#define USE_MATH_DEFINES

#include <iostream>
#include <fstream>
#include <string>
#include <vector>

#include <cxcore.h>
#include <cv.h>
#include <highgui.h>

#pragma comment(lib, "cxcore210.lib")
#pragma comment(lib, "cv210.lib")
#pragma comment(lib, "highgui210.lib")

void read_data(int framenum, char** argv, std::vector<CvMat*>* data2d)
{

	for(int i=0;i<framenum;i++){
		std::ifstream ifs(argv[i+2]);
		std::string line;
		int numpoints;
		float x, y;
		//double dummy;

		if(!ifs.is_open()){
			printf("can't open %s\n", argv[i+2]);
			exit(1);
		}

		ifs >> numpoints;

		//std::getline(ifs, line);

		CvMat *data = cvCreateMat(numpoints*2, 1, CV_32F );

		for(int i = 0; i < numpoints; i++){
			ifs >> x >> y;
			cvmSet(data, i*2, 0, x);
			cvmSet(data, i*2+1, 0, y);
		}
		data2d->push_back(data);
	}
}

void write_3dpoints(char* filename, CvMat* data)
{
    std::ofstream ofs(filename);

  for(int i=0;i<data->cols;i++){
	  for(int j=0;j<3;j++){
//		  printf("%f ", cvmGet(S, j, i));
		  ofs << cvmGet(data, j, i) << " ";
	  }
	  ofs << std::endl;

  }
}

void main(int argc, char** argv)
{
  std::vector<CvMat*> data2d;
  std::vector<CvMat*> data2d_c;
  int framenum, numpoint;

  if(argc < 4){
    printf("Usage: factorization <# of frames> <2Ddata file name>\n");
    exit(0);
  }

  framenum = atoi(argv[1]);
  if(argc-2 < framenum){
	  exit(0);
  }

  //１．トラッキングデータの読み込み
  read_data(framenum, argv, &data2d);
  numpoint = data2d[0]->rows/2;

  //１’．重心の計算
  for(int i=0;i<framenum;i++){
	  int cx =0, cy =0;
	  for(int j=0;j<numpoint;j++){
		  cx += cvmGet(data2d[i], j*2, 0);
		  cy += cvmGet(data2d[i], j*2+1, 0);
//		  printf("%d %d %f %f\n", i, j,	cvmGet(data2d[i], j*2, 0), cvmGet(data2d[i], j*2+1, 0));
	  }
	  CvMat* center = cvCreateMat(2, 1, CV_32F);
	  cvmSet(center, 0, 0, (float)cx/numpoint);
	  cvmSet(center, 1, 0, (float)cy/numpoint);
	  data2d_c.push_back(center);
  }

  //２．計測行列Dを作成
  CvMat* matD = cvCreateMat(framenum*2, numpoint, CV_32F);
  for(int i=0;i<framenum;i++){
	  for(int j=0;j<numpoint;j++){
		  cvmSet(matD, i*2, j, cvmGet(data2d[i], j*2, 0) - cvmGet(data2d_c[i], 0, 0));
		  cvmSet(matD, i*2+1, j, cvmGet(data2d[i], j*2+1, 0) - cvmGet(data2d_c[i], 1, 0));
	  }
  }

  //３．Dを特異値分解
  fprintf(stderr, "calculating SVD...\n");
  CvMat*W = cvCreateMat(framenum*2, numpoint, CV_32F);
  CvMat*U = cvCreateMat(framenum*2, framenum*2, CV_32F);
  CvMat*V = cvCreateMat(numpoint, numpoint, CV_32F);
  cvSVD(matD, W, U, V, CV_SVD_V_T);
  //for(int i=0;i<numpoint;i++){
	 // printf("%f ", cvmGet(W, i, i));
  //}
  //printf("\n");

  //３’．Rank3をWにあてはめ(特異値の上位3つ以外を削除)
  CvMat*W_3 = cvCreateMat(3, 3, CV_32F);
  for(int i=0;i<3;i++){
	  for(int j=0;j<3;j++){
		  cvmSet(W_3, i, j, sqrt(cvmGet(W, i, j)));
	  }
  }

  //３”．3次元座標を計算（射影復元）
  CvMat* V_3 = cvCreateMat(3, numpoint, CV_32F);
  for(int i=0;i<3;i++){
	  for(int j=0;j<numpoint;j++){
		  cvmSet(V_3, i, j, cvmGet(V, i, j));
	  }
  }
  CvMat*S_d = cvCreateMat(3, numpoint, CV_32F);
  cvMatMul(W_3, V_3, S_d);
  //for(int i=0;i<numpoint;i++){
	 // for(int j=0;j<3;j++){
		//  printf("%f ", cvmGet(S_d, j, i));
	 // }
	 // printf("\n");
  //}

  //４．メトリック拘束式を作成→簡単のため正射影

  //４’．モーション行列を計算（射影解）
  CvMat* U_3 = cvCreateMat(framenum*2, 3, CV_32F);
  for(int i=0;i<framenum*2;i++){
	  for(int j=0;j<3;j++){
		  cvmSet(U_3, i, j, cvmGet(U, i, j));
	  }
  }
  CvMat*M_d = cvCreateMat(framenum*2, 3, CV_32F);
  cvMatMul(U_3, W_3, M_d);
  //for(int i=0;i<framenum*2;i++){
	 // for(int j=0;j<3;j++){
		//  printf("%f ", cvmGet(M_d, i, j));
	 // }
	 // printf("\n");
  //}
  //printf("\n");

  //５．ユークリッドアップグレード
  //５’．拘束行列の作成
  CvMat* Met_C = cvCreateMat(framenum*3, 6, CV_32F);
  CvMat* B = cvCreateMat(framenum*3, 1, CV_32F);
  CvMat* X = cvCreateMat(6, 1, CV_32F);
  for(int i=0;i<framenum;i++){
	  //イコール1(x and y)
	  for(int j=0;j<2;j++){
		  cvmSet(Met_C, i*3+j, 0, cvmGet(M_d, i*2+j, 0)*cvmGet(M_d, i*2+j, 0));
		  cvmSet(Met_C, i*3+j, 1, 2*cvmGet(M_d, i*2+j, 0)*cvmGet(M_d, i*2+j, 1));
		  cvmSet(Met_C, i*3+j, 2, 2*cvmGet(M_d, i*2+j, 0)*cvmGet(M_d, i*2+j, 2));
		  cvmSet(Met_C, i*3+j, 3, cvmGet(M_d, i*2+j, 1)*cvmGet(M_d, i*2+j, 1));
		  cvmSet(Met_C, i*3+j, 4, 2*cvmGet(M_d, i*2+j, 1)*cvmGet(M_d, i*2+j, 2));
		  cvmSet(Met_C, i*3+j, 5, cvmGet(M_d, i*2+j, 2)*cvmGet(M_d, i*2+j, 2));
	  }
	  //イコール0
	  cvmSet(Met_C, i*3+2, 0, cvmGet(M_d, i*2, 0)*cvmGet(M_d, i*2+1, 0));
	  cvmSet(Met_C, i*3+2, 1, cvmGet(M_d, i*2, 0)*cvmGet(M_d, i*2+1, 1) + cvmGet(M_d, i*2+1, 0)*cvmGet(M_d, i*2, 1));
	  cvmSet(Met_C, i*3+2, 2, cvmGet(M_d, i*2, 0)*cvmGet(M_d, i*2+1, 2) + cvmGet(M_d, i*2+1, 0)*cvmGet(M_d, i*2, 2));
	  cvmSet(Met_C, i*3+2, 3, cvmGet(M_d, i*2, 1)*cvmGet(M_d, i*2+1, 1));
	  cvmSet(Met_C, i*3+2, 4, cvmGet(M_d, i*2, 1)*cvmGet(M_d, i*2+1, 2) + cvmGet(M_d, i*2+1, 1)*cvmGet(M_d, i*2, 2));
	  cvmSet(Met_C, i*3+2, 5, cvmGet(M_d, i*2, 2)*cvmGet(M_d, i*2+1, 2));
	  //Bにセット
	  cvmSet(B, i*3, 0, 1);
	  cvmSet(B, i*3+1, 0, 1);
	  cvmSet(B, i*3+2, 0, 0);
  }
  //５”．拘束式からX(=CC^Tの6自由度)を計算
  cvSolve(Met_C, B, X);
  //for(int i=0;i<6;i++){
	 // printf("%f ", cvmGet(X, i, 0));
  //}
  //printf("\n");
  //５”’．XからCを計算
  CvMat* mat = cvCreateMat(3, 3, CV_32F);
  CvMat* evals = cvCreateMat(3, 1, CV_32F);
  CvMat* evecs = cvCreateMat(3, 3, CV_32F);
  cvmSet(mat, 0, 0, cvmGet(X, 0, 0));
  cvmSet(mat, 0, 1, cvmGet(X, 1, 0));
  cvmSet(mat, 0, 2, cvmGet(X, 2, 0));
  cvmSet(mat, 1, 0, cvmGet(X, 1, 0));
  cvmSet(mat, 1, 1, cvmGet(X, 3, 0));
  cvmSet(mat, 1, 2, cvmGet(X, 4, 0));
  cvmSet(mat, 2, 0, cvmGet(X, 2, 0));
  cvmSet(mat, 2, 1, cvmGet(X, 4, 0));
  cvmSet(mat, 2, 2, cvmGet(X, 5, 0));
  cvEigenVV(mat, evecs, evals);
  cvTranspose(evecs, evecs);
  CvMat* evalmat = cvCreateMat(3, 3, CV_32F);
  //printf("\n");
  for(int i=0;i<3;i++){
	  for(int j=0;j<3;j++){
		  cvmSet(evalmat, i, j, 0);
		  //printf("%f ", cvmGet(evecs, i, j));
	  }
	  //printf("\n");
  }
  //printf("\n");

  for(int i=0;i<3;i++){
	  //printf("%f ", cvmGet(evals, i, 0));
	  cvmSet(evalmat, i, i, sqrt(cvmGet(evals, i, 0)));
  }
  //printf("\n");
  CvMat* M = cvCreateMat(framenum*2, 3, CV_32F);
  cvMatMul(M_d, evecs, M);
  cvMatMul(M, evalmat, M);
 //for(int i=0;i<framenum*2;i++){
	//  for(int j=0;j<3;j++){
	//	  printf("%f ", cvmGet(M, i, j));
	//  }
	//  printf("\n");
 // }
 // printf("\n");

  std::vector<CvMat*> R_e;
  R_e.resize(3);
  for(int i=0;i<2;i++){
	  R_e[i] = cvCreateMat(3, 1, CV_32F);
	  for(int j=0;j<3;j++){
		  cvmSet(R_e[i], j, 0, cvmGet(M, i, j));
	  }
  }
  R_e[2] = cvCreateMat(3, 1, CV_32F);
  cvCrossProduct(R_e[0], R_e[1], R_e[2]);
//  printf("dot prod %f\n", cvDotProduct(R_1, R_2));
  CvMat* R = cvCreateMat(3, 3, CV_32F);
  for(int i=0;i<3;i++){
	  for(int j=0;j<3;j++){
		  //printf("%f ", cvmGet(M, i, j));
		  cvmSet(R, i, j, cvmGet(R_e[i], j, 0));
	  }
	  //printf("\n");
  }
  //printf("\n---C---\n");

  CvMat* C = cvCreateMat(3, 3, CV_32F);
  cvMatMul(evecs, evalmat, C);
  cvMatMul(C, R, C);
  //for(int i=0;i<3;i++){
	 // for(int j=0;j<3;j++){
		//  printf("%f ", cvmGet(C, i, j));
	 // }
	 // printf("\n");
  //}
  //printf("\n");

  //６．3次元座標を計算
  CvMat* C_inv = cvCreateMat(3, 3, CV_32F);
  cvInvert(C, C_inv);

  CvMat*S = cvCreateMat(3, numpoint, CV_32F);
  cvMatMul(C_inv, S_d, S);
  //for(int i=0;i<numpoint;i++){
	 // for(int j=0;j<3;j++){
		//  printf("%f ", cvmGet(S, j, i));
	 // }
	 // printf("\n");
  //}

  //ファイル書き出し
  write_3dpoints("euclid.txt", S);

  //７．モーションを計算

}