/*  points.c  (C) 1997 Mark A. Livingston */
/* Any non-commercial use of this code is permitted so long as */
/* the above copyright line appears in the file.  Please contact */
/* the author regarding permission for commercial use. */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <quat.h>
#include "file.h"
#include "quatext.h"
#include "time.h"
#include "config.h"
#include "data.h"
#include "stax.h"
#include "cog.h"

#define HISTOSIZE 100
#define MAX_TO_AVG 250

enum { COGDISTPRE = 0, COGDISTPOST, NUMSTATS };

void
usage( char *Progname )
{
    printf( "Usage: %s [ options ] samples-file\n", Progname );
    printf( "\tOptions are:\n" );
    printf( "\t\t-cfg file\t = Read configurations from 'file'\n" );
    printf( "\t\t-file name\t = Write data to file (default = stdout)\n" );
    printf( "\t\t-verbose\t = Enter verbose mode\n" );
    printf( "\t\t-nographics\t = Build and write table only\n" );
    printf( "\t\t-help\t\t = Print this usage message\n" );

    return;
}

void
main( int argc, char *argv[] )
{
    int i, j, k, code;
    FILE *outfp = stdout;
    char *cfgFile = NULL, *outFile = NULL;
    int verbose = 0, showHisto = 1;
    ConfigRec *cR;
    /* samples and counters */
    int ptsAcc, ptsUsed;
    q_xyz_quat_type *fob, *faro;
    /* faster lvalue in resampling code */
    TableNode *tabQ;
    /* resampling variables */
    int xIndex, yIndex, zIndex;
    double x, y, z;
    q_vec_type curPt, posErr;
    double dist, minDist, weight, totalWeight;
    TableNode *tableQuat, nn;
    int gridCells;
    int usageHisto[ HISTOSIZE + 2 ];
    /* quat averaging */
    double angleQ, angleCum;
    q_vec_type axis, axisCum;
    q_xyz_quat_type methDiff;
    q_xyz_quat_type *q_to_avg;
    double *q_weights;
    /* COG measurements */
    q_vec_type cog, cog_incr, *samp_pts_vec;
    double cog_dist_pre, cog_dist_post;
    int cog_reduc_tries, cog_reduc_success;

    /* Parse arguments and options */
    for( i = 1; i < argc; i++ ) {
      if( !strcmp( argv[i], "-cfg" ) ) {
	cfgFile = argv[ ++i ];
      } else if( !strcmp( argv[i], "-verbose" ) ) {
        verbose = 1;
      } else if( !strcmp( argv[i], "-nohist" ) ) {
	showHisto = 0;
      } else if( !strcmp( argv[i], "-file" ) ) {
	outFile = argv[ ++i ];
      } else if( !strcmp( argv[i], "-help" ) ) {
        usage( argv[0] );
        exit( -1 );
      } else {
        fprintf( stderr, "%s: WARNING: Unknown argument '%s' ignored\n",
                 argv[0], argv[i] );
      }
    }
    cR = ParseConfigFile( cfgFile, verbose, outFile );

    /* Alloc */
    fob  = NEWXYZQUAT( cR->totalPts );
    faro = NEWXYZQUAT( cR->totalPts );
    /* Don't really know how many this should be */
    q_to_avg = NEWXYZQUAT( MAX_TO_AVG );
    q_weights = NEWDOUBLE( MAX_TO_AVG );
    samp_pts_vec = NEWVEC( MAX_TO_AVG );
    /* Read the data */
    ptsAcc = ReadDataFiles( cR, fob, faro );

    /* Count of number of points with N samples nearby */
    for( i = 0; i <= HISTOSIZE + 1; i++ )
      usageHisto[i] = 0;

    /* Initialize COG stats */
    stxInit( NUMSTATS );
    cog_reduc_tries = cog_reduc_success = 0;

    /* Allocate space for the table */
    gridCells = cR->xSteps * cR->ySteps * cR->zSteps;
    tableQuat = (TableNode *) malloc( gridCells * sizeof( TableNode ) );

    /* Now traverse the 3D grid and build the table */
    for( xIndex = 0; xIndex < cR->xSteps; xIndex++ )
      for( yIndex = 0; yIndex < cR->ySteps; yIndex++ )
	for( zIndex = 0; zIndex < cR->zSteps; zIndex++ ) {
	  /* Initialize data for this location */
	  ptsUsed = 0;
	  totalWeight = 0.0;
	  tabQ = &tableQuat[ INDEX( xIndex, yIndex, zIndex,
				    cR->xSteps, cR->ySteps, cR->zSteps ) ];
#ifdef NO_MORE_SVD
	  /* not using q_make b/c want all four to be zero */
	  q_vec_set( tabQ->error.xyz, 0.0, 0.0, 0.0 );
	  tabQ->error.quat[ Q_X ] = 0.0;
	  tabQ->error.quat[ Q_Y ] = 0.0;
	  tabQ->error.quat[ Q_Z ] = 0.0;
	  tabQ->error.quat[ Q_W ] = 0.0;
#endif
	  q_vec_set( cog, 0.0, 0.0, 0.0 );

	  /* Where am I? */
	  if( cR->xSteps == 1 ) x = cR->xMin;
	  else x = cR->xMin + ( cR->xMax - cR->xMin ) /
	                      (double)( cR->xSteps - 1 ) * (double) xIndex;
	  if( cR->ySteps == 1 ) y = cR->yMin;
	  else y = cR->yMin + ( cR->yMax - cR->yMin ) /
	                      (double)( cR->ySteps - 1 ) * (double) yIndex;
	  if( cR->zSteps == 1 ) z = cR->zMin;
	  else z = cR->zMin + ( cR->zMax - cR->zMin ) /
	                      (double)( cR->zSteps - 1 ) * (double) zIndex;
	  q_set_vec( curPt, x, y, z ); /* for using quatlib functions */

	  q_set_vec( axisCum, 0.0, 0.0, 0.0 );
	  angleCum = 0.0;

	  minDist = 10000.0;
	  /* Doing this the stupid way - no sorting or anything */
	  for( i = 0; i < ptsAcc; i++ ) {
	    /* Determine if this point is close enough */
	    dist = q_vec_distance( curPt, fob[i].xyz );
/* METHOD #1: interpolate all "near-enough" samples */
	    if( dist < cR->kernelRadius ) {
	      /* Compute the weight for this sample, and accumulate */
	      q_weights[ ptsUsed ] = KernelValue( cR->kernelSigma, dist );
	      totalWeight += q_weights[ ptsUsed ];

	      /*
	      ** Now add in contribution from this sample to table entry.
	      ** We want the position correction (faro[i].xyz - fob[i].xyz)
	      ** and the orientation correction - in fob[i].quat.
	      */
	      /* Selecting like this allows SVD averaging */
              q_vec_subtract( q_to_avg[ptsUsed].xyz, faro[i].xyz, fob[i].xyz );
	      q_copy( q_to_avg[ ptsUsed ].quat, fob[i].quat );
	      q_vec_copy( samp_pts_vec[ ptsUsed ], fob[i].xyz );

	      /* COG tracking */
	      q_vec_scale( cog_incr, q_weights[ ptsUsed ], fob[i].xyz );
	      q_vec_add( cog, cog, cog_incr );

	      ptsUsed++;
#ifdef NO_MORE_SVD
#ifdef ABS_POSN
	      q_vec_subtract( posErr, curPt, fob[i].xyz );
	      q_vec_add( posErr, faro[i].xyz, posErr );
#else
	      q_vec_subtract( posErr, faro[i].xyz, fob[i].xyz );
#endif
	      q_vec_scale( posErr, weight, posErr );
	      q_vec_add( tabQ->error.xyz, tabQ->error.xyz, posErr );
#if 1
	      tabQ->error.quat[ Q_X ] += weight * fob[i].quat[ Q_X ];
	      tabQ->error.quat[ Q_Y ] += weight * fob[i].quat[ Q_Y ];
	      tabQ->error.quat[ Q_Z ] += weight * fob[i].quat[ Q_Z ];
	      tabQ->error.quat[ Q_W ] += weight * fob[i].quat[ Q_W ];
#else
	      angleQ = 2.0 * acos( fob[i].quat[ Q_W ] );
	      q_set_vec( axis, fob[i].quat[ Q_X ], fob[i].quat[ Q_Y ],
			 fob[i].quat[ Q_Z ] );
	      q_vec_normalize( axis, axis );
	      q_vec_scale( axis, weight * angleQ, axis );
	      q_vec_add( axisCum, axisCum, axis );
	      angleCum += SQR( angleQ ) * weight;
#endif
#endif /* NO_MORE_SVD */

	      if( cR->verbose ) {
		fprintf( cR->outfp, "Using XYZ QUAT: (%lf) ", weight );
		qx_file_xyz_quat_print( cR->outfp, &fob[i] );

		if( dist < 0.002 ) {
		  fprintf( cR->outfp, "FOB TEST: " );
		  qx_file_xyz_quat_print( cR->outfp, &fob[i] );
		}
	      }
	    } /* end if point is close enough */

/* METHOD #2: use nearest-neighbor's correction */
	    if( dist < minDist ) {
	      minDist = dist;
	      q_vec_subtract( nn.error.xyz, faro[i].xyz, fob[i].xyz );
	      q_normalize( nn.error.quat, fob[i].quat );
	    }
	  } /* end for each sample point */

#ifndef NEARNEIGHBOR
	  /* Did we have enough points at this location? */
	  if( ptsUsed >= cR->minPoints && totalWeight >= cR->minWeight ) {
            tabQ->enoughData = 1;

	    /* COG tracking */
	    q_vec_scale( cog, 1.0 / totalWeight, cog );
	    cog_dist_pre = q_vec_distance( cog, curPt );
	    stxUpdate( COGDISTPRE, cog_dist_pre );
	    if( cog_dist_pre > cR->cogDistance && ptsUsed > 1 ) {
	      ptsUsed = cogReduce( q_to_avg, q_weights, samp_pts_vec,
				   ptsUsed, cog, curPt );
	      cog_reduc_tries++;
	    }
	    cog_dist_post = q_vec_distance( cog, curPt );
	    if( cog_dist_post < cog_dist_pre )
	      cog_reduc_success++;
	    stxUpdate( COGDISTPOST, cog_dist_post );

	    /* average using SVD method */
	    qx_xyz_quat_wgt_avg( &tabQ->error, q_to_avg, q_weights, ptsUsed );
#ifdef NO_MORE_SVD
	    /* YES: convert the sum into an average */
	    weight = 1.0 / totalWeight; /* faster rvalue */
	    q_vec_scale( tabQ->error.xyz, weight, tabQ->error.xyz );
#if 1
	    tabQ->error.quat[ Q_X ] *= weight;
	    tabQ->error.quat[ Q_Y ] *= weight;
	    tabQ->error.quat[ Q_Z ] *= weight;
	    tabQ->error.quat[ Q_W ] *= weight;
	    q_normalize( tabQ->error.quat, tabQ->error.quat );
#else
	    q_vec_normalize( axisCum, axisCum );
	    angleCum *= weight;
	    angleCum = sqrt( angleCum );
	    q_make( tabQ->error.quat, axisCum[ Q_X ], axisCum[ Q_Y ],
		    axisCum[ Q_Z ], angleCum );
#endif
#endif /* NO_MORE_SVD */
	    q_vec_copy( tabQ->loc, curPt );

	    if( cR->verbose ) {
	      fprintf( cR->outfp, "AVG QUAT = " );
	      qx_file_xyz_quat_print( cR->outfp, &tabQ->error );
	    }
	  } else {
	    tabQ->enoughData = 0;
	    /* NO: set the entry to identities */
	    q_set_vec( tabQ->error.xyz, 0.0, 0.0, 0.0 );
	    q_make( tabQ->error.quat, 0.0, 0.0, 0.0, 1.0 );
	    q_vec_copy( tabQ->loc, curPt );

#if 1
	    printf( "Not enough data (%d,%lf) at ", ptsUsed, totalWeight );
	    q_vec_print( curPt );
#endif

	    if( cR->verbose ) {
	      fprintf( cR->outfp, "Canceled grid cell\n" );
	    }
	  }
#else
	  /* Copy the nearest neighbor's correction */
	  q_vec_copy( tabQ->error.xyz, nn.error.xyz );
	  q_normalize( tabQ->error.quat, nn.error.quat );
#endif

#if 0
#ifndef NEARNEIGHBOR
/* COMPARE THE TWO METHODS */
	  q_vec_add( methDiff.xyz, curPt, tabQ->error.xyz );
	  q_vec_subtract( methDiff.xyz, methDiff.xyz, nn.error.xyz );
	  qx_diff( methDiff.quat, tabQ->error.quat, nn.error.quat );
	  printf( "methods differ: pos = %lf  ori = %lf\n",
		  q_vec_distance( curPt, methDiff.xyz ),
		  Q_RAD_TO_DEG( 2.0 * acos( methDiff.quat[Q_W] ) ) );
#endif
#endif
	  /* Some statistics about the resampling */
	  if( ptsUsed <= HISTOSIZE )
	    usageHisto[ ptsUsed ]++;
	  else
	    usageHisto[ HISTOSIZE + 1 ]++;
	} /* end for( z, y, x ) table positions */

    if( showHisto ) {
      /* histogram of number of samples used to determine a grid point */
      printf( "Number of grid points = %d\nSamples Used\tGrid Points\n",
	      gridCells );
      for( i = 0; i <= HISTOSIZE; i++ )
	printf( "     %2d      \t   %4d\n", i, usageHisto[i] );
      printf( "more than %3d\t   %4d\n", HISTOSIZE, usageHisto[ HISTOSIZE+1 ]);
    }

    /* center of gravity statistics - only valid if table complete */
    stxPrint( COGDISTPRE,  "",   "Ctr of grav PRE " );
    stxPrint( COGDISTPOST, NULL, "Ctr of grav POST" );
    printf( "Tried COG reduction %d times, worked %d times\n",
	    cog_reduc_tries, cog_reduc_success );

    /* If the user wants, write the table to a file */
    if( cR->outfp != stdout ) {
      /* Output the dimensions of the table */
      fprintf( cR->outfp, "%d %d %d\n", cR->xSteps, cR->ySteps, cR->zSteps );
      fprintf( cR->outfp, "%lf %lf %lf\n", cR->xMin, cR->yMin, cR->zMin );
      fprintf( cR->outfp, "%lf %lf %lf\n", cR->xMax, cR->yMax, cR->zMax );
      for( xIndex = 0; xIndex < cR->xSteps; xIndex++ )
	for( yIndex = 0; yIndex < cR->ySteps; yIndex++ )
	  for( zIndex = 0; zIndex < cR->zSteps; zIndex++ ) {
	    tabQ= &tableQuat[ INDEX( xIndex, yIndex, zIndex,
				     cR->xSteps, cR->ySteps, cR->zSteps ) ];

#ifdef DEBUG
	    fprintf( cR->outfp, "{ { %lf, %lf, %lf }, ", tabQ->loc[Q_X],
		     tabQ->loc[Q_Y], tabQ->loc[Q_Z] );
#endif
	    fprintf( cR->outfp, "{ %lf, %lf, %lf }, ", tabQ->error.xyz[ Q_X ],
		     tabQ->error.xyz[ Q_Y ], tabQ->error.xyz[ Q_Z ] );
	    fprintf( cR->outfp, "{ %lf, %lf, %lf, %lf }\n",
		     tabQ->error.quat[ Q_X ], tabQ->error.quat[ Q_Y ],
		     tabQ->error.quat[ Q_Z ], tabQ->error.quat[ Q_W ] );
	  }
      fclose( cR->outfp );
    } /* end writing table */

    printf( "Table written\n" );
          
    return;
}