#include "EKFCoords.h"
#include "FastSLAM.h"
#include "MotionClassifierCore.h"
#include "VOUtil.h"
#include "DualQuaternionEst.h"
#include "ProbMotionEst.h"
#include "VOSimulation.h"
#include <utils/UtilTimer.h>

using namespace std;
using namespace rtm;

void test_vosimulation();

////
void test_inverse_time();
void test_inverse_accuracy();
void test_getdquat();


ColumnVector make3DColumnVector(double x, double y, double z){
  ColumnVector ans(3);
  ans(0) = x;
  ans(1) = y;
  ans(2) = z;
  return ans;
}


int main(){
  srand((unsigned) time(NULL));
  //test_inverse_time();
  //test_inverse_accuracy();
  test_getdquat();
  //test_vosimulation();
  return 0;
}


void test_inverse_time(){
  UtilTimer utimer;
  utimer.start();
  Matrix mat(3,3,0), imat;

  utimer.tick();
  for(int i=0; i<1000; i++){
    for(int j=0; j<3; j++){
      for(int k=0; k<3; k++){
	mat(j,k) = (double) (rand() % 100);
      }}}
  cout << "time1 = " << utimer.tick()*1000 << endl;
  utimer.tick();

  for(int i=0; i<1000; i++){
    for(int j=0; j<3; j++){
      for(int k=0; k<3; k++){
	mat(j,k) = (double) (rand() % 100);
      }}
    imat = mat.inverse();
  }
  cout << "time2 = " << utimer.tick()*1000 << endl;

  utimer.tick();
  for(int i=0; i<1000; i++){
    for(int j=0; j<3; j++){
      for(int k=0; k<3; k++){
	mat(j,k) = (double) (rand() % 100);
      }}
    imat = inverseMatrix(mat);
  }
  cout << "time3 = " << utimer.tick()*1000 << endl;

  D33 dmat, idmat;
  utimer.tick();
  for(int i=0; i<1000; i++){
    for(int j=0; j<3; j++){
      for(int k=0; k<3; k++){
  	dmat(j,k) = (double) (rand() % 100);
      }}
    idmat = dmat.inverse();
  }
  cout << "time4 = " << utimer.tick()*1000 << endl;

}


void test_inverse_accuracy(){
  UtilTimer utimer;
  utimer.start();
  Matrix mat(3,3,0), imat1, imat2;

  for(int j=0; j<3; j++){
    for(int k=0; k<3; k++){
      mat(j,k) = (double) (rand() % 100);
    }}

  imat1 = mat.inverse();
  imat2 = inverseMatrix(mat);
  cout << "mat = " << endl << mat;
  cout << "imat1 = " << endl << imat1;
  cout << "imat2 = " << endl << imat2;
}


void test_vosimulation(){
  ColumnVector eval1(6,0);
  ColumnVector eval2(6,0);
  ColumnVector eval3(6,0);
  ColumnVector eval4(6,0);

  int max_tryn = 10000;

  for(int tryn = 0; tryn<max_tryn; tryn++){
    /* VOSimulationの初期化 */
    VOSimulation vosml;
    ColumnVector cnt = make3DColumnVector(2000,0,0);
    ColumnVector dsize = make3DColumnVector(1000, 1000, 1000);
    vosml.initFeatures(30, cnt, dsize);

    // cnt = make3DColumnVector(2000,-1000,0);
    // dsize = make3DColumnVector(500, 500, 1000);
    // vosml.addFeatures(10, cnt, dsize);

    // cnt = make3DColumnVector(2000,-1000,0);
    // dsize = make3DColumnVector(500, 500, 1000);
    // vosml.addFeatures(10, cnt, dsize);


    // cnt = make3DColumnVector(1000,30,0);
    // dsize = make3DColumnVector(500, 500, 1000);
    // vosml.addFeatures(10, cnt, dsize);

    // cnt = make3DColumnVector(3000,30,0);
    // dsize = make3DColumnVector(500, 500, 1000);
    // vosml.addFeatures(10, cnt, dsize);




    ProbMotionEst pme;
    DualQuaternionEst dqe2, dqe3, dqe4;
    dqe2.setCFFCMode(0);
    dqe3.setCFFCMode(1);
    dqe4.setCFFCMode(2);

    coords dco, estdco, estdco2, estdco3, estdco4;
    coords ansco, estco, estco2, estco3, estco4;
    Matrix estcov;
    ColumnVector rpy(3,0);
    vector<ColumnVector> pres, curs;
    vosml.update(dco);

    dco.worldpos(0) = (double) (rand()%50);
    dco.worldpos(1) = (double) (rand()%50);
    dco.worldpos(2) = (double) (rand()%50);
    rpy(0) = deg2rad((double) (rand()%3));
    rpy(1) = deg2rad((double) (rand()%3));
    rpy(2) = deg2rad((double) (rand()%3));

    dco.quat = rpy2quat(rpy);

    vosml.update(dco);
    vosml.getRealFeatures(pres, curs);
    //vosml.getIdealFeatures(pres, curs);

    // { /* debug */
    //   for(unsigned j=0;j<pres.size(); j++){
    // 	cout << "(" << pres[j].transpose() << ")";
    //   }
    //   cout << endl;
    // }


    pme.estMotion(pres, curs, dco.quat, 10, estdco, estcov);
    estdco2 = dqe2.estMotion(pres, curs, 10);
    estdco3 = dqe3.estMotion(pres, curs, 10);
    estdco4 = dqe4.estMotion(pres, curs, 10);

    estco.transform(estdco);
    estco2.transform(estdco2);
    estco3.transform(estdco3);
    estco4.transform(estdco4);
    ansco = vosml.getCoords();

    // { /* 表示 */
    //   cout << "fn  : " <<  vosml.getFN() << endl;

    //   cout << "ans  :" ;
    //   cout << " worldpos : " << ansco.worldpos.transpose();
    //   cout << "  rpy : " << (180.0/3.14) * quat2rpy(ansco.quat).transpose()  << endl;

    //   cout << "est1 :" ;
    //   cout << " worldpos : " << estco.worldpos.transpose();
    //   cout << "  rpy : " << (180.0/3.14) * quat2rpy(estco.quat).transpose()  << endl;
    //   cout << "est2 :";
    //   cout << " worldpos : " << estco2.worldpos.transpose();
    //   cout << "  rpy : " << (180.0/3.14) * quat2rpy(estco2.quat).transpose()  << endl;
    //   cout << "est3 :";
    //   cout << " worldpos : " << estco3.worldpos.transpose();
    //   cout << "  rpy : " << (180.0/3.14) * quat2rpy(estco3.quat).transpose()  << endl;
    //   cout << "est4 :";
    //   cout << " worldpos : " << estco4.worldpos.transpose();
    //   cout << "  rpy : " << (180.0/3.14) * quat2rpy(estco4.quat).transpose()  << endl;
    //   cout << endl;
    // }


    ColumnVector ansrpy = quat2rpy(ansco.quat);
    ColumnVector est1rpy = quat2rpy(estco.quat);
    ColumnVector est2rpy = quat2rpy(estco2.quat);
    ColumnVector est3rpy = quat2rpy(estco3.quat);
    ColumnVector est4rpy = quat2rpy(estco4.quat);
    for(int i=0; i<3; i++){
      eval1(i) += pow(estco.worldpos(i) - ansco.worldpos(i), 2);
      eval2(i) += pow(estco2.worldpos(i) - ansco.worldpos(i), 2);
      eval3(i) += pow(estco3.worldpos(i) - ansco.worldpos(i), 2);
      eval4(i) += pow(estco4.worldpos(i) - ansco.worldpos(i), 2);

      eval1(i+3) += pow(rad2deg(ansrpy(i) - est1rpy(i)), 2);
      eval2(i+3) += pow(rad2deg(ansrpy(i) - est2rpy(i)), 2);
      eval3(i+3) += pow(rad2deg(ansrpy(i) - est3rpy(i)), 2);
      eval4(i+3) += pow(rad2deg(ansrpy(i) - est4rpy(i)), 2);
    }
  }


  for(int i=0; i<6; i++){
    eval1(i) = sqrt(eval1(i) / max_tryn);
    eval2(i) = sqrt(eval2(i) / max_tryn);
    eval3(i) = sqrt(eval3(i) / max_tryn);
    eval4(i) = sqrt(eval4(i) / max_tryn);
  }
  /* 表示 */
  cout << eval1.transpose() << endl;
  cout << eval2.transpose() << endl;
  cout << eval3.transpose() << endl;
  cout << eval4.transpose() << endl;
}


void test_getdquat(){
  coords pco, cco, dco;
  ColumnVector rpy(3,0);

  dco.worldpos(0) = (double) (rand()%50);
  dco.worldpos(1) = (double) (rand()%50);
  dco.worldpos(2) = (double) (rand()%50);
  rpy(0) = deg2rad((double) (rand()%3));
  rpy(1) = deg2rad((double) (rand()%3));
  rpy(2) = deg2rad((double) (rand()%3));
  dco.quat = rpy2quat(rpy);

  cco = pco;
  cco.transform(dco, false);
  quaternion dquat1 = dco.quat;
  quaternion dquat2 = getDQuat(pco.quat, cco.quat);

  cout << "ans = " << dquat1 << endl;
  cout << "est = " << dquat2 << endl;
}
