/*  tabletest.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 <quat.h>
#include "quatext.h"
#include "table.h"
#include "stax.h"
#include "time.h"

enum { POSERRPRE = 0, POSERRPOST = 1, ANGERRPRE = 2, ANGERRPOST = 3,
       POSERRPRECUM = 108, POSERRPOSTCUM, ANGERRPRECUM, ANGERRPOSTCUM,
       POSBETTER, ANGBETTER, NUMSTATS
};
double hist_min[4] = { 0.020, 0.001, 1.0, 0.5 };
double hist_max[4] = { 0.150, 0.006, 5.0, 3.0 };
double hist_wid[4] = { 0.010, 0.001, 0.5, 0.5 };

q_vec_type origin = { 0.0, 0.0, 0.0 };

void
correctQuat( q_type q )
{
    if( q[Q_W] < 0.0 ) {
      q[Q_X] = -q[Q_X];
      q[Q_Y] = -q[Q_Y];
      q[Q_Z] = -q[Q_Z];
      q[Q_W] = -q[Q_W];
    }
    return;
}


int
codeToOctant( int code )
{
    int ret = 0;

    /*
    ** The octants are numbered with -X,-Y,-Z being 0, -X,-Y,CTR being 1,
    ** etc. up to +X,+Y,+Z being 26.
    */

    if( !( code & MIN_Z ) ) {
      ret++;
      if( code & MAX_Z )
	ret++;
    }

    if( !( code & MIN_Y ) ) {
      ret += 3;
      if( code & MAX_Y )
	ret += 3;
    }

    if( !( code & MIN_X ) ) {
      ret += 9;
      if( code & MAX_X )
	ret += 9;
    }

    return ret;
}

void
main( int argc, char *argv[] )
{
    int i, verbose = 0, code, oct;
    int unPos = 0, unOri = 0, unBoth = 0;
    int unPos13 = 0, unOri13 = 0, unBoth13 = 0;
    char buf[256];
    q_xyz_quat_type asceFsrot, asceFsens, asceFsrod;
    q_matrix_type oldQuat, newQuat;
    q_vec_type tabMin, tabMax;
    q_type sensFasceQ, srodFasceQ, srotFasceQ;
    q_type srodFsensQ, srotFsrodQ, sensFsrotQ;
    double distOld, distNew;
    double srodFsensA, srotFsrodA, sensFsrotA;
    char statName[80];
    double timeSpent = 0.0;
    int numSamp = 0;
    q_vec_type sampMin, sampMax; /* only inside table, not extrapolated */
    double maxDistOrigin, curDistOrigin; /* maximum dist of corrected sample */
    FILE *PREposVdist, *POSTposVdist; /* for scatter plots */
    FILE *PREoriVdist, *POSToriVdist;

    for( i = 1; i < argc - 1; i++ ) {
      if( !strcmp( argv[i], "-verbose" ) )
	verbose = 1;
      else
	fprintf( stderr, "%s: Unknown option '%s'\n", argv[0], argv[i] );
    }

    if( tabReadTable( argv[ argc - 1 ], tabMin, tabMax ) ) {
      fprintf( stderr, "Unable to read table from file '%s'\n", argv[argc-1] );
      exit( -1 );
    }

    printf( "Read table\n" );

    stxInit( NUMSTATS );
    stxHistInit( 4, hist_min, hist_max, hist_wid );
    TMXinit( 10 );
    /* these two init the measure of where the samples really are - it looks
       backward so that it starts from nothing and grows out */
    q_vec_copy( sampMin, tabMax );
    q_vec_copy( sampMax, tabMin );
    maxDistOrigin = 0.0;
    PREposVdist = fopen( "PREposVSdist.dat", "w" );
    POSTposVdist = fopen( "POSTposVSdist.dat", "w" );
    PREoriVdist = fopen( "PREoriVSdist.dat", "w" );
    POSToriVdist = fopen( "POSToriVSdist.dat", "w" );

    while( fgets( buf, 255, stdin ) ) {

      /* Read and interpret the input */
      sscanf( buf, "FOB XYZ   = ( %lf %lf %lf )", &asceFsens.xyz[Q_X],
	      &asceFsens.xyz[Q_Y], &asceFsens.xyz[Q_Z] );
      fgets( buf, 255, stdin );
      sscanf( buf, "FOB QUAT  = ( %lf %lf %lf %lf )",
	      &asceFsens.quat[Q_X], &asceFsens.quat[Q_Y],
	      &asceFsens.quat[Q_Z], &asceFsens.quat[Q_W] );
      fgets( buf, 255, stdin );
      sscanf( buf, "TRUE XYZ  = ( %lf %lf %lf )", &asceFsrod.xyz[Q_X],
	      &asceFsrod.xyz[Q_Y], &asceFsrod.xyz[Q_Z] );
      fgets( buf, 255, stdin );
      sscanf( buf, "TRUE QUAT = ( %lf %lf %lf %lf )",
	      &asceFsrod.quat[Q_X], &asceFsrod.quat[Q_Y],
	      &asceFsrod.quat[Q_Z], &asceFsrod.quat[Q_W] );

      /* Make a copy for error comparison */
      q_vec_copy( asceFsrot.xyz, asceFsens.xyz );
      correctQuat( asceFsens.quat );
      q_copy( asceFsrot.quat, asceFsens.quat );

      /* Correct the reading, if possible */
      TMXpush( );
      code = tabCorrectReading( &asceFsrot );
      timeSpent += TMXpop( );
      numSamp++;
      oct = codeToOctant( code );

      if( verbose )
	qx_file_xyz_quat_print( stdout, &asceFsrot );

      /* Analyze position error */
      distOld = q_vec_distance( asceFsens.xyz, asceFsrod.xyz );
      distNew = q_vec_distance( asceFsrot.xyz, asceFsrod.xyz );

      /* Update position error metrics */
      stxUpdate( POSERRPRE + 4 * oct, distOld );
      stxUpdate( POSERRPRECUM, distOld );
      stxUpdate( POSERRPOST + 4 * oct, distNew );
      stxUpdate( POSERRPOSTCUM, distNew );

      if( verbose )
	printf( "RAW pos error = %8lf\tCORRECTED pos error = %8lf\n",
	        distOld, distNew );

      /* Analyze orientation error */
      correctQuat( asceFsens.quat );
      q_invert( sensFasceQ, asceFsens.quat );
      correctQuat( asceFsrod.quat );
      q_invert( srodFasceQ, asceFsrod.quat );
      correctQuat( asceFsrot.quat );
      q_invert( srotFasceQ, asceFsrot.quat );
      qx_diff( srodFsensQ, sensFasceQ, srodFasceQ );
      q_normalize( srodFsensQ, srodFsensQ );
      srodFsensA = Q_RAD_TO_DEG( 2.0 * acos( srodFsensQ[Q_W] ) );
      qx_diff( srotFsrodQ, srodFasceQ, srotFasceQ );
      q_normalize( srotFsrodQ, srotFsrodQ );
      srotFsrodA = Q_RAD_TO_DEG( 2.0 * acos( srotFsrodQ[Q_W] ) );
      qx_diff( sensFsrotQ, srotFasceQ, sensFasceQ );
      q_normalize( sensFsrotQ, sensFsrotQ );
      sensFsrotA = Q_RAD_TO_DEG( 2.0 * acos( sensFsrotQ[Q_W] ) );

      /* Update orientation error metrics */
      stxUpdate( ANGERRPRE + 4 * oct, srodFsensA );
      stxUpdate( ANGERRPRECUM, srodFsensA );
      stxUpdate( ANGERRPOST + 4 * oct, srotFsrodA );
      stxUpdate( ANGERRPOSTCUM, srotFsrodA );

      if( verbose )
	printf( "srodFsensA = %8lf\tsrotFsrodA = %8lf\tsensFsrotA = %8lf\n",
	        srodFsensA, srotFsrodA, sensFsrotA );

      /* did we improve things? */
      if( distOld < distNew ) {
	if( srodFsensA < srotFsrodA )
	  unBoth++;
	else
	  unPos++;
      } else if( srodFsensA < srotFsrodA )
	unOri++;

      /* Update histogram and sample volume for inside table */
      if( oct == 13 ) {
	/* position hist */
	stxHistUpdate( POSERRPRE, distOld );
	stxHistUpdate( POSERRPOST, distNew );
	if( distOld < distNew ) {
	  stxUpdate( POSBETTER, distOld );
	  if( srodFsensA < srotFsrodA )
	    unBoth13++;
	  else
	    unPos13++;
	} else
	  stxUpdate( POSBETTER, distNew );
	/* orientation hist */
	stxHistUpdate( ANGERRPRE, srodFsensA );
	stxHistUpdate( ANGERRPOST, srotFsrodA );
	if( srodFsensA < srotFsrodA ) {
	  stxUpdate( ANGBETTER, srodFsensA );
	  unOri13++;
	} else
	  stxUpdate( ANGBETTER, srotFsrodA );

	/* extend sample volume */
        if( sampMin[ Q_X ] > asceFsens.xyz[ Q_X ] )
          sampMin[ Q_X ] = asceFsens.xyz[ Q_X ];
        if( sampMin[ Q_Y ] > asceFsens.xyz[ Q_Y ] )
          sampMin[ Q_Y ] = asceFsens.xyz[ Q_Y ];
        if( sampMin[ Q_Z ] > asceFsens.xyz[ Q_Z ] )
          sampMin[ Q_Z ] = asceFsens.xyz[ Q_Z ];

        if( sampMax[ Q_X ] < asceFsens.xyz[ Q_X ] )
          sampMax[ Q_X ] = asceFsens.xyz[ Q_X ];
        if( sampMax[ Q_Y ] < asceFsens.xyz[ Q_Y ] )
          sampMax[ Q_Y ] = asceFsens.xyz[ Q_Y ];
        if( sampMax[ Q_Z ] < asceFsens.xyz[ Q_Z ] )
          sampMax[ Q_Z ] = asceFsens.xyz[ Q_Z ];

	/* extend maximum distance */
	curDistOrigin = q_vec_distance( asceFsens.xyz, origin );
	if( curDistOrigin > maxDistOrigin )
	  maxDistOrigin = curDistOrigin;

	/* write out stuff for scatter plots */
	fprintf( PREposVdist, "%lf %lf\n", curDistOrigin, distOld );
	fprintf( POSTposVdist, "%lf %lf\n", curDistOrigin, distNew );
	fprintf( PREoriVdist, "%lf %lf\n", curDistOrigin, srodFsensA );
	fprintf( POSToriVdist, "%lf %lf\n", curDistOrigin, srotFsrodA );
      }
    }

    for( i = 0; i < 27; i++ ) {
      sprintf( statName, "in octant %d", i );
      stxPrint( 4 * i,     statName, "Position Without" );
      stxPrint( 4 * i + 1, NULL,     "Position With   " );
      stxPrint( 4 * i + 2, NULL,     "Angular Without " );
      stxPrint( 4 * i + 3, NULL,     "Angular With    " );
      printf( "\n" );
    }
    stxPrint( 108, "total", "Position Without" );
    stxPrint( 109, NULL,    "Position With   " );
    stxPrint( 110, NULL,    "Angular Without " );
    stxPrint( 111, NULL,    "Angular With    " );

    printf( "Octant 13 Position Pre Error Histogram:\n" );
    stxHistPrint( POSERRPRE );
    printf( "Octant 13 Position Post Error Histogram:\n" );
    stxHistPrint( POSERRPOST );
    printf( "Octant 13 Orientation Pre Error Histogram:\n" );
    stxHistPrint( ANGERRPRE );
    printf( "Octant 13 Orientation Post Error Histogram:\n" );
    stxHistPrint( ANGERRPOST );

    stxPrint( POSBETTER, NULL, "Position Some   " );
    stxPrint( ANGBETTER, NULL, "Orientation Some" );

    printf( "%d had more position error (%d in table)\n", unPos, unPos13 );
    printf( "%d had more orientation error (%d in table)\n", unOri, unOri13 );
    printf( "%d had both more pos and ori (%d in table)\n", unBoth, unBoth13 );

    printf( "%lf sec, %d samp ( %lf )\n",
	    timeSpent, numSamp, timeSpent / (double) numSamp );

    printf( "Bounding volume of samples:\n" );
    q_vec_print( sampMin );
    q_vec_print( sampMax );
    printf( "Maximum distance of sample from origin = %lf\n", maxDistOrigin );

    fclose( PREposVdist );
    fclose( POSTposVdist );
    fclose( PREoriVdist );
    fclose( POSToriVdist );

    return;
}