// -*- C++ -*-
#include "MotionClassifier.h"

using namespace std;
using namespace rtm;

static const char* motionestimator_spec[] =
  {
    "implementation_id", "MotionClassifier",
    "type_name",         "MotionClassifier",
    "description",       "MotionClassifier component",
    "version",           "1.0",
    "vendor",            "leus, JSK",
    "category",          "example",
    "activity_type",     "DataFlowComponent",
    "max_instance",      "10",
    "language",          "C++",
    "lang_type",         "compile",
    "conf.default.componentconf_file",  "MotionClassifierTest.conf",
    "conf.default.config", "MotionClassifier0",
    ""
  };


MotionClassifier::MotionClassifier(RTC::Manager* manager)
  : RTC::DataFlowComponentBase(manager),
    m_inFlowPort("in_flow", m_inFlow),
    m_inInertiaPort("in_inertia", m_inInertia),
    m_outCoordsPort("out_coords", m_outCoords),
    m_outStaticPCLPort("out_staticpcl", m_outStaticPCL),
    m_outMovingPCLPort("out_movingpcl", m_outMovingPCL)
{
}


MotionClassifier::~MotionClassifier()
{
}


RTC::ReturnCode_t MotionClassifier::onInitialize()
{
  /* Set InPort buffers */
  addInPort("in_flow", m_inFlowPort);
  addInPort("in_inertia", m_inInertiaPort);

  /* Set OutPort buffer */
  addOutPort("out_coords", m_outCoordsPort);
  addOutPort("out_staticpcl", m_outStaticPCLPort);
  addOutPort("out_movingpcl", m_outMovingPCLPort);

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

  return RTC::RTC_OK;
}


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

  /* MotionClassifier の実体インスタンス */
  double dt = 1 / 30.0;
  vo_.init(dt);
  /* その他 */
  orientp_ = false; /* 初期姿勢が決まるまでは待つ */
  count_ = 0;

  // Timer
  utimer_.start();

  return RTC::RTC_OK;
}


RTC::ReturnCode_t MotionClassifier::onDeactivated(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}


RTC::ReturnCode_t MotionClassifier::onExecute(RTC::UniqueId ec_id){
  if(m_inFlowPort.isNew()){
    if( m_inInertiaPort.isNew() ){
      readInertiaPort();
      readFlowPort();
      count_++;
    } else{
      cerr << "Error! Inertia Sensor can't find" << endl;
      sleep(1);
      readFlowPort();
      return RTC::RTC_OK;
    }

    /* 実時間 */
    if(orientp_){
      vo_.convert(in_flow_, in_avel_, in_quat_, in_tvel_,
		  in_odometry_, out_coords_, out_ftrs_);

      //worldpos
      m_outCoords.data.worldpos.x = out_coords_[0];
      m_outCoords.data.worldpos.y = out_coords_[1];
      m_outCoords.data.worldpos.z = out_coords_[2];
      //quaternion
      m_outCoords.data.quat.q1 = out_coords_[3];
      m_outCoords.data.quat.q2 = out_coords_[4];
      m_outCoords.data.quat.q3 = out_coords_[5];
      m_outCoords.data.quat.q4 = out_coords_[6];
      //write coords
      m_outCoordsPort.write();

      writePCL();

      /* 表示 */
      double tm = utimer_.tick();
      if(tm > 0.06) printf("30Hz cycle is overed. tick : %f\n", tm);
      quaternion quat;
      quat.t = static_cast<double>(out_coords_[3]);
      for(int i=0; i<3; i++) quat.V(i) = static_cast<double>(out_coords_[4+i]);
      ColumnVector rpy = quat2rpy(quat);
      
      fprintf(stderr, "==================================================\n");
      fprintf(stderr, " tick: %10.3f                                     \n", tm);
      fprintf(stderr, "  pos: %10.3f %10.3f %10.3f\n",
	      out_coords_[0], out_coords_[1], out_coords_[2]);
      fprintf(stderr, "  rpy: %10.3f %10.3f %10.3f\n", rpy(0), rpy(1), rpy(2));
      fprintf(stdout, "  quat: %10.3f %10.3f %10.3f %10.3f\n", quat.t, quat.V(0), quat.V(1), quat.V(2));
      fprintf(stderr, "\r\x1b[5A");
    } else {
      if(count_>10){
	vo_.initStartOrient(in_quat_);
	cout << "MotionClassifier start. First Pose is :"
	     << makeColumnVectorfromArray(4, in_quat_).transpose() << endl;
	orientp_ = true;
	utimer_.tick();
      }
    }
  }

  return RTC::RTC_OK;
}

void MotionClassifier::writePCL(){
  for(int i = 0; i < MAX_FN; i++){
    if((int)out_ftrs_[i * FEATURE_P_N] > 0){
      if(out_ftrs_[i * FEATURE_P_N + 2] <= 0.5){ //静止物体
	m_outStaticPCL.lpcl[i].count = out_ftrs_[i * FEATURE_P_N + 0];  //継続トラック数
	m_outStaticPCL.lpcl[i].objectlabel = out_ftrs_[i * FEATURE_P_N + 1];  //物体ラベル番号
	m_outStaticPCL.lpcl[i].pc_world.point.x = out_ftrs_[i * FEATURE_P_N + 3];  //ワールド座標x
	m_outStaticPCL.lpcl[i].pc_world.point.y = out_ftrs_[i * FEATURE_P_N + 4];  //ワールド座標y
	m_outStaticPCL.lpcl[i].pc_world.point.z = out_ftrs_[i * FEATURE_P_N + 5];  //ワールド座標z
	m_outStaticPCL.lpcl[i].pc_local.point.x = out_ftrs_[i * FEATURE_P_N + 6];  //ローカル座標x
	m_outStaticPCL.lpcl[i].pc_local.point.y = out_ftrs_[i * FEATURE_P_N + 7];  //ローカル座標y
	m_outStaticPCL.lpcl[i].pc_local.point.z = out_ftrs_[i * FEATURE_P_N + 8];  //ローカル座標z
	info2Color(out_ftrs_[i * FEATURE_P_N], out_ftrs_[i * FEATURE_P_N + 1], m_outStaticPCL.lpcl[i].pc_world.colour);
	info2Color(out_ftrs_[i * FEATURE_P_N], out_ftrs_[i * FEATURE_P_N + 1], m_outStaticPCL.lpcl[i].pc_local.colour);
	m_outMovingPCL.lpcl[i].count = 0;
      }else{ //移動物体
	m_outMovingPCL.lpcl[i].count = out_ftrs_[i * FEATURE_P_N + 0];  //継続トラック数
	m_outMovingPCL.lpcl[i].objectlabel = out_ftrs_[i * FEATURE_P_N + 1];  //物体ラベル番号
	m_outMovingPCL.lpcl[i].pc_world.point.x = out_ftrs_[i * FEATURE_P_N + 3];  //ワールド座標x
	m_outMovingPCL.lpcl[i].pc_world.point.y = out_ftrs_[i * FEATURE_P_N + 4];  //ワールド座標y
	m_outMovingPCL.lpcl[i].pc_world.point.z = out_ftrs_[i * FEATURE_P_N + 5];  //ワールド座標z
	m_outMovingPCL.lpcl[i].pc_local.point.x = out_ftrs_[i * FEATURE_P_N + 6];  //ローカル座標x
	m_outMovingPCL.lpcl[i].pc_local.point.y = out_ftrs_[i * FEATURE_P_N + 7];  //ローカル座標y
	m_outMovingPCL.lpcl[i].pc_local.point.z = out_ftrs_[i * FEATURE_P_N + 8];  //ローカル座標z
	info2Color(out_ftrs_[i * FEATURE_P_N], out_ftrs_[i * FEATURE_P_N + 1], m_outMovingPCL.lpcl[i].pc_world.colour);
	info2Color(out_ftrs_[i * FEATURE_P_N], out_ftrs_[i * FEATURE_P_N + 1], m_outMovingPCL.lpcl[i].pc_local.colour);
	m_outStaticPCL.lpcl[i].count = 0;
      }
    }else{
      m_outStaticPCL.lpcl[i].count = 0;
      m_outMovingPCL.lpcl[i].count = 0;
    }
  }

  m_outStaticPCLPort.write();
  m_outMovingPCLPort.write();
}

void MotionClassifier::info2Color(int ntr, int lbl, RTC::RGBColour &col){
  //トラック数とラベル番号から色を算出
  static const int MAX_LBL = 10;
  static const int MAX_NTR = 32;
  double h = 360 * lbl / MAX_LBL;
  double s = 1.0;
  double v = (ntr >= MAX_NTR ? 1.0 : (double)ntr / MAX_NTR);
  double hi = (int)h / 60;
  double f = h / 60 - hi;
  double p = v * (1 - s);
  double q = v * (1 - f * s);
  double t = v * (1 - (1 - f) * s);
  double r = 0.0, g = 0.0, b = 0.0;
  switch((int)hi)
    {
    case 0: r = v; g = t; b = p; break;
    case 1: r = q; g = v; b = p; break;
    case 2: r = p; g = v; b = t; break;
    case 3: r = p; g = q; b = v; break;
    case 4: r = t; g = p; b = v; break;
    case 5: r = v; g = p; b = q; break;
    }
  col.r = r; col.g = g; col.b = b;
}

bool MotionClassifier::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);

  /*
   * 初期値に0をいれておく. オドメトリは使わないので、0 0 0 1 0 0 0にしておく.
   * 一度１になったら外から命じてきに０を与えないと最後のオドメトリを信じつづける
   */
  for(int i=0; i<COORDS_N; i++){in_odometry_[i]=0;} 
  in_odometry_[3] = 1.0;
  for(int i=0; i<COORDS_N; i++) out_coords_[i] = 0; 
  out_coords_[3] = 1.0;
  for(int i=0; i<MAX_FN*FEATURE_P_N; i++) out_ftrs_[i] = 0;

  //worldpos
  m_outCoords.data.worldpos.x = out_coords_[0];
  m_outCoords.data.worldpos.y = out_coords_[1];
  m_outCoords.data.worldpos.z = out_coords_[2];
  //quaternion
  m_outCoords.data.quat.q1 = out_coords_[3];
  m_outCoords.data.quat.q2 = out_coords_[4];
  m_outCoords.data.quat.q3 = out_coords_[5];
  m_outCoords.data.quat.q4 = out_coords_[6];
  //write coords
  m_outCoordsPort.write();

  m_outStaticPCL.lpcl.length(MAX_FN);
  m_outMovingPCL.lpcl.length(MAX_FN);

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

  return true;
}

void MotionClassifier::readFlowPort(){
  m_inFlowPort.read();

  for(unsigned int i=0;i<m_inFlow.data.length();i++)
    in_flow_[i] = (double)m_inFlow.data[i];
}

void MotionClassifier::readInertiaPort(){
  m_inInertiaPort.read();
  int x=0, y=1, z=2;
  in_accl_[x] = m_inInertia.estimatedmotion.accel.x;
  in_accl_[y] = m_inInertia.estimatedmotion.accel.y;
  in_accl_[z] = m_inInertia.estimatedmotion.accel.z;

  in_avel_[x] = m_inInertia.estimatedmotion.angularvel.x;
  in_avel_[y] = m_inInertia.estimatedmotion.angularvel.y;
  in_avel_[z] = m_inInertia.estimatedmotion.angularvel.z;

  in_tvel_[x] = m_inInertia.estimatedmotion.translationalvel.x;
  in_tvel_[y] = m_inInertia.estimatedmotion.translationalvel.y;
  in_tvel_[z] = m_inInertia.estimatedmotion.translationalvel.z;

  int q1=0, q2=1, q3=2, q4=3;
  in_quat_[q1] = m_inInertia.estimatedmotion.quat.q1;
  in_quat_[q2] = m_inInertia.estimatedmotion.quat.q2;
  in_quat_[q3] = m_inInertia.estimatedmotion.quat.q3;
  in_quat_[q4] = m_inInertia.estimatedmotion.quat.q4;
  
  for(int i=0; i<4; i++)
    for(int j=0; j<4; j++){
      in_quat_[q4+1+4*i+j] = m_inInertia.estimatedmotion.quatcov[i][j];
    }

  in_isStatic_[0] = (double)m_inInertia.estimatedmotion.isstatic;
}

extern "C"
{
  void MotionClassifierInit(RTC::Manager* manager)
  {
    RTC::Properties profile(motionestimator_spec);
    manager->registerFactory(profile,
			     RTC::Create<MotionClassifier>,
			     RTC::Delete<MotionClassifier>);
  }
};


