// -*- C++ -*-
/*!
 * @file  vel_6dof.cpp
 * @brief vel controlled 6dof arm
 * @date $Date$
 *
 * $Id$
 */

#include "vel_6dof.h"

// Module specification
// <rtc-template block="module_spec">
static const char* vel_6dof_spec[] =
  {
    "implementation_id", "vel_6dof",
    "type_name",         "vel_6dof",
    "description",       "vel controlled 6dof arm",
    "version",           "1.0.0",
    "vendor",            "FSI",
    "category",          "vel_dof",
    "activity_type",     "PERIODIC",
    "kind",              "DataFlowComponent",
    "max_instance",      "1",
    "language",          "C++",
    "lang_type",         "compile",
    ""
  };
// </rtc-template>

/*!
 * @brief constructor
 * @param manager Maneger Object
 */
vel_6dof::vel_6dof(RTC::Manager* manager)
    // <rtc-template block="initializer">
  : RTC::DataFlowComponentBase(manager),
    m_joint_gripIn("joint_grip", m_joint_grip),
    m_th_posOut("th_pos", m_th_pos)

    // </rtc-template>
{
}

/*!
 * @brief destructor
 */
vel_6dof::~vel_6dof()
{
}

//-------------------------------------------------------------
// helper functions
//-------------------------------------------------------------
float calc_tm_dif(RTC::Time tm1, RTC::Time tm2) {
  return (float)(tm1.sec - tm2.sec)
    + (float)(1000000000 + tm1.nsec - tm2.nsec)/ 1000000000.0 - 1.0;
}
//
float calc_decay(float d_ftm, float l_ftm, float t_cnst) {
  if(d_ftm > l_ftm) {
    return exp((l_ftm - d_ftm)/ t_cnst);
  } else {
    return 1.0;
  }
}

RTC::ReturnCode_t vel_6dof::onInitialize()
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  addInPort("joint_grip", m_joint_gripIn);
  
  // Set OutPort buffer
  addOutPort("th_pos", m_th_posOut);
  
  // Set service provider to Ports
  
  // Set service consumers to Ports
  
  // Set CORBA Service Ports
  
  // </rtc-template>

  m_th_pos.data.length(7);

  int i;

  for(i=0;i<7;i++) {
    vel[i]=0.0;
    ref_vel[i]=0.0;
    th[i]=0.0;
  }
  //ソフトウェアリミット
  //プラス値
  plimth[0] = 160*PI/180.0;   //[rad]
  plimth[1] = 130*PI/180.0;   // :
  plimth[2] = 150*PI/180.0;
  plimth[3] = 159*PI/180.0;
  plimth[4] = 129*PI/180.0;   // :
  plimth[5] = 165*PI/180.0;   //[rad]
//  plimth[6] =  85;  //[mm/sec]
  plimth[6] =  0.085; //[m/sec]
  //マイナス値
  nlimth[0] = -160*PI/180.0;  //[rad]
  nlimth[1] =  -65*PI/180.0;  // :
  nlimth[2] =    0*PI/180.0;
  nlimth[3] = -159*PI/180.0;
  nlimth[4] =  -89*PI/180.0;  // :
  nlimth[5] = -165*PI/180.0;  //[rad]
//  nlimth[6] =    0; //[mm/sec]
  nlimth[6] =    0;   //[m/sec]
  
  return RTC::RTC_OK;
}

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

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


RTC::ReturnCode_t vel_6dof::onActivated(RTC::UniqueId ec_id)
{
  int i;
  coil::TimeValue tv;

  //時刻計測
  tv = coil::gettimeofday();
  n_tm.sec  = tv.sec();
  n_tm.nsec = tv.usec() * 1000;
  o_tm.sec = n_tm.sec;
  o_tm.nsec = n_tm.nsec;
  in_tm.sec = n_tm.sec;
  in_tm.nsec = n_tm.nsec;
  //減衰係数（初期値） = 1.0
  d_fact = 1.0;

  for(i=0;i<7;i++) {
    vel[i]=0.0;
    ref_vel[i]=0.0;
    m_th_pos.data[i]=th[i];
  }
  m_th_posOut.write();

  return RTC::RTC_OK;
  
}


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


RTC::ReturnCode_t vel_6dof::onExecute(RTC::UniqueId ec_id)
{
  int i;
  coil::TimeValue tv;

  //時刻計測
  tv = coil::gettimeofday();
  n_tm.sec  = tv.sec();
  n_tm.nsec = tv.usec() * 1000;
  //EC実行間隔測定
  d_tm = calc_tm_dif(n_tm, o_tm);  // delta time
  o_tm.sec = n_tm.sec;
  o_tm.nsec = n_tm.nsec;
  
  if(m_joint_gripIn.isEmpty()) {
    di_tm = calc_tm_dif(n_tm,in_tm);  //
    //速度減衰処理
    d_fact = calc_decay(di_tm, 0.1, 1.0);  // apply decay
  } else {
    in_tm.sec = n_tm.sec;
    in_tm.nsec = n_tm.nsec;
    m_joint_gripIn.read();
    for(i=0;i<7;i++) {
      ref_vel[i]=m_joint_grip.data[i];
    }
    d_fact = 1.0;
  }
  for(i=0;i<7;i++) {
    vel[i]=d_fact*ref_vel[i];
    th[i] += vel[i]*d_tm;
    if(th[i] > plimth[i]){
      RTC_INFO(("over plimth[%d]:%.4f, th[%d] = %.4f",i,plimth[i],i,th[i]));
      th[i] = plimth[i];
    }else if (th[i] < nlimth[i]){
      RTC_INFO(("over nlimth[%d]:%.4f, th[%d] = %.4f",i,nlimth[i],i,th[i]));
      th[i] = nlimth[i];
    }else {
      if(i < 6){
        RTC_INFO(("th[%d] = %.4f [rad]  ",i,th[i]*180.0/PI));
      }else {
        RTC_INFO(("th[%d] = %.4f [m/sec]  ",i,th[i]));
        //RTC_INFO(("th[%d] = %f [mm/sec]  ",i,th[i]));
       }
     }
    m_th_pos.data[i]=th[i];
  }
  m_th_posOut.write();

  return RTC::RTC_OK;
}

/*
RTC::ReturnCode_t vel_6dof::onAborting(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/
/*
RTC::ReturnCode_t vel_6dof::onError(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/
/*
RTC::ReturnCode_t vel_6dof::onReset(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/
/*
RTC::ReturnCode_t vel_6dof::onStateUpdate(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t vel_6dof::onRateChanged(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/



extern "C"
{
 
  void vel_6dofInit(RTC::Manager* manager)
  {
    coil::Properties profile(vel_6dof_spec);
    manager->registerFactory(profile,
                             RTC::Create<vel_6dof>,
                             RTC::Delete<vel_6dof>);
  }
  
};


