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

#ifndef RH_HANDWRAPPER_H
#define RH_HANDWRAPPER_H

#include <rtm/Manager.h>
#include <rtm/DataFlowComponentBase.h>
#include <rtm/CorbaPort.h>
#include <rtm/DataInPort.h>
#include <rtm/DataOutPort.h>
#include <rtm/idl/BasicDataTypeSkel.h>
#include <rtm/idl/ExtendedDataTypesSkel.h>
#include <rtm/idl/InterfaceDataTypesSkel.h>

// Service implementation headers
// <rtc-template block="service_impl_h">
#include "com_handSVC_impl.h"
#include <math.h>
// </rtc-template>

// Service Consumer stub headers
// <rtc-template block="consumer_stub_h">

// </rtc-template>

class ExecJointVel;
class ExecThPos;

using namespace RTC;

/*!
 * @class RH_HandWrapper
 * @brief RefHardハンド制御用ラッパーモジュール
 *
 */
class RH_HandWrapper
  : public RTC::DataFlowComponentBase
{
  friend class ExecJointVel;
  friend class ExecThPos;
 public:
  /*!
   * @brief constructor
   * @param manager Maneger Object
   */
  RH_HandWrapper(RTC::Manager* manager);

  /*!
   * @brief destructor
   */
  ~RH_HandWrapper();

  // <rtc-template block="public_attribute">
  
  // </rtc-template>

  // <rtc-template block="public_operation">
  
  // </rtc-template>

  /***
   *
   * The initialize action (on CREATED->ALIVE transition)
   * formaer rtc_init_entry() 
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
   virtual RTC::ReturnCode_t onInitialize();

  /***
   *
   * The finalize action (on ALIVE->END transition)
   * formaer rtc_exiting_entry()
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
  // virtual RTC::ReturnCode_t onFinalize();

  /***
   *
   * The startup action when ExecutionContext startup
   * former rtc_starting_entry()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
  // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id);

  /***
   *
   * The shutdown action when ExecutionContext stop
   * former rtc_stopping_entry()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
  // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id);

  /***
   *
   * The activated action (Active state entry action)
   * former rtc_active_entry()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
   virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);

  /***
   *
   * The deactivated action (Active state exit action)
   * former rtc_active_exit()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
   virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);

  /***
   *
   * The execution action that is invoked periodically
   * former rtc_active_do()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
  // virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);

  /***
   *
   * The aborting action when main logic error occurred.
   * former rtc_aborting_entry()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
   virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id);

  /***
   *
   * The error action in ERROR state
   * former rtc_error_do()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
   virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id);

  /***
   *
   * The reset action that is invoked resetting
   * This is same but different the former rtc_init_entry()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
  // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id);
  
  /***
   *
   * The state update action that is invoked after onExecute() action
   * no corresponding operation exists in OpenRTm-aist-0.2.0
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
  // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id);

  /***
   *
   * The action that is invoked when execution context's rate is changed
   * no corresponding operation exists in OpenRTm-aist-0.2.0
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
  // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id);


 protected:
  // <rtc-template block="protected_attribute">
  
  // </rtc-template>

  // <rtc-template block="protected_operation">
  
  // </rtc-template>

  // Configuration variable declaration
  // <rtc-template block="config_declare">
  /*!
   * 
   * - Name:  max_width
   * - DefaultValue: 0.07
   */
  double m_max_width;
  /*!
   * 
   * - Name:  grip_vel
   * - DefaultValue: 0.01
   */
  double m_grip_vel;
  /*!
   * 
   * - Name:  m_jointnum
   * - DefaultValue: 6
   */
  int m_m_jointnum;
  // </rtc-template>

  // DataInPort declaration
  // <rtc-template block="inport_declare">
  TimedDoubleSeq m_joint_vel;
  /*!
   */
  InPort<TimedDoubleSeq> m_joint_velIn;
  TimedDoubleSeq m_th_pos;
  /*!
   */
  InPort<TimedDoubleSeq> m_th_posIn;
  
  // </rtc-template>


  // DataOutPort declaration
  // <rtc-template block="outport_declare">
  TimedDoubleSeq m_th;
  /*!
   */
  OutPort<TimedDoubleSeq> m_thOut;
  TimedDoubleSeq m_joint_grip_vel;
  /*!
   */
  OutPort<TimedDoubleSeq> m_joint_grip_velOut;
  
  // </rtc-template>

  // CORBA Port declaration
  // <rtc-template block="corbaport_declare">
  /*!
   */
  RTC::CorbaPort m_ComRHHandWrapPort;
  
  // </rtc-template>

  // Service declaration
  // <rtc-template block="service_declare">
  /*!
   */
  ComHandSVC_impl m_com_hand;
  
  // </rtc-template>

  // Consumer declaration
  // <rtc-template block="consumer_declare">
  
  // </rtc-template>

 private:
  ExecJointVel* m_ejointvel;
  ExecThPos* m_ethpos;
  int act_flag;
  double hand_width;//現在の開口幅
  // <rtc-template block="private_attribute">
  
  // </rtc-template>

  // <rtc-template block="private_operation">
  
  // </rtc-template>

};

class ExecJointVel
  : public ConnectorDataListenerT<RTC::TimedDoubleSeq>
{
  RH_HandWrapper* m_handwrapper;
public :
  RTC::Logger rtclog;
  ExecJointVel(RH_HandWrapper* comp) : m_handwrapper(comp), rtclog("ExecJointVel") {};
  void operator()(const ConnectorInfo& info,
                  const RTC::TimedDoubleSeq& value) {
                 
    int i;
     
     //アーム関節角速度格納
    for(i=0;i<m_handwrapper->m_m_jointnum;i++){
      m_handwrapper->m_joint_grip_vel.data[i] = value.data[i];
     }
     //ハンド開閉速度格納
     switch (m_handwrapper->m_com_hand.action_mode){
       case 0://ready
              m_handwrapper->m_joint_grip_vel.data[m_handwrapper->m_m_jointnum] = 0.0;
              break;
       case 1://open
              m_handwrapper->m_joint_grip_vel.data[m_handwrapper->m_m_jointnum] = m_handwrapper->m_grip_vel;
              break;
       case 2://close
              m_handwrapper->m_joint_grip_vel.data[m_handwrapper->m_m_jointnum] = -m_handwrapper->m_grip_vel;
              break;
       default://others
              RTC_ERROR(("UNKNOWN action mode!"));
              break;
      }
    for(i=0;i<m_handwrapper->m_m_jointnum;i++){
      RTC_INFO(("JointGripVel[%d] = %.4f",i,m_handwrapper->m_joint_grip_vel.data[i]));
     }
    m_handwrapper->m_joint_grip_velOut.write();
  };
};

class ExecThPos
  : public ConnectorDataListenerT<RTC::TimedDoubleSeq>
{
  RH_HandWrapper* m_handwrapper;
public :
  RTC::Logger rtclog;
  ExecThPos(RH_HandWrapper* comp) : m_handwrapper(comp), rtclog("ExecThPos") {};
  void operator()(const ConnectorInfo& info,
                  const RTC::TimedDoubleSeq& value) {
    
    int i;
    coil::TimeValue tv;
    RTC::Time n_tm;   //現在時刻
    double d_tm;      //経過秒数
    
     //アーム関節角度格納
    for(i=0;i<m_handwrapper->m_m_jointnum;i++){
      m_handwrapper->m_th.data[i] = value.data[i];
     }
     //ハンド開口幅格納
    if(value.data[m_handwrapper->m_m_jointnum] > 0){
      m_handwrapper->hand_width = value.data[m_handwrapper->m_m_jointnum];
    }else {
      m_handwrapper->hand_width = 0.0;
     }     
     
    switch (m_handwrapper->m_com_hand.action_mode){
      case 0://ready
             break;
      case 1://open
               //開口幅 > 700[mm]なら完了
             if(m_handwrapper->hand_width >= m_handwrapper->m_max_width){
               m_handwrapper->m_com_hand.action_mode = 0;
               m_handwrapper->m_com_hand.condition = 1;    //開口完了
               RTC_INFO(("Open completed: hand_width = %.4f", m_handwrapper->hand_width));
               }
             break;
      case 2://close
             tv = coil::gettimeofday();
             n_tm.sec = tv.sec();
             d_tm = calc_tm_dif(n_tm,m_handwrapper->m_com_hand.s_tm);
               //閉口幅 < 1[mm]なら完了
             if(m_handwrapper->hand_width < diff_check(m_handwrapper->hand_width,0.0, 0.001)){
               m_handwrapper->m_com_hand.action_mode = 0;
               m_handwrapper->m_com_hand.condition = 0;    //閉口完了
               RTC_INFO(("Close completed: hand_width = %.4f", m_handwrapper->hand_width));
               //または、開始から8[sec]経過したら完了
             }else if(d_tm >= 8){
               m_handwrapper->m_com_hand.action_mode = 0;
               RTC_INFO(("Close completed(time out): hand_width = %.4f", m_handwrapper->hand_width));
               }
             break;
      default://others
             RTC_ERROR(("UNKNOWN action mode!"));
             break;
     }
     //テストログ
    for(i=0;i<m_handwrapper->m_m_jointnum;i++){
      RTC_INFO(("ThPos[%d] = %f",i,m_handwrapper->m_th.data[i]));
     }
    RTC_INFO(("ThPos[%d] = %.4f",i,m_handwrapper->hand_width));
    m_handwrapper->m_thOut.write();
  };
};

extern "C"
{
  DLL_EXPORT void RH_HandWrapperInit(RTC::Manager* manager);
};

#endif // RH_HANDWRAPPER_H
