Return to coord_guess.c CVS log

Jump to this file's LXR Page

Up to [CENS] / emstar / devel / loc / multilat

seems to mostly work...

/*
 *
 * Copyright (c) 2005 The Regents of the University of California.  All 
 * rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * - Redistributions of source code must retain the above copyright
 *   notice, this list of conditions and the following disclaimer.
 *
 * - Neither the name of the University nor the names of its
 *   contributors may be used to endorse or promote products derived
 *   from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS''
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 * PARTICULAR  PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
 * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */


#include "multilat_i.h"

QUEUE_FUNCTION_INSTANTIATIONS(guess_list,_,guesses,struct _coord_guess,struct _multilat_stages);





void coord_guess_add_or_update_neighbor(coord_guess_t *cgt, multilat_range_t *mrt) {
  /* check if have the neighbor first */
  int i = 0;
  for (i = 0; i < GUESS_MAX_NEIGHBORS; ++i) {
    if (cgt->neighs[i].inited == 0) {
      goto new;
    } else if (cgt->neighs[i].neighbor == mrt->chirp_from) {
      elog(LOG_ERR,"This should not be happening... duplicate in range list! (i=%d)  %d == %d",
	   i, cgt->neighs[i].neighbor, mrt->chirp_from);
      if (cgt->neighs[i].distance > mrt->distance) 
	goto up_distance;
      else
	goto out;
    }
  }
 new:
  cgt->neighs[i].inited = 1;
  cgt->total = cgt->total + 1;
 up_distance:
  cgt->neighs[i].distance = mrt->distance;
 out:
  cgt->neighs[i].neighbor = mrt->chirp_from;
  cgt->neighs[i].theta = mrt->theta;
  cgt->neighs[i].phi = mrt->phi;
  return;
}


int coord_guess_get_neighbor(coord_guess_t *cgt, uint32_t node) {
  int i = 0;
  for (i = 0; i < cgt->total; ++i) {
    if (cgt->neighs[i].neighbor == node) {
      return i;
    }
  }
  return -1;
}


/* lookup a coord_guess in the list, and figure out if */
coord_guess_t *coord_guess_get_init(ml_state_t *mls, uint32_t node, int init) {
  coord_guess_t *cgt = guess_list_top(mls->multilat_lists);
  for ( ; cgt != NULL; cgt = guess_list_next(cgt)) {
    if (cgt->node == node) {
      goto out;
    }
  }
  if (init) {
    cgt = g_new0(coord_guess_t, 1);
    cgt->node = node;
    guess_list_push(mls->multilat_lists, cgt);
  }
 out:
  return cgt;
}

void corrd_guess_initnode(ml_state_t *mls, uint32_t node) {
  coord_guess_get_init(mls, node, 1);
}

coord_guess_t *coord_guess_get(ml_state_t *mls, uint32_t node) {
  return coord_guess_get_init(mls, node, 0);
}


/* used to sort the cgt_array on total number of neighbors */
int sort_cgt_array_on_neighs(const void *a, const void *b) {
  return ((coord_guess_t *) b)->total - ((coord_guess_t *) a)->total;

  //  if (((coord_guess_t *) a)->total > ((coord_guess_t *) b)->total)
  //   return 1;
  //else if (((coord_guess_t *) a)->total > ((coord_guess_t *) b)->total) 
  // return -1;
  //  else if (((coord_guess_t *) a)->total == ((coord_guess_t *) b)->total)
  //return 0;
}




/* public functions */

void initialize_guess_stage(ml_state_t *mls) {
  
  multilat_range_t *mrt = range_list_top(mls->multilat_lists);
  
  for ( ; mrt != NULL; mrt = range_list_next(mrt)) {
    coord_guess_t *cgt = NULL;
    cgt = coord_guess_get_init(mls, mrt->data_from, 1);
    coord_guess_add_or_update_neighbor(cgt, mrt);
  }

  /* now keep only the short distances between two nodes */
#if 0  
  coord_guess_t *cgt = guess_list_top(mls->multilat_lists);
  coord_guess_t *cgt_temp = NULL;
  for ( ; cgt != NULL; cgt = guess_list_next(cgt)) {
    int i = 0;
    for ( i = 0; i < cgt->total; ++i ) {
      cgt_temp = coord_guess_find(mls, cgt->neigh[i].neighbor, 0);
      int loc = coord_guess_has_neighbor(cgt_temp, cgt->node);
      if (loc != -1) {
	/* set the distance to the shorter of the two */
	if (cgt->neigh[i].distance <= cgt_temp->neigh[loc].distance) {
	  cgt_temp->neigh[loc].distance = cgt->neigh[i].distance;
	} else {
	  cgt->neigh[i].distance = cgt_temp->neigh[loc].distance;
	}
	//	cgt_temp->neigh[loc].neighbor_cgt = cgt; /* carefull */
      }
//      cgt->neigh[i].neighbor_cgt = cgt_temp; /* carefull! */
    }
  }
#endif

}
 
#if 0
void fix_no_hear(ml_state_t *mls, uint32_t node) {

  coord_guess_t *cgt = coord_guess_get(mls, node);
  multilat_result_t *my_res = result_list_get(mls, node);
  int i = 0;
  mreal x = 0, y = 0, z = 0;
  for (i = 0; i < cgt->total; ++i) {
    multilat_result_t *neigh_coords = result_list_get(mls, cgt->neighs[i]);
    x = sinf(cgt->neighs[i].theta) - 

  }



}
#endif
/* VERY INEFFICIENT... but clear for now */
void calculate_coords_and_yaw(ml_state_t *mls, coord_guess_t *cgt) {

  multilat_result_t *listener_result = result_list_get(mls, cgt->node);

  if (listener_result == NULL) {
    elog(LOG_WARNING, "unable to look up result for node %d", cgt->node);
    return;
  }

  if (listener_result->total_heard_me == 0) {
    elog(LOG_WARNING, "no-one has heard me.. giving up");
    return;
  }

  //  elog(LOG_WARNING, "\n\nFind coords for listener: %d at (%f, %f, %f) %f", cgt->node, listener_result->x, listener_result->y, listener_result->z, listener_result->yaw);

  /* first find coords  and yaw of each of the neighs */
  int i = 0;
  for (i = 0; i < cgt->total; ++i) {
    multilat_result_t *chirper_result = result_list_get(mls, cgt->neighs[i].neighbor);

    if (chirper_result->state == RESULT_COORD_ROOT) {
      continue;
    }

#ifdef USEZ
    /* distance */
    chirper_result->x += cgt->neighs[i].distance 
      * cosf(listener_result->yaw + cgt->neighs[i].theta)
      * cosf(cgt->neighs[i].phi)
      + listener_result->x / (listener_result->total_heard_me + 0.0);
    
    chirper_result->y += cgt->neighs[i].distance 
      * sinf(listener_result->yaw + cgt->neighs[i].theta)
      * cosf(cgt->neighs[i].phi)
      + listener_result->y / (listener_result->total_heard_me + 0.0);

    chirper_result->z += cgt->neighs[i].distance 
      * sinf(cgt->neighs[i].phi)
      + listener_result->z / (listener_result->total_heard_me + 0.0);
#else
    /* distance */
    chirper_result->x += cgt->neighs[i].distance 
      * cosf(listener_result->yaw + cgt->neighs[i].theta)
      + listener_result->x / (listener_result->total_heard_me + 0.0);
    
    chirper_result->y += cgt->neighs[i].distance 
      * sinf(listener_result->yaw + cgt->neighs[i].theta)
      + listener_result->y / (listener_result->total_heard_me + 0.0);
#endif

    chirper_result->total_heard_me = chirper_result->total_heard_me + 1;
#if 0
    elog(LOG_WARNING, "& coords of node %d are: ", cgt->neighs[i].neighbor);
    elog(LOG_WARNING, "--- x = %f = dist %f * cosf(orr %f + theta %f) + cur x %f", 
	 chirper_result->x / chirper_result->total_heard_me, 
	 cgt->neighs[i].distance, listener_result->yaw,cgt->neighs[i].theta,
	 listener_result->x / (listener_result->total_heard_me + 0.0));   
    elog(LOG_WARNING, "--- y = %f = dist %f * sinf(orr %f + theta %f) + cur y %f",
	 chirper_result->y / chirper_result->total_heard_me,
	 cgt->neighs[i].distance, listener_result->yaw,cgt->neighs[i].theta,
	 listener_result->y / (listener_result->total_heard_me + 0.0));   
#endif

    /* yaw */
    coord_guess_t *chirper_cgt = coord_guess_get(mls, cgt->neighs[i].neighbor);
    if (chirper_cgt == NULL) {
      elog(LOG_CRIT, "oops!!! null cgt for neighbor %d -- skipping", i);
      continue;
    }
    int chirper_neigh_offset_listener = coord_guess_get_neighbor(chirper_cgt, cgt->node);

    if (chirper_neigh_offset_listener == -1 && chirper_result->state == RESULT_INITIALIZED) {
      chirper_result->state = RESULT_NO_YAW;
    } else if (chirper_result->state == RESULT_INITIALIZED ||
	       chirper_result->state == RESULT_NO_YAW) {
      chirper_result->yaw =
	misc_normalize_angle_rad(listener_result->yaw +
				 cgt->neighs[i].theta + M_PI
				 - chirper_cgt->neighs[chirper_neigh_offset_listener].theta);
#if 0
      elog(LOG_WARNING, "--- orr = %f = orr %f + thetac %f + pi - thetal %f",
	   chirper_result->yaw, listener_result->yaw, cgt->neighs[i].theta, 
	   chirper_cgt->neighs[chirper_neigh_offset_listener].theta);
#endif
      chirper_result->state = RESULT_YAW_COMPLETE;
    }
#if 1
    elog(LOG_WARNING, "Guessing node %d from neigh %d: %f, %f, %f  theta: %f   status %d", 
	 chirper_result->node, cgt->node, 
	 chirper_result->x, chirper_result->y,
	 chirper_result->z, chirper_result->yaw, chirper_result->state);
#endif
  }
  cgt->done_processing = 1;
  /* then call recursivly on cgt's that are not 'done' */
  for (i = 0; i < cgt->total; ++i) {
    coord_guess_t *chirper_cgt = coord_guess_get(mls, cgt->neighs[i].neighbor);
    if (chirper_cgt == NULL) {
      elog(LOG_CRIT, "oops2, n %d", i);
      continue;
    }
    if (!chirper_cgt->done_processing) {
      calculate_coords_and_yaw(mls, chirper_cgt);
    }
  }
}


int guess_coords(ml_state_t *mls) {
  
  /* first sort on total number of neighbors */
  int i = 0;
  int arr_size = guess_list_qlen(mls->multilat_lists);

  if (arr_size < 3) return -1;

  coord_guess_t *cgt = guess_list_top(mls->multilat_lists);

  coord_guess_t *(cgt_arrayview[arr_size]);
  int first = -1;
  int second = -1;
  /* build an array out of the list so we can sort and then iterate */
  for ( i = 0; i < arr_size; ++i) {
    cgt_arrayview[i] = cgt;
    if (cgt->node == mls->anode1) {
      //      elog(LOG_WARNING, "fixed node %i at %i", mls->anode1, i);
      multilat_result_t *temp_res = result_list_get(mls, mls->anode1);
      temp_res->state = RESULT_COORD_ROOT;
      temp_res->total_heard_me = 1;
      temp_res->x = mls->x1;
      temp_res->y = mls->y1;
      temp_res->z = mls->z1;
      temp_res->yaw = mls->a1;
      cgt->done_processing = 1;
      first = i;
    }
    if (cgt->node == mls->anode2) {
      //      elog(LOG_WARNING, "fixed node %i at %i", mls->anode2, i);
      multilat_result_t *temp_res = result_list_get(mls, mls->anode2);
      temp_res->state = RESULT_COORD_ROOT;
      temp_res->total_heard_me = 1;
      temp_res->x = mls->x2;
      temp_res->y = mls->y2;
      temp_res->z = mls->z2;
      temp_res->yaw = mls->a2;
      cgt->done_processing = 1;
      second = i;
    }    
    cgt = guess_list_next(cgt);    
  }
  if (first == -1 && second == -1) {
    qsort(cgt_arrayview, arr_size, sizeof(coord_guess_t *),
	  sort_cgt_array_on_neighs);
  }
  
  /* mark the first node at 0,0,0 */  
  if (first == -1 && second == -1) {
    elog(LOG_WARNING, "Marking %i as 0,0,0", cgt_arrayview[0]->node);
    multilat_result_t *temp_res = result_list_get(mls, cgt_arrayview[0]->node);
    temp_res->state = RESULT_COORD_ROOT;
    temp_res->total_heard_me = 1;
    cgt_arrayview[0]->done_processing = 1;
  }

  /* now SOHCAHTOA until you drop */
  if (first != -1) {
        elog(LOG_WARNING, "Wroking on fixed node %i", cgt_arrayview[first]->node);
    calculate_coords_and_yaw(mls,cgt_arrayview[first]);
  }
  if (second != -1) {
        elog(LOG_WARNING, "Wroking on fixed node %i", cgt_arrayview[second]->node);
    calculate_coords_and_yaw(mls,cgt_arrayview[second]);
  }
  if (first == -1 && second == -1) {
    calculate_coords_and_yaw(mls,cgt_arrayview[0]);
  }

  for ( i = 0 ; i < arr_size; ++i) {
        elog(LOG_WARNING, "\n\nTop level processing %d\n", cgt_arrayview[i]->node);
    if (!cgt_arrayview[i]->done_processing) {
      calculate_coords_and_yaw(mls, cgt_arrayview[i]);
    } else {
      elog(LOG_WARNING, "skipping\n");
    }
  }
  
  /* now average out all the resulting positions */
  multilat_result_t *res = result_list_top(mls->multilat_lists);
  for ( ; res != NULL; res = result_list_next(res)) {
    if (res->total_heard_me != 0) {
      res->x = res->x / (res->total_heard_me + 0.0);
      res->y = res->y / (res->total_heard_me + 0.0);
      res->z = res->z / (res->total_heard_me + 0.0);
    } else {
      elog(LOG_WARNING, "No one has heard from node %d", res->node);
      //      fix_no_hear(mls, res->node);
      res->x = 0;
      res->y = 0;
      res->z = 0;
      res->yaw = 0;
      /*
	res->x = 700;
	res->y = 5500;
	res->z = 0;
	res->yaw = M_PI;
      */
    }
    res->col = -1;
  }

  return 1;

}