#include "cv.h"
#include "highgui.h"

#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <iostream.h>
#include <fstream.h>

#include "eightPoint.h"

int EightPoint::numOfRuns = 100;
char EightPoint::leftWindowName[] = "Left_Image";
char EightPoint::rightWindowName[] = "Right_Image";

EightPoint::EightPoint()
{
  isNormalized = false;

  leftImage = 0;
  rightImage = 0;
  fMat = cvMat(3, 3, CV_64FC1);
  unnormFMat = cvMat(3, 3, CV_64FC1);
  normFMat = cvMat(3, 3, CV_64FC1);
  leftNormT = cvMat(3, 3, CV_64FC1);
  rightNormT = cvMat(3, 3, CV_64FC1);
  
  cvmAlloc(&leftNormT);
  cvmAlloc(&rightNormT);
  cvmAlloc(&fMat);
  cvmAlloc(&unnormFMat);
  cvmAlloc(&normFMat);
  
  unnormConditionNum = 0.0;
  normConditionNum = 0.0;
  unnormActiveError = 0.0;
  normActiveError = 0.0;
  
  tU = cvMat(3, 3, CV_64FC1);
  tD = cvMat(3, 3, CV_64FC1);
  tV = cvMat(3, 3, CV_64FC1);
  ttM = cvMat(3, 3, CV_64FC1);
  cvmAlloc(&tU); cvmAlloc(&tD); 
  cvmAlloc(&tV); cvmAlloc(&ttM);

  m1 = cvMat(3, 1, CV_64FC1);
  m2 = cvMat(3, 1, CV_64FC1);
  m1x3 = cvMat(1, 3, CV_64FC1);
  m1x1 = cvMat(1, 1, CV_64FC1);
  m3x3 = cvMat(3, 3, CV_64FC1);
  cvmAlloc(&m1); cvmAlloc(&m2); 
  cvmAlloc(&m1x1); cvmAlloc(&m1x3);
  cvmAlloc(&m3x3);


}

EightPoint::~EightPoint()
{

  cvmFree(&fMat);
  cvmFree(&unnormFMat);
  cvmFree(&normFMat);
  cvmFree(&leftNormT);
  cvmFree(&rightNormT);

  delete leftPoints;
  delete rightPoints;

  unnormConditionNumFS.close();
  normConditionNumFS.close();
  unnormActiveErrorFS.close();
  normActiveErrorFS.close();

  cvmFree(&tU); 
  cvmFree(&tD);
  cvmFree(&tV);
  cvmFree(&ttM);

  cvmFree(&m1);
  cvmFree(&m2);
  cvmFree(&m1x3);
  cvmFree(&m1x1);
  cvmFree(&m3x3);

}


void EightPoint::initialize(char *title)
{
  int i, j, k;
  ifstream lfs,rfs;
  char fileBase[80], fileN[80];
  
  strcpy(fileBase, "eight_point/");
  strcpy(leftImageFN, "leftImage_");
  strcpy(rightImageFN, "rightImage_");
  strcat(leftImageFN, title);
  strcat(leftImageFN, ".jpg");
  strcat(rightImageFN, title);
  strcat(rightImageFN, ".jpg");
  
  
  if (strcmp(title, "jig") == 0) { //do for jib
    strcat(fileBase, "calibraton_jig/");
    strcpy(fileN, fileBase);strcat(fileN, "dub00.pgm");
    if ( (leftImage = cvLoadImage(fileN)) == 0 ) {
      cout<<fileN<<endl;
      cout<<" Cannot load calibration_jig1 image. "<<endl;
      exit(0);
    }
    strcpy(fileN, fileBase);strcat(fileN, "dub01.pgm");
    if ( (rightImage = cvLoadImage(fileN)) == 0 ) {
      cout<<" Cannot load calibration_jig2 image. "<<endl;
      exit(0);
    }
    strcpy(fileN, fileBase);strcat(fileN, "f1");
    lfs.open(fileN);
    strcpy(fileN, fileBase);strcat(fileN, "f2");
    rfs.open(fileN);
    unnormConditionNumFS.open("unnormConditionNum_jig.dat");
    normConditionNumFS.open("normConditionNum_jig.dat");
    unnormActiveErrorFS.open("unnormActiveError_jig.dat");
    normActiveErrorFS.open("normActiveError_jig.dat");
  }
  else if (strcmp(title, "corridor") == 0) {  //do for corridor
    strcat(fileBase, "corridor/");
    strcpy(fileN, fileBase);strcat(fileN, "base00.pgm");
    if ( (leftImage = cvLoadImage(fileN)) == 0 ) {
      cout<<" Cannot load corridor1 image. "<<endl;
      exit(0);
    }
    strcpy(fileN, fileBase);strcat(fileN, "base01.pgm");
    if ( (rightImage = cvLoadImage(fileN)) == 0 ) {
      cout<<" Cannot load corridor2 image. "<<endl;
      exit(0);
    }
    strcpy(fileN, fileBase);strcat(fileN, "g1");
    lfs.open(fileN);
    strcpy(fileN, fileBase);strcat(fileN, "g2");
    rfs.open(fileN);
    unnormConditionNumFS.open("unnormConditionNum_corridor.dat");
    normConditionNumFS.open("normConditionNum_corridor.dat");
    unnormActiveErrorFS.open("unnormActiveError_corridor.dat");
    normActiveErrorFS.open("normActiveError_corridor.dat");
  }
  else if (strcmp(title, "house") == 0) {     //do for house
    strcat(fileBase, "house/");
    strcpy(fileN, fileBase);strcat(fileN, "house1.pgm");
    if ( (leftImage = cvLoadImage(fileN)) == 0 ) {
      cout<<" Cannot load house1 image. "<<endl;
      exit(0);
    }
    strcpy(fileN, fileBase);strcat(fileN, "house2.pgm");
    if ( (rightImage = cvLoadImage(fileN)) == 0 ) {
      cout<<" Cannot load house2 image. "<<endl;
      exit(0);
    }
    strcpy(fileN, fileBase);strcat(fileN, "f1");
    lfs.open(fileN);
    strcpy(fileN, fileBase);strcat(fileN, "f2");
    rfs.open(fileN);
    unnormConditionNumFS.open("unnormConditionNum_house.dat");
    normConditionNumFS.open("normConditionNum_house.dat");
    unnormActiveErrorFS.open("unnormActiveError_house.dat");
    normActiveErrorFS.open("normActiveError_house.dat");
  }
  else {
    cout<<"!!!!!!! Can decide data's file name!!!!!!!!!!"<<endl;
    exit(0);
  }

  lfs >>  totalNumPoints;
  rfs >> k;
  if (k != totalNumPoints) {
    cout << "!!!!! Left and right image point number does NOT match!!!!"<<endl;
    exit(0);
  }
  
  cvNamedWindow(leftWindowName, CV_WINDOW_AUTOSIZE);
  cvNamedWindow(rightWindowName, CV_WINDOW_AUTOSIZE);
  cvShowImage(leftWindowName, leftImage);
  cvShowImage(rightWindowName, rightImage);

  leftPoints = new CvPoint2D64d [totalNumPoints];
  rightPoints = new CvPoint2D64d [totalNumPoints];
  pointIndices = new int [totalNumPoints];
  
  for (i = 0; i < totalNumPoints; i++) {
    lfs >> leftPoints[i].x;
    lfs >> leftPoints[i].y;
    rfs >> rightPoints[i].x;
    rfs >> rightPoints[i].y;
    pointIndices[i] = -1;
  }

  cout<<"------Finish reading data from:  ";
  cout<<fileBase<<" ! ----------"<<endl;

  lfs.close(); rfs.close();
}


void EightPoint::epipolarG(int step)
{
  int i, j, k;

  for (i = 9; i < totalNumPoints *3 /4; i += step) {
    unnormConditionNum = 0.0;
    normConditionNum = 0.0;
    unnormActiveError = 0.0;
    normActiveError = 0.0;

    allocateMats(i);

    //begin numOfRuns LOOP
    for (j = 0; j < numOfRuns; j++) {
      choosePoints(i);

      constructAMat();
      findUnnormFMat();

      isNormalized = true; 
      constructNormTs(); 
      constructAMat();
      findNormFMat();

      /*//xxx
      cout<<" unnorm mat: "<<endl;
      printMat(&unnormFMat, 3, 3);
      cout<<" Norm De: "<<endl;
      printMat(&normFMat, 3, 3);
      cout<<endl;
      */
      
      isNormalized = false;

    } // end numOfRuns LOOP
    
    freeMats();
    //xxx----------------------
    //cout<<" unNo condi ....";
    unnormConditionNumFS<<i<<" "<<unnormConditionNum/numOfRuns<<endl;
    // cout<<" Nor ...........";
    normConditionNumFS<<i<<" "<<normConditionNum/numOfRuns<<endl;
    //cout<<" unNo ACtov E ..";
    unnormActiveErrorFS<<i<<" "<<unnormActiveError/numOfRuns<<endl;
    //cout<<"  No ACtov E ..";
    normActiveErrorFS<<i<<" "<<normActiveError/numOfRuns<<endl;
    
  } //end of for(i = start---end by step);

  cout<<"-----finished in recording data.--------"<<endl;

  cvmTranspose(&normFMat, &m3x3);
  cvSVD(&m3x3, &tD, &tU, &tV, 0);
  cout<<"----------Left Epiloar for "<<leftImageFN<<endl;
  cout<<"  " <<cvmGet(&tV, 0, 2)/cvmGet(&tV, 2, 2)<<"  ";
  cout<<"  " <<cvmGet(&tV, 1, 2)/cvmGet(&tV, 2, 2)<<endl;
  
  cvSVD(&normFMat, &tD, &tU, &tV, 0);
  cout<<"---------Right Epipolar for "<<rightImageFN<<endl;
  cout<<"  " <<cvmGet(&tV, 0, 2)/cvmGet(&tV, 2, 2)<<"  ";
  cout<<"  " <<cvmGet(&tV, 1, 2)/cvmGet(&tV, 2, 2)<<endl;
  
  //xxx
  //printMat(&tV, 3,3);
  
  cvmTranspose(&normFMat, &m3x3);
  for (i = 0; i < totalNumPoints/3; i++) {
    epipolarLine(&m3x3, &rightPoints[i], leftImage);
    cvShowImage(leftWindowName, leftImage);

    epipolarLine(&normFMat, &leftPoints[i], rightImage);
    cvShowImage(rightWindowName, rightImage);
  }
  cvSaveImage(leftImageFN, leftImage);
  cvSaveImage(rightImageFN, rightImage);

  cout<<"------------Finished in saving images.-------"<<endl<<endl;
  
}


void EightPoint::findUnnormFMat()
{
  int i, j, k;
  double dl, dr;

  dl = 0.0; dr = 0.0;

  cvSVD(&AMat, &WMat, &UMat, &VMat, 0);

  //condition number
  dl = cvmGet(&WMat, 0, 0) / cvmGet(&WMat, 7, 7);
  dl = dl * dl;
  unnormConditionNum += dl;
  dl = 0.0;
  
  //initialize and create nunormFMat
  create3x3MatFromCol(&unnormFMat, &VMat, 8); 
  cvSVD(&unnormFMat, &tD, &tU, &tV, 0);
  cvmSet(&tD, 2, 2, 0.0);
  cvmMul(&tU, &tD, &ttM);
  cvTranspose(&tV, &tV);
  cvmMul(&ttM, &tV, &unnormFMat);
  
  
  //calculate Average Error
  cvTranspose(&unnormFMat, &m3x3);
  for (i = 0; i < totalNumPoints; i++) {
    //left-to-right
    cvmSet(&m1, 0, 0, leftPoints[i].x);
    cvmSet(&m1, 1, 0, leftPoints[i].y);
    cvmSet(&m1, 2, 0, 1.0);
    cvmMul(&unnormFMat, &m1, &m2);
    cvmSet(&m1, 0, 0, rightPoints[i].x);
    cvmSet(&m1, 1, 0, rightPoints[i].y);
    cvmSet(&m1, 2, 0, 1.0);
    cvTranspose(&m1, &m1x3);

    cvmMul(&m1x3, &m2, &m1x1);
    dl = cvmGet(&m1x1, 0, 0) / sqrt( 
      (cvmGet(&m2, 0, 0) * cvmGet(&m2, 0, 0) +
       cvmGet(&m2, 1, 0) * cvmGet(&m2, 1, 0)));
    if (dl < 0) dl *= -1.0;
    dr += dl;

    //right-to-left
    cvmSet(&m1, 0, 0, rightPoints[i].x);
    cvmSet(&m1, 1, 0, rightPoints[i].y);
    cvmSet(&m1, 2, 0, 1.0);
    cvmMul(&m3x3, &m1, &m2);
    cvmSet(&m1, 0, 0, leftPoints[i].x);
    cvmSet(&m1, 1, 0, leftPoints[i].y);
    cvmSet(&m1, 2, 0, 1.0);
    cvTranspose(&m1, &m1x3);
    cvmMul(&m1x3, &m2, &m1x1);
    dl = cvmGet(&m1x1, 0, 0) / sqrt(
      (cvmGet(&m2, 0, 0) * cvmGet(&m2, 0, 0) +
       cvmGet(&m2, 1, 0) * cvmGet(&m2, 1, 0)));
    if (dl < 0) dl *= -1.0;
    dr += dl;
  }
  
  dr /= (double) (totalNumPoints * 2);
  unnormActiveError += dr; 
}


void EightPoint::findNormFMat()
{
  int i, j, k;
  double dl, dr;
  
  dl = 0.0; dr = 0.0;
  cvSVD(&AMat, &WMat, &UMat, &VMat, 0);
  dl = cvmGet(&WMat, 0, 0) / cvmGet(&WMat, 7, 7);
  dl = dl * dl;
  normConditionNum += dl;
  dl = 0.0;

  //initialize and create normFMat
  create3x3MatFromCol(&normFMat, &VMat, 8); 
  cvSVD(&normFMat, &tD, &tU, &tV, 0);
  cvmSet(&tD, 2, 2, 0.0);
  cvmMul(&tU, &tD, &ttM);
  cvTranspose(&tV, &tV);
  cvmMul(&ttM, &tV, &normFMat);
  
  //DENORMALIZE FMat
  cvTranspose(&rightNormT, &m3x3);
  cvmMul(&m3x3, &normFMat, &ttM);
  cvmMul(&ttM, &leftNormT, &normFMat);
  
  
  //calculate Average Error
  cvTranspose(&normFMat, &m3x3);
  for (i = 0; i < totalNumPoints; i++) {
    //left-to-right
    cvmSet(&m1, 0, 0, leftPoints[i].x);
    cvmSet(&m1, 1, 0, leftPoints[i].y);
    cvmSet(&m1, 2, 0, 1.0);
    cvmMul(&normFMat, &m1, &m2);
    cvmSet(&m1, 0, 0, rightPoints[i].x);
    cvmSet(&m1, 1, 0, rightPoints[i].y);
    cvmSet(&m1, 2, 0, 1.0);
    cvTranspose(&m1, &m1x3);

    cvmMul(&m1x3, &m2, &m1x1);
    dl = cvmGet(&m1x1, 0, 0) / sqrt( 
      (cvmGet(&m2, 0, 0) * cvmGet(&m2, 0, 0) +
       cvmGet(&m2, 1, 0) * cvmGet(&m2, 1, 0)));
    if (dl < 0) dl *= -1.0;
    dr += dl;

    //right-to-left
    cvmSet(&m1, 0, 0, rightPoints[i].x);
    cvmSet(&m1, 1, 0, rightPoints[i].y);
    cvmSet(&m1, 2, 0, 1.0);
    cvmMul(&m3x3, &m1, &m2);
    cvmSet(&m1, 0, 0, leftPoints[i].x);
    cvmSet(&m1, 1, 0, leftPoints[i].y);
    cvmSet(&m1, 2, 0, 1.0);
    cvTranspose(&m1, &m1x3);
    cvmMul(&m1x3, &m2, &m1x1);
    dl = cvmGet(&m1x1, 0, 0) / sqrt(
      (cvmGet(&m2, 0, 0) * cvmGet(&m2, 0, 0) +
       cvmGet(&m2, 1, 0) * cvmGet(&m2, 1, 0)));
    if (dl < 0) dl *= -1.0;
    dr += dl;
  }
  
  dr /= (double) (totalNumPoints * 2);
  normActiveError += dr; 

}



void EightPoint::constructAMat()
{
  int i, j, k;
  double ul, vl, ur, vr;

  for (i = 0; i < numPoints; i++) {
    j = pointIndices[i];
    ul = leftPoints[j].x; 
    vl = leftPoints[j].y;
    ur = rightPoints[j].x; 
    vr = rightPoints[j].y;
    if (isNormalized) {             //if normalized
      cvmSet(&m1, 0, 0, ul);        //left image points
      cvmSet(&m1, 1, 0, vl);
      cvmSet(&m1, 2, 0, 1.0);
      cvmMul(&leftNormT, &m1, &m2);      
      ul = cvmGet(&m2, 0, 0);
      vl = cvmGet(&m2, 1, 0);
      cvmSet(&m1, 0, 0, ur);         //right image points
      cvmSet(&m1, 1, 0, vr);
      cvmSet(&m1, 2, 0, 1.0);
      cvmMul(&rightNormT, &m1, &m2);
      ur = cvmGet(&m2, 0, 0);
      vr = cvmGet(&m2, 1, 0);
    }                           //end normalized points
    // set AMat
    cvmSet(&AMat, i, 0, ur * ul);
    cvmSet(&AMat, i, 1, ur * vl);
    cvmSet(&AMat, i, 2, ur);
    cvmSet(&AMat, i, 3, vr * ul);
    cvmSet(&AMat, i, 4, vr * vl);
    cvmSet(&AMat, i, 5, vr);
    cvmSet(&AMat, i, 6, ul);
    cvmSet(&AMat, i, 7, vl);
    cvmSet(&AMat, i, 8, 1.0);
  }

}


void EightPoint::allocateMats(int n)
{
  AMat = cvMat(n, 9, CV_64FC1);
  UMat = cvMat(n, n, CV_64FC1);
  WMat = cvMat(n, 9, CV_64FC1);
  VMat = cvMat(9, 9, CV_64FC1);

  cvmAlloc(&AMat);
  cvmAlloc(&UMat);
  cvmAlloc(&WMat);
  cvmAlloc(&VMat);

}

void EightPoint::freeMats()
{
  
  cvmFree(&AMat);
  cvmFree(&UMat);
  cvmFree(&WMat);
  cvmFree(&VMat);

}

void EightPoint::constructNormTs()
{
  int i, j;
  double rms, xo, yo,dd;
  
  for (i = 0; i < 3; i++)
    for (j = 0; j < 3; j++) {
      cvmSet(&leftNormT, i, j, 0.0);
      cvmSet(&rightNormT, i, j, 0.0);
    }

  //set left normalized T
  rms = 0.0; xo = 0.0; yo = 0.0;
  for (i = 0; i < numPoints; i++) {
    xo += leftPoints[pointIndices[i]].x; 
    yo += leftPoints[pointIndices[i]].y;
  }
  xo /= numPoints; yo /= numPoints;
  for (i = 0; i < numPoints; i++) {
    j = pointIndices[i];
    dd = (leftPoints[j].x - xo) * (leftPoints[j].x - xo) +
      (leftPoints[j].y - yo) * (leftPoints[j].y - yo);
    //dd = sqrt(dd);
    rms += dd;
  }
  rms = sqrt(rms/numPoints);
  rms = sqrt(2.0) / rms;
  cvmSet(&leftNormT, 0, 0, rms);
  cvmSet(&leftNormT, 1, 1, rms);
  cvmSet(&leftNormT, 2, 2, 1.0);
  cvmSet(&leftNormT, 0, 2, -rms * xo);
  cvmSet(&leftNormT, 1, 2, -rms * yo);

  //set right normalized T
  rms = 0.0; xo = 0.0; yo = 0.0;
  for (i = 0; i < numPoints; i++) {
    j = pointIndices[i];
    xo += rightPoints[j].x; 
    yo += rightPoints[j].y;
  }
  xo /= numPoints; yo /= numPoints;
  for (i = 0; i < numPoints; i++) {
    j = pointIndices[i];
    dd = (rightPoints[j].x - xo) * (rightPoints[j].x - xo) +
      (rightPoints[j].y - yo) * (rightPoints[j].y - yo);
    //dd = sqrt(dd);
    rms += dd;
  }
  rms = sqrt(rms/numPoints);
  rms = sqrt(2.0) / rms;
  cvmSet(&rightNormT, 0, 0, rms);
  cvmSet(&rightNormT, 1, 1, rms);
  cvmSet(&rightNormT, 2, 2, 1.0);
  cvmSet(&rightNormT, 0, 2, -rms * xo);
  cvmSet(&rightNormT, 1, 2, -rms * yo);

}


void EightPoint::printMat(CvMat *m, int nRow, int nCol)
{
  for (int i = 0; i < nRow; i++) {
    for (int j = 0; j < nCol; j++)
      cout << cvmGet(m, i, j) <<" ";
    cout<<endl;
  }

}


void EightPoint::create3x3MatFromCol(CvMat *dstM, CvMat *srcM, int colN)
{
  cvmSet(dstM, 0, 0, cvmGet(srcM, 0, colN));
  cvmSet(dstM, 0, 1, cvmGet(srcM, 1, colN));
  cvmSet(dstM, 0, 2, cvmGet(srcM, 2, colN));
  cvmSet(dstM, 1, 0, cvmGet(srcM, 3, colN));
  cvmSet(dstM, 1, 1, cvmGet(srcM, 4, colN));
  cvmSet(dstM, 1, 2, cvmGet(srcM, 5, colN));
  cvmSet(dstM, 2, 0, cvmGet(srcM, 6, colN));
  cvmSet(dstM, 2, 1, cvmGet(srcM, 7, colN));
  cvmSet(dstM, 2, 2, cvmGet(srcM, 8, colN));
  
}


void EightPoint::choosePoints(int n)
{
  int i,j;

  numPoints = n;
  
  if (n <= 7) {
    cout << "!!!!!!! Input fewer than 8 point to calculate matrix.!!!!!"<<endl;
    exit(0);
  }
  pointIndices[0] = rand() % totalNumPoints;
  for (i = 1; i < numPoints + 1; i++) {
    j = rand() % totalNumPoints;
    while (inPointIndices(j, i)) j = rand() % totalNumPoints;
    pointIndices[i] = j; 
   
  }
  
  
}

bool EightPoint::inPointIndices(int n, int m)
{

  for (int i = 0; i < m; i++)
    if (n == pointIndices[i])
      return true;

  return false;

}
  

void EightPoint::epipolarLine(CvMat *mat, CvPoint2D64d *p, IplImage *img)
{
  int rgbC;
  double x1, y1, x2, y2, a, b, c;
  int lx1, ly1, lx2, ly2;

  CvSize imgSize = cvGetSize(img);
  
  cvmSet(&m1, 0, 0, p->x);
  cvmSet(&m1, 1, 0, p->y);
  cvmSet(&m1, 2, 0, 1.0);

  cvmMul(mat, &m1, &m2);
  a = cvmGet(&m2, 0, 0);
  b = cvmGet(&m2, 1, 0);
  c = cvmGet(&m2, 2, 0);
  
  if ( (a==0.0) || (b==0.0)) {
    cout<<"WWWWW---"<<endl;
    return;
  }
 

  y1 = - c / b; x1 = - c / a;
  y2 = - (imgSize.width * a + c) / b;
  x2 = - (imgSize.height * b + c) / a;
  

  if (y1 < 0) {
    if (y2 < 0) {
      cout<<" No line----1"<<endl;
      exit(0);
    }
    else {
      lx1 = (int) x1; ly1  = 0;
      if (y2 > imgSize.height) {
	lx2 = (int) x2; ly2 = imgSize.height;
      }
      else {
	lx2 = imgSize.width; ly2 = (int) y2;
      }
    }
  }
  else if (y1 > imgSize.height) {
    if (y2 > imgSize.height) {
      cout<<" No line----2"<<endl;
      exit(0);
    }
    else {
      lx1 = (int) x2; ly1 = imgSize.height;
      if (y2 < 0) {
	lx2 = (int) x1; ly2 = 0;
      }
      else {
	lx2 = imgSize.width; ly2 = (int) y2;
      }
    }
  }
  else {
    lx1 = 0; ly1 = (int) y1;
    if (y2 < 0) {
      lx2 = (int) x1; ly2 = 0;
    }
    else if (y2 > imgSize.height) {
      lx2 = (int) x2; ly2 = imgSize.height;
    }
    else {
      lx2 = imgSize.width, ly2 = (int) y2;
    }
  }

  //draw line
  rgbC = CV_RGB(255, 255, 255);
  cvLine(img, cvPoint(lx1, ly1),
	 cvPoint(lx2, ly2), rgbC);

}