// -*- C++ -*-
/*!
 * @file  NxMotionEstimator.cpp
 * @brief Sequence InPort component
 *
 */

#include "NxMotionEstimator.h"


using namespace std;
using namespace rtm;

double gettimeofday_sec(){
  struct timeval tv;
  gettimeofday(&tv,NULL);
  return tv.tv_sec + (double)tv.tv_usec*1e-6;
}

// Module specification
// <rtc-template block="module_spec">
static const char* rotation_estimator_spec[] =
  {
    "implementation_id", "NxMotionEstimator",
    "type_name",         "NxMotionEstimator",
    "description",       "NxMotionEstimator component",
    "version",           "1.0",
    "vendor",            "leus, JSK",
    "category",          "example",
    "activity_type",     "DataFlowComponent",
    "max_instance",      "10",
    "language",          "C++",
    "lang_type",         "compile",
    "conf.default.componentconf_file", "NxMotionEstimatorTest.conf",
    "conf.default.config", "NxMotionEstimator0",
    ""
  };
// </rtc-template>

NxMotionEstimator::NxMotionEstimator(RTC::Manager* manager)
  : RTC::DataFlowComponentBase(manager),
    // <rtc-template block="initializer">
    m_inSensorPort("in_sensor", m_inSensor),
    m_EstimatedResultPort("out_result", m_EstimatedResult)
{
}

NxMotionEstimator::~NxMotionEstimator()
{
}



RTC::ReturnCode_t NxMotionEstimator::onInitialize()
{
  /* Inport */
  addInPort("in_sensor",m_inSensorPort);

  /* OutPort */
  addOutPort("out_result", m_EstimatedResultPort);

  /* Service Port */

  /* Bind variables and configuration variable */
  bindParameter("componentconf_file", m_componentconf_file, "NxMotionEstimatorTest.conf");
  bindParameter("config", m_config, "NxMotionEstimator0");

  return RTC::RTC_OK;
}


RTC::ReturnCode_t NxMotionEstimator::onActivated(RTC::UniqueId ec_id)
{
  /* State Service */
  if(!this->setState()){
    std::cerr << "failed to set config" << endl;
    return RTC::RTC_ERROR;
  }

  pthread_mutex_init(&mutex, NULL);
  mtnest_ = new MECore();
  mtnest_->init(dt_/input_packets_); /* 1周期にinput_packtes_個のパケット入っている */
  cerr << "@Motion Estimator is created" << endl;

  /* 変数リセット */
  accs_.assign(input_packets_, ColumnVector(3,0));
  gyros_.assign(input_packets_, ColumnVector(3,0));
  avel_.assign(input_packets_, ColumnVector(3,0));
  quat_ = ColumnVector(4,0);
  m_first_ = true;
  count_ = 0;

  cerr << "@NxMotionEstimator is onActivated" << endl;

  return RTC::RTC_OK;
}


RTC::ReturnCode_t NxMotionEstimator::onDeactivated(RTC::UniqueId ec_id)
{
  delete mtnest_;
  free(in_accs_);
  free(in_gyros_);
  return RTC::RTC_OK;
}


RTC::ReturnCode_t NxMotionEstimator::onExecute(RTC::UniqueId ec_id){
  if(m_inSensorPort.isNew()){
    count_++;
    readSensorPort();
    this->getInertiaData();

    if(m_first_){
      for(int i=0; i<input_packets_; i++){
	cout << "@debug  : " << isStatic_ << endl;
	if( mtnest_->initFirstPose(accs_[i], gyros_[i]) ){
	  m_first_ = false;
	  utimer.start();
	  for(int j=0; j<input_packets_; j++){
	    mtnest_->convert(accs_[i], gyros_[i], quat_, Cov_, avel_[i], tvel_, isStatic_);
	  }
	  break;
	}
	else{
	  cout << "@debug  : " << isStatic_ << endl;
	}
      }
    } else {
      /*  推定 */
      for(int i=0; i<input_packets_; i++){
	mtnest_->convert(accs_[i], gyros_[i], quat_, Cov_, avel_[i], tvel_, isStatic_);
      }
      this->writeEstimationResult();
      m_EstimatedResultPort.write();

      double tm = utimer.tick();

      if(tm > dt_ + 5.0){
	cerr << "@NxMotionEstimator lap time is " << tm*1000 << " msec and overed" << endl;
      }


      {
	ColumnVector rpy = quat2rpy(quat_);
	for(int i=0; i<3; i++) rpy(i) = rad2deg(rpy(i));
	fprintf(stderr, "==================================================\n");
	fprintf(stderr, "     tick: %10.3f                    \n", tm);
	fprintf(stderr, "      rpy: %10.3f %10.3f %10.3f\n", rpy(0), rpy(1), rpy(2));
	fprintf(stderr, "trans vel: %10.3f %10.3f %10.3f\n", tvel_(0), tvel_(1), tvel_(2));
	fprintf(stderr, "\r\x1b[4A");
      }
    }}

  return RTC::RTC_OK;
}


/*
 * コマンドからの取得用
 */
std::string NxMotionEstimator::quat_result(const std::string &argv){
  char buf[32768];
  //mutex lock
  pthread_mutex_lock(&mutex);
  ColumnVector out_quat = quat_;
  pthread_mutex_unlock(&mutex);

  sprintf(buf,"#f(%f %f %f %f)", out_quat(0), out_quat(1), out_quat(2), out_quat(3));
  std::string str(buf);
  return str;
}


bool NxMotionEstimator::setState(){
  //// 設定ファイルの読み込み
  coil::Properties fprop;
  std::ifstream compf(m_componentconf_file.c_str());

  if (compf.fail()) {
    std::cerr << "failed to open component configuration file: "
	      << m_componentconf_file << std::endl;
    return false;
  }
  fprop.load(compf);

  // 設定ファイルから自分(RTC)に関係する部分を探す
  if (fprop.findNode(m_config)==0) {
    std::cerr << "failed to find config node" << std::endl;
    return false;
  }
  coil::Properties prop;
  prop << fprop.getNode(m_config);

  out_quat_[0] =  1.0;
  out_sttc_[0] = -1.0;
  for(int i=1; i<13; i++) out_quat_[i] = 0.0;

  for(int i=0; i<3; i++){
    out_accl_[i] = 0.0;
    out_avel_[i] = 0.0;
  }

  std::cerr << std::setw(40) << std::setfill('=') << " " << std::endl;

  /********** 周期 **********/
  double freq;
  try {
    freq = atof(prop["frequency"].c_str());
    dt_ = 1 / freq;
  } catch(std::runtime_error &e) {
    std::cerr << e.what() << ":: frequency must be specified!" << std::endl;
    return false;
  }

  /********** 何パケットのデータが来るのか **********/
  try {
    input_packets_ = atoi(prop["inputpackets"].c_str());
  } catch(std::runtime_error &e) {
    std::cerr << e.what() << ":: inputpackets must be specified!" << std::endl;
    return false;
  }

  in_accs_ = (double*)malloc(sizeof(double)*3*input_packets_);;
  in_gyros_ = (double*)malloc(sizeof(double)*3*input_packets_);;

  return true;
}


void NxMotionEstimator::readSensorPort(){
  m_inSensorPort.read();
  for(int i=0; i<input_packets_; i++){
    int idx = i*3;
    int x=0,y=1,z=2;

    in_accs_[idx + x] = m_inSensor.motion3dsensors[i].accs.x;
    in_accs_[idx + y] = m_inSensor.motion3dsensors[i].accs.y;
    in_accs_[idx + z] = m_inSensor.motion3dsensors[i].accs.z;

    in_gyros_[idx + x] = m_inSensor.motion3dsensors[i].gyros.x;
    in_gyros_[idx + y] = m_inSensor.motion3dsensors[i].gyros.y;
    in_gyros_[idx + z] = m_inSensor.motion3dsensors[i].gyros.z;
  }
}

void NxMotionEstimator::getInertiaData(){
  for(int i=0; i<input_packets_; i++){
    for(int j=0; j<3; j++){
      int idx = i*3;
      gyros_[i](j) = static_cast<double>(in_gyros_[idx + j]) * PI / 180.0;
      accs_[i](j) = static_cast<double>(in_accs_[idx + j]);
    }
  }
}

void NxMotionEstimator::writeEstimationResult(){
  /* state */
  ColumnVector accl(3,0), avel(3,0);
  for(int i=0; i<input_packets_; i++){
    accl = accl + accs_[i];
    avel = avel + avel_[i];
  }
  accl = accl / input_packets_;
  avel = avel / input_packets_;

  for(int i=0; i<3; i++){
    out_accl_[i] = static_cast<double>(accl(i));
    out_avel_[i] = static_cast<double>(avel(i));
    out_tvel_[i] = static_cast<double>(tvel_(i));
  }
  if(isStatic_) out_sttc_[0] = 1.0;
  else out_sttc_[0] = -1.0;

  for(int i=0; i<4; i++) out_quat_[i] = static_cast<double>(quat_(i));
  for(int i=0; i<4; i++){
    for(int j=0; j<4; j++){
      out_quat_[i*4+j+4] = static_cast<double>(Cov_(i,j));
    }}

  getAvelAverage();

  int x=0, y=1, z=2;
  m_EstimatedResult.estimatedmotion.accel.x = static_cast<double>(accs_[0](x));
  m_EstimatedResult.estimatedmotion.accel.y = static_cast<double>(accs_[0](y));
  m_EstimatedResult.estimatedmotion.accel.z = static_cast<double>(accs_[0](z));

  m_EstimatedResult.estimatedmotion.angularvel.x = static_cast<double>(ave_avel_(x));
  m_EstimatedResult.estimatedmotion.angularvel.y = static_cast<double>(ave_avel_(y));
  m_EstimatedResult.estimatedmotion.angularvel.z = static_cast<double>(ave_avel_(z));

  m_EstimatedResult.estimatedmotion.translationalvel.x = out_tvel_[x];
  m_EstimatedResult.estimatedmotion.translationalvel.y = out_tvel_[y];
  m_EstimatedResult.estimatedmotion.translationalvel.z = out_tvel_[z];

  m_EstimatedResult.estimatedmotion.isstatic = (short)out_sttc_[0];

  int q1=0, q2=1, q3=2, q4=3;
  m_EstimatedResult.estimatedmotion.quat.q1 = static_cast<double>(quat_(q1));
  m_EstimatedResult.estimatedmotion.quat.q2 = static_cast<double>(quat_(q2));
  m_EstimatedResult.estimatedmotion.quat.q3 = static_cast<double>(quat_(q3));
  m_EstimatedResult.estimatedmotion.quat.q4 = static_cast<double>(quat_(q4));

  for(int i=0; i<4; i++){
    for(int j=0; j<4; j++){
      m_EstimatedResult.estimatedmotion.quatcov[i][j] = static_cast<double>(Cov_(i,j));
    }}
}

/* 最近傍30msec分の平均角速度  */
void NxMotionEstimator::getAvelAverage(){
  for(int i=0; i<input_packets_; i++) gyro_stck_.push_back(avel_[i]);
  int stck = static_cast<int>(gyro_stck_.size());
  if(stck - 15 > 0){
    for(int i=0; i<stck-15; i++){
      gyro_stck_.pop_front();
    }
    stck = 15;
  }

  ave_avel_ = ColumnVector(3,0);
  list<ColumnVector>::iterator it = gyro_stck_.begin();

  for( ; it!=gyro_stck_.end(); it++){
    ave_avel_ = ave_avel_ + (*it);
  }
  ave_avel_ = ave_avel_ / static_cast<double>(stck);
}



extern "C"
{
  void NxMotionEstimatorInit(RTC::Manager* manager)
  {
    RTC::Properties profile(rotation_estimator_spec);
    manager->registerFactory(profile,
			     RTC::Create<NxMotionEstimator>,
			     RTC::Delete<NxMotionEstimator>);
  }
};

