// -*- C++ -*-
/*!
 * @file  RH_HandWrapper.cpp
 * @brief RefHardハンド制御用ラッパーモジュール
 * @date $Date$
 *
 * $Id$
 */

#include "RH_HandWrapper.h"

// Module specification
// <rtc-template block="module_spec">
static const char* rh_handwrapper_spec[] =
  {
    "implementation_id", "RH_HandWrapper",
    "type_name",         "RH_HandWrapper",
    "description",       "RefHardハンド制御用ラッパーモジュール",
    "version",           "1.0.0",
    "vendor",            "FSI",
    "category",          "RH_HandWrapper",
    "activity_type",     "PERIODIC",
    "kind",              "DataFlowComponent",
    "max_instance",      "0",
    "language",          "C++",
    "lang_type",         "compile",
    // Configuration variables
    "conf.default.max_width", "0.07",
    "conf.default.grip_vel", "0.01",
    "conf.default.jointnum", "6",
    // Widget
    "conf.__widget__.max_width", "text",
    "conf.__widget__.grip_vel", "text",
    "conf.__widget__.jointnum", "text",
    // Constraints
    ""
  };
// </rtc-template>

/*!
 * @brief constructor
 * @param manager Maneger Object
 */
RH_HandWrapper::RH_HandWrapper(RTC::Manager* manager)
    // <rtc-template block="initializer">
  : RTC::DataFlowComponentBase(manager),
    m_joint_velIn("JointVel", m_joint_vel),
    m_th_posIn("ThPos", m_th_pos),
    m_thOut("Th", m_th),
    m_joint_grip_velOut("JointGripVel", m_joint_grip_vel),
    m_ComRHHandWrapPort("ComRHHandWrap")

    // </rtc-template>
{
}

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



RTC::ReturnCode_t RH_HandWrapper::onInitialize()
{
  m_ejointvel=new ExecJointVel(this);
  m_joint_velIn.addConnectorDataListener(ON_BUFFER_WRITE, m_ejointvel);
  m_ethpos=new ExecThPos(this);
  m_th_posIn.addConnectorDataListener(ON_BUFFER_WRITE, m_ethpos);
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  addInPort("JointVel", m_joint_velIn);
  addInPort("ThPos", m_th_posIn);
  
  // Set OutPort buffer
  addOutPort("Th", m_thOut);
  addOutPort("JointGripVel", m_joint_grip_velOut);
  
  // Set service provider to Ports
  m_ComRHHandWrapPort.registerProvider("com_hand", "ComHand", m_com_hand);
  
  // Set service consumers to Ports
  
  // Set CORBA Service Ports
  addPort(m_ComRHHandWrapPort);
  
  // </rtc-template>

  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  bindParameter("max_width", m_max_width, "0.07");
  bindParameter("grip_vel", m_grip_vel, "0.01");
  bindParameter("jointnum", m_m_jointnum, "6");
  m_com_hand.condition = 0;
  
  // </rtc-template>
  
  m_joint_grip_vel.data.length(m_m_jointnum+1);
  m_th.data.length(m_m_jointnum);
  
  
  return RTC::RTC_OK;
}

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

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

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


RTC::ReturnCode_t RH_HandWrapper::onActivated(RTC::UniqueId ec_id)
{
  act_flag = 1;
  m_com_hand.action_mode = 0;
  
  //開口幅ソフトウェアリミット = 850[mm]とする
  if(m_max_width <= 0.0850){  
    RTC_INFO(("config max_width is set: %.4f.", m_max_width));
  }else {
    RTC_ERROR(("OVER software limit:0.0850, max_width = %.4f.", m_max_width));
    //RTC_ERROR(("OVER software limit:850, max_width = %.4f.", m_max_width));
    return RTC::RTC_ERROR;
  }
  //開口（閉口）速度ソフトウェアリミット = 200[mm]とする
  if(m_grip_vel <= 0.0200){  
    RTC_INFO(("config grip_vel is set: %.4f.", m_grip_vel));
  }else {
    RTC_ERROR(("OVER software limit:0.0200, grip_vel = %.4f.", m_grip_vel));
    //RTC_ERROR(("OVER software limit:200, grip_vel = %.4f.", m_grip_vel));
    return RTC::RTC_ERROR;
  }
  
  return RTC::RTC_OK;
}


RTC::ReturnCode_t RH_HandWrapper::onDeactivated(RTC::UniqueId ec_id)
{
  act_flag = 1;
  m_com_hand.action_mode = 0;

  return RTC::RTC_OK;
}

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


RTC::ReturnCode_t RH_HandWrapper::onAborting(RTC::UniqueId ec_id)
{
  act_flag = 0;
  m_com_hand.action_mode = 0;

  return RTC::RTC_OK;
}



RTC::ReturnCode_t RH_HandWrapper::onError(RTC::UniqueId ec_id)
{
  act_flag = 1;
  m_com_hand.action_mode = 0;

  return RTC::RTC_OK;
}


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

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

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



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


