// -*-C++-*-
/*!
 * @file  com_RMRCSVC_impl.h
 * @brief Service implementation header of com_RMRC.idl
 *
 */


#include "com_RMRCSkel.h"
#include "rtm/idl/BasicDataType.hh"
#include <rtm/SystemLogger.h>

#ifndef COM_RMRCSVC_IMPL_H
#define COM_RMRCSVC_IMPL_H

#include "fk.h"
#include <math.h>
#include <geo_std.h>
 
using namespace RTC;
 
/*!
 * @class ComFkSVC_impl
 * Example class implementing IDL interface ComFk
 */
class ComFkSVC_impl
 : public virtual POA_ComFk,
   public virtual PortableServer::RefCountServantBase
{
 private:
   // Make sure all instances are built on the heap by making the
   // destructor non-public
   //virtual ~ComFkSVC_impl();
   RTC::Logger rtclog;

 public:
  /*!
   * @brief standard constructor
   */
   ComFkSVC_impl();
  /*!
   * @brief destructor
   */
   virtual ~ComFkSVC_impl();

   // attributes and operations
   short set_tool(const Frame& frm);
   Frame get_cur_frm();
   short get_joint_num();

   short jnum;
   Frame current_frame;   // 現在の手先位置・姿勢
   FK f_kinema;

};

/*!
 * @class ComFrmCtrlSVC_impl
 * Example class implementing IDL interface ComFrmCtrl
 */
class ComFrmCtrlSVC_impl
 : public virtual POA_ComFrmCtrl,
   public virtual PortableServer::RefCountServantBase
{
 private:
   // Make sure all instances are built on the heap by making the
   // destructor non-public
   //virtual ~ComFrmCtrlSVC_impl();
   RTC::Logger rtclog;

 public:
  /*!
   * @brief standard constructor
   */
   ComFrmCtrlSVC_impl();
  /*!
   * @brief destructor
   */
   virtual ~ComFrmCtrlSVC_impl();

   // attributes and operations
   void set_param(CORBA::Double k_pv, CORBA::Double v_m, CORBA::Double k_pr);
   int calc_vel(FRAME geo_ref, FRAME geo_cur, VECTOR p_vel, VECTOR r_vel);

  double k_gain, v_max, k_pos_rot;
  double settle;
  int state;

};

/*!
 * @class ComMoveSVC_impl
 * Example class implementing IDL interface ComMove
 */
class ComMoveSVC_impl
 : public virtual POA_ComMove,
   public virtual PortableServer::RefCountServantBase
{
 private:
   // Make sure all instances are built on the heap by making the
   // destructor non-public
   //virtual ~ComMoveSVC_impl();
   RTC::Logger rtclog;

 public:
  /*!
   * @brief standard constructor
   */
   ComMoveSVC_impl();
  /*!
   * @brief destructor
   */
   virtual ~ComMoveSVC_impl();
   
   // attributes and operations
   short move_to(const Frame& frm);
   short set_max_joint_vel(const DoubleSeq& max_vel);
   short move_joint(const DoubleSeq& ref_angles);
   short get_softlimit(CORBA::Double& x_max, CORBA::Double& y_max, CORBA::Double& z_max, CORBA::Double& x_min, CORBA::Double& y_min, CORBA::Double& z_min);
   short set_softlimit(CORBA::Double x_max, CORBA::Double y_max, CORBA::Double z_max, CORBA::Double x_min, CORBA::Double y_min, CORBA::Double z_min);

   int ref_flag;
   int mvjnt_flag;
   int joint_num;
   FRAME ref_frm;
   DoubleSeq max_joint_vel;//関節動作時最大角速度
   DoubleSeq min_joint_vel;//関節動作時最小角速度
   DoubleSeq joint_velocity;//関節動作時角速度
   DoubleSeq joint_th;      //各軸角度
   //目標値と現在値の差分
   double sum;             //送出関節角速度の和
   double k_pos;
   double lim;             //目標値への収束判定角度[rad]
   double x_maximum;     //X軸方向ソフト上限値[mm]
   double y_maximum;     //Y軸方向ソフト上限値[mm]
   double z_maximum;     //Z軸方向ソフト上限値[mm]
   double x_minimum;     //X軸方向ソフト下限値[mm]
   double y_minimum;     //Y軸方向ソフト下限値[mm]
   double z_minimum;     //Z軸方向ソフト下限値[mm]
   
};



#endif // COM_RMRCSVC_IMPL_H


