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

#ifndef RMRC_H
#define RMRC_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>

#include <math.h>

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

// </rtc-template>

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

// </rtc-template>

#include "svd.h"

float calc_tm_dif(RTC::Time tm1, RTC::Time tm2);
class ExecTh;

using namespace RTC;

/*!
 * @class RMRC
 * @brief 分解運動速度制御
 *
 */
class RMRC
  : public RTC::DataFlowComponentBase
{
  friend class ExecTh;
 public:
  /*!
   * @brief constructor
   * @param manager Maneger Object
   */
  RMRC(RTC::Manager* manager);

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

  // <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:  n_size
   * - DefaultValue: 7
   */
  int m_n_size;
  /*!
   * 
   * - Name:  m_max
   * - DefaultValue: 100
   */
  int m_m_max;
  
  std::vector<double> m_min_joint_vel;
  // </rtc-template>

  // DataInPort declaration
  // <rtc-template block="inport_declare">
  TimedDoubleSeq m_th;
  /*!
   */
  InPort<TimedDoubleSeq> m_thIn;
  
  // </rtc-template>


  // DataOutPort declaration
  // <rtc-template block="outport_declare">
  TimedDoubleSeq m_vel;
  /*!
   */
  OutPort<TimedDoubleSeq> m_velOut;
  
  // </rtc-template>

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

  // Service declaration
  // <rtc-template block="service_declare">
  /*!
   */
  ComFrmCtrlSVC_impl m_com_frm_ctrl;
  /*!
   */
  ComFkSVC_impl m_com_fk;
  /*!
   */
  ComMoveSVC_impl m_com_move;
  
  // </rtc-template>

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

 private:
  // <rtc-template block="private_attribute">
  ExecTh* m_eth;
 
  FRAME ref_frm, cur_frm;
  double *j_list;
  int o_n_size;
  int o_m_max;
  SVD * j_inv;

  // </rtc-template>

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

};


class ExecTh
  : public ConnectorDataListenerT<RTC::TimedDoubleSeq>
{
  RMRC* m_rmrc;
public :
  RTC::Logger rtclog;
  ExecTh(RMRC* comp) : m_rmrc(comp), rtclog("ExecTh") {};
  void operator()(const ConnectorInfo& info,
                  const RTC::TimedDoubleSeq& value) {

    int i,j;
    int n_size;                      //6*(n+1)行列の成分数
    int n_row;	      	              //逆ヤコビ行列計算で使用：行数
    int row_size = m_rmrc->o_n_size;	//逆ヤコビ行列計算で使用：列数
    int y_row = row_size +1;	       //逆ヤコビ行列計算で使用：
    double tmp_jcb[row_size];	       //逆ヤコビ行列計算で使用：
    double tmp_vel[row_size];	       //逆ヤコビ行列計算で使用：算出速度格納配列
    double d_tm;
    RTC::Time s_tm, e_tm;
    coil::TimeValue tv;
    Jacob jcb;			          //順運動学計算で使用：算出ヤコビ行列格納配列
    REAL *tmp_th;
    VECTOR p_vel;			        //関節角度の並進速度
    VECTOR r_vel;			        //関節角度の回転速度

     //時刻を記録
    tv = coil::gettimeofday();
    s_tm.sec = tv.sec();
    s_tm.nsec = tv.usec() * 1000;
    
    for(i=0;i<m_rmrc->m_n_size;i++) {
      tmp_vel[i]=0.0;
     }

     //データインポート（関節角度）読み込み
    tmp_th = new REAL[m_rmrc->m_n_size];
    for(i=0;i<m_rmrc->m_n_size;i++) {
      tmp_th[i]=value.data[i];
      m_rmrc->m_com_move.joint_th.data[i]=value.data[i];
     }
     
     //順運動学計算 現在の手先位置・姿勢を算出
    m_rmrc->m_com_fk.f_kinema.calc_fk(tmp_th);
    m_rmrc->m_com_fk.f_kinema.link_frame(m_rmrc->m_n_size, &(m_rmrc->cur_frm));
    //変換 FRAME型 -> Frame型
    for(i=0;i<3;i++) {
      for(j=0;j<3;j++) {
        m_rmrc->m_com_fk.current_frame.mat[i][j]=m_rmrc->cur_frm.mtr[i][j];
       }
      m_rmrc->m_com_fk.current_frame.pos[i]=m_rmrc->cur_frm.vtr[i];
     }
      
     //目標手先位置・姿勢が入力されている（分解運動速度制御モード）
    if(m_rmrc->m_com_move.ref_flag) {
       
       //順運動学計算 ヤコビ行列算出
      m_rmrc->m_com_fk.f_kinema.jacob(jcb);
       
       //手先並進・回転速度計算
      m_rmrc->m_com_frm_ctrl.calc_vel(m_rmrc->m_com_move.ref_frm, m_rmrc->cur_frm, p_vel, r_vel);
      
       //動作状態：0 ならば
      if(m_rmrc->m_com_frm_ctrl.state == 0) {
        //6*(n+1)行分のシーケンスに速度格納
        for(i=0;i<3;i++) {
          m_rmrc->j_list[i*(m_rmrc->m_n_size+1)]=p_vel[i];
          m_rmrc->j_list[(i+3)*(m_rmrc->m_n_size+1)]=r_vel[i];
         }
        //6*(n+1)行分のシーケンスにヤコビ行列成分格納
        for(i=0;i<6;i++) {
          for(j=0;j<m_rmrc->m_n_size;j++) {
            m_rmrc->j_list[i*(m_rmrc->m_n_size+1)+1+j]=jcb[i][j];
           }
         }
         
         //逆運動学計算
        n_size = 6*(m_rmrc->m_n_size+1);	//6*(n+1)行列の成分数
        n_row = n_size / y_row;
        m_rmrc->j_inv -> set_m(n_row);
        m_rmrc->j_inv -> set_m(n_row);
        if(n_row > 0) {
          for(i=0;i<n_row;i++) {
            for(j=0;j<row_size;j++) {
              tmp_jcb[j]=m_rmrc->j_list[y_row*i+1+j];
              }
            m_rmrc->j_inv -> put_row_and_y(i, m_rmrc->j_list[y_row*i], tmp_jcb);
           }
          m_rmrc->j_inv -> do_svdcmp();
          m_rmrc->j_inv -> do_round_off(0.0001);
          m_rmrc->j_inv -> do_svbksb();
          m_rmrc->j_inv -> get_result(tmp_vel);
          
            //手先速度＝逆運動学計算値
          for(i=0;i<row_size;i++) {
            if(tmp_vel[i]*tmp_vel[i] >= m_rmrc->m_min_joint_vel[i]*m_rmrc->m_min_joint_vel[i]){
              m_rmrc->m_vel.data[i] = tmp_vel[i];
              //（正の方向）速度下限値制限
            }else if(tmp_vel[i] > 0){
              m_rmrc->m_vel.data[i] = m_rmrc->m_min_joint_vel[i];
              //（負の方向）速度下限値制限
            }else {
              m_rmrc->m_vel.data[i] = -m_rmrc->m_min_joint_vel[i];
              }
           }
         }
       //動作終了状態：1 ならば
      }else {
         //目標値入力フラグ＝0
        m_rmrc->m_com_move.ref_flag=0;
         
         //ログ出力
        RTC_INFO(("\n"
         "    current position:[%f, %f, %f ]\n"
         "    current atitude :[%f, %f, %f \n"
         "                      %f, %f, %f \n"
         "                      %f, %f, %f] \n"
         ,m_rmrc->cur_frm.vtr[0],m_rmrc->cur_frm.vtr[1],m_rmrc->cur_frm.vtr[2]
         ,m_rmrc->cur_frm.mtr[0][0],m_rmrc->cur_frm.mtr[0][1],m_rmrc->cur_frm.mtr[0][2]
         ,m_rmrc->cur_frm.mtr[1][0],m_rmrc->cur_frm.mtr[1][1],m_rmrc->cur_frm.mtr[1][2]
         ,m_rmrc->cur_frm.mtr[2][0],m_rmrc->cur_frm.mtr[2][1],m_rmrc->cur_frm.mtr[2][2]));
          
         //手先速度＝0
        for(i=0;i<m_rmrc->m_n_size;i++) {
          m_rmrc->m_vel.data[i] = tmp_vel[i];   //tmp_vel[i] = 0.0
          }
        }
     //目標関節角度が入力されている（関節角度制御モード）
    }else if(m_rmrc->m_com_move.mvjnt_flag){
      //関節動作速度設定
      for(i=0;i<m_rmrc->m_n_size;i++) {
        m_rmrc->m_vel.data[i] = m_rmrc->m_com_move.joint_velocity.data[i];
       }
    }else {
      //手先速度＝0
      for(i=0;i<m_rmrc->m_n_size;i++) {
        m_rmrc->m_vel.data[i] = tmp_vel[i];     //tmp_vel[i] = 0.0
       }
     }
     //
    m_rmrc->m_velOut.write();

    delete [] tmp_th;
    
    tv = coil::gettimeofday();
    e_tm.sec = tv.sec();
    e_tm.nsec = tv.usec() * 1000;
    d_tm = calc_tm_dif(e_tm, s_tm);
    RTC_INFO(("_Interval: %f[sec]", d_tm));
   
  };
};
         
extern "C"
{
  DLL_EXPORT void RMRCInit(RTC::Manager* manager);
};

#endif // RMRC_H
