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

using namespace std;
using namespace rtm;
using namespace RTC;

static const char* classifiedpointsconverter_spec[] =
  {
    "implementation_id", "ClassifiedPointsConverter",
    "type_name",         "ClassifiedPointsConverter",
    "description",       "ClassifiedPointsConverter component",
    "version",           "1.0",
    "vendor", "leus",
    "category",          "example",
    "activity_type",     "DataFlowComponent",
    "max_instance",      "10",
    "language",          "C++",
    "lang_type",         "compile",
    "conf.default.componentconf_file",  "ClassifiedPointsConverterTest.conf",
    "conf.default.config", "ClassifiedPointsConverter0",
    "conf.default.box_depth", "300",
    "conf.default.box_width", "40",
    "conf.default.box_height", "40",
    ""
  };


ClassifiedPointsConverter::ClassifiedPointsConverter(RTC::Manager* manager)
  : RTC::DataFlowComponentBase(manager),
    m_inCoordsPort("in_coords", m_inCoords),
    m_inStaticPCLPort("in_staticpcl", m_inStaticPCL),
    m_inMovingPCLPort("in_movingpcl", m_inMovingPCL),
    m_Box3DOut("Box3D", m_Box3D),
    m_Points3DOut("Points3D", m_Points3D)
{
}


ClassifiedPointsConverter::~ClassifiedPointsConverter()
{
}


RTC::ReturnCode_t ClassifiedPointsConverter::onInitialize()
{
  /* Set InPort buffers */
  addInPort("in_coords", m_inCoordsPort);
  addInPort("in_staticpcl", m_inStaticPCLPort);
  addInPort("in_movingpcl", m_inMovingPCLPort);

  /* Set OutPort buffer */
  addOutPort("Box3D", m_Box3DOut);
  addOutPort("Points3D", m_Points3DOut);

  /* Bind variables and configuration variable */
  bindParameter("componentconf_file", m_componentconf_file, "ClassifiedPointsConverterTest.conf");
  bindParameter("config", m_config, "ClassifiedPointsConverter0");
  bindParameter("box_depth", m_box_depth, "300" );
  bindParameter("box_width", m_box_width, "40" );
  bindParameter("box_height", m_box_height, "40" );

  return RTC::RTC_OK;
}


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

  // Timer
  utimer_.start();

  return RTC::RTC_OK;
}


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


RTC::ReturnCode_t ClassifiedPointsConverter::onExecute(RTC::UniqueId ec_id){
  if(m_inCoordsPort.isNew()){
    if(m_inStaticPCLPort.isNew()){
      if(m_inMovingPCLPort.isNew()){
	m_inCoordsPort.read();
	m_inStaticPCLPort.read();
	m_inMovingPCLPort.read();

	//Odmetry
	m_state_coords[0] = (float)m_inCoords.data.worldpos.x;
	m_state_coords[1] = (float)m_inCoords.data.worldpos.y;
	m_state_coords[2] = (float)m_inCoords.data.worldpos.z;
	m_state_coords[3] = (float)m_inCoords.data.quat.q1;
	m_state_coords[4] = (float)m_inCoords.data.quat.q2;
	m_state_coords[5] = (float)m_inCoords.data.quat.q3;
	m_state_coords[6] = (float)m_inCoords.data.quat.q4;

	quaternion quat;
	quat.t = static_cast<double>(m_state_coords[3]);
	for(int i=0; i<3; i++) quat.V(i) = static_cast<double>(m_state_coords[4+i]);
	ColumnVector rpy = quat2rpy(quat);

	//box is (tx ty tz rx ry rz depth width height)
	m_Box3D.data[0] = m_state_coords[0];
	m_Box3D.data[1] = m_state_coords[1];
	m_Box3D.data[2] = m_state_coords[2];
	m_Box3D.data[3] = (float)rpy(0);
	m_Box3D.data[4] = (float)rpy(1);
	m_Box3D.data[5] = (float)rpy(2);
	m_Box3D.data[6] = m_box_depth;
	m_Box3D.data[7] = m_box_width;
	m_Box3D.data[8] = m_box_height;

	int cnt = 0;

	//FeaturePoints
	for(unsigned int i=0;i<m_inStaticPCL.lpcl.length();i++){
	  if(m_inStaticPCL.lpcl[i].count > 0){
	    m_Points3D.points[i].point.x = (float)m_inStaticPCL.lpcl[i].pc_world.point.x;
	    m_Points3D.points[i].point.y = (float)m_inStaticPCL.lpcl[i].pc_world.point.y;
	    m_Points3D.points[i].point.z = (float)m_inStaticPCL.lpcl[i].pc_world.point.z;
	    m_Points3D.points[i].colour.r = (float)m_inStaticPCL.lpcl[i].pc_world.colour.r;
	    m_Points3D.points[i].colour.g = (float)m_inStaticPCL.lpcl[i].pc_world.colour.g;
	    m_Points3D.points[i].colour.b = (float)m_inStaticPCL.lpcl[i].pc_world.colour.b;
	    cnt++;
	  }
	}

	for(unsigned int i=0;i<m_inMovingPCL.lpcl.length();i++){
	  if(m_inMovingPCL.lpcl[i].count > 0){
	    m_Points3D.points[i].point.x = (float)m_inMovingPCL.lpcl[i].pc_world.point.x;
	    m_Points3D.points[i].point.y = (float)m_inMovingPCL.lpcl[i].pc_world.point.y;
	    m_Points3D.points[i].point.z = (float)m_inMovingPCL.lpcl[i].pc_world.point.z;
	    m_Points3D.points[i].colour.r = (float)m_inMovingPCL.lpcl[i].pc_world.colour.r;
	    m_Points3D.points[i].colour.g = (float)m_inMovingPCL.lpcl[i].pc_world.colour.g;
	    m_Points3D.points[i].colour.b = (float)m_inMovingPCL.lpcl[i].pc_world.colour.b;
	    cnt++;
	  }
	}

	//outport
	m_Box3DOut.write();
	m_Points3DOut.write();

	double tm = utimer_.tick();
	fprintf(stdout, "==================================================\n");
	fprintf(stdout, " tick: %10.3f\n", tm);
	fprintf(stdout, "  pos: %10.3f %10.3f %10.3f\n",
		m_state_coords[0], m_state_coords[1], m_state_coords[2]);
	fprintf(stdout, "  rpy: %10.3f %10.3f %10.3f\n", rpy(0), rpy(1), rpy(2));
	fprintf(stdout, "\r\x1b[4A");
	m_state_pclnum[0] = (short)cnt;
      }
    }
  }

  return RTC::RTC_OK;
}

bool ClassifiedPointsConverter::setState(){
  /**** read config ****/
  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);

  std::cerr << m_config << std::endl;

  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);

  //make state
  float m_state_coords[7];
  float m_state_allpcls[1800];
  short m_state_pclnum[1];

  {
    //Initialize
    int plen;
    plen = 7;
    for(int i=0;i<plen;i++){m_state_coords[i] = 0;} m_state_coords[3] = 1;
    plen = 1800;
    for(int i=0;i<plen;i++){m_state_allpcls[i] = 0;}
    m_state_pclnum[0] = 0;
    m_Points3D.points.length(200);
    m_Box3D.data.length(9);
  }

  return true;
}


extern "C"
{
  void ClassifiedPointsConverterInit(RTC::Manager* manager)
  {
    RTC::Properties profile(classifiedpointsconverter_spec);
    manager->registerFactory(profile,
			     RTC::Create<ClassifiedPointsConverter>,
			     RTC::Delete<ClassifiedPointsConverter>);
  }
};


