// -*- C++ -*-
/*!
 * @file  RMRC.cpp
 * @brief 分解運動速度制御
 * @date $Date$
 *
 * $Id$
 */

#include "RMRC.h"
#include "VectorConvert.h"

// Module specification
// <rtc-template block="module_spec">
static const char* rmrc_spec[] =
  {
    "implementation_id", "RMRC",
    "type_name",         "RMRC",
    "description",       "分解運動速度制御",
    "version",           "1.0.0",
    "vendor",            "FSI",
    "category",          "RMRC",
    "activity_type",     "PERIODIC",
    "kind",              "DataFlowComponent",
    "max_instance",      "0",
    "language",          "C++",
    "lang_type",         "compile",
    // Configuration variables
    "conf.default.n_size", "7",
    "conf.default.m_max", "100",
    "conf.default.min_joint_vel", "0.0,0.0,0.0,0.0,0.0,0.0,0.0",
    // Widget
    "conf.__widget__.n_size", "text",
    "conf.__widget__.m_max", "text",
    // Constraints
    ""
  };
// </rtc-template>

/*!
 * @brief constructor
 * @param manager Maneger Object
 */
RMRC::RMRC(RTC::Manager* manager)
    // <rtc-template block="initializer">
  : RTC::DataFlowComponentBase(manager),
    m_thIn("Th", m_th),
    m_velOut("Vel", m_vel),
    m_RMRCProvPort("RMRCProv")

    // </rtc-template>
{
}

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



RTC::ReturnCode_t RMRC::onInitialize()
{
  m_eth=new ExecTh(this);
  m_thIn.addConnectorDataListener(ON_BUFFER_WRITE, m_eth);
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  addInPort("Th", m_thIn);
  
  // Set OutPort buffer
  addOutPort("Vel", m_velOut);
  
  // Set service provider to Ports
  m_RMRCProvPort.registerProvider("com_frm_ctrl", "ComFrmCtrl", m_com_frm_ctrl);
  m_RMRCProvPort.registerProvider("com_fk", "ComFk", m_com_fk);
  m_RMRCProvPort.registerProvider("com_move", "ComMove", m_com_move);
  
  // Set service consumers to Ports
  
  // Set CORBA Service Ports
  addPort(m_RMRCProvPort);
  
  // </rtc-template>

  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  bindParameter("n_size", m_n_size, "7");
  bindParameter("m_max", m_m_max, "100");
  bindParameter("min_joint_vel", m_min_joint_vel, "0.0,0.0,0.0,0.0,0.0,0.0,0.0");
  
  m_vel.data.length(m_n_size);
  // </rtc-template>

  /*  
   *  frm_ctrl code start
   *
   * default
   * k_gain = 1.0, means time constant is 1 sec.
   * v_max = 200[mm/sec]
   * k_pos_rot = 200[mm], means that sizes of objects are about this size.
   */
  m_com_frm_ctrl.set_param(1.0, 200, 200);
  m_com_frm_ctrl.settle = 1.0;
  m_com_frm_ctrl.state = 1;
  m_com_move.set_softlimit(850.0, 850.0, 850.0, -850.0, -850.0, -850.0);
  m_com_move.ref_flag = 0;       //目標手先位置・姿勢未入力
  m_com_move.mvjnt_flag = 0;     //目標関節角度未入力
  /*  
   *  frm_ctrl code end
   */
   
  return RTC::RTC_OK;
}

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

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

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


RTC::ReturnCode_t RMRC::onActivated(RTC::UniqueId ec_id)
{
  int i;
  
 
  m_com_fk.jnum = m_n_size;
  m_com_move.ref_flag = 0;
  m_com_move.mvjnt_flag = 0;     
  m_com_move.max_joint_vel.data.length(m_n_size); 
  m_com_move.min_joint_vel.data.length(m_n_size); 
  m_com_move.joint_velocity.data.length(m_n_size);
  m_com_move.joint_th.data.length(m_n_size);      
  j_list = new double[6*(m_n_size+1)];

  if((o_n_size != m_n_size) || (o_m_max != m_m_max)) {
    o_n_size = m_n_size;
    o_m_max = m_m_max;
    m_vel.data.length(o_n_size); // angular vel of o_n_size joints
    j_inv = new SVD(o_m_max,o_n_size);
  }

  m_com_move.joint_num = m_n_size;

  //関節動作時の最大、最小関節動作速度設定
  for(i=0;i<m_n_size;i++) {
    m_com_move.max_joint_vel.data[i] = 0.4;  //[rad/sec]
    m_com_move.min_joint_vel.data[i] = m_min_joint_vel[i];
  }
  m_com_move.set_max_joint_vel(m_com_move.max_joint_vel);
   
  return RTC::RTC_OK;
}


RTC::ReturnCode_t RMRC::onDeactivated(RTC::UniqueId ec_id)
{

  m_com_move.ref_flag = 0;
  m_com_move.mvjnt_flag = 0;     

  delete [] j_list;

  return RTC::RTC_OK;
}
/*
RTC::ReturnCode_t RMRC::onExecute(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

RTC::ReturnCode_t RMRC::onAborting(RTC::UniqueId ec_id)
{
  m_com_move.ref_flag = 0;
  m_com_move.mvjnt_flag = 0;

  return RTC::RTC_OK;
}


RTC::ReturnCode_t RMRC::onError(RTC::UniqueId ec_id)
{

  m_com_move.ref_flag = 0;
  m_com_move.mvjnt_flag = 0;

  return RTC::RTC_OK;
}


RTC::ReturnCode_t RMRC::onReset(RTC::UniqueId ec_id)
{

  m_com_move.ref_flag = 0;
  m_com_move.mvjnt_flag = 0;

  return RTC::RTC_OK;
}

/*
RTC::ReturnCode_t RMRC::onAborting(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t RMRC::onError(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t RMRC::onReset(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t RMRC::onStateUpdate(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

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



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


