Return to multilat.c CVS log

Jump to this file's LXR Page

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

modifications needed for rerunning data from marmots demo

/*
 *
 * 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 <stdio.h>
#include <math.h>
#include <emrun/emrun.h>
#include <libmisc/misc.h>
#include "multilat_i.h"
#include <gsl/gsl_multifit_nlin.h>
#include <gsl/gsl_blas.h>



QUEUE_FUNCTION_INSTANTIATIONS(range_list,_,ranges,multilat_range_t,struct _multilat_stages);

float killpairs_prob = 0;

#define PHI_DIST_VARIATION 6.0
#define PSIG  d2r(3.5/2.0)
#define TSIG  d2r(0.96/2.0)
#define MAX_BIDIR_DIFF  300
#define MIN_BIDIR_DIFF  5
#undef AVERAGE_SMALL
#undef EMBEDDED_USECONF

int isnormal(double d)
{
  if (isnan(d)) return 0;
  if (isinf(d)) return 0;
  return 1;
}

double misc_normalize_angle_rad_d(double rad_angle)
{
  double result = fmod(rad_angle, 2.0 * M_PI);
  if (result > M_PI) result -= (2.0 * M_PI);
  else if (result < - M_PI) result += (2.0 * M_PI);
  return result;
}

inline float m_pow_2(float x) { return x*x; }

void multilatbuf_to_file(ml_state_t *mls, char *filename, buf_t *buf) {
  if (!mls->logs_off) {
    //  int thetime = time(NULL);
    char f[4096];
    //  sprintf(f, "/tmp/%s-%d", filename, thetime);
    sprintf(f, "%s/%04i-%s", mls->outputprefix, mls->global_run, filename);
    elog(LOG_WARNING, "outputing to %s", f);
    buf_to_file(f, buf);
  }
  else 
    elog(LOG_WARNING, "suppressing log file output..");
}


void print_matrix(buf_t *buf, gsl_matrix *m) {
  int i = 0;
  int j = 0;
  for (i = 0; i < m->size1; ++i) {
    for (j = 0; j < m->size2; ++j) {
      bufprintf(buf, "%f ", gsl_matrix_get(m, i, j));
    }
    bufprintf(buf, "\n");
  }
}
void print_vector(buf_t *buf, gsl_vector *v) {
  int i = 0;
  for (i = 0; i < v->size; ++i) {
    bufprintf(buf, "%f ", gsl_vector_get(v, i));
  }
  bufprintf(buf, "\n");
}

void print_multitlat_state(ml_state_t *mls,
			   gsl_matrix *A,
			   gsl_matrix *new_A,
			   gsl_matrix *V,
			   gsl_vector *s,
			   gsl_vector *b,
			   gsl_vector *x, 
			   int state)
{
  buf_t *buf = buf_new();
  char file[4096];

  bufprintf(buf, "Current State:\n");
  bufprintf(buf, "A was:\n");
  print_matrix(buf, A);
  bufprintf(buf, "\n\n");
	     
  bufprintf(buf, "A is:\n");
  print_matrix(buf, new_A);

  bufprintf(buf, "\nV is:\n");
  print_matrix(buf, V);
	     
  bufprintf(buf,"\n\n s is:\n");
  print_vector(buf, s);

  bufprintf(buf,"\n\n b is:\n");
  print_vector(buf, b);

  bufprintf(buf,"\n\n x is:\n");
  print_vector(buf, x);
  bufprintf(buf, "\n\n\n");
  
  sprintf(file, "state%i", state);
  multilatbuf_to_file(mls, file, buf);
  buf_free(buf);
}


struct aw {
  mreal angle;
  mreal weight;
  mreal total;
};
struct aw angles[100];
int angle_count;

void init_cm()
{
  angle_count = 0;
}

void add_cm(mreal theta, mreal weight)
{
  angles[angle_count].angle = misc_normalize_angle_rad_d(theta);
  angles[angle_count].weight = weight;
  angles[angle_count].total = 0;
  angle_count++;
}

int aw_cmp(const void *a,const void *b)
{
  return ((struct aw *)a)->angle - ((struct aw *)b)->angle;
}

int realcmp(const void *a,const void *b)
{
  mreal cmp = *((mreal *)a) - *((mreal *)b);
  if (cmp < 0) return -1;
  if (cmp > 0) return 1;
  return 0;
}

mreal finish_cm()
{
  qsort(angles, angle_count, sizeof(struct aw), aw_cmp);

  int i;
  float max_weight = 0;
  int max_w_index = -1;
  for (i=0; i<angle_count; i++) {
    int j;
    for (j=0; j<angle_count; j++) 
      if (fabs(misc_normalize_angle_rad_d(angles[j].angle - angles[i].angle)) < d2r(10)) 
	angles[i].total += angles[j].weight;
    if (angles[i].total > max_weight || max_w_index < 0) {
      max_weight = angles[i].total;
      max_w_index = i;
    }
  }

  if (max_w_index >= 0) {
    mreal t[2] = {0,0};

    int j;
    for  (j=0; j<angle_count; j++) {
      if (fabs(misc_normalize_angle_rad_d(angles[j].angle - angles[max_w_index].angle)) < d2r(10)) {
	t[0] += angles[j].weight*cos(angles[j].angle);
	t[1] += angles[j].weight*sin(angles[j].angle);
      }
    }

    return atan2(t[1], t[0]);
  }
  
  elog(LOG_WARNING, "couldnt compute sloppy mode");
  return 0;
}

struct error_stats {
  mreal avg_range_diff;
  mreal rms_range_diff;
  mreal avg_theta_diff;
  mreal rms_theta_diff;
  mreal avg_phi_diff;
  mreal rms_phi_diff;
  int range_count;
  int theta_count;
  int phi_count;
};


buf_t *mlat_errors_to_buf(ml_state_t *mls, struct error_stats *stats)
{
  mreal avg_range_diff = 0;
  mreal rms_range_diff = 0;
  mreal avg_theta_diff = 0;
  mreal rms_theta_diff = 0;
  mreal avg_phi_diff = 0;
  mreal rms_phi_diff = 0;
  int range_count = 0;
  int theta_count = 0; 
  int phi_count = 0;
  multilat_range_t *mtr;

  buf_t *err_summ = buf_new();
  bufprintf(err_summ, "#h chirper receiver r r_diff user t terr uset p perr usep yaw pitch roll\n");
  
  for (mtr = range_list_top(mls->multilat_lists) ;
       mtr != NULL; mtr = range_list_next(mtr)) 
    if (mtr->distance > 0) {
      
      multilat_result_t * mrr1 = result_list_get(mls, mtr->chirp_from);
      multilat_result_t * mrr2 = result_list_get(mls, mtr->data_from);
      
      mreal dist = sqrt(sqrf(mrr1->x - mrr2->x) +
			sqrf(mrr1->y - mrr2->y) +
			sqrf(mrr1->z - mrr2->z));
      
      mreal phi = atan2(mrr1->z - mrr2->z,
			sqrt(sqrf(mrr1->x - mrr2->x) +
			     sqrf(mrr1->y - mrr2->y)));
      
      mreal theta = atan2(mrr1->y - mrr2->y, mrr1->x - mrr2->x);
      
      mreal rerr = (mtr->userange != 0) ? mtr->userange - dist :
	mtr->distance - dist;
      mreal perr = misc_normalize_angle_rad_d(mtr->phi) - 
	misc_normalize_angle_rad_d(phi);
      mreal terr = 
	misc_normalize_angle_rad_d
	(misc_normalize_angle_rad_d(mtr->theta) -
	 misc_normalize_angle_rad_d(mrr2->yaw) -
	 misc_normalize_angle_rad_d(theta));
      
      bufprintf(err_summ, "%d\t%d\t%f\t%f\t%c\t",
		mtr->chirp_from, mtr->data_from, mtr->distance, rerr, 
		(mtr->state == RANGE_DROPPED || mtr->drop_r) ? 'N' : 
#ifdef RTHETA
		'Y'
#else
		((mtr->use_r) ? 'Y' : 'M') 
#endif
		);
      
#ifdef RTHETA
      if (!(mtr->state == RANGE_DROPPED || mtr->drop_r))
#else
      if (mtr->use_r) 
#endif
	{
	  range_count++;
	  avg_range_diff += fabs(rerr);
	  rms_range_diff += sqrf(rerr);
	}

#ifdef RTHETA
      if (mtr->state != RANGE_DROPPED)
#else
      if (mtr->use_t) 
#endif
	{
	  avg_theta_diff += fabs(terr);
	  rms_theta_diff += sqrf(terr);
	  theta_count++;
	  bufprintf(err_summ, "%f\t%f\tY\t", r2d(mtr->theta), r2d(terr));
	}
      else bufprintf(err_summ, "%f\t%f\t%c\t", r2d(mtr->theta), r2d(terr),
		     (mtr->state == RANGE_DROPPED || mtr->drop_t) ? 'N' : 'M');
#ifdef RTHETA
      if (mtr->state != RANGE_DROPPED)
#else
      if (mtr->use_p) 
#endif
	{
	  phi_count++;
	  avg_phi_diff += fabs(perr);
	  rms_phi_diff += sqrf(perr);
	  bufprintf(err_summ, "%f\t%f\tY\t", r2d(mtr->phi), r2d(perr));
	}	
      else bufprintf(err_summ, "%f\t%f\t%c\t", r2d(mtr->phi), r2d(perr),
		     (mtr->state == RANGE_DROPPED || mtr->drop_p) ? 'N' : 'M');
      bufprintf(err_summ, "%f\t%f\t%f\n", r2d(-mrr2->yaw), r2d(-mrr2->pitch), r2d(-mrr2->roll));
    }

  if (stats) {
    stats->avg_range_diff = avg_range_diff;
    stats->rms_range_diff = rms_range_diff;
    stats->avg_theta_diff = avg_theta_diff;
    stats->rms_theta_diff = rms_theta_diff;
    stats->avg_phi_diff = avg_phi_diff;
    stats->rms_phi_diff = rms_phi_diff;
    stats->range_count = range_count;
    stats->theta_count = theta_count;
    stats->phi_count = phi_count;
  }

  return err_summ;
}

  
float last_drop_value = 0;
float min_drop_value = -1;
float min_xyz = -1;
float min_xyz_resid = 0;
int min_xyz_passes = 0;
int iterations = 0;

void dump_errors(ml_state_t *mls, int k)
{
  buf_t *err_summ = mlat_errors_to_buf(mls, NULL);
  char thefile[4096];
  sprintf(thefile, "err-%d", k);
  multilatbuf_to_file(mls, thefile, err_summ);
  buf_free(err_summ);
}


/* compare the results to the ground truth.. */
void analyse_result(ml_state_t *mls)
{
  /* compute raw average differences */
  mreal avg_range_diff = 0;
  mreal rms_range_diff = 0;
  mreal avg_theta_diff = 0;
  mreal rms_theta_diff = 0;
  mreal avg_phi_diff = 0;
  mreal rms_phi_diff = 0;
  int range_count = 0;
  int theta_count = 0; 
  int phi_count = 0;

  struct error_stats stats = {};
  buf_t *err_summ = mlat_errors_to_buf(mls, &stats);

  avg_range_diff = stats.avg_range_diff;
  rms_range_diff = stats.rms_range_diff;
  avg_theta_diff = stats.avg_theta_diff;
  rms_theta_diff = stats.rms_theta_diff;
  avg_phi_diff = stats.avg_phi_diff;
  rms_phi_diff = stats.rms_phi_diff;
  range_count = stats.range_count;
  theta_count = stats.theta_count;
  phi_count = stats.phi_count;  
  
  if (range_count > 0) {
    avg_range_diff /= (mreal)range_count;
    avg_theta_diff /= (mreal)theta_count;
    avg_phi_diff /= (mreal)phi_count;
    rms_range_diff = sqrt(rms_range_diff / (mreal)range_count);
    rms_theta_diff = sqrt(rms_theta_diff / (mreal)theta_count);
    rms_phi_diff = sqrt(rms_phi_diff / (mreal)phi_count);
    elog(LOG_WARNING, "AVG:  r=%f t=%f p=%f",
	 avg_range_diff, r2d(avg_theta_diff), r2d(avg_phi_diff));
    elog(LOG_WARNING, "RMS:  r=%f t=%f p=%f",
	 rms_range_diff, r2d(rms_theta_diff), r2d(rms_phi_diff));
    
  }

  char thefile[4096];
  sprintf(thefile, "final-err");
  multilatbuf_to_file(mls, thefile, err_summ);
  buf_free(err_summ);

#ifdef PROCRUSTES

  /* compute centroid */

  mreal cG[3] = {0,0,0};
  mreal c[3] = {0,0,0};

  multilat_result_t *node1, *node2;
  
  int count = 0;
  
  for (node1 = result_list_top(mls->multilat_lists);
       node1; node1 = result_list_next(node1)) {

    if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) {
      elog(LOG_WARNING, "skipping node %d that isn't in the matrix..", node1->node);
      continue;
    }

    node1->X[0] = node1->x;
    node1->X[1] = node1->y;
    node1->X[2] = node1->z;

    node1->XG[0] = node1->real_x;
    node1->XG[1] = node1->real_y;
    node1->XG[2] = node1->real_z;

    count++;
    for (i=0; i<3; i++) {
      c[i] += X[i];
      cG[i] += XG[i];
    }
  }

  for (i=0; i<3; i++) {
    c[i] /= (mreal)count;
    cG[i] /= (mreal)count;
  }

  /* compute centroid sizes */
  
  mreal S = 0;
  mreal SG = 0;

  for (node1 = result_list_top(mls->multilat_lists);
       node1; node1 = result_list_next(node1)) {

    if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) {
      elog(LOG_WARNING, "skipping node %d that isn't in the matrix..", node1->node);
      continue;
    }

    for (i=0; i<3; i++) {
      S += sqrf(node1->X[i] - c[i]);
      SG += sqrf(node1->XG[i] - cG[i]);
    }
  }

  S = sqrt(S);
  SG = sqrt(SG);

  /* pre-shape */
  
  for (node1 = result_list_top(mls->multilat_lists);
       node1; node1 = result_list_next(node1)) {

    if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) {
      elog(LOG_WARNING, "skipping node %d that isn't in the matrix..", node1->node);
      continue;
    }

    for (i=0; i<3; i++) {
      node1->X[i] = (node1->X[i] - c[i]) / S;
      node1->XG[i] = (node1->XG[i] - cG[i]) / SG;
    }
  }

  /* rotations.. */
  /* $$$ */


#else
  /* compute scaling factor */
  mreal tot_comp = 1.0;
  mreal tot_gt = 1.0;
  mreal V;

  multilat_result_t *node1, *node2;
  
  mreal c[3] = {0,0,0};
  mreal cG[3] = {0,0,0};
  int count = 0;
  
  for (node1 = result_list_top(mls->multilat_lists);
       node1; node1 = result_list_next(node1)) {

    if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) {
      elog(LOG_WARNING, "skipping node %d that isn't in the matrix..", node1->node);
      continue;
    }

    if (node1->nogt) {
      elog(LOG_WARNING, "skipping node %d, no gt..", node1->node);
      continue;
    }

    count++;
    c[0] += node1->x;
    c[1] += node1->y;
    c[2] += node1->z;

    cG[0] += node1->real_x;
    cG[1] += node1->real_y;
    cG[2] += node1->real_z;

    mreal nD1=-1.0, nD2=-1.0;
    
    for (node2 = result_list_top(mls->multilat_lists);
	 node2; node2 = result_list_next(node2)) {

      if (node2->col < 0 && node2->state != RESULT_COORD_ROOT) continue;
      if (node2->nogt) continue;

      if (node1 != node2) {
	mreal D1 = sqrt
	  (sqrf(node1->x - node2->x) +
	   sqrf(node1->y - node2->y) +
	   sqrf(node1->z - node2->z));

	mreal D2 = sqrt
	  (sqrf(node1->real_x - node2->real_x) +
	   sqrf(node1->real_y - node2->real_y) +
	   sqrf(node1->real_z - node2->real_z));

	if (nD1 < 0 || D1 < nD1) {
	  nD1 = D1;
	  nD2 = D2;
	}
      }
    }

    if (nD1 > 0) {
      tot_comp += nD1;
      tot_gt += nD2;
    }
  }

  V = tot_comp / tot_gt;

  /* centroid */
  c[0] /= (mreal)count;
  c[1] /= (mreal)count;
  c[2] /= (mreal)count;
  
  cG[0] /= (mreal)count;
  cG[1] /= (mreal)count;
  cG[2] /= (mreal)count;

#ifndef ROTATE_ABOUT_CENTROID
  mreal min_dist = -1;

  /* find center point */
  for (node1 = result_list_top(mls->multilat_lists);
       node1; node1 = result_list_next(node1)) {

    if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
    if (node1->nogt) continue;

    mreal D1 = sqrt
      (sqrf(node1->x - c[0]) +
       sqrf(node1->y - c[1]) +
       sqrf(node1->z - c[2]));
    if (min_dist < 0 || min_dist > D1) {
      min_dist = D1;
      node2 = node1;
    }
  }

  /* node2 is closest to centroid */
  elog(LOG_WARNING, "centroid is node %d", node2->node);

  /* translate to center */
  for (node1 = result_list_top(mls->multilat_lists);
       node1; node1 = result_list_next(node1)) {

    if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;

    if (node1 != node2) {
      node1->tx = node1->x - node2->x;
      node1->ty = node1->y - node2->y;
      node1->tz = node1->z - node2->z;
      
      node1->trx = node1->real_x - node2->real_x;
      node1->try = node1->real_y - node2->real_y;
      node1->trz = node1->real_z - node2->real_z;
    }
  }

  node2->tx = 0;
  node2->ty = 0;
  node2->tz = 0;

  node2->trx = 0;
  node2->try = 0;
  node2->trz = 0;
#else
  /* translate to centroid */
  for (node1 = result_list_top(mls->multilat_lists);
       node1; node1 = result_list_next(node1)) {

    if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;

    node1->tx = node1->x - c[0];
    node1->ty = node1->y - c[1];
    node1->tz = node1->z - c[2];
      
    node1->trx = node1->real_x - cG[0];
    node1->try = node1->real_y - cG[1];
    node1->trz = node1->real_z - cG[2];
  }
  node2=NULL;
#endif

  /* scale and init tyaw */
  for (node1 = result_list_top(mls->multilat_lists);
       node1; node1 = result_list_next(node1)) {

    if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;

    node1->tx /= V;
    node1->ty /= V;
    node1->tz /= V;
  }

  mreal rot;
  mreal cumrot = 0;

  goto retry_z;

 flip:
  /* FLIP */
  elog(LOG_WARNING, "Flipped!");
  cumrot = M_PI-cumrot;

 retry_z:

#ifdef USEZ
  init_cm();

  /* determine Z axis rotation */
  for (node1 = result_list_top(mls->multilat_lists);
       node1; node1 = result_list_next(node1)) {

    if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
    if (node1->nogt) continue;

    if (node1 != node2) {
      mreal D1 = sqrt(sqrf(node1->tx) + sqrf(node1->ty));

      mreal dt = 
	atan2(node1->ty, node1->tx) -
	atan2(node1->try, node1->trx);

      add_cm(dt, D1);
    }
  }

  rot = -finish_cm();
  
  elog(LOG_WARNING, "Z axis rotation = %f", r2d(rot));

#endif

  /* rotate */
  for (node1 = result_list_top(mls->multilat_lists);
       node1; node1 = result_list_next(node1)) {

    if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) {
      elog(LOG_WARNING, "node %d is a root?", node1->node);
      continue;
    }

    if (node1 != node2) {
      mreal tmpx = node1->tx;
      mreal tmpy = node1->ty;
      node1->tx = (cos(rot)*tmpx - sin(rot)*tmpy);
      node1->ty = (sin(rot)*tmpx + cos(rot)*tmpy);
    }
  } 

  cumrot += rot;

  /* does our ground truth have a z axis? */
  int have_gtz = 0;
  for (node1 = result_list_top(mls->multilat_lists);
       node1; node1 = result_list_next(node1)) {

    if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
    if (node1->nogt) continue;

    if (node1->real_z != 0) {
      have_gtz = 1;
      break;
    }
  }

  if (have_gtz) {
    
    /* determine Y axis rotation */
    
    init_cm();
    
    for (node1 = result_list_top(mls->multilat_lists);
	 node1; node1 = result_list_next(node1)) {
      
      if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
      if (node1->nogt) continue;
      
      if (node1 != node2) {
	mreal D1 = sqrt(sqrf(node1->tx) + sqrf(node1->tz));
	
	mreal dt = 
	  atan2(node1->tz, node1->tx) -
	  atan2(node1->trz, node1->trx);
	
	add_cm(dt, D1);
      }
    }
    
    rot = -finish_cm();

    elog(LOG_WARNING, "Y axis rotation = %f", r2d(rot));
      
    /* rotate */
    for (node1 = result_list_top(mls->multilat_lists);
	 node1; node1 = result_list_next(node1)) {
      
      if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;

      if (node1 != node2) {
	mreal tmpx = node1->tx;
	mreal tmpz = node1->tz;
	node1->tx = (cos(rot)*tmpx - sin(rot)*tmpz);
	node1->tz = (sin(rot)*tmpx + cos(rot)*tmpz);
      }
    }
 
    if (r2d(rot) < -160 || r2d(rot) > 160) {
      goto flip;
    }
    
    /* determine X axis rotation */
    
    init_cm();
    
    for (node1 = result_list_top(mls->multilat_lists);
	 node1; node1 = result_list_next(node1)) {
      
      if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
      if (node1->nogt) continue;

      if (node1 != node2) {
	mreal D1 = sqrt(sqrf(node1->tz) + sqrf(node1->ty));
	
	mreal dt = 
	  atan2(node1->ty, node1->tz) -
	  atan2(node1->try, node1->trz);
	
	add_cm(dt, D1);
      }
    }
    
    rot = -finish_cm();

    elog(LOG_WARNING, "X axis rotation = %f", r2d(rot));
          
    /* rotate */
    for (node1 = result_list_top(mls->multilat_lists);
	 node1; node1 = result_list_next(node1)) {
      
      if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;

      if (node1 != node2) {
	mreal tmpz = node1->tz;
	mreal tmpy = node1->ty;
	node1->tz = (cos(rot)*tmpz - sin(rot)*tmpy);
	node1->ty = (sin(rot)*tmpz + cos(rot)*tmpy);
      }
    }

    if (r2d(rot) < -160 || r2d(rot) > 160) {
      goto flip;
    }
  }    

  /* add in original yaw */
  for (node1 = result_list_top(mls->multilat_lists);
       node1; node1 = result_list_next(node1)) {
    node1->tyaw = -node1->yaw + cumrot;
  }
  
  /* average translation */
  mreal tr[3] = {0,0,0};
  for (node1 = result_list_top(mls->multilat_lists);
       node1; node1 = result_list_next(node1)) {

    if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
    if (node1->nogt) continue;

    mreal D = sqrt(sqrf(node1->trx-node1->tx) +
		   sqrf(node1->try-node1->ty) +
		   sqrf(node1->trz-node1->tz));
   
    /* only add into average if < 1 meter away */
    if (D < 100) {
      tr[0] += node1->trx-node1->tx;
      tr[1] += node1->try-node1->ty;
      tr[2] += node1->trz-node1->tz;
    }
  }

  tr[0] /= (mreal)count;
  tr[1] /= (mreal)count;
  tr[2] /= (mreal)count;

  /* adjust nodes for translation */
  for (node1 = result_list_top(mls->multilat_lists);
       node1; node1 = result_list_next(node1)) {

    if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;

    node1->tx += tr[0];
    node1->ty += tr[1];
    node1->tz += tr[2];
  }
#endif

  /* compute the avg dist */
  mreal avg_dist = 0;
  mreal rms_dist = 0;
  mreal avg_zdist = 0;
  mreal rms_zdist = 0;
  mreal worst_dist = 0;
  mreal avg_zdist_d1 = 0;

  mreal avg_yaw_err = 0;

  mreal distarr[100];
  mreal zdistarr[100];
  mreal yawarr[100];
  int distcount = 0;

  buf_t *xybuf = buf_new();
  buf_t *xyzbuf = buf_new();
  buf_t *obuf = buf_new();

  for (node1 = result_list_top(mls->multilat_lists);
       node1; node1 = result_list_next(node1)) {

    if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
    if (node1->nogt) continue;

    mreal DZ = sqrt
      (sqrf(node1->tx - node1->trx) +
       sqrf(node1->ty - node1->try) +
       sqrf(node1->tz - node1->trz));

    mreal DXY = sqrt
      (sqrf(node1->tx - node1->trx) +
       sqrf(node1->ty - node1->try));

    avg_dist += DXY;
    rms_dist += sqrf(DXY);
    
    avg_zdist += DZ;
    rms_zdist += sqrf(DZ);
    
    distarr[distcount] = DXY;
    zdistarr[distcount] = DZ;

    if (DZ > worst_dist) worst_dist = DZ;

    mreal yaw_err = fmod(node1->real_yaw - r2d(node1->tyaw), 360.0);
    if (yaw_err > 180) yaw_err -= 360.0;
    if (yaw_err < -180) yaw_err += 360.0;
    
    avg_yaw_err += fabs(yaw_err);
    yawarr[distcount] = fabs(yaw_err);
    distcount++;
 
    elog(LOG_WARNING, "dist %d %f, yawerr=%f", node1->node, DXY, yaw_err);

    bufprintf(xybuf, "%f %d\n", DXY, node1->node);
    bufprintf(xyzbuf, "%f %d\n", DZ, node1->node);
    bufprintf(obuf, "%f %d\n", yaw_err, node1->node);

  }

  qsort(distarr, distcount, sizeof(mreal), realcmp);
  qsort(zdistarr, distcount, sizeof(mreal), realcmp);
  qsort(yawarr, distcount, sizeof(mreal), realcmp);

  mreal median_dist = distarr[distcount/2];
  mreal median_zdist = zdistarr[distcount/2];
  mreal median_yaw = yawarr[distcount/2];

#if 0
  elog(LOG_WARNING, "distcount=%d, medianyaw=%f", distcount, median_yaw);
  int klkl;
  for (klkl=0;klkl<distcount;klkl++)
    elog(LOG_WARNING, "  yaw %d %f", klkl, yawarr[klkl]);
#endif

  avg_dist /= (mreal)count;
  rms_dist = sqrt(rms_dist / (mreal)count);
  avg_yaw_err/= (mreal)count;
  
  avg_zdist_d1 = (avg_zdist - worst_dist) / (mreal)(count-1);

  avg_zdist /= (mreal)count;
  rms_zdist = sqrt(rms_zdist / (mreal)count);

  elog(LOG_WARNING, " V = %f", V);
  elog(LOG_WARNING, " DIST = %f, RMS = %f, median = %f", avg_dist, rms_dist, median_dist);
  elog(LOG_WARNING, " ZDIST = %f, RMS = %f, median = %f", avg_zdist, rms_zdist, median_zdist);
  elog(LOG_WARNING, " YAWERR = %f", avg_yaw_err);

  /* print out */
  buf_t *firstpass = buf_new();
  sprintf(thefile, "final");
  bufprintf(firstpass, "#h node x y z yaw roll pitch gt_x gt_y gt_z\n");
  multilat_result_t *rlt = result_list_top(mls->multilat_lists);
  for ( ; rlt != NULL; rlt = result_list_next(rlt) ) {

    if (rlt->col < 0 && rlt->state != RESULT_COORD_ROOT) continue;

    bufprintf(firstpass, "%d\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n", rlt->node, rlt->tx, rlt->ty, rlt->tz, 
	      r2d(rlt->tyaw), r2d(-rlt->roll), r2d(-rlt->pitch), 
	      rlt->trx, rlt->try, rlt->trz);
  }

  if (min_drop_value < 0 || last_drop_value < min_drop_value) min_drop_value = last_drop_value;
  if (min_xyz < 0 || avg_zdist < min_xyz) {
    min_xyz = avg_zdist;
    min_xyz_resid = min_drop_value;
    min_xyz_passes = mls->global_run;
  }

  bufprintf(firstpass, "# V = %f\n", V);
  bufprintf(firstpass, "# avg_pos_err(xy)  = %f, RMS = %f, med %f\n", avg_dist, rms_dist, median_dist);
  bufprintf(firstpass, "# avg_pos_err(xyz) = %f, RMS = %f, med %f\n", avg_zdist, rms_zdist, median_zdist);
  bufprintf(firstpass, "# avg_constraint_err:  r=%f t=%f p=%f\n",
	    avg_range_diff, r2d(avg_theta_diff), r2d(avg_phi_diff));
  bufprintf(firstpass, "# rms_constraint_err:  r=%f t=%f p=%f\n",
	    rms_range_diff, r2d(rms_theta_diff), r2d(rms_phi_diff));
  bufprintf(firstpass, "# yaw_avg %f median %f \n",
	    avg_yaw_err, median_yaw);

  bufprintf(firstpass, "##%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%d\t%f\t%f\t%d\t%d\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n\n",
	    V, avg_dist, avg_zdist, avg_range_diff, r2d(avg_theta_diff), r2d(avg_phi_diff),
	    last_drop_value, min_drop_value, count,
	    min_xyz, min_xyz_resid, min_xyz_passes, iterations, avg_zdist_d1,
	    median_dist, median_zdist, avg_yaw_err, median_yaw,
	    killpairs_prob, distarr[distcount-1]
	    );
  multilatbuf_to_file(mls, thefile, firstpass);
  buf_free(firstpass);  

  multilatbuf_to_file(mls, "3d-dist", xyzbuf);
  multilatbuf_to_file(mls, "2d-dist", xybuf);
  multilatbuf_to_file(mls, "yaw-dist", obuf);
  buf_free(xyzbuf);
  buf_free(xybuf);
  buf_free(obuf);
}


#ifdef RTHETA

void update_minmax(mreal *min, mreal *max, mreal value)
{
  if (value < *min) *min = value;
  else if (value > *max) *max = value;
}

mreal do_xweight(multilat_range_t *mtr)
{
  mreal min=0;
  mreal max=0;
  min = max = cos(mtr->theta + TSIG)*cos(mtr->phi + PSIG);
  update_minmax(&min,&max,cos(mtr->theta - TSIG)*cos(mtr->phi + PSIG));
  update_minmax(&min,&max,cos(mtr->theta + TSIG)*cos(mtr->phi - PSIG));
  update_minmax(&min,&max,cos(mtr->theta - TSIG)*cos(mtr->phi - PSIG));
  
  mreal w = sqrt(mtr->distance * (max-min));
  if (w == 0) return 0;
  return 1.0/w;
}

mreal do_yweight(multilat_range_t *mtr)
{
  mreal min=0;
  mreal max=0;
  min = max = sin(mtr->theta + TSIG)*cos(mtr->phi + PSIG);
  update_minmax(&min,&max,sin(mtr->theta - TSIG)*cos(mtr->phi + PSIG));
  update_minmax(&min,&max,sin(mtr->theta + TSIG)*cos(mtr->phi - PSIG));
  update_minmax(&min,&max,sin(mtr->theta - TSIG)*cos(mtr->phi - PSIG));
  
  mreal w = sqrt(mtr->distance * (max-min));
  if (w == 0) return 0;
  return 1.0/w;
}

mreal do_zweight(multilat_range_t *mtr)
{
  mreal min=0;
  mreal max=0;
  min = max = sin(mtr->phi + PSIG);
  update_minmax(&min,&max,sin(mtr->phi + PSIG));
  update_minmax(&min,&max,sin(mtr->phi - PSIG));
  update_minmax(&min,&max,sin(mtr->phi - PSIG));
  
  mreal w = sqrt(mtr->distance * (max-min));
  if (w == 0) return 0;
  return 1.0/w;
}

void multilat_solver(ml_state_t *mls) {

  int i = 0;
  
  /* the result_view array is the mapping of a 0 to num_nodes index of
     all the results... it gives us a convienient way to address the
     nodes without having to deal with incosistent node id's */
  int num_nodes = result_list_qlen(mls->multilat_lists);
  multilat_result_t *(result_view[num_nodes]);
  result_list_make_array(mls, result_view, num_nodes);

#ifdef USEZ
  int axes = 3;
#else
  int axes = 2;
#endif

  /* row is chirp_from, column is data_from */

  int num_ranges = range_list_qlen(mls->multilat_lists);
  multilat_range_t *mtr;
  int total_dups = 0;
  
  int runagain = 0;
  int dropped_rows = 0;
  int num_roots = 0;
  
  buf_t *droppedbuf = buf_new();

  /******************/
  /* get dimensions */
  /******************/
 repeate:

  if (mls->global_run != 0) {
    result_load(mls);
  }

  num_roots = 0;
  for (i = 0; i < num_nodes; ++i) {
    if (result_view[i]->state == RESULT_COORD_ROOT)
      num_roots++;
  }

  /* $$$ this would change with tiedowns.. not really right anyway */
  int num_rows = (num_ranges - total_dups - (num_roots*(num_roots-1)))*axes - dropped_rows;
  int num_cols = (num_nodes - num_roots)*axes;

  elog(LOG_CRIT, "total ranges %i, dropped %i, rows,cols= %i,%i", num_ranges, dropped_rows, 
       num_rows, num_cols);
  
  if (num_cols > num_rows) {
    elog(LOG_CRIT, "Can not continue with SVD only have %i by %i matrix", num_rows, num_cols);
    exit(1);
  }

  //mreal VV = 1;

  /***********/
  /* iterate */
  /***********/
  int k = 0;
  int dobreak=0;
  for (k = 0; k < 100; ++k) {
    
    /* setup the matrix */
    gsl_matrix *A;
    gsl_vector *b;

    elog(LOG_DEBUG(0), "allocing %d,%d matrix", num_rows, num_cols); 
    A = gsl_matrix_calloc(num_rows, num_cols);
    b = gsl_vector_calloc(num_rows);
    
    int node1_loc = 0, node2_loc = 0, curr_row = 0, curr_col = 0;
    for (mtr = range_list_top(mls->multilat_lists), i = 0;
	 i < num_ranges; ++i, mtr = range_list_next(mtr) ) {

      //elog(LOG_CRIT, "currrow: %i, i is %i", curr_row, i);
      //elog(LOG_CRIT, "looking for df: %i  cf: %i", mtr->data_from, mtr->chirp_from);
      node1_loc = result_list_array_lookup(mls, result_view, mtr->data_from);
      node2_loc = result_list_array_lookup(mls, result_view, mtr->chirp_from);

      mtr->last_row = -1;
      
      /* skip ranges between anchors */
      if ( (mtr->data_from == mls->anode1 && mtr->chirp_from == mls->anode2) ||
	   (mtr->data_from == mls->anode2 && mtr->chirp_from == mls->anode1)
	   ) {
	//	elog(LOG_WARNING, "!!!! found the anchors");
	continue;
      }
      
      /* skip if dropped */
      if ( mtr->state == RANGE_DROPPED ) {
	continue;
      }

      mtr->last_row = curr_row;

      /* find the columns if they exist... if not, set them */
      if (result_view[node1_loc]->col == -1 && result_view[node1_loc]->state != RESULT_COORD_ROOT) {
	result_view[node1_loc]->col = curr_col;
	curr_col += axes;
      }
      if (result_view[node2_loc]->col == -1 && result_view[node2_loc]->state != RESULT_COORD_ROOT) {
	result_view[node2_loc]->col = curr_col;
	curr_col += axes;
      }

      /* node1_loc represents x1, y1, z1 ...
	 node2_loc represents x2, y2, z2 */
      // node 2 chirps
      // node 1 receives

      /* $$$ missing VV and tiedowns */

      mtr->userange = mtr->distance + PHI_DIST_VARIATION*sin(mtr->phi);

      if (result_view[node1_loc]->state == RESULT_COORD_ROOT) {
	/* add constraints */
	mreal w = do_xweight(mtr);
	gsl_matrix_set(A, curr_row, result_view[node2_loc]->col + 0, +1.0*w);
	gsl_vector_set(b, curr_row, 
		       (mtr->userange*cos(mtr->phi)*
			cos(mtr->theta-result_view[node1_loc]->yaw) +
			result_view[node1_loc]->x)*w);
	curr_row++;
	
	w = do_yweight(mtr);
	gsl_matrix_set(A, curr_row, result_view[node2_loc]->col + 1, +1.0*w);
	gsl_vector_set(b, curr_row, 
		       (mtr->userange*cos(mtr->phi)*
			sin(mtr->theta-result_view[node1_loc]->yaw) +
			result_view[node1_loc]->y)*w);
	curr_row++;
	
	if (axes>2) {
	  w = do_zweight(mtr);
	  gsl_matrix_set(A, curr_row, result_view[node2_loc]->col + 2, +1.0*w);
	  gsl_vector_set(b, curr_row, (mtr->userange*sin(mtr->phi) +
				       result_view[node1_loc]->z)*w);
	  curr_row++;
	}
      }

      else if (result_view[node2_loc]->state == RESULT_COORD_ROOT) {
	/* add constraints */
	mreal w = do_xweight(mtr);
	gsl_matrix_set(A, curr_row, result_view[node1_loc]->col + 0, -1.0*w);
	gsl_vector_set(b, curr_row, 
		       (mtr->userange*cos(mtr->phi)*
			cos(mtr->theta-result_view[node1_loc]->yaw) -
			result_view[node2_loc]->x)*w);
	curr_row++;
	
	w = do_yweight(mtr);
	gsl_matrix_set(A, curr_row, result_view[node1_loc]->col + 1, -1.0*w);
	gsl_vector_set(b, curr_row, 
		       (mtr->userange*cos(mtr->phi)*
			sin(mtr->theta-result_view[node1_loc]->yaw) -
			result_view[node2_loc]->y)*w);
	curr_row++;
	
	if (axes>2) {
	  w = do_zweight(mtr);
	  gsl_matrix_set(A, curr_row, result_view[node1_loc]->col + 2, -1.0*w);
	  gsl_vector_set(b, curr_row, (mtr->userange*sin(mtr->phi) -
				       result_view[node2_loc]->z)*w);
	  curr_row++;
	}
      }

      else {
	/* add constraints */
	mreal w = do_xweight(mtr);
	gsl_matrix_set(A, curr_row, result_view[node1_loc]->col + 0, -1.0*w);
	gsl_matrix_set(A, curr_row, result_view[node2_loc]->col + 0, +1.0*w);
	gsl_vector_set(b, curr_row, 
		       (mtr->userange*cos(mtr->phi)*
			cos(mtr->theta-result_view[node1_loc]->yaw))*w);
	curr_row++;
	
	w = do_yweight(mtr);
	gsl_matrix_set(A, curr_row, result_view[node1_loc]->col + 1, -1*w);
	gsl_matrix_set(A, curr_row, result_view[node2_loc]->col + 1, +1*w);
	gsl_vector_set(b, curr_row, 
		       (mtr->userange*cos(mtr->phi)*
			sin(mtr->theta-result_view[node1_loc]->yaw))*w);
	curr_row++;

	if (axes>2) {
	  w = do_zweight(mtr);
	  gsl_matrix_set(A, curr_row, result_view[node1_loc]->col + 2, -1*w);
	  gsl_matrix_set(A, curr_row, result_view[node2_loc]->col + 2, +1*w);
	  gsl_vector_set(b, curr_row, mtr->userange*sin(mtr->phi)*w);
	  curr_row++;
	}
      }
      
    }
    elog(LOG_CRIT, "Just added %i rows", curr_row);


    /************/
    /* SVDecomp */
    /************/

    gsl_matrix *new_A;
    gsl_matrix *V;
    gsl_vector *s;
    gsl_vector *x_res;

    new_A = gsl_matrix_calloc(num_rows, num_cols);
    V = gsl_matrix_calloc(num_cols, num_cols);
    s = gsl_vector_calloc(num_cols);
    x_res = gsl_vector_calloc(num_cols);
    
    int res = gsl_matrix_memcpy(new_A, A);
    if (res != GSL_SUCCESS) {
      elog(LOG_CRIT, "Error copying new matrix: %s\n", gsl_strerror(res));
      exit(1);
    }

    elog(LOG_WARNING, "Starting svdecomp");
    res = gsl_linalg_SV_decomp_jacobi(new_A, V, s);

    if (res != GSL_SUCCESS) {
      elog(LOG_CRIT, "Error doing svdecomp: %s\n", gsl_strerror(res));
      exit(1);
    }
    elog(LOG_WARNING, "Starting SV solve");
    res = gsl_linalg_SV_solve(new_A, V, s, b, x_res);
    if (res != GSL_SUCCESS) {
      elog(LOG_CRIT, "Error doing svdecomp Solve: %s\n", gsl_strerror(res));
      exit(1);
    }
    elog(LOG_WARNING, "Printing state");
    print_multitlat_state(mls, A, new_A, V, s, b, x_res, k);
    elog(LOG_WARNING, "Done");

    /*********************************/
    /* update the results with x_res */
    /*********************************/
    for (i = 0; i < num_nodes; ++i) {
      if (result_view[i]->state != RESULT_COORD_ROOT && result_view[i]->col >= 0) {
	result_view[i]->x = gsl_vector_get(x_res, result_view[i]->col + 0);
	result_view[i]->y = gsl_vector_get(x_res, result_view[i]->col + 1);
	if (axes>2)
	  result_view[i]->z = gsl_vector_get(x_res, result_view[i]->col + 2);
      }
    }
    
    /*************************/
    /* update yaw/pitch/roll */
    /*************************/

    mreal totx=0;
    mreal toty=0;
    int totsum=0;
    //mreal max_err=0;

    int converged = 1;
    multilat_result_t *node;
    for (i = 0; i < num_nodes; ++i) {
      multilat_result_t *node = result_view[i];
      /* compute orientation for roots if there is more than one of them.
       * otherwise, lock in specified orientation */
      if (num_roots > 1 || (result_view[i]->state != RESULT_COORD_ROOT)) {
	mreal yaw[2] = {0,0};
	mreal pitch[2] = {0,0};
	mreal roll[2] = {0,0};

	for (mtr = range_list_top(mls->multilat_lists);	
	     mtr != NULL; mtr = range_list_next(mtr)) {
	  
	  if (mtr->data_from == result_view[i]->node) {
	    int chirp_index = result_list_array_lookup(mls, result_view, mtr->chirp_from);
	    multilat_result_t *chirp_node = result_view[chirp_index];
	    
	    mreal c[3];
	    mreal ctheta,cphi;
	    mreal dtheta,dphi;

	    /* unit vector of computed angle */
	    c[0] = chirp_node->x - node->x;
	    c[1] = chirp_node->y - node->y;
	    c[2] = chirp_node->z - node->z;

	    mreal D = sqrt(sqrf(c[0]) + sqrf(c[1]) + sqrf(c[2]));
	    
	    c[0] /= D;
	    c[1] /= D;
	    c[2] /= D;
	    
	    /* zenith and azimuth angles */
	    ctheta = atan2(c[1], c[0]);
	    cphi = misc_normalize_angle_rad_d(atan2(c[2], sqrt(sqrf(c[0])+sqrf(c[1]))));
	    
	    /* diffs */
	    dtheta = ctheta - mtr->theta;
	    dphi = cphi - misc_normalize_angle_rad_d(mtr->phi);

	    if (!mtr->drop_t) {
	      /* sum (could weight by confidence..) */
	      yaw[0] += cos(dtheta)*cos(cphi);
	      yaw[1] += sin(dtheta)*cos(cphi);
	    }

	    if (!mtr->drop_p) {// && fabs(dphi) < (6/180.0*M_PI)) {
	      elog(LOG_DEBUG(20), 
		   "dphi %f ctheta %f p += %f %f ",
		   r2d(dphi), r2d(ctheta),
		   fabs(cos(ctheta))*cos(dphi),
		   cos(ctheta)*sin(dphi));
	      elog(LOG_DEBUG(20), 
		   "dphi %f ctheta %f r += %f %f ",
		   r2d(dphi), r2d(ctheta),
		   fabs(sin(ctheta))*cos(dphi),
		   sin(ctheta)*sin(dphi));
	      
	      pitch[0] += fabs(cos(ctheta))*cos(dphi);
	      pitch[1] += cos(ctheta)*sin(dphi);
	      
	      roll[0] += fabs(sin(ctheta))*cos(dphi);
	      roll[1] += sin(ctheta)*sin(dphi);
	      
	      elog(LOG_DEBUG(20), 
		   " == %f,%f (%f)", 
		   pitch[0],pitch[1],
		   r2d(atan2(pitch[1],pitch[0])));
	      elog(LOG_DEBUG(20), 
		   " == %f,%f (%f)", 
		   roll[0],roll[1],
		   r2d(atan2(roll[1],roll[0])));
	    }
	    else {
	      elog(LOG_DEBUG(11), "skipping dphi=%f", dphi*180.0/M_PI);
	    }
	  }
	}
      
	mreal new_yaw = -atan2(yaw[1],yaw[0]);
	if (fabs(node->yaw - new_yaw) > 0.01) 
	  converged = 0;
	node->yaw = new_yaw;
	node->pitch = -atan2(pitch[1],pitch[0]);
	node->roll = -atan2(roll[1],roll[0]);
	
	elog(LOG_DEBUG(11), "****node %d: avg yaw=%f pitch=%f roll=%f  [%f,%f,%f] ",
	     node->node, r2d(node->yaw),
	     r2d(node->pitch),
	     r2d(node->roll),
	     sqrt(sqrf(yaw[0])+sqrf(yaw[1])),
	     sqrt(sqrf(pitch[0])+sqrf(pitch[1])),
	     sqrt(sqrf(roll[0])+sqrf(roll[1])));

	totx += cos(node->yaw);
	toty += sin(node->yaw);
	totsum++;
      }
    }
    
    mreal avgor = atan2(toty,totx);
    elog(LOG_WARNING, "average orientation is %f", avgor);

#ifndef ORIENT_BY_ROOT
    if (converged) {
      mreal max_diff = -1;
      
      /* rotate all points and unorient */
      for (node = result_list_top(mls->multilat_lists);
	   node; node = result_list_next(node)) {
	node->yaw -= avgor;
	mreal tmpx = node->x;
	mreal tmpy = node->y;
	node->x = cos(avgor)*tmpx - sin(avgor)*tmpy;
	node->y = sin(avgor)*tmpx + cos(avgor)*tmpy;
	
	if (node->state != RESULT_COORD_ROOT && node->col >= 0) {
	  mreal diff;
	  diff = fabs(gsl_vector_get(x_res, node->col + 0) +
		      (node->x - tmpx));
	  if (diff > max_diff) max_diff = diff;
	  diff = fabs(gsl_vector_get(x_res, node->col + 1) +
		      (node->y - tmpy));
	  if (diff > max_diff) max_diff = diff;
	}    
      }

      elog(LOG_WARNING, "MAX DIFF = %f", max_diff);
    }
#endif
    if (converged) dobreak = 1;

    /******************/
    /* out put errors */
    /******************/

    /*
     * This can be done in the mess above, but to keep it from getting
     * too messy, we just repeat down here
     */

    buf_t *resbuf = buf_new();
    bufprintf(resbuf, "#h receiver chirper axis resid\n");

    // declared earlier    int node1_loc = 0, node2_loc = 0;
    //    for (i = 0; i < b->size; ++i) {
    for (i = 0; i < num_rows; i+=axes) {
      
      /* find the ranges */
      for (mtr = range_list_top(mls->multilat_lists) ;
	   mtr != NULL; mtr = range_list_next(mtr)) {
	if (i == mtr->last_row) {
	  break;
	}
      }
      
      if (mtr == NULL) {
	elog(LOG_WARNING, "Could not find range for that row!");
	exit(1);	  
      }
      
      /* find the new calculated positions and the range */
      node1_loc = result_list_array_lookup(mls, result_view, mtr->data_from);
      node2_loc = result_list_array_lookup(mls, result_view, mtr->chirp_from);

      int k;
      for (k=0; k<axes; k++) {
	int kk;
	double sum = 0;
	for (kk=0; kk<num_cols; kk++) 
	  sum += (gsl_matrix_get(A, i+k, kk) * gsl_vector_get(x_res, kk));
	sum -= gsl_vector_get(b, i+k);
	bufprintf(resbuf, "%i %i %i %f\n",
		  mtr->data_from, mtr->chirp_from, k, sum);
      
	/* $$ could also include range, angle error */
      }
    }

#if 0
    char angleres[4096];
    sprintf(angleres, "angleres%i", k);
    multilatbuf_to_file(mls, angleres, ares);
    buf_free(ares);
#endif
    
    char errors[4096];
    sprintf(errors, "errors%i", k);
    multilatbuf_to_file(mls, errors, resbuf);
    buf_free(resbuf);

    /************************/
    /* studentized residual */
    /************************/
    // H = X ( XT X )-1 XT.
    if (dobreak && (mls->studthresh != 0)) {

      /* num_rows is the number of ranges */

      int l = 0;
      gsl_matrix *X = gsl_matrix_calloc(num_rows, 2);
      gsl_matrix *X2 = gsl_matrix_calloc(num_rows, 2);
      gsl_matrix *XTrans = gsl_matrix_calloc(2, num_rows);
      gsl_matrix *XTrans2 = gsl_matrix_calloc(2, num_rows);
      gsl_matrix *XXTrans = gsl_matrix_calloc(2, 2);
      gsl_matrix *H = gsl_matrix_calloc(num_rows, num_rows);
      gsl_vector *stud = gsl_vector_calloc(num_rows);
      double mean = 0.0;
      double var = 0.0;
      

      for (l = 0; l < num_rows; l++) {
	mean += gsl_vector_get(b, l);
	gsl_matrix_set(X, l, 0, 1);
	gsl_matrix_set(X, l, 1, gsl_vector_get(b, l));
	gsl_matrix_set(X2, l, 0, 1);
	gsl_matrix_set(X2, l, 1, gsl_vector_get(b, l));
	gsl_matrix_set(XTrans, 0, l, 1);
	gsl_matrix_set(XTrans, 1, l, gsl_vector_get(b, l));
	gsl_matrix_set(XTrans2, 0, l, 1);
	gsl_matrix_set(XTrans2, 1, l, gsl_vector_get(b, l));
      }
      mean = mean / (num_rows + 0.0);

      elog(LOG_WARNING, "mean is %f", mean);
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, XTrans, X, 1.0, XXTrans);
      gsl_blas_dtrsm(CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit, 1.0, XXTrans, X2);
      // upper or lower does not matter // gsl_blas_dtrsm(CblasRight, CblasLower, CblasNoTrans, CblasNonUnit, 1.0, XXTrans, X2);
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, X2, XTrans2, 1.0, H);
      
      /* calculate the variance */
      for (l = 0; l < num_rows; ++l) {
	var += m_pow_2(gsl_vector_get(b,l) - mean);
	//	elog(LOG_WARNING, "%f", gsl_matrix_get(H,l,l));	
      }
      var = var / (num_rows + 0.0);
      elog(LOG_WARNING, "var is %f", var);


      buf_t *studbuf = buf_new();
      bufprintf(studbuf, "#h res stud real chirpfrom datafrom\n");

      mreal maxstud = 0;
      for (l = 0; l < num_rows; l++) {
	double a = gsl_vector_get(b, l) / (sqrtf(var) * sqrtf(1.0 - gsl_matrix_get(H,l,l)));
	//	gsl_vector_set(stud, l, a);
	gsl_vector_set(stud, l, a >= 0 ? a : -a);

	if (gsl_vector_get(stud, l) > mls->studthresh) {
	  if (gsl_vector_get(stud, l) > maxstud) {
	    maxstud = gsl_vector_get(stud, l);
	  }
	}
      }

      if (maxstud > mls->studthresh) 
	for (l = 0; l < num_rows; l++) {
	  int range_index = (l/axes)*axes;
	  
	  if (gsl_vector_get(stud, l) == maxstud) {
	    
	    /* important! */
	    runagain = 1;
	    
	    elog(LOG_WARNING, "val is %f at row %i", gsl_vector_get(stud, l), l);
	    last_drop_value = maxstud;
	    
	    for (mtr = range_list_top(mls->multilat_lists) ; 
		 mtr != NULL; mtr = range_list_next(mtr)) {
	      if (range_index == mtr->last_row) {
		break;
	      }
	    }
	    
	    if (mtr) {
	      if (mtr->state != RANGE_DROPPED) {
		dropped_rows += axes;
		mtr->state = RANGE_DROPPED;
		int from = mtr->chirp_from;
		int to = mtr->data_from;
		
		bufprintf(droppedbuf, "%i %i %f %f %i\n", from, to,
			  gsl_vector_get(b,l),
			  gsl_vector_get(stud,l),
			  mls->global_run
			  );
		
		for (mtr = range_list_top(mls->multilat_lists) ;
		     mtr != NULL; mtr = range_list_next(mtr)) {
		  if ((from == mtr->chirp_from && to == mtr->data_from) || 
		      (to == mtr->chirp_from && from == mtr->data_from)) {
		    if (mtr->state != RANGE_DROPPED) {
		      dropped_rows += axes;
		      mtr->state = RANGE_DROPPED;
		    }
		  }
		}
	      }
	    }
	    else
	      elog(LOG_CRIT, "cant' find range.. ");
	  }
	  
	  for (mtr = range_list_top(mls->multilat_lists) ; 
	       mtr != NULL; mtr = range_list_next(mtr)) {
	    if (range_index == mtr->last_row) {
	      break;
	    }
	  }
	  
	  if (mtr) {
	    multilat_result_t * mrr1 = result_list_get(mls, mtr->chirp_from);
	    multilat_result_t * mrr2 = result_list_get(mls, mtr->data_from);
	    bufprintf(studbuf, "%f %f %f %i %i\n",
		      gsl_vector_get(b,l),
		      gsl_vector_get(stud,l),
		      mtr->real_distance - 
		      sqrtf(m_pow_2(mrr1->x - mrr2->x) + 
			    m_pow_2(mrr1->y - mrr2->y) +
			    m_pow_2(mrr1->z - mrr2->z)),
		      mtr->chirp_from,
		      mtr->data_from
		      );
	  }
	  else
	    elog(LOG_CRIT, "cant' find range for index %d (%d).. 2",
		 range_index, l);
	}
      
      multilatbuf_to_file(mls, "compare", studbuf);
      buf_free(studbuf);

      elog(LOG_WARNING, " ----- All done!");

      gsl_matrix_free(X);
      gsl_matrix_free(X2);
      gsl_matrix_free(XTrans);
      gsl_matrix_free(XTrans2);
      gsl_matrix_free(XXTrans);
      gsl_matrix_free(H);
      gsl_vector_free(stud);
    }
    
    mreal avg_resid = 0;
    mreal rms_resid = 0;
    int range_count = 0;
    int l;
    for (l = 0; l < num_rows; ++l) {
      avg_resid += fabs(gsl_vector_get(b,l));
      rms_resid += sqrf(gsl_vector_get(b,l));
      range_count++;
    }

    if (range_count > 0) {
      avg_resid /= (float)range_count;
      rms_resid = sqrt(rms_resid/(float)range_count);      
    }

    elog(LOG_CRIT, "**** average residuals:");
    elog(LOG_CRIT, "AVG:  %f  RMS:  %f",
	 avg_resid, rms_resid);
    

    /**************/
    /* free stuff */
    /**************/
    
    gsl_matrix_free(A);
    gsl_vector_free(b);
    gsl_matrix_free(new_A);
    gsl_matrix_free(V);
    gsl_vector_free(s);
    //    gsl_vector_free(work);
    gsl_vector_free(x_res);
    //    gsl_vector_free(check);

    buf_t *firstpass = buf_new();
    char thefile[4096];
    memset(thefile, 0, 4096);
    sprintf(thefile, "pass%i", k);
    result_list_print(mls, firstpass);
    multilatbuf_to_file(mls, thefile, firstpass);
    buf_free(firstpass);  

    if (dobreak) {
      dobreak = 0;
      break;
    }
  }

  /* print out the final error stuff */
  buf_t *ce = buf_new();
  bufprintf(ce, "#h node coorderror\n");
  multilat_result_t *mlr = result_list_top(mls->multilat_lists);
  mreal rms = 0;
  int count = 0;
  for ( ; mlr != NULL; mlr = result_list_next(mlr) ) {
    if (mlr->state != RESULT_COORD_ROOT) {
      bufprintf(ce, "%i %f\n", mlr->node,
		sqrtf(m_pow_2(mlr->x - mlr->real_x) +
		      m_pow_2(mlr->y - mlr->real_y) +
		      m_pow_2(mlr->z - mlr->real_z))
		);
      rms += m_pow_2(mlr->x - mlr->real_x) +
	m_pow_2(mlr->y - mlr->real_y) +
	m_pow_2(mlr->z - mlr->real_z);
      count++;
    }
    
  }
  bufprintf(ce, "# rms: %f\n", sqrtf(rms / (count + 0.0)));
  bufprintf(ce, "# Grms: %f\n", mls->guess_rms);
  multilatbuf_to_file(mls, "coorderr", ce);
  buf_free(ce);

  iterations = k;
  analyse_result(mls);

  /*************/
  /* increment */
  /*************/
 
  if (runagain) {
    mls->global_run++;
    runagain = 0;
    goto repeate;
  }

  multilatbuf_to_file(mls, "dropped", droppedbuf);
  buf_free(droppedbuf);

  mls->global_run = 1000;

  {
    buf_t *firstpass = buf_new();
    char thefile[4096];
    memset(thefile, 0, 4096);
    sprintf(thefile, "last");
    result_list_print(mls, firstpass);
    multilatbuf_to_file(mls, thefile, firstpass);
    buf_free(firstpass);  
  }

  analyse_result(mls);
  
  return;
}


/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
/******************************************************************************/


#else  //!RTHETA


void reset_matrix(ml_state_t *mls, int num_rows)
{
  if (mls->row_info) free(mls->row_info);
  mls->row_info = g_new0(struct row_info, num_rows);
  mls->row_count = 0;
}

int addrow(ml_state_t *mls, int type, multilat_range_t *new_range) 
{
  mls->row_info[mls->row_count].c_type = type;
  mls->row_info[mls->row_count].range = new_range;
  mls->row_count++;
  return mls->row_count;
}

int get_row_type(ml_state_t *mls, int row)
{
  return mls->row_info[row].c_type;
}

multilat_range_t *get_row_range(ml_state_t *mls, int row)
{
  return mls->row_info[row].range;
}


/* compare the forward and reverse paths and compute 
 * estimates of the common delta for each node.
 * the theory is that there may be a fixed delay for each node
 * that accounts for the fwd/rev differences 
 *
 *  (Rij, i is the receiver)
 *   Rij = T + Di - Dj
 *   Rji = T + Dj - Di
 *   Rij - Di + Dj = Rji - Dj + Di
 *   Rij - Rji = 2Di - 2Dj
 */

void compute_deltas(ml_state_t *mls) 
{
  multilat_result_t *node;
  multilat_range_t *mtr;
  int rows = 0;

  /* clear delta_in flags */
  for (node = result_list_top(mls->multilat_lists);
       node; node = result_list_next(node)) {
    node->delta_in = 0;
    node->col = -1;
  }

  /* count number of rows */
  for (mtr = range_list_top(mls->multilat_lists);
       mtr; mtr = range_list_next(mtr)) {

    mtr->last_row = -1;

    /* sort out ranges */
    if (!mtr->drop_r && !mtr->peer->drop_r) {
      if (fabs(mtr->peer->distance - mtr->distance) > MAX_BIDIR_DIFF ||
	  (mtr->peer->distance > MAX_BIDIR_DIFF &&
	   fabs(mtr->peer->distance - mtr->distance) > (mtr->distance * 0.1))) {
	elog(LOG_WARNING, "dropping bogus range and angles..");
	if (mtr->peer->distance > mtr->distance) {
	  mtr->peer->drop_r = mtr->peer->drop_p = mtr->peer->drop_t = 1;
	}
	else {
	  mtr->drop_r = mtr->drop_p = mtr->drop_t = 1;
	}
      }
    }

    if (!mtr->drop_r && !mtr->peer->drop_r) {
      if (fabs(mtr->peer->distance - mtr->distance) < 20) {
	mtr->result->delta_in++;
	mtr->peer->result->delta_in++;
	rows++;
      }
    }
  }

  rows /= 2;

#ifdef COMPUTE_DELTAS
  /* count cols */
  int cols = 0;
  for (node = result_list_top(mls->multilat_lists);
       node; node = result_list_next(node)) {
    if (node->delta_in) cols++;
  }

  /* set up matrices */
  gsl_matrix *A;
  gsl_vector *b;

  elog(LOG_WARNING, "allocing %d by %d matrix", rows, cols);
  A = gsl_matrix_calloc(rows, cols); 
  b = gsl_vector_calloc(rows);

  int curr_row = 0;
  int curr_col = 0;
  for (mtr = range_list_top(mls->multilat_lists);
       mtr; mtr = range_list_next(mtr)) {
    
    /* skip if... */
    if (mtr->last_row >= 0) continue;
    if (mtr->drop_r || mtr->peer->drop_r) continue;
    if (fabs(mtr->peer->distance - mtr->distance) >= 20) continue;

    /* ok, add it */
    if (mtr->result->col < 0)
      mtr->result->col = curr_col++;
    if (mtr->peer->result->col < 0)
      mtr->peer->result->col = curr_col++;

    mtr->last_row = curr_row;
    mtr->peer->last_row = curr_row;

    /* b = Rij - Rji */
    gsl_vector_set(b, curr_row, mtr->distance - mtr->peer->distance);
    /* A = 2Di - 2Dj */
    gsl_matrix_set(A, curr_row, mtr->result->col, +2); 
    gsl_matrix_set(A, curr_row, mtr->peer->result->col, -2); 

    curr_row++;
  }

  gsl_matrix *new_A;
  gsl_matrix *V;
  gsl_vector *s;
  gsl_vector *x_res;
  
  new_A = gsl_matrix_calloc(rows, cols);
  V = gsl_matrix_calloc(cols, cols);
  s = gsl_vector_calloc(cols);
  x_res = gsl_vector_calloc(cols);

  buf_t *bn = buf_new();
  print_matrix(bn, A);
  multilatbuf_to_file(mls, "DeltatAis", bn);
  buf_free(bn);
   
  int res = gsl_matrix_memcpy(new_A, A);
  if (res != GSL_SUCCESS) {
    elog(LOG_CRIT, "Error copying new matrix: %s\n", gsl_strerror(res));
    exit(1);
  }

  elog(LOG_WARNING, "Starting svdecomp");
  res = gsl_linalg_SV_decomp_jacobi(new_A, V, s);
  
  if (res != GSL_SUCCESS) {
    elog(LOG_CRIT, "Error doing svdecomp: %s\n", gsl_strerror(res));
    goto mlat_failed;
  }
  elog(LOG_WARNING, "Starting SV solve");
  res = gsl_linalg_SV_solve(new_A, V, s, b, x_res);
  if (res != GSL_SUCCESS) {
    elog(LOG_CRIT, "Error doing svdecomp Solve: %s\n", gsl_strerror(res));
    goto mlat_failed;
  }
  
  /* ok, should now have the answers! */
  for (node = result_list_top(mls->multilat_lists);
       node; node = result_list_next(node)) {
    if (node->col >= 0) {
      node->delta = gsl_vector_get(x_res, node->col);
      elog(LOG_WARNING, "node %d, delta=%.2f", node->node, node->delta);
    }
  }
#endif

}


int multilat_solver(ml_state_t *mls) {

  buf_t *studbuf = NULL;
  mls->failed_convergence = 3;
  int total_iterations = 0;
  int total_passes = 0;

  /* 
   * configure peer relationships 
   */

  multilat_range_t *mtr;

  LOG_ENTRY("****starting multilat");
  
  /* clear peers */
  for (mtr = range_list_top(mls->multilat_lists);
       mtr; mtr = range_list_next(mtr)) {
    mtr->peer = NULL;
    if (mtr->distance > 0)
      mtr->drop_r = mtr->drop_t = mtr->drop_p = 0;
  }

  /* set peers and results */
  for (mtr = range_list_top(mls->multilat_lists);
       mtr; mtr = range_list_next(mtr)) {
    if (mtr->peer == NULL) {
      multilat_range_t *mtr2;
      for (mtr2 = range_list_top(mls->multilat_lists);
	   mtr2; mtr2 = range_list_next(mtr2)) {
	if (mtr != mtr2 && 
	    mtr->chirp_from == mtr2->data_from &&
	    mtr->data_from == mtr2->chirp_from && 
	    mtr2->peer == NULL) {
	  mtr->peer = mtr2;
	  mtr2->peer = mtr;
	  break;
	}
      }
      
      mtr->result = result_list_get(mls, mtr->data_from);
      if (mtr->peer == NULL) {
	mtr2 = g_new0(multilat_range_t, 1);
	range_list_push(mls->multilat_lists, mtr2);
	mtr->peer = mtr2;
	mtr2->peer = mtr;
	mtr2->chirp_from = mtr->data_from;
	mtr2->data_from = mtr->chirp_from;
	mtr2->drop_r = mtr2->drop_t = mtr2->drop_p = 1;
      }
      mtr->peer->result = result_list_get(mls, mtr->peer->data_from);
    }
  }

  /* compute the deltas.. */
  compute_deltas(mls);

  for (mtr = range_list_top(mls->multilat_lists);
       mtr; mtr = range_list_next(mtr)) {

    if (!mtr->drop_r && !mtr->peer->drop_r) {
      if (mtr->peer->distance > mtr->distance) {
	mtr->peer->userange = mtr->userange = mtr->distance;
      }
      else {
	mtr->peer->userange = mtr->userange = mtr->peer->distance;
      }
      
      /* 8cm*sin(phi) accounts for excess length when coming in off plane */
      mtr->userange += sin(mtr->phi) * PHI_DIST_VARIATION;
      mtr->peer->userange += sin(mtr->peer->phi) * PHI_DIST_VARIATION;
    }
    
    else {
      if (!mtr->drop_r)
	mtr->userange = mtr->distance + sin(mtr->phi) * PHI_DIST_VARIATION;
      if (!mtr->peer->drop_r)
	mtr->peer->userange = mtr->peer->distance + sin(mtr->peer->phi) * PHI_DIST_VARIATION;
    }
    
  }

  /* clear column assignments */
  multilat_result_t *node;
  for (node = result_list_top(mls->multilat_lists);
       node; node = result_list_next(node)) {
    node->col = -1;
  }
  
  /* compute number of roots */
  int num_roots = 0;
  multilat_result_t *mlr;
  for (mlr = result_list_top(mls->multilat_lists);
       mlr; mlr = result_list_next(mlr)) {
    if (mlr->state == RESULT_COORD_ROOT)
      num_roots++;
  }

  int runagain = 0;
  int num_rows = 0;
  int num_cols = 0;

  buf_t *droppedbuf = buf_new();

  int useVV = 0;
#ifdef USEZ
  int axes = 3;
#else
  int axes = 2;
#endif

  /******************/
  /* get dimensions */
  /******************/
 repeate:

  if (mls->global_run != 0) {
    result_load(mls);
  }

  mreal VV = 1;


  /***********/
  /* iterate */
  /***********/
  int k = 0;
  int orient_count = 0;
  for (k = 0; k < 100; ++k) { 

    elog(LOG_WARNING, "starting iterations %d", k);
    total_iterations++;

    /* reset usage bits and columns */
    for (mtr = range_list_top(mls->multilat_lists);
	 mtr; mtr = range_list_next(mtr)) {
      mtr->use_r = mtr->use_t = mtr->use_p = mtr->use_set = 0;
      mtr->result->col = -1;
    }

    /* for each pair, decide which to use */
    for (mtr = range_list_top(mls->multilat_lists);
	 mtr; mtr = range_list_next(mtr)) {
      
      /* continue if already processed */
      if (mtr->use_set) continue;

      /* set bit */
      mtr->use_set = 1;
      if (mtr->peer) mtr->peer->use_set = 1;

      /* continue if between two roots */
      if (mtr->result->state == RESULT_COORD_ROOT &&
	  mtr->peer && mtr->peer->result->state == RESULT_COORD_ROOT)
	continue;
      
      /* if range is not dropped, use it */
      if (!mtr->drop_r)
	mtr->use_r = 1;
      else if (!mtr->peer->drop_r)
	mtr->peer->use_r = 1;
	
      if (mls->useangles) {      

	/* use this theta angle if not dropped */
	if (!mtr->drop_t && mtr->peer && !mtr->peer->drop_t) {
	  if (mtr->angle_conf > mtr->peer->angle_conf)
	    mtr->use_t = 1;
	  else
	    mtr->peer->use_t = 1;
	}
	else if (!mtr->drop_t) 
	  mtr->use_t = 1;
	else if (mtr->peer && !mtr->peer->drop_t)
	  mtr->peer->use_t = 1;

#ifndef HACK_OUT_PHI
	if (axes > 2) {
	  /* use this phi angle if not dropped */
	  if (!mtr->drop_p && mtr->peer && !mtr->peer->drop_p) {
	    if (mtr->angle_conf > mtr->peer->angle_conf)
	      mtr->use_p = 1;
	    else
	      mtr->peer->use_p = 1;
	  }
	  else if (!mtr->drop_p) 
	    mtr->use_p = 1;
	  else if (mtr->peer && !mtr->peer->drop_p)
	    mtr->peer->use_p = 1;
	}
#endif
      }
    }

    /* count rows and columns */
    num_rows = 0;
    num_cols = 0;
    for (mtr = range_list_top(mls->multilat_lists);
	 mtr; mtr = range_list_next(mtr)) {

      int eqns = (mtr->use_r + mtr->use_p + mtr->use_t);
      num_rows += eqns;

      if (mtr->peer)
	eqns += (mtr->peer->use_r + mtr->peer->use_p + mtr->peer->use_t);

      if (eqns > 0 && mtr->result->col == -1 && mtr->result->state != RESULT_COORD_ROOT) {
	mtr->result->col = num_cols;
	num_cols += axes;
      }
    }
    
    elog(LOG_CRIT, "total rows %d, num cols %d", num_rows, num_cols);
    if (num_cols > num_rows) {
      elog(LOG_CRIT, "Can not continue with SVD only have %i by %i matrix", num_rows, num_cols);
      goto mlat_failed;
    }

    if (num_cols == 0 || num_rows == 0) {
      elog(LOG_CRIT, "must have rows or cols");
      goto mlat_failed;
    }

    /* clear row map */
    reset_matrix(mls, num_rows);
    
    /* setup the matrix */
    gsl_matrix *A;
    gsl_vector *b;

    int Vcol;
    elog(LOG_DEBUG(0), "allocing %d,%d matrix", num_rows, num_cols+useVV); 
    A = gsl_matrix_calloc(num_rows, num_cols+useVV); 
    b = gsl_vector_calloc(num_rows);
    Vcol = num_cols;

    int curr_row = 0;
    for (mtr = range_list_top(mls->multilat_lists);
	 mtr; mtr = range_list_next(mtr)) {
      
      if (mtr->peer == NULL) {
	elog(LOG_WARNING, "no peer??");
	continue;
      }
      
      multilat_result_t *node1 = mtr->result;
      multilat_result_t *node2 = mtr->peer->result;

      mreal DX = node2->x - node1->x;
      mreal DY = node2->y - node1->y;
      mreal DZ = node2->z - node1->z;
      mreal D = sqrtf(sqrf(DX) + sqrf(DY) + sqrf(DZ));
      
      if (mtr->use_r) {

	/* protect against explosion */
	if (D < 1.0) {
	  D = 1.0;
	}
	
	mreal w = 1.0; //(mls->include_conf ? mtr->conf / 10.0 : 1.0);
	
	if (node1->state != RESULT_COORD_ROOT) {
	  gsl_matrix_set(A, curr_row, node1->col + 0, -VV/D*DX*w);
	  gsl_matrix_set(A, curr_row, node1->col + 1, -VV/D*DY*w);
	  if (axes > 2)
	    gsl_matrix_set(A, curr_row, node1->col + 2, -VV/D*DZ*w);
	}

	if (node2->state != RESULT_COORD_ROOT) {
	  gsl_matrix_set(A, curr_row, node2->col + 0, VV/D*DX*w);
	  gsl_matrix_set(A, curr_row, node2->col + 1, VV/D*DY*w);
	  if (axes > 2)
	    gsl_matrix_set(A, curr_row, node2->col + 2, VV/D*DZ*w);
	}

	if (useVV)
	  gsl_matrix_set(A, curr_row, Vcol, D*w);

	gsl_vector_set(b, curr_row, (mtr->userange - VV*D)*w);

	curr_row = addrow(mls, ML_CONSTRAINT_RANGE, mtr);
      }
	
      /************/
      /*  angle   */
      /************/

      // node 2 chirps
      // node 1 send data

      if (mls->useangles) {

	if (mtr->use_t) {

	  if (fabsf(DX) < 1.0 || fabsf(DY) < 1.0) {
	    elog(LOG_WARNING, "skipping angle for %d->%d", node1->node, node2->node);
	    if (k>10) {
	      elog(LOG_CRIT, "DROPPING angle for %d->%d", node1->node, node2->node);
	      mtr->drop_t = 1;
	    }
	    goto skipangle;
	  }
  
	  double T = 1.0/(1.0+sqrf(DY/DX));
	  double mply = 226 * mtr->angle_conf;
	  
	  if (node1->state != RESULT_COORD_ROOT) {
	    gsl_matrix_set(A, curr_row, node1->col + 0, mply * +T * (DY / sqrf(DX)));
	    gsl_matrix_set(A, curr_row, node1->col + 1, mply * -T * (1 / DX));	  
	  }
	  
	  if (node2->state != RESULT_COORD_ROOT) {
	    gsl_matrix_set(A, curr_row, node2->col + 0, mply * -T * (DY / sqrf(DX)));
	    gsl_matrix_set(A, curr_row, node2->col + 1, mply * +T * (1 / DX));	  
	  }
	  
	  gsl_vector_set(b, curr_row, mply* misc_normalize_angle_rad_d
			 ((mtr->theta - node1->yaw) - (atan2f(DY, DX))));
	  
      skipangle:
	  curr_row = addrow(mls, ML_CONSTRAINT_THETA, mtr);
	}

	if (axes > 2 && mtr->use_p) {

	  /*
	   *  weighing scheme:  
	   *     sigma for range 3.8 cm
	   *     sigma for AZI 0.96 deg = 226x
	   *     sigma for ZEN 45-45 3.6 deg = 60x
	   *     sigma for ZEN 45-90 4.5 deg = 48x
	   */
	  
	  mreal use_phi = misc_normalize_angle_rad_d(mtr->phi);

#ifdef CORRECT_PHI
	  mreal corr = misc_normalize_angle_rad_d
	    (sin(mtr->theta)*node1->pitch + cos(mtr->theta)*node1->roll);
	  elog(LOG_DEBUG(11), "phi was %f, roll correction %f, now is %f",
	       r2d(use_phi), r2d(corr), r2d(corr+use_phi));
	  use_phi += corr;
#endif
	  
	  double w = ((fabs(use_phi) > (45/180.0*M_PI)) ? 48.0 : 60.0) * 
	    3.0 * mtr->angle_conf;  //3.0
  
	  float D_sqr = sqrf(DX) + sqrf(DY);
	  if (D_sqr < 1.0) D_sqr = 1.0;
	  
	  float D_prime = sqrtf(D_sqr);
	  
	  float T_prime = 1.0/(1.0 + sqrf(DZ/D_prime));
	  
	  float K = DZ * powf(D_sqr, -3.0/2.0);
	  
	  if (node1->state != RESULT_COORD_ROOT) {
	    gsl_matrix_set(A, curr_row, node1->col + 0, w*T_prime*K*DX);
	    gsl_matrix_set(A, curr_row, node1->col + 1, w*T_prime*K*DY);
	    gsl_matrix_set(A, curr_row, node1->col + 2, -w*T_prime/D_prime);
	  }
	  
	  if (node2->state != RESULT_COORD_ROOT) {
	    gsl_matrix_set(A, curr_row, node2->col + 0, -w*T_prime*K*DX);
	    gsl_matrix_set(A, curr_row, node2->col + 1, -w*T_prime*K*DY);
	    gsl_matrix_set(A, curr_row, node2->col + 2, +w*T_prime/D_prime);
	  }
	  
	  gsl_vector_set(b, curr_row,
			 w*misc_normalize_angle_rad_d
			 (use_phi - misc_normalize_angle_rad_d(atan2f(DZ, D_prime))));

	  curr_row = addrow(mls, ML_CONSTRAINT_PHI, mtr);
	}
      }
    }
    elog(LOG_CRIT, "Just added %i rows", curr_row);
    
    
    /************/
    /* SVDecomp */
    /************/

    gsl_matrix *new_A;
    gsl_matrix *V;
    gsl_vector *s;
    gsl_vector *x_res;

    new_A = gsl_matrix_calloc(num_rows, num_cols + useVV);
    V = gsl_matrix_calloc(num_cols + useVV, num_cols + useVV);
    s = gsl_vector_calloc(num_cols + useVV);
    x_res = gsl_vector_calloc(num_cols + useVV);
    
    buf_t *bn = buf_new();
    print_matrix(bn, A);
    multilatbuf_to_file(mls, "Ais", bn);
    buf_free(bn);

    int res = gsl_matrix_memcpy(new_A, A);
    if (res != GSL_SUCCESS) {
      elog(LOG_CRIT, "Error copying new matrix: %s\n", gsl_strerror(res));
      exit(1);
    }

    elog(LOG_WARNING, "Starting svdecomp");
    res = gsl_linalg_SV_decomp_jacobi(new_A, V, s);

    if (res != GSL_SUCCESS) {
      elog(LOG_CRIT, "Error doing svdecomp: %s\n", gsl_strerror(res));
      goto mlat_failed;
    }
    elog(LOG_WARNING, "Starting SV solve");
    res = gsl_linalg_SV_solve(new_A, V, s, b, x_res);
    if (res != GSL_SUCCESS) {
      elog(LOG_CRIT, "Error doing svdecomp Solve: %s\n", gsl_strerror(res));
      goto mlat_failed;
    }
    elog(LOG_WARNING, "Printing state");
    print_multitlat_state(mls, A, new_A, V, s, b, x_res, k);
    elog(LOG_WARNING, "Done");



    /*********************************/
    /* update the results with x_res */
    /*********************************/
    int dobreak = 1;
    if (useVV) {
      VV += gsl_vector_get(x_res, Vcol);
      elog(LOG_WARNING, "V is now %f", VV);
    } 

    multilat_result_t *node;
    for (node = result_list_top(mls->multilat_lists);
	 node; node = result_list_next(node)) {
      if (node->state != RESULT_COORD_ROOT && node->col >= 0) {
	node->x += (gsl_vector_get(x_res, node->col + 0));
	node->y += (gsl_vector_get(x_res, node->col + 1));
	if (axes > 2)
	  node->z += (gsl_vector_get(x_res, node->col + 2));
	if (fabs(gsl_vector_get(x_res, node->col + 0)) > 0.01 ||
	    fabs(gsl_vector_get(x_res, node->col + 1)) > 0.01 || 
	    ((axes > 2) && fabs(gsl_vector_get(x_res, node->col + 2)) > 0.01)) {
	  dobreak = 0;
	}
      }
    }

#ifdef STANDALONE_MULTILAT
    /* allow outlier rejection even if convergence failed */
    if (k > 50) dobreak = 1;
#endif
    
    /* only update orienation for first 10 iterations */
    if (orient_count<10) {

      orient_count++;

      /*************************/
      /* update yaw/pitch/roll */
      /*************************/
      
      mreal totx=0;
      mreal toty=0;
      int totsum=0;
      //mreal max_err=0;
      //int reset_orient_count = 0;

      /* for angle-check-mode */
      multilat_range_t *worst_angle = NULL;
      mreal worst_angle_value = 0;

      for (node = result_list_top(mls->multilat_lists);
	   node; node = result_list_next(node)) {
	/* compute orientation for roots if there is more than one of them.
	 * otherwise, lock in specified orientation */
	//if (num_roots > 1 || (result_view[i]->state != RESULT_COORD_ROOT)) 
	{
	  mreal yaw[2] = {0,0};
	  mreal pitch[2] = {0,0};
	  mreal roll[2] = {0,0};

	  for (mtr = range_list_top(mls->multilat_lists);	
	       mtr != NULL; mtr = range_list_next(mtr)) {
	    
	    if (mtr->result == node && mtr->peer) {
	      
	      multilat_result_t *chirp_node = mtr->peer->result;
	      
	      mreal c[3];
	      mreal ctheta,cphi;
	      mreal dtheta,dphi;
	      
	      /* unit vector of computed angle */
	      c[0] = chirp_node->x - node->x;
	      c[1] = chirp_node->y - node->y;
	      c[2] = chirp_node->z - node->z;
	      
	      mreal D = sqrt(sqrf(c[0]) + sqrf(c[1]) + sqrf(c[2]));
	      
	      c[0] /= D;
	      c[1] /= D;
	      c[2] /= D;
	      
	      /* zenith and azimuth angles */
	      ctheta = atan2(c[1], c[0]);
	      cphi = misc_normalize_angle_rad_d(atan2(c[2], sqrt(sqrf(c[0])+sqrf(c[1]))));
	      
	      /* diffs */
	      dtheta = ctheta - mtr->theta;
	      dphi = cphi - misc_normalize_angle_rad_d(mtr->phi);
	
	      /* if orient_count == 10, then we are in angle-check mode. */
	      if (orient_count == 10 && !mtr->drop_t) {		
		mreal diff = misc_normalize_angle_rad_d
		  (fabs(misc_normalize_angle_rad_d(dtheta) + 
			misc_normalize_angle_rad_d(node->yaw)));
		if (diff > worst_angle_value) {
		  worst_angle_value = diff;
		  worst_angle = mtr;
		}
	      }
      
	      if (!mtr->drop_t) {
		/* sum (could weight by confidence..) */
		yaw[0] += cos(dtheta)*cos(cphi);
		yaw[1] += sin(dtheta)*cos(cphi);
	      }
	      
	      if (!mtr->drop_p) { // && fabs(dphi) < (6/180.0*M_PI)) {
		elog(LOG_DEBUG(20), 
		     "dphi %f ctheta %f p += %f %f ",
		     r2d(dphi), r2d(ctheta),
		     fabs(cos(ctheta))*cos(dphi),
		     cos(ctheta)*sin(dphi));
		elog(LOG_DEBUG(20), 
		     "dphi %f ctheta %f r += %f %f ",
		     r2d(dphi), r2d(ctheta),
		     fabs(sin(ctheta))*cos(dphi),
		     sin(ctheta)*sin(dphi));
		
		pitch[0] += fabs(cos(ctheta))*cos(dphi);
		pitch[1] += cos(ctheta)*sin(dphi);
		
		roll[0] += fabs(sin(ctheta))*cos(dphi);
		roll[1] += sin(ctheta)*sin(dphi);
		
		elog(LOG_DEBUG(20), 
		     " == %f,%f (%f)", 
		     pitch[0],pitch[1],
		     r2d(atan2(pitch[1],pitch[0])));
		elog(LOG_DEBUG(20), 
		     " == %f,%f (%f)", 
		     roll[0],roll[1],
		     r2d(atan2(roll[1],roll[0])));
	      }
	      else {
		elog(LOG_DEBUG(11), "skipping dphi=%f", dphi*180.0/M_PI);
	      }
	    }
	  }

	  node->yaw = -atan2(yaw[1],yaw[0]);
	  node->pitch = -atan2(pitch[1],pitch[0]);
	  node->roll = -atan2(roll[1],roll[0]);
	  
	  elog(LOG_DEBUG(11), "****node %d: avg yaw=%f pitch=%f roll=%f  [%f,%f,%f] ",
	       node->node, r2d(node->yaw),
	       r2d(node->pitch),
	       r2d(node->roll),
	       sqrt(sqrf(yaw[0])+sqrf(yaw[1])),
	       sqrt(sqrf(pitch[0])+sqrf(pitch[1])),
	       sqrt(sqrf(roll[0])+sqrf(roll[1])));
	  
	  totx += cos(node->yaw);
	  toty += sin(node->yaw);
	  totsum++;
	}
      }

      /* if orient_count == 10, then we are in angle-check mode. */
      if (orient_count == 10) {		
	if (!mls->noreject && worst_angle_value > d2r(20)) {
	  elog(LOG_CRIT, "***DROPPING RANGE %d->%d because angle is off by %f!!!!",
	       worst_angle->chirp_from, worst_angle->data_from, r2d(worst_angle_value));
	  worst_angle->drop_r = worst_angle->drop_t = worst_angle->drop_p = 1;
	  //reset_orient_count = 1;
	  /* keep doing orientation if we dropped a range */
	  //      if (reset_orient_count) 
	  orient_count = 7;
	}
      }
	      
      mreal avgor = atan2(toty,totx);
      elog(LOG_WARNING, "average orientation is %f", avgor);
      
#ifdef ORIENT_BY_ROOT
      if (num_roots == 1) {
	for (node = result_list_top(mls->multilat_lists);
	     node; node = result_list_next(node)) {
	  if (node->state == RESULT_COORD_ROOT) {
	    avgor = node->yaw;
	    elog(LOG_WARNING, "root node orientation is %f", avgor);
	  }
	}
      }
#endif
      
      mreal max_diff = -1;
      
      /* rotate all points and unorient */
      for (node = result_list_top(mls->multilat_lists);
	   node; node = result_list_next(node)) {

	if (node->col < 0 && node->state != RESULT_COORD_ROOT) continue;

	node->yaw -= avgor;
	mreal tmpx = node->x;
	mreal tmpy = node->y;
	node->x = cos(avgor)*tmpx - sin(avgor)*tmpy;
	node->y = sin(avgor)*tmpx + cos(avgor)*tmpy;
	
	if (node->state != RESULT_COORD_ROOT && node->col >= 0) {
	  mreal diff;
	  diff = fabs(gsl_vector_get(x_res, node->col + 0) +
		      (node->x - tmpx));
	  if (diff > max_diff) max_diff = diff;
	  diff = fabs(gsl_vector_get(x_res, node->col + 1) +
		      (node->y - tmpy));
	  if (diff > max_diff) max_diff = diff;
	}    
      }
      
      elog(LOG_WARNING, "MAX DIFF = %f", max_diff);
      if (max_diff >= 0 && max_diff < 0.01) dobreak = 1;

      /* don't break until we've completed orientation stuff */
      dobreak = 0;
    }

#if 0
    /******************/
    /* out put errors */
    /******************/

    /*
     * This can be done in the mess above, but to keep it from getting
     * too messy, we just repeat down here
     */

    buf_t *resbuf = buf_new();
    bufprintf(resbuf, 
	      "#h chirper listener residual realrange realtheta realphi "
	      "calcrange calctheta calcphi real-calcrange real-calctheta real-calcphi "
	      "introdisterr introthetaerr introphierr\n");

    buf_t *ares = buf_new();
    bufprintf(ares, "#h c l angle res type dz\n");

    for (i = 0; i < num_rows; ++i) {
      mtr = get_row_range(mls, i);
      if (mtr == NULL) {
	elog(LOG_WARNING, "can't find range for row %d!!", i);
	exit(1);
      }
      multilat_result_t *node1 = mtr->result;
      multilat_result_t *node2 = mtr->peer->result;

      /******************/

      switch (get_row_type(mls, i)) {
      case ML_CONSTRAINT_RANGE:
	bufprintf(resbuf, "%i %i %f %f %f %f %f %f %f %f %f %f %f %f %f\n", 
		  mtr->chirp_from,
		  mtr->data_from,
		  gsl_vector_get(b, i), 
		  mtr->real_distance,
		  mtr->real_theta,
		  mtr->real_phi,
		  sqrtf(m_pow_2(node1->x - node2->x) + m_pow_2(node1->y - node2->y)),
		  atan2f(node2->y - node1->y, node2->x - node1->x),
		  0.0,
		  mtr->real_distance -
		  sqrtf(m_pow_2(node1->x - node2->x) +
			m_pow_2(node1->y - node2->y)),
		  mtr->real_theta -
		  atan2f(node2->y - node1->y, node2->x - node1->x),
		  0.0,
		  mtr->introerr_distance,
		  mtr->introerr_theta,
		  mtr->introerr_phi);
	break;
	
      case ML_CONSTRAINT_THETA:
	bufprintf(ares, "%i %i %f %f theta 0\n", 
		  mtr->chirp_from,
		  mtr->data_from,
		  r2d(mtr->theta),
		  gsl_vector_get(b, i));
	break;

      case ML_CONSTRAINT_PHI:
	bufprintf(ares, "%i %i %f %f phi %f\n", 
		  mtr->chirp_from,
		  mtr->data_from,
		  r2d(mtr->phi),
		  gsl_vector_get(b, i),
		  mtr->peer->result->z - mtr->result->z);
	break;
	
      default:
	break;
      }
    }

    char angleres[4096];
    sprintf(angleres, "angleres%i", k);
    multilatbuf_to_file(mls, angleres, ares);
    buf_free(ares);
    
    char errors[4096];
    sprintf(errors, "errors%i", k);
    multilatbuf_to_file(mls, errors, resbuf);
    buf_free(resbuf);
#endif

    /************************/
    /* studentized residual */
    /************************/
    // H = X ( XT X )-1 XT.
    {
      studbuf = buf_new();

#if 0   /* use martin's weird but working version.. */
      bufprintf(studbuf, "#h res stud real chirpfrom datafrom\n");
      
      int l = 0;
      gsl_matrix *X = gsl_matrix_calloc(num_rows, num_cols);
      gsl_matrix *X2 = gsl_matrix_calloc(num_rows, num_cols);
      gsl_matrix *XTrans = gsl_matrix_calloc(num_cols, num_rows);
      gsl_matrix *XTrans2 = gsl_matrix_calloc(num_cols, num_rows);
      gsl_matrix *XXTrans = gsl_matrix_calloc(num_cols, num_cols);
      gsl_matrix *H = gsl_matrix_calloc(num_rows, num_rows);
      gsl_vector *stud = gsl_vector_calloc(num_rows);
      gsl_vector *eps = gsl_vector_calloc(num_rows);
      double sum = 0.0;
            
      int res = gsl_matrix_memcpy(X, A);
      if (res != GSL_SUCCESS) {
	elog(LOG_CRIT, "Error copying new matrix: %s\n", gsl_strerror(res));
	exit(1);
      }

      res = gsl_matrix_transpose_memcpy(XTrans, X);
      if (res != GSL_SUCCESS) {
	elog(LOG_CRIT, "Error transposing new matrix: %s\n", gsl_strerror(res));
	exit(1);
      }

      gsl_matrix_memcpy(XTrans2, XTrans);
      gsl_matrix_memcpy(X2, X);
      
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, XTrans, X, 1.0, XXTrans);
      gsl_blas_dtrsm(CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit, 1.0, XXTrans, X2);
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, X2, XTrans2, 1.0, H);

      /* compute residuals and the sum of squares */
      sum = 0;
      for (l = 0; l < num_rows; ++l) {
	mreal eps_sum = 0;
	int k;
	for (k = 0; k < num_cols; ++k) {
	  eps_sum += gsl_matrix_get(A, l, k) * gsl_vector_get(x_res, k);
	}
	//elog(LOG_WARNING, "epssum=%f, b=%f", eps_sum, gsl_vector_get(b, l));
	eps_sum -= gsl_vector_get(b, l);
	gsl_vector_set(eps, l, eps_sum);
	sum += sqrf(eps_sum);
      }
      
      /* compute the weighted residuals */
      mreal max_stud = 0;
      for (l = 0; l < num_rows; ++l) {
	mreal sigma = sqrt((sum - sqrf(gsl_vector_get(eps, l))) / (mreal)(num_rows - 3));
	mreal hat = sqrt(1.0 - gsl_matrix_get(H, l, l));
	mreal a = fabs(gsl_vector_get(eps, l) / (sigma * hat));
	gsl_vector_set(stud, l, a);
	
	if (a > max_stud) max_stud = a;
	
	mtr = get_row_range(mls, l);
	multilat_result_t *mrr2 = mtr->result;
	multilat_result_t *mrr1 = mtr->peer->result;

	bufprintf(studbuf, "%f %f %f %i %i\n",
		  gsl_vector_get(eps,l),
		  gsl_vector_get(stud,l),
		  /* $$$ customize based on type!! */
		  mtr->real_distance - sqrtf(m_pow_2(mrr1->x - mrr2->x) + m_pow_2(mrr1->y - mrr2->y)),
		  mtr->chirp_from,
		  mtr->data_from
		  );
      }
      
      bufprintf(studbuf, "\n");
      gsl_vector_free(eps);      
#else
      bufprintf(studbuf, "#h res stud measured chirpfrom datafrom\n");
      
      int l = 0;
      gsl_matrix *X = gsl_matrix_calloc(num_rows, 2);
      gsl_matrix *X2 = gsl_matrix_calloc(num_rows, 2);
      gsl_matrix *XTrans = gsl_matrix_calloc(2, num_rows);
      gsl_matrix *XTrans2 = gsl_matrix_calloc(2, num_rows);
      gsl_matrix *XXTrans = gsl_matrix_calloc(2, 2);
      gsl_matrix *H = gsl_matrix_calloc(num_rows, num_rows);
      gsl_vector *stud = gsl_vector_calloc(num_rows);
      double mean = 0.0;
      double var = 0.0;
            
      for (l = 0; l < num_rows; ++l) {
	mreal resid = gsl_vector_get(b, l);
	mean += resid;
	gsl_matrix_set(X, l, 0, 1);
	gsl_matrix_set(X, l, 1, resid);
	gsl_matrix_set(X2, l, 0, 1);
	gsl_matrix_set(X2, l, 1, resid);
	gsl_matrix_set(XTrans, 0, l, 1);
	gsl_matrix_set(XTrans, 1, l, resid);
	gsl_matrix_set(XTrans2, 0, l, 1);
	gsl_matrix_set(XTrans2, 1, l, resid);
      }
      mean = mean / (num_rows + 0.0);
      
      elog(LOG_WARNING, "mean is %f", mean);
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, XTrans, X, 1.0, XXTrans);
      gsl_blas_dtrsm(CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit, 1.0, XXTrans, X2);
      // upper or lower does not matter // gsl_blas_dtrsm(CblasRight, CblasLower, CblasNoTrans, CblasNonUnit, 1.0, XXTrans, X2);
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, X2, XTrans2, 1.0, H);
	
      /* calculate the variance */
      for (l = 0; l < num_rows; ++l) {
	var += m_pow_2(gsl_vector_get(b,l) - mean);
	//	elog(LOG_WARNING, "%f", gsl_matrix_get(H,l,l));	
      }
      var = var / (num_rows + 0.0);
      elog(LOG_WARNING, "var is %f", var);
      
      mreal max_stud = 0;
      for (l = 0; l < num_rows; ++l) {
	double a = gsl_vector_get(b, l) / (sqrtf(var) * sqrtf(1.0 - gsl_matrix_get(H,l,l)));
	//	gsl_vector_set(stud, l, a);
	gsl_vector_set(stud, l, a >= 0 ? a : -a);
	
	if (gsl_vector_get(stud, l) > max_stud)
	  max_stud = gsl_vector_get(stud, l);
	
	mtr = get_row_range(mls, l);
	//multilat_result_t *mrr2 = mtr->result;
	//multilat_result_t *mrr1 = mtr->peer->result;

	if (get_row_type(mls, l) == ML_CONSTRAINT_RANGE)
	  bufprintf(studbuf, "%f %f %f %i %i\n",
		    gsl_vector_get(b,l),
		    gsl_vector_get(stud,l),
		    mtr->distance,
		    mtr->chirp_from,
		    mtr->data_from
		    );
      }
      
      bufprintf(studbuf, "\n");
#endif

      if (dobreak && mls->studthresh != 0) {      
	if (runagain == 0 && max_stud > mls->studthresh) {
	  for (l = 0; l < num_rows; ++l) {
	    if (gsl_vector_get(stud, l) == max_stud) {
	      
	      last_drop_value = max_stud;
	      
	      /* important! */
	      runagain = 1;
	      
	      mtr = get_row_range(mls, l);
	      char *type = NULL;
	      
	      switch (get_row_type(mls, l)) {
	      case ML_CONSTRAINT_RANGE:
		type = "range";
		mtr->drop_r = 1; mtr->drop_t = 1; mtr->drop_p = 1; 
		break;
	      case ML_CONSTRAINT_THETA:
		type = "theta";
		mtr->drop_t = 1; break;
	      case ML_CONSTRAINT_PHI:
		type = "phi";
		mtr->drop_p = 1; break;
	      }
	      
	      elog(LOG_WARNING, "dropping %s %d->%d (val is %f at row %i)", 
		   type, mtr->chirp_from, mtr->data_from, gsl_vector_get(stud, l), l);
	      
	      bufprintf(droppedbuf, "%i %i %s %f %f %i\n", mtr->chirp_from, mtr->data_from, type,
			gsl_vector_get(b,l),
			gsl_vector_get(stud,l),
			mls->global_run
			);
	      
	      multilatbuf_to_file(mls, "dropped", droppedbuf);
	    }
	  }
	}

	gsl_matrix_free(X);
	gsl_matrix_free(X2);
	gsl_matrix_free(XTrans);
	gsl_matrix_free(XTrans2);
	gsl_matrix_free(XXTrans);
	gsl_matrix_free(H);
	gsl_vector_free(stud);      
	
      }
      
      multilatbuf_to_file(mls, "compare", studbuf);

      elog(LOG_WARNING, " ----- All done!");      
    }
    
    mreal avg_range_resid = 0;
    mreal avg_angle_resid = 0;
    mreal avg_phi_resid = 0;
    mreal rms_range_resid = 0;
    mreal rms_angle_resid = 0;
    mreal rms_phi_resid = 0;
    int range_count = 0;
    int theta_count = 0;
    int phi_count = 0;
    int l;
    for (l = 0; l < num_rows; ++l) {
      switch (get_row_type(mls, l)) {
      case ML_CONSTRAINT_RANGE:
	avg_range_resid += fabs(gsl_vector_get(b,l));
	rms_range_resid += sqrf(gsl_vector_get(b,l));
	range_count++;
	break;
      case ML_CONSTRAINT_THETA:
	avg_angle_resid += fabs(gsl_vector_get(b,l));
	rms_angle_resid += sqrf(gsl_vector_get(b,l));
	theta_count++;
	break;	
      case ML_CONSTRAINT_PHI:
	avg_phi_resid += fabs(gsl_vector_get(b,l));
	rms_phi_resid += sqrf(gsl_vector_get(b,l));
	phi_count++;
	break;
      }
    }

    if (range_count > 0) {
      avg_range_resid /= (float)range_count;
      rms_range_resid = sqrt(rms_range_resid/(float)range_count);      
    }

    if (theta_count > 0) {
      avg_angle_resid /= (float)theta_count;
      rms_angle_resid = sqrt(rms_angle_resid/(float)theta_count);      
    }

    if (phi_count > 0) {
      avg_phi_resid /= (float)phi_count;
      rms_phi_resid = sqrt(rms_phi_resid/(float)phi_count);      
    }

    elog(LOG_CRIT, "**** average residuals:");
    elog(LOG_CRIT, "    range: %f angle: %f phi %f",
	 avg_range_resid, avg_angle_resid, avg_phi_resid);
    elog(LOG_CRIT, "RMS range: %f angle: %f phi %f",
	 rms_range_resid, rms_angle_resid, rms_phi_resid);
    
    /**************/
    /* free stuff */
    /**************/
    
    gsl_matrix_free(A);
    gsl_vector_free(b);
    gsl_matrix_free(new_A);
    gsl_matrix_free(V);
    gsl_vector_free(s);
    //    gsl_vector_free(work);
    gsl_vector_free(x_res);
    //    gsl_vector_free(check);

    buf_t *firstpass = buf_new();
    char thefile[4096];
    memset(thefile, 0, 4096);
    sprintf(thefile, "pass%i", k);
    result_list_print(mls, firstpass);
    multilatbuf_to_file(mls, thefile, firstpass);
    buf_free(firstpass);  

#ifndef STANDALONE_MULTILAT
    /* 
     * check for explosion and update the status! 
     */

    /* pack up the answers and push them to the main thread */
    buf_t *result = buf_new();
    buf_t *answer = buf_new();
    
    multilat_result_t *node1; 
    for (node1 = result_list_top(mls->multilat_lists);
	 node1; node1 = result_list_next(node1)) {
      
      if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) {
	elog(LOG_WARNING, "skipping node %d that isn't in the matrix..", node1->node);
	continue;
      }

      if (fabs(node1->x) > 100000 || fabs(node1->y) > 100000 || 
	  fabs(node1->z) > 100000 || fabs(node1->yaw) > 100000 || 
	  !isnormal(node1->x) || !isnormal(node1->y) || 
	  !isnormal(node1->z) || !isnormal(node1->yaw)) {
	elog(LOG_CRIT, "oops.. it blew up!");
	mls->failed_convergence = 1;
	buf_free(answer);
	buf_free(result);
	goto mlat_failed;
      }
      
      coord_entry_t ce = {
	node: node1->node,
	coord: { node1->x*10, node1->y*10, node1->z*10 },
	rpy: { r2d(-node1->roll)*10, r2d(-node1->pitch)*10, r2d(-node1->yaw)*10 },
	valid: 1
      };
      
      bufcpy(answer, &ce, sizeof(ce));
    }
    
    buf_t *errors = mlat_errors_to_buf(mls, NULL);
    
    bufcpy(result, &answer, sizeof(answer));
    bufcpy(result, &errors, sizeof(errors));
    bufcpy(result, &studbuf, sizeof(studbuf));
    studbuf = NULL;

    g_msg_queue_push(mls->results_queue, result);
#endif

    if (dobreak) {
      dobreak = 0;
      break;
    }

    //#ifdef STANDALONE_MULTILAT
    dump_errors(mls, k);
    //#endif
  }

  /* print out the final error stuff */
  buf_t *ce = buf_new();
  bufprintf(ce, "#h node coorderror\n");
  mreal rms = 0;
  int count = 0;
  for (mlr = result_list_top(mls->multilat_lists);
       mlr; mlr = result_list_next(mlr)) {
    if (mlr->state != RESULT_COORD_ROOT) {
      bufprintf(ce, "%i %f\n", mlr->node,
		sqrtf(m_pow_2(mlr->x - mlr->real_x) +
		      m_pow_2(mlr->y - mlr->real_y) +
		      m_pow_2(mlr->z - mlr->real_z))
		);
      rms += m_pow_2(mlr->x - mlr->real_x) +
	m_pow_2(mlr->y - mlr->real_y) +
	m_pow_2(mlr->z - mlr->real_z);
      count++;
    }

  }
  bufprintf(ce, "# rms: %f\n", sqrtf(rms / (count + 0.0)));
  bufprintf(ce, "# Grms: %f\n", mls->guess_rms);
  multilatbuf_to_file(mls, "coorderr", ce);
  buf_free(ce);

  iterations = k;
#ifdef STANDALONE_MULTILAT
  analyse_result(mls);
#endif

  /*************/
  /* increment */
  /*************/
 
  if (runagain) {
    total_passes++;
    mls->global_run++;
    runagain = 0;
    goto repeate;
  }

  multilatbuf_to_file(mls, "dropped", droppedbuf);
  buf_free(droppedbuf);
  
  mls->global_run = 1000;

  {
    buf_t *firstpass = buf_new();
    char thefile[4096];
    memset(thefile, 0, 4096);
    sprintf(thefile, "last");
    result_list_print(mls, firstpass);
    multilatbuf_to_file(mls, thefile, firstpass);
    buf_free(firstpass);  
  }

#ifdef STANDALONE_MULTILAT
  analyse_result(mls);
#endif
  mls->failed_convergence = 2;

  LOG_ENTRY("****multilat success, %d, %d", total_iterations, total_passes);  
  return 0;

 mlat_failed:
  LOG_ENTRY("****multilat failed, %d, %d", total_iterations, total_passes);  
#ifdef STANDALONE_MULTILAT
  exit(1);
#else
  return -1;
#endif
}
#endif

void *multilat_thread_main(void *arg) {
  
  ml_state_t *mls = (ml_state_t *) arg;

  /* nice down */
  nice(10);
  
 do_multilat:
  
#ifndef STANDALONE_MULTILAT
  /* wait for new data to appear */
  pthread_mutex_lock(&(mls->range_snapshot_mutex));
  while (!mls->master_mode || mls->latest_table == NULL) {
    pthread_cond_wait(&(mls->range_snapshot_cond),
		      &(mls->range_snapshot_mutex));
  }

  int i;
  multilat_range_t *mlr;

  /* clear the last range set */
  while ((mlr = range_list_pop(mls->multilat_lists))) {
    free(mlr);
  }
  coord_guess_t *cgt;
  while ((cgt = guess_list_pop(mls->multilat_lists))) {
    free(cgt);
  }
  multilat_result_t *mrt;
  while ((mrt = result_list_pop(mls->multilat_lists))) {
    free(mrt);
  }
  mls->global_run = 0;

  /* fill the new range set */
  for (i = 0; i < mls->latest_table_count; ++i) {
    mlr = g_new0(multilat_range_t, 1);

    mlr->chirp_from = mls->latest_table[i].range_entry.source;
    mlr->data_from = mls->latest_table[i].header.flow_id.src;
    
    if (mlr->chirp_from == mlr->data_from || 
	mlr->chirp_from == 0 || mlr->data_from == 0)
      goto free_it;
    
    /* from tenths of degrees to radians */
    mlr->theta = (M_PI * (float) mls->latest_table[i].range_entry.theta) / 1800.0;
    mlr->phi = (M_PI * (float) mls->latest_table[i].range_entry.phi) / 1800.0;
    
    mlr->distance = mls->latest_table[i].range_entry.distance / 10.0;
    
    if (mlr->distance == 0) 
      goto free_it;

#ifdef EMBEDDED_USECONF
    mlr->angle_conf = mls->latest_table[i].range_entry.a_conf / 10.0;
    mlr->conf = mls->latest_table[i].range_entry.conf / 10.0;
#else
    mlr->angle_conf = 1;
    mlr->conf = 10;
#endif

    elog(LOG_WARNING, "Processing %d->%d, r=%f (cm) t=%f (%f) p=%f",
	 mlr->chirp_from, mlr->data_from, mlr->distance, mlr->theta, r2d(mlr->theta), mlr->phi);

    range_list_push(mls->multilat_lists, mlr);
    continue;
    
  free_it:
    free(mlr);
  }

  free(mls->latest_table);
  mls->latest_table = NULL;
  mls->latest_table_count = 0;
  pthread_mutex_unlock(&(mls->range_snapshot_mutex));
#endif

  elog(LOG_WARNING, "Other thread got new data, %d entries", range_list_qlen(mls->multilat_lists));
  update_result_list(mls); /* add new nodes to the result list */
  
  /* dump the current range table */
  multilat_range_t *tmp;
  elog(LOG_WARNING, "DUMPING RANGE TABLE:");
  elog(LOG_WARNING, "src dst distance theta phi aconf conf state ur dr dt dp ur ut up us:");
  for (tmp = range_list_top(mls->multilat_lists); tmp; tmp=range_list_next(tmp)) {
    elog(LOG_WARNING, "%d %d %.1f %.2f %.2f %.1f %.1f %d %.1f %d %d %d %d %d %d %d",
	 tmp->chirp_from, tmp->data_from, tmp->distance, tmp->theta, tmp->phi,
	 tmp->angle_conf, tmp->conf, tmp->state, tmp->userange,
	 tmp->drop_r, tmp->drop_t, tmp->drop_p,
	 tmp->use_r, tmp->use_t, tmp->use_p, tmp->use_set);
  }

  initialize_guess_stage(mls);
  if (!guess_coords(mls)) {
    goto do_multilat;
  }
  
  buf_t *guessx = buf_new();
  result_list_print(mls, guessx);
  multilatbuf_to_file(mls, "step1", guessx);
  buf_free(guessx);
  
#ifdef STANDALONE_MULTILAT
  if (mls->addfuzz) {
    add_fuzz_to_results(mls);
  }
#endif

  guessx = buf_new();
  result_list_print(mls, guessx);
  multilatbuf_to_file(mls, "step2", guessx);
  buf_free(guessx);

#ifdef STANDALONE_MULTILAT
  multilat_result_t *res = result_list_top(mls->multilat_lists);
  mreal rms = 0;
  int count = 0;
  for ( ; res != NULL; res = result_list_next(res) ) {
    if (res->state != RESULT_COORD_ROOT) {
      rms += m_pow_2(res->x - res->real_x) +
	m_pow_2(res->y - res->real_y) +
	m_pow_2(res->z - res->real_z);
      count++;
    }
    
  }
  mls->guess_rms = sqrtf(rms / (count + 0.0));
#endif

  /* note counts of nodes and ranges */
  mls->result_element_count = result_list_qlen(mls->multilat_lists);
  mls->range_element_count = range_list_qlen(mls->multilat_lists);

  result_save(mls);
  multilat_solver(mls);

#ifdef STANDALONE_MULTILAT
  exit(1);
#endif

  goto do_multilat;

}


void multilat_thread_init(ml_state_t *mls) {
  
  /* setup mutex and cond */
  pthread_mutex_init(&(mls->range_snapshot_mutex), NULL);
  pthread_cond_init(&(mls->range_snapshot_cond), NULL);

  /* spawn thread */
  pthread_attr_t thread_attr;
  pthread_attr_init(&thread_attr);
  if (pthread_create(&(mls->multilat_thread), &thread_attr, multilat_thread_main, mls) < 0) {
    elog(LOG_CRIT, "Failed to spawn compute thread: %m");
    exit(1);
  }

}



#ifdef STANDALONE_MULTILAT


int killpairs[3][20];
int killpairs_count = 0;
char *killpairs_file = NULL;

void read_killpairs()
{
  FILE *f = fopen(killpairs_file, "r");
  while (2 == fscanf(f, "%d %d", &(killpairs[0][killpairs_count]),
		     &(killpairs[1][killpairs_count]))) {
    printf("read killpair %d %d\n",
	   killpairs[0][killpairs_count],
	   killpairs[1][killpairs_count]);
    killpairs[2][killpairs_count] = (random()%100 < killpairs_prob*100);
    killpairs_count++;
  }
  fclose(f);
}


/*
 * for median finding
 */

struct sorter {
  int index;
  float value;
};

int sorter_cmp(const void *x, const void *y) {
  return ((struct sorter *)x)->value - ((struct sorter *)y)->value;
}


int read_in_file(ml_state_t *mls)
{
  if ((mls->fakedata = fopen(mls->filename, "r")) == NULL) {
    elog(LOG_CRIT, "Unable to open file %s", mls->filename);
    exit(1);
  }


  int res = 0;
  int chirper = 0;
  int listener = 0;
  float distance = 0;
  float theta = 0;
  float phi = 0;
  float temp = 0;
  float real_distance = 0;
  float real_theta = 0;
  float real_phi = 0;
  float real_x = 0;
  float real_y = 0;
  float real_z = 0;
  float real_yaw = 0;
  float conf = 10;
  float aconf = 1;
  
  char input[8192];
  memset(&input, 0, 8192);

  while (1) {
    
    if (fgets(input,8192,mls->fakedata) == NULL) {
      break;
    }
    
    if (input[0] == '#') {
      continue;
    }

    if (mls->include_conf) {
      res = sscanf(input, "%d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
		   &chirper, &listener, &distance, &theta, &phi,
		   &real_x, &real_y, &real_z, &temp, &temp, &temp, &temp, 
		   &temp, &real_distance, &real_theta, &real_phi,
		   &conf, &aconf);
    } else {
      res = sscanf(input, "%d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f",
		   &chirper, &listener, &distance, &theta, &phi,
		   &real_x, &real_y, &real_z, &temp, &temp, &temp, &temp, 
		   &temp, &real_distance, &real_theta, &real_phi);
    }
    if (res == EOF) {
      elog(LOG_WARNING, "End of input file %s", mls->filename);
      break;
    }
    if (res != (mls->include_conf ? 18:16)) {
      elog(LOG_WARNING, "error in file %s; status was %d", mls->filename, res);
      continue;
    }
    
    multilat_range_t *mlr = g_new0(multilat_range_t, 1);
    mlr->chirp_from = chirper;
    mlr->data_from = listener;
    if (mls->input_in_degrees) {
      mlr->theta = theta/180.0*M_PI;
      mlr->phi = phi/180.0*M_PI;
    }
    else {
      mlr->theta = theta;
      mlr->phi = phi;
    }
    mlr->distance = distance;
    mlr->real_distance = real_distance;
    mlr->real_theta = real_theta;
    mlr->real_phi = real_phi;
    mlr->introerr_distance = mlr->real_distance - mlr->distance;
    mlr->introerr_theta = mlr->real_theta - mlr->theta;
    mlr->introerr_phi = mlr->real_phi - mlr->phi;
    mlr->conf = conf;
    mlr->angle_conf = aconf;
    
    int ll;
    for (ll=0; ll<killpairs_count; ll++) {
      if ((chirper == killpairs[0][ll] &&
	   listener == killpairs[1][ll]) ||
	  (chirper == killpairs[1][ll] &&
	   listener == killpairs[0][ll])) {
	if (killpairs[2][ll]) {

	  /*
	   *  used for simulating reflections.\
	   *  you can set a list of pairs to remove in a file you
	   *  pass in.  then you can also specify specific
	   *  ranges to cause to reflect.
	   *  reflections add 10m and may or may not add a 45 degree
	   *  angular error.
	   */

	  int disterr = 1000;
	  int angleerr = 0 * (chirper == killpairs[1][ll] ? +1 : -1);
	  printf("offsetting %d,%d by %d cm, %d deg\n",
		 chirper, listener, disterr, angleerr);
	  mlr->distance += disterr;
	  mlr->theta += angleerr*M_PI/180.0;
	}
	else {
	  printf("nuking %d,%d\n",
		 chirper, listener);
	  mlr->conf = 0;
	}
	break;
      }
    }

    if (mlr->conf == 0 || mlr->distance == 0) {
      elog(LOG_WARNING, "skipping 0 range %d->%d", 
	   mlr->chirp_from, mlr->data_from);
      free(mlr);
      goto skip;
    }
    
    range_list_push(mls->multilat_lists, mlr);
    
    result_list_initnode(mls, listener);
    result_list_initnode(mls, chirper);
    multilat_result_t *mtr = result_list_get(mls, chirper);
    mtr->real_x = real_x;
    mtr->real_y = real_y;
    mtr->real_z = real_z;
    //mtr->nogt = 0;  // get it from the map file!!
    
    //  update_result_list(mls); /* add new nodes to the result list */
    
  skip:
    elog(LOG_DEBUG(0), "read entry: %d %d %f %f %f", chirper, listener, theta, phi, distance);
  }

  fclose(mls->fakedata);

  if (mls->gt_filename) {
    FILE *f = fopen(mls->gt_filename, "r");
    if (f == NULL) {
      elog(LOG_WARNING, "unable to read gt from file %s", mls->gt_filename);
      exit(1);
    }

    while (1) {
      if (fgets(input,8192,f) == NULL) {
	break;
      }
      
      if (input[0] == '#') {
	continue;
      }
      
      int node;
      res = sscanf(input, "%d %f %f %f %f",
		   &node, &real_x, &real_y, &real_z, &real_yaw);
      if (res == EOF) {
	elog(LOG_WARNING, "End of input file %s", mls->gt_filename);
	break;
      }
      if (res != 5) {
	elog(LOG_WARNING, "error in file %s", mls->filename);
	continue;
      }

      result_list_initnode(mls, node);
      multilat_result_t *mtr = result_list_get(mls, node);
      mtr->real_x = real_x*100.0;
      mtr->real_y = real_y*100.0;
      mtr->real_z = real_z*100.0;
      mtr->real_yaw = real_yaw;
      mtr->nogt = 0;

      elog(LOG_WARNING, "read %d %f %f %f", 
	   node, real_x, real_y, real_z);
    }

    fclose(f);  
  }

  /* prune duplicates, keep median phi angle */
  multilat_range_t *ptr;
  multilat_range_t *tmp;
  for (ptr = range_list_top(mls->multilat_lists); ptr; ptr = tmp) {
    
    multilat_range_t *ptr2;
    multilat_range_t *tmp2;
    multilat_stages_t lists;
    memset(&lists, 0, sizeof(lists));
    
    /* move dups into list */
    for (ptr2 = range_list_next(ptr); ptr2; ptr2 = tmp2) {
      tmp2 = range_list_next(ptr2);
      if (ptr->chirp_from == ptr2->chirp_from &&
	  ptr->data_from == ptr2->data_from) {
	range_list_remove(mls->multilat_lists, ptr2);
	range_list_push(&lists, ptr2);
      }
    }
    
    /* inc and move this one into list */
    tmp = range_list_next(ptr);
    range_list_remove(mls->multilat_lists, ptr);
    range_list_push(&lists, ptr);
    int r_count = range_list_qlen(&lists);
    struct sorter s[r_count];
    
    /* now process list */
    int i;
    int q[5] = {0,0,0,0,0};
    for (i=0, ptr2 = range_list_top(&lists); ptr2; i++, ptr2 = range_list_next(ptr2)) {
      s[i].index = i;
      s[i].value = ptr2->theta;
      if (s[i].value < 0)
	s[i].value += (M_PI*2.0);
      q[(int)(s[i].value/(M_PI/2.0))]++;
    }
    
    q[3] += q[4];
    q[4] = 0;
    
    int maxq;
    int maxind=-1;
    for (i=0; i<4; i++) {
      int thisq = q[i] + q[(i+1)%4];
      if (maxind < 0 || thisq > maxq) {
	maxind = i;
	maxq = thisq;
      }
    }
    
    /* flip? */
    if (maxind == 3) {
      for (i=0; i<r_count; i++) 
	if (s[i].value > M_PI/2.0 && s[i].value < 3.0*M_PI/2.0)
	  s[i].index = -1;
	else
	  if (s[i].value > M_PI)
	    s[i].value -= (2.0*M_PI);
    }
    
    else {
      for (i=0; i<r_count; i++) {
	int q_num = ((int)(s[i].value / (M_PI/2.0)));
	if (q_num == 4) q_num = 3;
	if (q_num != maxind && q_num != maxind+1)
	  s[i].index = -1;
      }
    }
    
    /* filter stuff out of 2 quadrants */
    int drops=0;
    for (i=0; i<r_count; i++) {
      if (s[i].index < 0) {
	elog(LOG_WARNING, "dropping outlier value %f, for pair %d->%d",
	     s[i].value, ptr->chirp_from, ptr->data_from);
	drops++;
      }
      else if (drops > 0) {
	s[i-drops] = s[i];
      }
    }
    r_count -= drops;
    
    /* sort and keep median */
    if (r_count > 0) {
      qsort(&s, r_count, sizeof(struct sorter), sorter_cmp);
      
      /* keep median */
      int med_idx = s[r_count/2].index;
      for (i=0, ptr2=range_list_top(&lists); i<med_idx; 
	   i++, ptr2=range_list_next(ptr2));
      
      range_list_remove(&lists, ptr2);
      range_list_push_front(mls->multilat_lists, ptr2);
      elog(LOG_WARNING, "keeping median: %d->%d %f %f %f", 
	   ptr2->chirp_from, ptr2->data_from, ptr2->theta, 
	   ptr2->phi, ptr2->distance);
    }
    
    /* free dups */
    while ((ptr2 = range_list_pop(&lists))) free(ptr2);
  }
    
  return 0;
}

#define OUTPUTPREFIX "/tmp/"

int main(int argc, char **argv) {
  
  
  ml_state_t mls = {
    master_mode: 1
  };

  misc_init(&argc, argv, CVSTAG);
  char *tempstr1;
  char *tempstr2;
  float studthresh = 0;

  mls.filename = misc_parse_out_option(&argc, argv, "file", 'f');
  if (mls.filename == NULL || strcmp(mls.filename, "") == 0) {
    elog(LOG_CRIT, "Please input valid filename!");
    exit(1);
  }

  char *kp;
  if ((kp = misc_parse_out_option(&argc, argv, "kp", 0))) {
    sscanf(kp, "%d,%d", &killpairs[0][0], &killpairs[1][0]);
    killpairs[2][0] = 1;
    killpairs_count = 1;
  }

  misc_parse_option_as_float(&argc, argv, "kprob", 0, &killpairs_prob);
  killpairs_file = misc_parse_out_option(&argc, argv, "killpairs", 0);
  if (killpairs_file)
    read_killpairs(killpairs_file);
  
  mls.gt_filename = misc_parse_out_option(&argc, argv, "gt", 0);
  
  mls.outputprefix = misc_parse_out_option(&argc, argv, "outputprefix", 'p');
  if (mls.outputprefix == NULL || strcmp(mls.outputprefix, "") == 0) {
    elog (LOG_CRIT, "no output prefix set, setting to /tmp");
    mls.outputprefix = OUTPUTPREFIX;
  }
  
  mls.noreject = misc_parse_out_switch(&argc, argv, "no_reject", 0);
  mls.addfuzz = misc_parse_out_switch(&argc, argv, "fuzz", 'z');
  mls.useangles = misc_parse_out_switch(&argc, argv, "useangle", 'a');
  mls.input_in_degrees = misc_parse_out_switch(&argc, argv, "degrees", 0);
  mls.include_conf = misc_parse_out_switch(&argc, argv, "useconf", 0);

  if (misc_parse_option_as_float(&argc, argv, "studres", 's', &studthresh) != 0) {
    studthresh = 0;
  }
  mls.studthresh = studthresh;

  tempstr1 = misc_parse_out_option(&argc, argv, "one", '1');
  if (tempstr1 != NULL) {
    sscanf(tempstr1, "%i:%lf,%lf,%lf,%lf", 
	   &mls.anode1, &mls.x1, &mls.y1, &mls.z1, &mls.a1);
    mls.a1 = mls.a1/180.0*M_PI;
    elog(LOG_WARNING, "fixed node %i at %f %f %f, angle %f", 
	 mls.anode1, mls.x1, mls.y1, mls.z1, mls.a1);
  }

  tempstr2 = misc_parse_out_option(&argc, argv, "two", '2');
  if (tempstr2 != NULL) {
    sscanf(tempstr2, "%i:%lf,%lf,%lf,%lf", 
	   &mls.anode2, &mls.x2, &mls.y2, &mls.z2,&mls.a2);
    mls.a2 = mls.a2/180.0*M_PI;
    elog(LOG_WARNING, "fixed node %i at %f %f %f angle %f",
	 mls.anode2, mls.x2, mls.y2, mls.z2, mls.a2);
  }

  mls.multilat_lists = g_new0(multilat_stages_t, 1);
  range_list_init(mls.multilat_lists);  
  guess_list_init(mls.multilat_lists);  
  result_list_init(mls.multilat_lists);  

  read_in_file(&mls);

  multilat_thread_main(&mls);

  exit(1);
}


#endif