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

#include "NxWMSReader.h"
#define DEBUG

// Module specification
// <rtc-template block="module_spec">
static const char* nxwms_spec[] =
  {
    "implementation_id", "NxWMSReader",
    "type_name",         "NxWMSReader",
    "description",       "NxWMSReader component",
    "version",           "1.0",
    "vendor",            "Tomoaki Yoshikai, JSK",
    "category",          "example",
    "activity_type",     "DataFlowComponent",
    "max_instance",      "10",
    "language",          "C++",
    "lang_type",         "compile",
    // Configuration variables
    "conf.default.componentconf_file",         "NxWMSReaderTest.conf",
    "conf.default.config", "NxWMSReader0",
    ""
  };
// </rtc-template>

NxWMSReader::NxWMSReader(RTC::Manager* manager)
  : RTC::DataFlowComponentBase(manager),
    // <rtc-template block="initializer">
    m_outResultPort("out_result", m_outResult)
    // </rtc-template>
{
}

NxWMSReader::~NxWMSReader()
{    
}

RTC::ReturnCode_t NxWMSReader::onInitialize()
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers

  // Set OutPort buffer
  addOutPort("out_result", m_outResultPort);

  // Set service provider to Ports
  // Set service consumers to Ports
  
  // Set CORBA Service Ports

  // Bind variables and configuration variable
  bindParameter("componentconf_file", m_componentconf_file, "NxWMSReaderTest.conf");
  bindParameter("config", m_config, "NxWMSReader0");

  m_outResult.motion3dsensors.length(5);

  return RTC::RTC_OK;
}

/*
  RTC::ReturnCode_t NxWMSReader::onFinalize()
  {
  return RTC::RTC_OK;
  }
*/

/*
  RTC::ReturnCode_t NxWMSReader::onStartup(RTC::UniqueId ec_id)
  {
  return RTC::RTC_OK;
  }
*/

/*
  RTC::ReturnCode_t NxWMSReader::onShutdown(RTC::UniqueId ec_id)
  {
  return RTC::RTC_OK;
  }
*/


RTC::ReturnCode_t NxWMSReader::onActivated(RTC::UniqueId ec_id)
{
  this->setState();

  /* デバイスのオープン */
  nxwmsd.OpenNxwms(devicename_);

  /* デバイスの初期化 */
  nxwmsd.InitializeNxwms(prdct_id_, module_id_, freq_);
  nxwmsd.setAccsOffset(accs_offset_);
  nxwmsd.setAccsConvMat(accs_cmat_);
  m_timer.start();


  return RTC::RTC_OK;
}


RTC::ReturnCode_t NxWMSReader::onDeactivated(RTC::UniqueId ec_id)
{
  nxwmsd.FinalizeNxwms();

  return RTC::RTC_OK;
}

RTC::ReturnCode_t NxWMSReader::onExecute(RTC::UniqueId ec_id)
{
  double tmpaccs[15],tmpgyros[15],tmpmags[15];
  static int debug_count = 0;
  debug_count++;

  //デバイスドライバを介して各値の読み込み
  try
    {
      int mode = 2; // all packet      
      nxwmsd.GetSensordata(mode);
      nxwmsd.CopyAccs(tmpaccs);
      nxwmsd.CopyGyros(tmpgyros);
      nxwmsd.CopyMags(tmpmags);

      for(int i=0; i<5; i++){
	int idx = 3*i;
	int x=0,y=1,z=2;
	  
	m_outResult.motion3dsensors[i].accs.x = axis_pm_[x] * static_cast<double>(tmpaccs[idx + axis_ord_[x]]);
	m_outResult.motion3dsensors[i].accs.y = axis_pm_[y] * static_cast<double>(tmpaccs[idx + axis_ord_[y]]);
	m_outResult.motion3dsensors[i].accs.z = axis_pm_[z] * static_cast<double>(tmpaccs[idx + axis_ord_[z]]);

	m_outResult.motion3dsensors[i].gyros.x = axis_pm_[x] * static_cast<double>(tmpgyros[idx + axis_ord_[x]]);
	m_outResult.motion3dsensors[i].gyros.y = axis_pm_[y] * static_cast<double>(tmpgyros[idx + axis_ord_[y]]);
	m_outResult.motion3dsensors[i].gyros.z = axis_pm_[z] * static_cast<double>(tmpgyros[idx + axis_ord_[z]]);

	m_outResult.motion3dsensors[i].mags.x = axis_pm_[x] * static_cast<double>(tmpmags[idx + axis_ord_[x]]);
	m_outResult.motion3dsensors[i].mags.y = axis_pm_[y] * static_cast<double>(tmpmags[idx + axis_ord_[y]]);
	m_outResult.motion3dsensors[i].mags.z = axis_pm_[z] * static_cast<double>(tmpmags[idx + axis_ord_[z]]);
      }
      m_outResultPort.write();

      double tm = m_timer.tick();
      fprintf(stderr, "                                                  \n");
      fprintf(stderr, "     tick: %10.3f                                 \n",tm);
      fprintf(stderr, "     accs: %10.5f %10.5f %10.5f\n",
	      m_outResult.motion3dsensors[0].accs.x, m_outResult.motion3dsensors[0].accs.y, m_outResult.motion3dsensors[0].accs.z);
      fprintf(stderr, "    gyros: %10.1f %10.1f %10.1f\n",
	      m_outResult.motion3dsensors[0].gyros.x, m_outResult.motion3dsensors[0].gyros.y, m_outResult.motion3dsensors[0].gyros.z);
      fprintf(stderr, "     mags: %10.1f %10.1f %10.1f\n",
	      m_outResult.motion3dsensors[0].mags.x, m_outResult.motion3dsensors[0].mags.y, m_outResult.motion3dsensors[0].mags.z);
      std::cerr << "\r\x1b[5A";

    } catch(const std::runtime_error &e) {
    // 失敗しても続行
    std::cerr << e.what() << std::endl;
  }

  return RTC::RTC_OK;
}

bool
NxWMSReader::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 RTC::RTC_ERROR;
    }
  fprop.load(compf);

  // 設定ファイルから自分に関係する部分を探す
  // なければエラー
  if (fprop.findNode(m_config)==0)
    {
      std::cerr << "failed to find config node" << std::endl;
      return RTC::RTC_ERROR;
    }

  coil::Properties prop;
  prop << fprop.getNode(m_config);

  
  if (prop.findNode("deviceconfig")!=0)
    {
      coil::Properties p;
      p << prop.getNode("deviceconfig");
        
      std::cerr << std::setw(40) << std::setfill('=') << " " << std::endl;

      try {
	devicename_ = p["devicename"];
      } catch(std::runtime_error &e) {
	    std::cerr << e.what() << ":: ignored" << std::endl;
      }
    } else {
    devicename_ = std::string(DefaultDevice);
  }

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

  /********** パラメータの入力 **********/
  /**** 加速度のオフセット ****/
  for(int i=0; i<3; i++){ accs_offset_[i] = 0.0;}
  if(prop.findNode("accs_offset") != 0){
    coil::Properties p;
    p << prop.getNode("accs_offset");
    try{
      accs_offset_[0] = atof(p["axis_x"].c_str());
      accs_offset_[1] = atof(p["axis_y"].c_str());
      accs_offset_[2] = atof(p["axis_z"].c_str());
    }
    catch(std::runtime_error &e) {
      std::cerr << e.what() << ":: accs_offset ignored" << std::endl;
    }}

  /**** 加速度の変換 ****/
  for(int i=0; i<9; i++) accs_cmat_[i] = 0;
  for(int i=0; i<3; i++) accs_cmat_[4*i] = 1.0;
  if(prop.findNode("ConvMat") != 0){
    coil::Properties p;
    p << prop.getNode("ConvMat");
    try{
      accs_cmat_[0] = atof(p["xx"].c_str());
      accs_cmat_[1] = atof(p["xy"].c_str());
      accs_cmat_[2] = atof(p["xz"].c_str());
      accs_cmat_[3] = atof(p["yx"].c_str());
      accs_cmat_[4] = atof(p["yy"].c_str());
      accs_cmat_[5] = atof(p["yz"].c_str());
      accs_cmat_[6] = atof(p["zx"].c_str());
      accs_cmat_[7] = atof(p["zy"].c_str());
      accs_cmat_[8] = atof(p["zz"].c_str());
    }
    catch(std::runtime_error &e) {
      std::cerr << e.what() << ":: ConvMat ignored" << std::endl;
    }}


  /**** get axis order and plusminus ****/
  string axstr[3];
  /* x_axis */
  if(prop.findNode("axis_x")) axstr[0] = prop["axis_x"];
  else axstr[0] = "x";
  /* y_axis */
  if(prop.findNode("axis_y")) axstr[1] = prop["axis_y"];
  else axstr[1] = "y";
  /* z_axis */
  if(prop.findNode("axis_z")) axstr[2] = prop["axis_z"];
  else axstr[2] = "z";
  /* 座標軸の順番と正負の取得 */
  for(int i=0; i<3; i++){
    axis_ord_[i] = getInertiaAxis(axstr[i]);
    axis_pm_[i] = getInertiaPM(axstr[i]);
  }

  if(prop.findNode("ProductID"))
    prdct_id_ = static_cast<unsigned char>(atoi(prop["ProductID"].c_str()));
  else prdct_id_ = 0x01;

  if(prop.findNode("ModuleID"))
    module_id_ = static_cast<unsigned char>(atoi(prop["ModuleID"].c_str()));
  else module_id_ = 0x01;

  if(prop.findNode("Frequency"))
    freq_ = atoi(prop["Frequency"].c_str());
  else freq_ = 100;

  cout << setw(30) << setfill('=') << " Parameter " << setw(20) << setfill('=') << " " << endl;
  printf("accs offset : %10.3f  %10.3f %10.3f\n",
	 accs_offset_[0], accs_offset_[1], accs_offset_[2]);
  printf("accs conv mat:\n");
  printf("%10.5f  %10.5f  %10.5f\n", accs_cmat_[0], accs_cmat_[1], accs_cmat_[2]);
  printf("%10.5f  %10.5f  %10.5f\n", accs_cmat_[3], accs_cmat_[4], accs_cmat_[5]);
  printf("%10.5f  %10.5f  %10.5f\n", accs_cmat_[6], accs_cmat_[7], accs_cmat_[8]);
  cout << setw(50) << setfill('=') << " " << endl;

  { int tmp[3];
    for(int i=0; i<3; i++) tmp[i] = (int) (axis_ord_[i] * axis_pm_[i]);
    printf("accs order  : %3d %3d %3d\n", tmp[0], tmp[1], tmp[2]);
  }
  printf("ProductID: %d,  ModuleID: %d, Frequency: %d\n", prdct_id_, module_id_, freq_);

  std::string wflag;
  if(prop.findNode("Communication")){
    wflag = prop["Communication"];
    if(wflag==std::string("Wireless")){
      nxwmsd.SetWirelessFlag(1);
      printf("Communication: Wireless\n");
    } else if(wflag==std::string("Wired")){
      nxwmsd.SetWirelessFlag(0);
      printf("Communication: Wired\n");
    }
  } else {
    nxwmsd.SetWirelessFlag(0);
    printf("No configuration, \n Communication: Wireless\n");
  }

  return true;
}


/**** センサの置かれ方による座標軸方向の設定 ****/
int NxWMSReader::getInertiaAxis(string str){
  int axis = -1;
  if     (str == "x" || str == "-x") axis = 0;
  else if(str == "y" || str == "-y") axis = 1;
  else if(str == "z" || str == "-z") axis = 2;
  return axis;
}


/*
 * センサの置かれ方による座標軸の+-の設定
 * 恣意的にdouble型にしている
 */
double NxWMSReader::getInertiaPM(string str){
  int pm = 0;
  if(str == "x" || str == "y" || str == "z") pm = 1;
  else if (str == "-x" || str == "-y" || str == "-z") pm = -1;
  return pm;
}


extern "C"
{
 
  void NxWMSReaderInit(RTC::Manager* manager)
  {
    RTC::Properties profile(nxwms_spec);
    manager->registerFactory(profile,
                             RTC::Create<NxWMSReader>,
                             RTC::Delete<NxWMSReader>);
  }
  
};


