#include "TrueValueEstimatorCore.h"

using namespace rtm;

TVECore::TVECore(){
}


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


TVECore::~TVECore(){
}


void TVECore::init(double dt){
  dt_ = dt;
  avel_offset_ = ColumnVector(3,0);
  lp_accs_ = ColumnVector(3,0);
  Cov_ = pow(deg2rad(0.5), 2) * make_unit_matrix(3);

  for(int i=0; i<3; i++) lp_bwf_[i].create(dt_, 2, 2, LP);
}

void TVECore::convert(const ColumnVector &accs, const ColumnVector &gyros,
		      ColumnVector &avel, bool &isStatic){

  accs_ = accs;
  in_gyros_ = gyros;
  for(int i=0; i<3; i++) lp_accs_(i) = lp_bwf_[i].pass(accs_(i));
  /* 仮 */
  avel_ = getEstAvel();
  isStatic_ = evalIsStatic();
  if( isStatic_ ) avel_ = ColumnVector(3,0);
  updateOffset();

  /* 書き出し */
  avel = avel_;
  isStatic = isStatic_;
}


ColumnVector TVECore::getEstAvel(){
  return (in_gyros_ - avel_offset_);
}


bool TVECore::evalIsStatic(){
  static int count = 0;

  /* 実機合わせによるパラメータ */
  const double kAVel = deg2rad(5.0);
  const double kDAccs = 0.2;
  const double kNAccs = 0.05;

  double avel_nrm = avel_.norm(2.0);
  double daccs_nrm = (accs_ - lp_accs_).norm(2.0);
  double accs_nrm = abs(1.0 - accs_.norm(2.0));

  //cout << (avel_nrm < kAVel) << " " << ( daccs_nrm < kDAccs) << " " << (accs_nrm <  kNAccs) << endl;//
  //cout << avel_nrm  << " " <<  daccs_nrm  << " " << accs_nrm  << endl;//
  if( avel_nrm < kAVel && daccs_nrm < kDAccs && accs_nrm < kNAccs) count++;
  else count = 0;
  if(count * dt_ > 0.1) return true;

  return false;
}


void TVECore::updateOffset(){
  if( isStatic_ ) avel_offset_ = 0.99 * avel_offset_ + 0.01 * in_gyros_;
}
