//-----------------------------------------------------------------------------
//
//  sfmest.c++
//
//  Reads in tracking data and runs the SFM algorithm on it.
//
//  Tony Jebara (Copyright (C) 1998)
//
//-----------------------------------------------------------------------------

#include <stdio.h>
#include <stdlib.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <netdb.h>
#include <termios.h>
#include <unistd.h>
#include <sys/file.h>
#include <sys/filio.h>
#include <string.h>
#include <limits.h>
#include <math.h>
#include <fstream.h>
#include <strstream.h>
#include <libc.h>
#include "SFMEKF.h++"
#include "SFMEKFb.h++"


double width  = 1.0;
double height = 1.0;
char   infile[256];
char   outfile[256];
char   in3file[256];
int    bias   = -1;

void
parse_args (int argc, char **argv)
{
  int i;

  strcpy(infile, "");
  strcpy(in3file, "");
  strcpy(outfile, "");

  // Set the command line arguments
  for(i=1; i<argc; i++)
    {
      if  (!strcmp(argv[i], "-in"))
	strcpy(infile, argv[++i]);
      else if (!strcmp(argv[i], "-out"))
	strcpy(outfile, argv[++i]);
      else if (!strcmp(argv[i], "-3d"))
	strcpy(in3file, argv[++i]);
      else if (!strcmp(argv[i], "-width"))
	width  = atof(argv[++i]);
      else if (!strcmp(argv[i], "-height"))
	height = atof(argv[++i]);
      else if (!strcmp(argv[i], "-bias"))
	bias   = 1;
      else 
	{
	  fprintf(stderr,"Usage: %s -in <fname> -out <fname> -3d <fname> -width <int> -height <int> -bias\n",argv[0]);
	  fprintf(stderr,"sfmest.\n");
	  fprintf(stderr,"Tony Jebara 1998\n");
	  exit (2);
	}
    }
}

#define MAXLINE 4096

void
main(int argc, char *argv[]) 
{
  int     N, D, i, j;
  FILE    *fpIN, *fpOUT, *fp3;
  char    line[MAXLINE+1];
  char    b2[50];

  // Parse Arguments
  parse_args(argc,argv);

  // Open the File to Determine its Dimensions
  if ((fpIN = fopen(infile,"r"))==NULL)
    {
      fprintf(stderr,"%s can't open %s for input.\n",argv[0],infile);
      exit(0);
    }
  if (strlen(outfile)>0)
    {
      if ((fpOUT = fopen(outfile,"w"))==NULL)
	{
	  fprintf(stderr,"%s can't open %s for output.\n",argv[0],outfile);
	  exit(0);
	}
    }
  if (strlen(in3file)>0)
    {
      if ((fp3 = fopen(in3file,"w"))==NULL)
	{
	  fprintf(stderr,"%s can't open %s for output.\n",argv[0],in3file);
	  exit(0);
	}
    }

  fgets(line,MAXLINE,fpIN);
  istrstream ist(line,strlen(line));
  D = 0;
  while (ist>>b2) D++;
  N = 1;
  while (fgets(line, MAXLINE, fpIN) != NULL) N++;
  fclose(fpIN);


  // Allocate a measurement vector and the SFMEKF
  Vector measurement = VectorCreate(D);
  if (bias<1)
    {
      SFMEKF sfm(D/2,width,height);
  
      // Reread the File and Step the EKF along side it
      fpIN = fopen(infile,"r");
      i    = 0;
      while (fgets(line, MAXLINE, fpIN) != NULL)
	{
	  // Make a measurement
	  istrstream ist(line,strlen(line));
	  j = 0;
	  while (ist>>b2)
	    {
	      measurement->d[j] = atof(b2);
	      j++;
	    }
	  
	  if (i==0)
	    {
	      // Initialize the EKF
	      sfm.initialize(measurement);
	      sfm.initializeEKF();
	    }
	  
	  // Step the SFMEKF
	  
	  sfm.measurement(measurement);
	  
	  sfm.Update();
	  // Write the state to the output file
	  if (strlen(outfile)>0)
	    {
	      VectorPrint(fpOUT, sfm.xp);
	      VectorPrint(fpOUT, sfm.qp);
	      fprintf(fpOUT,"\n");
	    }
	  // Write the 3D coords to another file
	  if (strlen(in3file)>0)
	    {
	      sfm.compute3DCoords();
	      VectorPrint(fp3, sfm.coords3D);
	      fprintf(fp3,"\n");
	    }
	  i++;
	}
      fclose(fpIN);
      if (strlen(outfile)>0) fclose(fpOUT);
      if (strlen(in3file)>0) fclose(fp3);
      VectorFree(measurement);
      sfm.destroy();
    }
  else
    {
      SFMEKFb sfm(D/2,width,height);
  
      // Reread the File and Step the EKF along side it
      fpIN = fopen(infile,"r");
      i    = 0;
      while (fgets(line, MAXLINE, fpIN) != NULL)
	{
	  // Make a measurement
	  istrstream ist(line,strlen(line));
	  j = 0;
	  while (ist>>b2)
	    {
	      measurement->d[j] = atof(b2);
	      j++;
	    }
	  
	  if (i==0)
	    {
	      // Initialize the EKF
	      sfm.initialize(measurement);
	      sfm.initializeEKF();
	    }
	  
	  // Step the SFMEKF
	  
	  sfm.measurement(measurement);
	  
	  sfm.Update();
	  // Write the state to the output file
	  if (strlen(outfile)>0)
	    {
	      VectorPrint(fpOUT, sfm.xp);
	      VectorPrint(fpOUT, sfm.qp);
	      fprintf(fpOUT,"\n");
	    }
	  // Write the 3D coords to another file
	  if (strlen(in3file)>0)
	    {
	      sfm.compute3DCoords();
	      VectorPrint(fp3, sfm.coords3D);
	      fprintf(fp3,"\n");
	    }
	  i++;
	}
      fclose(fpIN);
      if (strlen(outfile)>0) fclose(fpOUT);
      if (strlen(in3file)>0) fclose(fp3);
      VectorFree(measurement);
      sfm.destroy();
    }
}