//=============================================================================
#include "movieIO.h"
#include "rgbImage.h"
#include "featureCorrelator.h"
#include "subWindow.h"
#include "percentBar.h"
#include "math.h"
#include "stereoRecon.h"


extern "C" void svdcmp( float **a, int m, int n, float *w, float **v );
extern "C" void svbksb( float **u, float *w, float **v, int m, int n,
			float *b, float *x );
extern "C" void gaussj( float **a, int n, float **b, int m );

// C = A.B
#define MATMULT_3X3( A, B, C ) \
  (C)[0] = (A)[0]*(B)[0] + (A)[1]*(B)[3] + (A)[2]*(B)[6]; \
  (C)[1] = (A)[0]*(B)[1] + (A)[1]*(B)[4] + (A)[2]*(B)[7]; \
  (C)[2] = (A)[0]*(B)[2] + (A)[1]*(B)[5] + (A)[2]*(B)[8]; \
  (C)[3] = (A)[3]*(B)[0] + (A)[4]*(B)[3] + (A)[5]*(B)[6]; \
  (C)[4] = (A)[3]*(B)[1] + (A)[4]*(B)[4] + (A)[5]*(B)[7]; \
  (C)[5] = (A)[3]*(B)[2] + (A)[4]*(B)[5] + (A)[5]*(B)[8]; \
  (C)[6] = (A)[6]*(B)[0] + (A)[7]*(B)[3] + (A)[8]*(B)[6]; \
  (C)[7] = (A)[6]*(B)[1] + (A)[7]*(B)[4] + (A)[8]*(B)[7]; \
  (C)[8] = (A)[6]*(B)[2] + (A)[7]*(B)[5] + (A)[8]*(B)[8]
  
#define MATMULT_3X3b( A, B, c1, c2, c3 ) \
  (c1)[0] = (A)[0]*(B)[0] + (A)[1]*(B)[3] + (A)[2]*(B)[6]; \
  (c1)[1] = (A)[0]*(B)[1] + (A)[1]*(B)[4] + (A)[2]*(B)[7]; \
  (c1)[2] = (A)[0]*(B)[2] + (A)[1]*(B)[5] + (A)[2]*(B)[8]; \
  (c2)[0] = (A)[3]*(B)[0] + (A)[4]*(B)[3] + (A)[5]*(B)[6]; \
  (c2)[1] = (A)[3]*(B)[1] + (A)[4]*(B)[4] + (A)[5]*(B)[7]; \
  (c2)[2] = (A)[3]*(B)[2] + (A)[4]*(B)[5] + (A)[5]*(B)[8]; \
  (c3)[0] = (A)[6]*(B)[0] + (A)[7]*(B)[3] + (A)[8]*(B)[6]; \
  (c3)[1] = (A)[6]*(B)[1] + (A)[7]*(B)[4] + (A)[8]*(B)[7]; \
  (c3)[2] = (A)[6]*(B)[2] + (A)[7]*(B)[5] + (A)[8]*(B)[8]

// Determinant of a 3X3 matrix
#define DET_3X3(a1,a2,a3,b1,b2,b3,c1,c2,c3) \
  (  (a1)*((b2)*(c3)-(b3)*(c2)) \
    -(b1)*((a2)*(c3)-(a3)*(c2)) \
    +(c1)*((a2)*(b3)-(a3)*(b2)) )
    
#define DET_3X3b(A,B,C) \
  (  (A)[0] * ((B)[1]*(C)[2]-(B)[2]*(C)[1]) \
    -(B)[0] * ((A)[1]*(C)[2]-(A)[2]*(C)[1]) \
    +(C)[0] * ((A)[1]*(B)[2]-(A)[2]*(B)[1]) )

#define DET_3X3c(A) \
  DET_3X3b( (A)[0], (A)[3], (A)[6] )

// Determinant of a 4X4 matrix
#define DET_4X4(A,B,C,D) \
  (  (A)[0] * DET_3X3b( &(B)[1], &(C)[1], &(D)[1] ) \
    -(B)[0] * DET_3X3b( &(A)[1], &(C)[1], &(D)[1] ) \
    +(C)[0] * DET_3X3b( &(A)[1], &(B)[1], &(D)[1] ) \
    -(D)[0] * DET_3X3b( &(A)[1], &(B)[1], &(C)[1] ) )

#define DET_4X4b(A) DET_4X4( &(A)[0], &(A)[4], &(A)[8], &(A)[12] )

void computeM( const float *F, const float *e1, const float *e2,
	       float *P1, float *P2 )
{
  int i;
	  
  // P1 is set to be I
  for ( i=1; i<12; i++ )
    P1[i]=0.0f;
  P1[0] = 1.0f;
  P1[5] = 1.0f;
  P1[10] = 1.0f;

  // P2 = [ [e2]XF | e2 ]
  float e2XMat[3][3];
  e2XMat[0][0]= 0.0f;   e2XMat[0][1]=-e2[2];  e2XMat[0][2]= e2[1];
  e2XMat[1][0]= e2[2];  e2XMat[1][1]= 0.0f;   e2XMat[1][2]=-e2[0];
  e2XMat[2][0]=-e2[1];  e2XMat[2][1]= e2[0];  e2XMat[2][2]= 0.0f;

  MATMULT_3X3b( e2XMat[0], F, &P2[0], &P2[4], &P2[8] );
  P2[ 3] = e2[0];
  P2[ 7] = e2[1];
  P2[11] = e2[2];
}

void matrixPseudoInverse( const float *A, float *Res )
{
  //          +      T      T  -1
  // Res  =  A   =  A  (A  A  )
  int i,j,k;
  
  float comb[3][3];
  for ( i=0; i<3; i++ )
    for ( j=0; j<3; j++ )
  {
    comb[i][j] = A[i*4]*A[j*4];
    for ( k=1; k<4; k++ )
      comb[i][j] += A[i*4+k]*A[j*4+k];
  }

  float *c[4];
  for ( i=0; i<3; i++ )
    c[i+1] = comb[i]-1;

  if ( DET_3X3c(comb)!=0 )
    gaussj( c, 3, 0, 0 );
  
  for ( i=0; i<4; i++ )
    for ( j=0; j<3; j++ )
      Res[i*3+j] = A[i]*comb[0][j] + A[4+i]*comb[1][j] + A[8+i]*comb[2][j];
}

void computeMfromP( const float *P1,  const float *P2,
		    const float *P2p, const float *P3p,
		    float *P3 )
{
  int i,j,k;
  // P3 = P2.pseudoinv(P2p).P3p
  // P3 = pseudoinv(P2p).P2.P3p
  float invP2p[12], tmp[4][4];
  matrixPseudoInverse( P2p, invP2p );

  for ( i=0; i<4; i++ )
    for ( j=0; j<4; j++ )
  {
    tmp[i][j] = invP2p[i*3]*P2[j];
    for ( k=1; k<3; k++ )
      tmp[i][j] += invP2p[i*3+k]*P2[k*4+j];
  }
  
  for ( i=0; i<3; i++ )
    for ( j=0; j<4; j++ )
      P3[i*4+j] = P3p[i*4  ]*tmp[0][j] + P3p[i*4+1]*tmp[1][j] +
	          P3p[i*4+2]*tmp[2][j] + P3p[i*4+3]*tmp[3][j];
  
//   for ( i=0; i<3; i++ )
//   {
//     printf("| ");
//     for ( int j=0; j<4; j++ )
//       printf("%+6.4f ",P2[i*4+j] );
//     if (i==1)
//       printf("|   =   | ");
//     else
//       printf("|       | ");
//     for (  j=0; j<4; j++ )
//       printf("%+6.4f ",P3[i*4+j] );
//     printf("|\n");
//   }
//   printf("\n");
}

void computeFp( const float *F, float *Fp, float *e1, float *e2 )
{
  int i,j,k;
  
  float aBuf[3][3];
  float *a[4];
  for ( i=0; i<3; i++ )
  {
    a[i+1] = aBuf[i]-1;
    for ( j=0; j<3; j++ )
      aBuf[i][j] = F[i*3+j];
  }
  
  float vBuf[3][3];
  float *v[4];
  for ( i=0; i<3; i++ )
    v[i+1]=vBuf[i]-1;

  float w[4];

  svdcmp( a, 3, 3, w, v );

  w[3] = 0.0f;

  for ( i=0; i<3; i++ )
    for ( j=0; j<3; j++ )
  {
    Fp[i*3+j] = 0.0f;
    for ( k=0; k<3; k++ )
      Fp[i*3+j] += aBuf[i][k]*w[k+1]*vBuf[j][k];
  }

  for ( i=0; i<3; i++ )
    e1[i] = vBuf[i][2];
  
  for ( i=0; i<3; i++ )
    e2[i] = aBuf[i][2];
}

void computeFfromP( const float *P1, const float *P2, float *F )
{
  F[0] =  DET_4X4( &P1[4], &P1[8], &P2[4], &P2[8] );
  F[1] = -DET_4X4( &P1[0], &P1[8], &P2[4], &P2[8] );
  F[2] =  DET_4X4( &P1[0], &P1[4], &P2[4], &P2[8] );
  
  F[3] = -DET_4X4( &P1[4], &P1[8], &P2[0], &P2[8] );
  F[4] =  DET_4X4( &P1[0], &P1[8], &P2[0], &P2[8] );
  F[5] = -DET_4X4( &P1[0], &P1[4], &P2[0], &P2[8] );
  
  F[6] =  DET_4X4( &P1[4], &P1[8], &P2[0], &P2[4] );
  F[7] = -DET_4X4( &P1[0], &P1[8], &P2[0], &P2[4] );
  F[8] =  DET_4X4( &P1[0], &P1[4], &P2[0], &P2[4] );
}

void computeF( const float *F12, const float *F23, float *F13 )
{
  float F12p[9], F23p[9];
  float e1[3], e2[3], e2p[3], e3p[3];
  float P1[12], P2[12], P3[12], P1p[12], P2p[12], P3p[12];
  
  computeFp( F12, F12p, e1, e2 );
  computeM( F12p, e1, e2, P1, P2 );

  computeFp( F23, F23p, e2p, e3p );
  computeM( F23p, e2p, e3p, P2p, P3p );
  
  //  computeMfromP( P1, P2, P2p, P3p, P3 );
  //  computeFfromP( P1, P3, F13 );
  
  computeMfromP( P3p, P2p, P2, P1, P1p );
  computeFfromP( P1p, P3p, F13 );
  
  for ( int i=0; i<3; i++ )
  {
    printf("|");
    for ( int j=0; j<4; j++ )
      printf("%+5.3f ",P1[i*4+j] );
    printf("| |");
    for (  j=0; j<4; j++ )
      printf("%+5.3f ",P2[i*4+j] );
    printf("| |");
    for (  j=0; j<4; j++ )
      printf("%+5.3f ",P2p[i*4+j] );
    printf("| |");
    for (  j=0; j<4; j++ )
      printf("%+5.3f ",P3p[i*4+j] );
    printf("| |");
    for (  j=0; j<4; j++ )
      printf("%+5.3f ",P3[i*4+j] );
    printf("|\n");
  }
  printf("\n");
//   computeFp( F13, F12p, e1, e2 );
//   computeM( F12p, e1, e2, P1, P2 );
//   for ( i=0; i<3; i++ )
//   {
//     printf("|");
//     for ( int j=0; j<4; j++ )
//       printf("%+5.3f ",P1[i*4+j] );
//     printf("|     |");
//     for (  j=0; j<4; j++ )
//       printf("%+5.3f ",P2[i*4+j] );
//     printf("|\n");
//   }
  printf("\n");
  for ( i=0; i<3; i++ )
  {
    printf("| ");
    for ( int j=0; j<3; j++ )
      printf("%+6.4f ",F12p[i*3+j] );
    if (i==1)
      printf("|  \"+\"  | ");
    else
      printf("|       | ");
    for (  j=0; j<3; j++ )
      printf("%+6.4f ",F23p[i*3+j] );
    if (i==1)
      printf("|   =   | ");
    else
      printf("|       | ");
    for (  j=0; j<3; j++ )
      printf("%+6.4f ",F13[i*3+j] );
    printf("|\n");
  }
  printf("\n");
}

bool computeF( const vector<subWindowListAnim> &features,
	       int frame1, int frame2, int imgSizeX, int imgSizeY, float *F )
{
  float x1, y1, x2, y2, compX, compY, compW;
  vector<subWindowListAnim>::const_iterator iter;
  int i, j;
  
  int numFeatures=0;
  for ( iter = features.begin(); iter!=features.end(); iter++ )
  {
    if ( (iter->includesFrame(frame1)) &&
	 (iter->includesFrame(frame2)) )
    { // feature is present in both images
      numFeatures++;
    }
  }
  if (numFeatures<9) return false;
  //  printf("Using %i feature corespondences\n", numFeatures);

  float *pCBuf = (float*)malloc( numFeatures * 4 * sizeof(float) );
  float **pointCorresp = (float**)malloc( numFeatures * sizeof(float*) );
  for ( i=0; i<numFeatures; i++ )
    pointCorresp[i]=&pCBuf[i*4];
  for ( iter = features.begin(), i=0; iter!=features.end(); iter++ )
  {
    if ( (iter->includesFrame(frame1)) &&
	 (iter->includesFrame(frame2)) )
    { // feature is present in both images
      pointCorresp[i][0] = ((float)iter->getSubWindow(frame1).centerX()) /
	(float)imgSizeX * 2.0f - 1.0f;
      pointCorresp[i][1] = ((float)iter->getSubWindow(frame1).centerY()) /
	(float)imgSizeY * 2.0f - 1.0f;
      pointCorresp[i][2] = ((float)iter->getSubWindow(frame2).centerX()) /
	(float)imgSizeX * 2.0f - 1.0f;
      pointCorresp[i][3] = ((float)iter->getSubWindow(frame2).centerY()) /
	(float)imgSizeY * 2.0f - 1.0f;
      i++;
    }
  }
  
  float *aBuf = (float*)malloc( numFeatures * 9 * sizeof(float) );
  float **a = (float**)malloc( (numFeatures+1) * sizeof(float*) );
  for ( i=0; i<numFeatures; i++ )
    a[i+1] = &aBuf[i*9]-1;
  for ( i=0; i<numFeatures; i++ )
  {
    x1 = pointCorresp[i][0];
    y1 = pointCorresp[i][1];
    x2 = pointCorresp[i][2];
    y2 = pointCorresp[i][3];
    
    a[i+1][1] = x2*x1;
    a[i+1][2] = x2*y1;
    a[i+1][3] = x2;
    a[i+1][4] = y2*x1;
    a[i+1][5] = y2*y1;
    a[i+1][6] = y2;
    a[i+1][7] = x1;
    a[i+1][8] = y1;
    a[i+1][9] = 1.0f;
  }

  float vBuf[9][9];
  float *v[10];
  for ( i=0; i<9; i++ )
    v[i+1]=vBuf[i]-1;

  float w[10];

  svdcmp( a, numFeatures, 9, w, v );

  float f[3][3];

  for ( i=0; i<3; i++ )
    for ( j=0; j<3; j++ )
      F[i*3+j] = f[i][j]  = vBuf[8][i*3+j];

  printf("Fundamental matrix from SVD directly\n");
  for ( i=0; i<numFeatures; i++ )
  {
    x1 = pointCorresp[i][0];
    y1 = pointCorresp[i][1];
    x2 = pointCorresp[i][2];
    y2 = pointCorresp[i][3];
    printf("(%5.2f %5.2f) <=> (%5.2f %5.2f)",x1,y1,x2,y2);
      
    compX = x1*f[0][0] + y1*f[1][0] + f[2][0];
    compY = x1*f[0][1] + y1*f[1][1] + f[2][1];
    compW = x1*f[0][2] + y1*f[1][2] + f[2][2];
    compX *= x2;
    compY *= y2;
    compW *= 1.0f;

    printf( "diff %i: (%5.2f, %5.2f, %5.2f)\n",
	    i, compX, compY, compW );
  }

  free(aBuf);
  free(a);
  free(pCBuf);
  free(pointCorresp);
  return true;
}


int main( int argc, char *argv[] )
{
  int width, height, length;
  int i,j;
  vector<subWindowListAnim> features;
  if ( argc>=2 )
  {
    int minPerFrame=10;
    int minTrackLen=5;
    
    if ( argc>=3 )
    {
      minPerFrame = atoi(argv[2]);
      if ( minPerFrame<=0 )
      {
	printf("Error in specification of minimum number of features \n" );
	printf("to track per frame: %i\n", minPerFrame );
	exit(0);
      }
    }
    
    if ( argc>=4 )
    {
      minTrackLen = atoi(argv[3]);
      if ( minTrackLen<=0 )
      {
	printf("Error in specification of minimum length of feature \n" );
	printf("tracking: %i\n", minTrackLen );
	exit(0);
      }
    }

    movieIO movie;
    printf( "Preparing \"%s\"...\n", argv[1] );
    if ( !movie.open( argv[1] ) )
    {
      printf( "Error in preparing %s\n", argv[1] );
      exit(0);
    }
    width=movie.getWidth();
    height=movie.getHeight();
    length=movie.getLength();

    printf("\n");
    featureCorrelator corl( "Initializing correlator: ", movie, true );
    printf("\n");
    features = corl.buildTrackedFeatureList( "Tracking Features: ",
					     minPerFrame, minTrackLen );
    printf("\n");
  }
  else
  {
    printf("Loading previously tracked feature data...\n");
    #define FEATURE_LIST_NAME features
    #define WIDTH_VAR width
    #define HEIGHT_VAR height
    #define LENGTH_VAR length
    #include "track4.out"
    printf("Loaded %i tracked features\n", features.size() );
  }

  movieIO movie;
  printf( "Preparing \"%s\"...\n", argv[1] );
  if ( !movie.open( "dino.avi" ) )
  {
    printf( "Error in preparing %s\n", argv[1] );
    exit(0);
  }
  rgbImage *rgbBuf;
  percentBar bar(" Adding squares: " );
  vector<subWindowListAnim>::iterator iter = features.begin();
  unsigned char colour[3];
  float curr=0.0f, end=features.size();
  rgbBuf = movie.getBlankRGBFrame();
  for ( i=0; i<movie.getLength(); i++ )
  {
    movie.setRGBFrame( i, rgbBuf );
  }
  movie.releaseRGBFrame( rgbBuf );
  for ( ; iter!=features.end(); iter++ )
  {
    colour[0] = rand() % 256;
    colour[1] = rand() % 256;
    colour[2] = rand() % 256;
    for ( int srcFrame=iter->getInterval().getStart();
	  srcFrame<=iter->getInterval().getEnd(); srcFrame++ )
    {
      rgbBuf = movie.getRGBFrame(srcFrame);
      rgbBuf->addSquare(
	(*iter)[srcFrame].centerX()-1,
	(*iter)[srcFrame].centerY()-1,
	(*iter)[srcFrame].centerX()+1,
	(*iter)[srcFrame].centerY()+1,
	colour[0], colour[1], colour[2], 1, true);
      if ( srcFrame > iter->getInterval().getStart() )
	rgbBuf->addLineSegment( (*iter)[srcFrame-1].centerX(),
				(*iter)[srcFrame-1].centerY(),
				(*iter)[srcFrame].centerX(),
				(*iter)[srcFrame].centerY(),
				colour[0], colour[1], colour[2], 1 );
      if ( srcFrame-1 > iter->getInterval().getStart() )
	rgbBuf->addLineSegment( (*iter)[srcFrame-2].centerX(),
				(*iter)[srcFrame-2].centerY(),
				(*iter)[srcFrame-1].centerX(),
				(*iter)[srcFrame-1].centerY(),
				colour[0], colour[1], colour[2], 0.5f );
      movie.setRGBFrame( srcFrame, rgbBuf );
      movie.releaseRGBFrame( rgbBuf );
    }
    bar.updatePercent( (curr+=1.0f)/end );
  }
  bar.endProgressBar();
  movie.save("tracked.qt");
  movie.play();
  movie.close();
  exit(0);
  
  stereoMovieRecon recon( features, width/2, height/2, length );
    
  printf("   ");
  for ( i=0; i<length-1; i++ )
    printf("%3i",i);
  printf("\n");
  for ( i=0; i<length-1; i++ )
  {
    printf("%2i ",i);
    for ( j=0; j<length-1; j++ )
    {
      if ( recon.getStereoRecon(i,j)==NULL )
	printf("   ");
      else if ( !recon.getStereoRecon(i,j)->isValid() )
	printf(" I!");
      else
	printf("%3i", (int)recon.getStereoRecon(i,j)->avgPntReconError() );
    }
    printf("\n");
  }
  
  movieIO inmovie;
  printf( "Preparing \"%s\"...\n", argv[1] );
  if ( !inmovie.open( "dino.avi" ) )
  {
    printf( "Error in preparing %s\n", argv[1] );
    exit(0);
  }

  movieIO outmovie( inmovie.getWidth()*2, inmovie.getHeight(),
		    length );
  
  rgbImage *inBuf;
  rgbImage *firstFrame = inmovie.getRGBFrame(0);
  rgbImage *outBuf = outmovie.getBlankRGBFrame();
  outBuf->paste( 0, 0, firstFrame );
  outBuf->paste( inmovie.getWidth(), 0, firstFrame );
  outmovie.setRGBFrame(0,outBuf);
  
  for ( i=1; i<length; i++ )
  {
    point3D epip = recon.getStereoRecon(0,i)->getLeftE();
    printf("Camera center at frame %2i: (%3i, %3i, %7.4f) err=%.6f, %.6f\n",
	   i, (int)epip[0], (int)epip[1], epip[2],
	   recon.getStereoRecon(0,i)->avgPntReconError(),
	   recon.getStereoRecon(0,i)->FReconError() );
    
    inBuf = inmovie.getRGBFrame(i);
    outBuf->paste( 0, 0, inBuf );
    inmovie.releaseRGBFrame( inBuf );
    outBuf->paste( inmovie.getWidth(), 0, firstFrame );
    
//     outBuf->addSquare( inmovie.getWidth()+epip[X]-1, epip[Y]-1,
// 		       inmovie.getWidth()+epip[X]+1, epip[Y]+1,
// 		       255, 0, 0, 1, true );

    for ( j=recon.getStereoRecon(0,i)->numMeasuredPntMatches()-1; j>=0; j--)
    {
      point3D line( recon.getStereoRecon(0,i)->getEpipolarLineRight(j) );
      outBuf->addLine( line[0], line[1], line[2], 255, 255, 0, 1,
		       0,0,width, height);
    }
    
    for ( j=recon.getStereoRecon(0,i)->numMeasuredPntMatches()-1; j>=0; j--)
      outBuf->addSquare(
	recon.getStereoRecon(0,i)->getMeasuredPntRight(j)[X]-1,
	recon.getStereoRecon(0,i)->getMeasuredPntRight(j)[Y]-1,
	recon.getStereoRecon(0,i)->getMeasuredPntRight(j)[X]+1,
	recon.getStereoRecon(0,i)->getMeasuredPntRight(j)[Y]+1,
	0,255,0,1,true);
    for ( j=recon.getStereoRecon(0,i)->numMeasuredPntMatches()-1; j>=0; j--)
      outBuf->addSquare(
	inmovie.getWidth()+recon.getStereoRecon(0,i)->getMeasuredPntLeft(j)[X]-1,
	recon.getStereoRecon(0,i)->getMeasuredPntLeft(j)[Y]-1,
	inmovie.getWidth()+recon.getStereoRecon(0,i)->getMeasuredPntLeft(j)[X]+1,
	recon.getStereoRecon(0,i)->getMeasuredPntLeft(j)[Y]+1,
	0,255,0,1,true);
//     for ( j=recon.getStereoRecon(0,i)->numIdealPntMatches()-1; j>=0; j--)
//       outBuf->addSquare(
// 	recon.getStereoRecon(0,i)->getReconPntRight(j)[X]-1,
// 	recon.getStereoRecon(0,i)->getReconPntRight(j)[Y]-1,
// 	recon.getStereoRecon(0,i)->getReconPntRight(j)[X]+1,
// 	recon.getStereoRecon(0,i)->getReconPntRight(j)[Y]+1,
// 	0,0,255,1,true);
    outmovie.setRGBFrame(i,outBuf);
  }
  inmovie.releaseRGBFrame(firstFrame);
  inmovie.close();
  outmovie.releaseRGBFrame(outBuf);
  outmovie.play();
  outmovie.close();
  
//   stereoRecon **Fbuf = (stereoRecon**)malloc( length*length*sizeof(stereoRecon*) );
//   for ( i=length*length-1; i>=0; i-- )
//     Fbuf[i] = NULL;
//   stereoRecon ***F = (stereoRecon***)malloc( length*sizeof(stereoRecon**) );
//   for ( i=length-1; i>=0; i-- )
//     F[i] = &Fbuf[i*length];

//   stereoRecon *Ftemp;
//   for ( i=0; i<length-1; i++ )
//   {
//     for ( j=i+1; j<length-1; j++ )
//     {
//       Ftemp = new stereoRecon( features, i, j, width, height );
//       if ( Ftemp->isValid() )
//       {
// 	F[i][j] = Ftemp;
//       }
//       else
//       {
// 	delete Ftemp;
// 	break;
//       }
//     }
//   }

//   stereoRecon **F2buf = (stereoRecon**)malloc( length*length*sizeof(stereoRecon*) );
//   memcpy( F2buf, Fbuf, length*length*sizeof(stereoRecon*) );
//   stereoRecon ***F2 = (stereoRecon***)malloc( length*sizeof(stereoRecon**) );
//   for ( i=length-1; i>=0; i-- )
//     F2[i] = &F2buf[i*length];

//   for ( i=0; i<length-1; i++ )
//   {
//     for ( j=i+1; j<length-1; j++ )
//     {
//     }
//   }

  
  
//   for ( i=0; i<length-1; i++ )
//   {
//     for ( j=0; j<length-1; j++ )
//       printf( F2[i][j]==NULL ? " " : "*" );
//     printf("\n");
//   }
  
  
//   stereoRecon *Ftemp;
//   int steps;
//   int *step = (int*)malloc(length*sizeof(int));
//   bool found;
  
//   step[0]=0;
//   //  for ( i=length-1; i>0; i-- )
//   for ( i=1; i<length; i++ )
//   {
//     found=false;
//     for ( steps=1; (steps<=i) && (!found); steps++ )
//     {
//       step[steps]=i;
//       for ( j=0; (j<i-steps+1) && ((steps>1)||(j==0)); j++ )
//       {
// 	for ( k=1; k<steps; k++ )
// 	{
// 	  step[k] = k*i/steps + ( ((j/k)%2)==0 ? -j/steps : j/steps+1 );
// 	  if ( step[k]<=step[k-1]) break;
// 	}
// 	if ( (k<steps) || (step[k-1]>=i) ) continue;
// 	for ( k=0; k<steps; k++ )
// 	{
// 	  if ( F[step[k]][step[k+1]]==NULL )
// 	  {
// 	    Ftemp = new stereoRecon( features, step[k], step[k+1],
// 				     width, height );
// 	    if ( Ftemp->isValid() )
// 	    {
// 	      F[step[k]][step[k+1]] = Ftemp;
// 	    }
// 	    else
// 	    {
// 	      delete Ftemp;
// 	      break;
// 	    }
// 	  }
// 	}
// 	if ( k>=steps )
// 	{
// 	  found=true;
// 	  printf( "Found: (%i steps)", steps );
// 	  for ( k=0; k<=steps; k++ ) printf("%3i", step[k]);
// 	  printf("\n");
// 	  break;
// 	}
//       }
//       if ( found ) break;
//     }
//     if ( !found )
//     {
//       printf( "ERROR: Couldn't find fundamental matrices spanning from %i to %i\n",
// 	      0, i );
//       break;
//     }
// //     for ( k=2; k<=steps; k++ )
// //     {
// //       if ( F[0][step[k]]==NULL )
// //       {
// //  	F[0][step[k]] =
// // 	  new stereoRecon( F[0][step[k-1]], F[step[k-1]][step[k]] );
// //       }
// //     }
//   }

//   for ( i=1; i<length; i++ )
//   {
//     point3D e1 = F[0][i]->getLeftE();
//     point3D e2 = F[0][i]->getRightE();
//     printf("Camera center at frame %2i: (%3i, %3i, %7.4f) err=%.6f\n",
// 	   i, (int)e2[0], (int)e2[1], e2[2], F[0][i]->avgReconPntError() );
//   }
    
    
//     if ( computeF( features, 0, i, width, height, Ftemp ) )
//     {
//       F[0][i] = Ftemp;
//       Ftemp = (float*)malloc( 9*sizeof(float) );
//     }
//     else
//     {
      
//     }
  

//   percentBar bar(" Adding squares: " );
//   vector<subWindowListAnim>::iterator iter = features.begin();
//   unsigned char colour[3];
//   float curr=0.0f, end=features.size();
//   for ( ; iter!=features.end(); iter++ )
//   {
//     colour[0] = rand() % 256;
//     colour[1] = rand() % 256;
//     colour[2] = rand() % 256;
//     for ( int srcFrame=iter->getInterval().getEnd();
// 	  srcFrame>=iter->getInterval().getStart(); srcFrame-- )
//     {
//       rgbBuf = movie.getRGBFrame(srcFrame);
//       rgbBuf->addSquare( (*iter)[srcFrame], colour[0], colour[1], colour[2],
// 			 1, true );
//       movie.setRGBFrame( srcFrame, rgbBuf );
//       movie.releaseRGBFrame( rgbBuf );
//     }
//     bar.updatePercent( (curr+=1.0f)/end );
//   }
//   bar.endProgressBar();
  
//   printf("\n\n");
//   movie.play();
//   movie.close();
}