/*************************************************
 *                   test-oct.cpp
 *
 * 
 * written by T.Goto
 *************************************************/


#include "octave_rtmex.h"
#include <iostream>
#include <malloc.h>
#include <iostream>
#include <stdlib.h>
#include <time.h>
#include <list>

using namespace std;
using namespace rtm;

#ifndef PI
#define PI 3.141592653589793238462643383279
#endif

// test normalize_vecetor
void func1();
//test vector_angle
void func2();
// test quaternion
void func3();
// test make_quat;
void func4();
// test vector_op;
void func5();
// クォータニオン演算子のチェック
void func6();
// Matrixの試し
void func7();
// test make coords
void func8();
//test quat to mat
void func9();

// rpy <=> R
void func10();
// test==
void func11();

void test_rotatevector();

void test_norm();
//主成分分析などの試し
void test_eigen();
void test_eigen2();

void test_bestquat();
void test_getrot();

//ベクトルの要素のソート
void test_sort();



void test_DQ();
void test_vo();

// translateの試し
void test_translate();
void test_transform_vector();
//transform
void test_transform();

// quat2matrixとかその他もろもろ
void test_convert();




// removeのテスト
// type : 0=>row 1=>column 2=>both
void test_remove_matrix(){
  int type = 0;
  printf("enter remove type\n");
  printf("rows=>0, columns=>1, matrix=>2  :  "); scanf("%d", &type);

  double elm = 0.0;
  Matrix mat1(10,10,0);
  for(int i=0; i<10; i++){
    for(int j=0; j<10; j++){
      mat1(i,j) = elm;
      elm += 1.0;
    }}
  cout << "mat1" << endl << mat1 << endl;

  int r,c,nr,nc;
  Matrix mat2;

  switch (type){
  case 0:
    printf("enter start row of remove : "); scanf("%d", &r);
    printf("enter row num of remove   : "); scanf("%d", &nr);
    mat2 = remove_rows(mat1, r, nr);
    cout << "mat2" << endl << mat2 << endl;
    break;
  case 1:
    printf("enter start column of remove : "); scanf("%d", &c);
    printf("enter column num of remove   : "); scanf("%d", &nc);
    mat2 = remove_columns(mat1, c, nc);
    cout << "mat2" << endl << mat2 << endl;
    break;
  case 2:
    printf("enter start row and column : "); scanf("%d %d", &r, &c);
    printf("enter row and column num of remove : "); scanf("%d %d", &nr, &nc);
    mat2 = remove_matrix(mat1, r, c, nr, nc);
    cout << "mat2" << endl << mat2 << endl;
    break;
  }
}


void test_vector_functions(){
  ColumnVector v1(3), v2(3);
  printf("enter vector1 : "); scanf("%lf %lf %lf", &v1(0), &v1(1), &v1(2));
  printf("enter vector2 : "); scanf("%lf %lf %lf", &v2(0), &v2(1), &v2(2));

  double ip = vector_ip(v1, v2);
  ColumnVector op = vector_op(v1, v2);
  double angle = rad2deg(vector_angle(v1,v2));

  cout << "vector1 : " << v1.transpose() << endl;
  cout << "vector2 : " << v2.transpose() << endl;
  cout << "inner p : " << ip << endl;
  cout << "outer p : " << op.transpose() << endl;
  cout << "angle   : " << angle << endl;

  normalize_vector(v1);
  normalize_vector(v2);
  cout << endl << "....vector is normalized" << endl;
  cout << "vector1 : " << v1.transpose() << endl;
  cout << "vector2 : " << v2.transpose() << endl;
}



void test_eigen(){
  srand((unsigned) time(NULL));
  Matrix mat1(4,4);
  int i,j;
  for(i=0; i<4; i++){
    for(j=0; j<4; j++){
      mat1(i,j) = (double) (rand() % 100);
    }
  }
  mat1(1,0) = mat1(0,1);
  mat1(2,0) = mat1(0,2);
  mat1(2,1) = mat1(1,2);
  mat1(3,0) = mat1(0,3);
  mat1(3,1) = mat1(1,3);
  mat1(3,2) = mat1(2,3);

  EIG eig(mat1);
  Matrix evecs = real(eig.eigenvectors());
  ColumnVector evals = real(eig.eigenvalues());


  cout << "mat1" << endl << mat1 << endl;
  cout << "Eigen Vectors" << endl << evecs << endl;
  cout << "Eigen Values" << endl << evals << endl;
  cout << "Recomposed Matrix" << endl << evecs * Matrix(DiagMatrix(evals)) * evecs.inverse() << endl;

  for(i=0; i<4; i++){
    cout << "EVAL " << i << "th" << endl;
    cout << "Eigen Value  : " << evals(i) << endl;
    cout << "Eigen Vector : " << evecs.column(i).transpose() << endl;
    cout << "qt*N*q       : "
	 << evecs.column(i).transpose() * mat1 * evecs.column(i)
	 << endl << endl;
  }
}


void test_eigen2(){
  srand((unsigned) time(NULL));
  Matrix mat1(4,4);
  int i,j;
  for(i=0; i<4; i++){
    for(j=0; j<4; j++){
      mat1(i,j) = (double) (rand() % 100);
    }
  }
  mat1(1,0) = mat1(0,1);
  mat1(2,0) = mat1(0,2);
  mat1(2,1) = mat1(1,2);
  mat1(3,0) = mat1(0,3);
  mat1(3,1) = mat1(1,3);
  mat1(3,2) = mat1(2,3);

  EIG eig(mat1);
  Matrix evecs = real(eig.eigenvectors());
  ColumnVector evals = real(eig.eigenvalues());

  cout << "mat1 : " << endl << mat1;
  cout << "evals : " << evals.transpose() << endl;
  cout << "evecs^-1*mat1*evec" << endl << evecs.inverse() * mat1 * evecs << endl;

  Matrix mat2(4,4,0);
  for(i=0; i<4; i++){
    mat2(i,i) = evals(i);
  }
  cout << "mat2 form eig" << endl << evecs * mat2 * evecs.inverse();

  // ColumnVector => DiagMatrixは以下の様に作った方が綺麗だが，時々ばくる気がする．
  // mat2 = Matrix(DiagMatrix(evals));
  ColumnVector tmp(3,0);
  for(int i=0; i<3; i++){
    tmp(i) = static_cast<double>(i);
  }
  DiagMatrix tmp2(tmp);
  cout << "tmp2" << endl << tmp2 << endl;
  Matrix tmp3(tmp2);
  cout << "tmp3" << endl << tmp3 << endl;
}


void test_translate(){
  coords co;
  ColumnVector pos(3,0);

  ColumnVector trans0(3,0), trans1(3,0), trans2(3,0);
  trans0(0) = 100;
  trans1(1) = 100;
  trans2(2) = 100;

  double rad = deg2rad(45);
  ColumnVector axis0(3,0),axis1(3,0),axis2(3,0);
  axis0(0) = 1;
  axis1(1) = 1;
  axis2(2) = 1;
  quaternion rot0(rad,axis0);
  quaternion rot1(rad,axis1);
  quaternion rot2(rad,axis2);

  co.rotate(rot0, false);
  co.rotate(rot1, false);
  co.translate(trans0, true);

  co.translate(trans0, false);
  co.translate(trans1, false);
  co.rotate(rot2, true);

  co.rotate(rot2, false);
  co.rotate(rot1, true);
  co.translate(trans2, false);

  cout << "worldpos   : " << co.worldpos.transpose() << endl;
  cout << "inverse 0  : " << co.inverse_transform_vector(pos).transpose() << endl;
  cout << "transform  : " << co.transform_vector(ColumnVector(3,100)).transpose() << endl;
}


void test_transform_vector(){
  // logの値
  ColumnVector tmp_rot(4,0);
  tmp_rot(0) = 0.991575;
  tmp_rot(1) = 0.0132376;
  tmp_rot(2) = -0.130002;
  tmp_rot(3) = -0.000399916;

  ColumnVector tmp_pos(3,0);
  tmp_pos = tmp_pos.stack(tmp_rot);

  coords co(tmp_pos);
  ColumnVector sample_vec(3,0);
  sample_vec(0) = 334.910034;
  sample_vec(1) = -547.280090;
  sample_vec(2) = 2658.164307;
  ColumnVector ans = co.transform_vector(sample_vec);
  cout << "ans = " << ans.transpose() << endl;

  Matrix rotmat = quat2matrix(co.quat);
  ans = rotmat * sample_vec;
  cout << "ans2 = " << ans.transpose() << endl;
  ColumnVector rpy = matrix2rpy(rotmat);
  cout << "rpy : "  << rpy.transpose() << endl;
}


void test_transform(){
  ColumnVector rpy(3,0);
  rpy(0) = 0.05; //rool
  rpy(1) = 0.10; //pitch
  rpy(2) = 0.15; //yaw
  Matrix rotmat = rpy2matrix(rpy);
  quaternion quat = matrix2quat(rotmat);
  ColumnVector pos(3,0);
  pos(0) = 100;
  pos(1) = 200;
  pos(2) = 300;

  coords trans(pos, quat.to_vector());
  coords co;

  co.transform(trans,true);
  co.transform(trans,false);
  co.transform(trans,false);
  co.transform(trans,true);

  cout << "rotmat" << endl << rotmat << endl;
  cout << "coords" << endl << co.to_vector().transpose() << endl;

}


void test_angle_convert(){
  ColumnVector rpy(3,0);
  rpy(0) = 0.05; //rool
  rpy(1) = 0.10; //pitch
  rpy(2) = 0.15; //yaw
  cout << "rpy = " << rpy.transpose() << endl;

  Matrix rotmat = rpy2matrix(rpy);
  cout << "rotmat = " << endl << rotmat << endl;

  quaternion quat = matrix2quat(rotmat);
  cout << "quat = " << quat.to_vector().transpose() << endl;

  Matrix rotmat2 = quat2matrix(quat);
  cout << "rotmat2 = " << endl << rotmat2 << endl;

  ColumnVector rpy2 = matrix2rpy(rotmat2);
  cout << "rpy2 = " << rpy2.transpose() << endl;
}


// // 変数名はReview and Analysis of Solutions of the Three Point Perspectiveに準拠
// void tpoints_core(vector<ColumnVector> curs, vector<ColumnVector> pres,
// 		  const vector<ColumnVector> dists, coords ans){
//   //double f = 488.0; //３次元から無理やり２次元に落としているので関係ない．
//   // 特徴点群の距離

//   double a = dists[0];
//   double b = dists[1];
//   double c = dists[2];


//   //距離情報をリセット
//   vector<ColumnVector> uvecs;
//   pres_3d.assin(3, ColumnVector(3,0));
//   for(int i=0; i<3; i++){
//     uvecs[i] = curs[i];
//     normalize_vector(uvecs[i]);
//   }

//   double s[3];

//   // 特徴点群へのベクトルの成す角
//   double angs[3];
//   for(int i=0; i<3; i++) angs[i] = vector_angle(uvecs[(i+1)%3], uvecs[(i+2)%3]);

//   // lambdaを求める
//   double G = pow(c,2) * ( pow(c,2)*pow(sin(angs[1]),2) - pow(b,2)*pow(sin(angs[2]),2) );
//   double H = 0.0;
//   H += pow(b,2) * ( pow(b,2) - pow(a,2) ) * pow(sin(angs[2]), 2);
//   H += pow(c,2) * ( pow(c,2) + 2*pow(a,2) ) * pow(sin(angs[1],2),2);
//   H += 2 * pow(b,2) + pow(c,2) * (-1 + cos(ang[0])*cos(ang[1])*cos(ang[2]));
//   double I = 0.0;
//   I += pow(b,2) * ( pow(b,2) - pow(c,2) ) * pow(sin(angs[0]), 2);
//   I += pow(a,2) * ( pow(a,2) + 2*pow(c,2) ) * pow(sin(angs[1],2),2);
//   I += 2 * pow(a,2) + pow(b,2) * (-1 + cos(ang[0])*cos(ang[1])*cos(ang[2]));
//   double J = pow(a,2) * ( pow(a,2) * pow(sin(ang[1]), 2) - pow(b,2)* pow(sin(ang[0]),2) );
//   double lmd = calc3DEquation(G,H,I.J,1000,0);


//   // 係数を求める
//   double A = 1 + lmd;
//   double B = - cos(angs[0]);
//   double C =  (pow(b,2) - pow(a,2))/pow(b,2) - (lmd * pow(c,2)) / pow(b,2);
//   double D = - (lmd * cos(ang[2]));
//   double E = ( (pow(a,2)+ (lmd * pow(c,2)))/pow(b,2) ) * cos(angs[1]);
//   double F = (- pow(a,2))/pow(b,2) + (lmd * (pow(b,2) + pow(c,2)))/pow(b,2);

//   double p = sqrt( pow(B,2) - A*C );
//   double q = sgn(B*E-C*D) * sqrt(pow(E,2) - C*F);

//   // ここから+- を考慮する
//   double m[2], n[2];
//   m[0] = (-B+p)/C;
//   m[1] = (-B-p)/C;
//   n[0] = (-(E-q))/C;
//   n[1] = (-(E+q))/C;

//   double _A[2];
//   double _B[2];
//   double _C[2];
//   double u_l[2];
//   double u_s[2];
//   double v_l[2];
//   double v_s[2];

//   for(int i=0; i<2; i++){
//     _A[i] = pow(b,2) - m[i]*pow(c,2);
//     _B[i] = pow(c,2) * (cos(angs[1]) - n[i]) * m[i] - pow(b,2) * cos(angs[2]);
//     _C[i] = (- c * pow(n[i], 2)) + 2*pow(c,2)*n[i]*cos(angs[1]) + pow(b,2) - pow(c,2);
//     // u
//     u_l[i] = -sgn(_B) * (abs(_B) + sqrt(pow(_B,2) - _A*_C)) / A;
//     u_s[i] = _C/(A*u_l[i]);

//     // v
//     v_l[i] = u_l[i] + n[i];
//     v_s[i] = u_s[i] + n[i];
//   }


// }


// //３次方程式（必ず解がある）
// double calc3DEquation(double a, double b, double c, double d, int N, double x0){
//   double x;
//   double xold = x0;
//   for (k=0;k<=N;k++) {
//     F=a*pow(xold,3)+b*pow(xold,2)+c*xold+d;  /* ３次方程式 */
//     Df=3*a*pow(xold,2)+2*b*xold+c;  /* Df : F を微分したもの */
//     if (fabs(Df)<eps) {  /* <math.h>の中にあるdouble型絶対値を返す関数 */
//       x=xold;      /* ３次式の微分値が極微になったら x とする */
//       break;
//     }
//     else {
//       x=xold-F/Df;  /* これはよく分からん */
//       if (fabs(x-xold)<eps) break;
//       else xold=x;
//     }
//   }
//   double F=a*pow(x,3)+b*pow(x,2)+c*x+d;  /* ax^3+bx^2+cx+d=0 の確認 */
//   return x;
// }

int main(){
    srand((unsigned) time(NULL));    
    //test_remove_matrix();
    //test_vector_functions();
    //test_eigen();
    return 0;
}
