/*************************************************
 *                   MotionEstimatorCore.cpp
 *
 *
 *************************************************/
#include "MotionEstimatorCore.h"

using namespace std;
using namespace rtm;


MECore::MECore(){
  tvest_ = NULL;
  rotest_ = NULL;
  trsest_ = NULL;
}


MECore::~MECore(){
  delete tvest_;
  delete rotest_;
  delete trsest_;
  tvest_ = NULL;
  rotest_ = NULL;
  trsest_ = NULL;
}


MECore::MECore(double dt){
  MECore();
  this->init(dt);
}


void MECore::init(double dt){
  dt_ = dt;

  if(tvest_ == NULL) tvest_ = new TVECore(dt_);
  if(rotest_ == NULL) rotest_ = new RECore(dt_);
  if(trsest_ == NULL) trsest_ = new TECore(dt_);
}


/* オドメトリ無 */
void MECore::convert(const ColumnVector &accs, const ColumnVector &gyros,
		     ColumnVector &quat, Matrix &Cov,
		     ColumnVector &avel, ColumnVector &tvel, bool &isStatic){
  accs_ = accs;
  gyros_ = gyros;
  tvest_->convert(accs_, gyros_, avel_, isStatic_);
  rotest_->convert(accs_, avel_, quat_, Cov_);
  trsest_->convert(accs_, quat_, isStatic_, tvel_);

  quat = quat_;
  Cov = Cov_;
  avel = avel_;
  tvel = tvel_;
  isStatic = isStatic_;
}


/* オドメトリ有 */
void MECore::convert(const ColumnVector &accs, const ColumnVector &gyros,
		     const ColumnVector &ref_tvel, const ColumnVector &ref_var,
		     ColumnVector &quat, Matrix &Cov,
		     ColumnVector &avel, ColumnVector &tvel, bool &isStatic){
  accs_ = accs;
  gyros_ = gyros;
  tvest_->convert(accs_, gyros_, avel_, isStatic_);
  rotest_->convert(accs_, avel_, quat_, Cov_);
  trsest_->convert(accs_, quat_, ref_tvel, ref_var, isStatic, tvel_);

  quat = quat_;
  Cov = Cov_;
  avel = avel_;
  tvel = tvel_;
  isStatic = isStatic_;
}




bool MECore::initFirstPose(const ColumnVector &accs, const ColumnVector &gyros){
  accs_ = accs;
  gyros_ = gyros;

  tvest_->convert(accs_, gyros_, avel_, isStatic_);
  isStatic_ = true; //force to start
  if( isStatic_ ){
    return (rotest_->initFirstPose(accs_));
  }
  return false;
}
