#include "VOSimulation.h"

using namespace std;
using namespace rtm;

/**************************************************
 *
 * VOSimulation Class
 * ステレオの量子化誤差を考慮したものとと理想的なものの測観値を得る．
 *
 **************************************************/

VOSimulation::VOSimulation(){
  bsln_ = 90.0;
  fcs_ = 480.0;
}


VOSimulation::~VOSimulation(){
}


void VOSimulation::init(double bsln, double fcs){
  bsln_ = bsln;
  fcs_ = fcs;
}


void VOSimulation::initFeatures(int fn, const ColumnVector &cnt, const ColumnVector &dsize){
  points_.clear();
  this->makeNewFeatures(fn, cnt, dsize, points_);
  this->initNTrack();
  this->initLocalPositions();
}


void VOSimulation::addFeatures(int fn, const  ColumnVector &cnt, const ColumnVector &dsize){
  vector<ColumnVector> new_points;
  this->makeNewFeatures(fn, cnt, dsize, new_points);
  for(int i=0; i<fn; i++){
    points_.push_back(new_points[i]);
  }
  this->initNTrack();
  this->initLocalPositions();
}


void VOSimulation::update(const coords &dco){
  dco_ = dco;
  co_.transform(dco_, false);
  this->updateLocalPositions();
  this->updateNTrack();
  this->update3DFlow();
}


void VOSimulation::getIdealFeatures(vector<ColumnVector> &pres, vector<ColumnVector> &curs){
  pres.clear(); curs.clear();
  pres = pres_;
  curs = curs_;
}


void VOSimulation::getRealFeatures(vector<ColumnVector> &pres, vector<ColumnVector> &curs){
  pres.clear(); curs.clear();
  pres.assign(fn_, ColumnVector(3,0));
  curs.assign(fn_, ColumnVector(3,0));

  for(int i=0; i<fn_; i++){
    curs[i] = this->getQuantizationPostion(curs_[i]);
    pres[i] = this->getQuantizationPostion(pres_[i]);
  }
}


// void VOSimulation::getOtherFeatures(vector<Matrix> &SCovs, vector<Matrix> &iSCovs,
// 				    vector<Matrix> &iSQCovs){
//   SCovs.clear(); iSCovs.clear(); iSQCovs.clear();

//   SCovs.assign(fn_, Matrix(3,3,0));
//   iSCovs.assign(fn_, Matrix(3,3,0));
//   iSQCovs.assign(fn_, Matrix(3,3,0));

//   /* 回転誤差 */
//   double dt = 1 / 30.0;
//   ColumnVector avel = 2 * (dco_.quat.V) / dt;
//   Matrix qcov = makeAVelCov(avel);
//   Matrix J1 = dpQuatbyAVel(co_.quat.to_vector(), dt);
//   qcov = J1 * qcov * J1.transpose();

//   for(int i=0; i<fn_; i++){
//     Matrix SCov = makeStereoErrorCov(curs_[i], bsln_, fcs_, 1.0, 1.0, 1.0);
//     Matrix iSCov = SCov.inverse();
//     Matrix J = dpWorldPointbyQuat(curs_[i], dco_.quat.to_vector());
//   }
// }


void VOSimulation::print(){
  vector<ColumnVector>::iterator it = points_.begin();
  cout << "@VOSimulation World Points = " << endl;
  for(int i=0 ; it!=points_.end(); it++, i++ ){
    cout << i << " th : "<< (*it).transpose() << endl;
  }
  cout << "average = " << getAverage(points_).transpose() << endl;
}


/****************************************
 * ここから private関数
 ****************************************/
/* 特徴点を作る */
void VOSimulation::makeNewFeatures(int fn, const ColumnVector &cnt, const ColumnVector &dsize,
				   vector<ColumnVector> &points){
  points.clear();
  points.assign(fn, ColumnVector(3,0));

  /* -1.0 -- 1.0の間でランダムに作る */
  for(int i=0; i<fn; i++){
    for(int j=0; j<3; j++){
      points[i](j) = 1.0 - ((rand() % 2000) / 1000.0);
    }}

  for(int i=0; i<fn; i++){
    for(int j=0; j<3; j++){
      points[i](j) = dsize(j) * points[i](j);
    }
    points[i] = points[i] + cnt;
  }
}


void VOSimulation::initNTrack(){
  ntrack_.clear();
  ntrack_.assign(points_.size(), -1);
}


void VOSimulation::initLocalPositions(){
  crnt_lpos_.clear();
  crnt_lpos_.assign(points_.size(), ColumnVector(3,0));
  prvs_lpos_.clear();
  prvs_lpos_.assign(points_.size(), ColumnVector(3,0));
}


/* ローカル座標系の更新 */
void VOSimulation::updateLocalPositions(){
  prvs_lpos_ = crnt_lpos_;
  for(unsigned int i=0; i<crnt_lpos_.size(); i++){
    crnt_lpos_[i] = co_.inverse_transform_vector(points_[i]);
  }
}


/* 特徴点がカメラ画像内にあるかの判断を行う */
void VOSimulation::updateNTrack(){
  fn_ = 0;
  for(unsigned int i=0; i<points_.size(); i++){
    double u = fcs_ * (crnt_lpos_[i](1)/crnt_lpos_[i](0));
    double v = fcs_ * (crnt_lpos_[i](2)/crnt_lpos_[i](0));
    if( crnt_lpos_[i](0) > 0 && abs(u) < 320 && abs(v) < 240 ) ntrack_[i]++;
    else ntrack_[i] = -1;

    if( ntrack_[i] >= 1) fn_++;
  }
}


void VOSimulation::update3DFlow(){
  pres_.clear(); curs_.clear();
  pres_.reserve(fn_); curs_.reserve(fn_);

  for(unsigned int i=0; i<points_.size(); i++){
    if(ntrack_[i] >= 1){
      pres_.push_back(prvs_lpos_[i]);
      curs_.push_back(crnt_lpos_[i]);
    }}
}


/* 中はカメラ座標系で考えている */
ColumnVector VOSimulation::getQuantizationPostion(const ColumnVector &pos){
  double x = -pos(1);
  double y = -pos(2);
  double z =  pos(0);
  int u = static_cast<int>( (fcs_ * x / z) + 0.5 );
  int v = static_cast<int>( (fcs_ * y / z) + 0.5 );
  int d = u - static_cast<int>( (fcs_*(x - bsln_)/z) + 0.5);

  x = (u * bsln_) / d;
  y = (v * bsln_) / d;
  z = fcs_ * bsln_ / d;

  ColumnVector ans(3,0);
  ans(0) = z;
  ans(1) = -x;
  ans(2) = -y;

  return ans;
}
