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

#include "com_RMRCSVC_impl.h"

/*
 * Example implementational code for IDL interface ComFk
 */
ComFkSVC_impl::ComFkSVC_impl()
:rtclog("ComFkSVC_impl")
{
  // Please add extra constructor code here.
}


ComFkSVC_impl::~ComFkSVC_impl()
{
  // Please add extra destructor code here.
}


/*
 * Methods corresponding to IDL attributes and operations
 */
short ComFkSVC_impl::set_tool(const Frame& frm)
{
  int i,j;
  FRAME tmp;
  for(i=0;i<3;i++) {
    for(j=0;j<3;j++) {
      tmp.mtr[i][j]=frm.mat[i][j];
    }
    tmp.vtr[i]=frm.pos[i];
  }
  f_kinema.set_tool(&tmp);
  RTC_INFO(("set_tool() was completed.\n"
             "    tool position:[%f, %f, %f ]\n"
             "    tool atitude :[%f, %f, %f \n"
             "                      %f, %f, %f \n"
             "                      %f, %f, %f] \n"
             ,tmp.vtr[0],tmp.vtr[1],tmp.vtr[2]
             ,tmp.mtr[0][0],tmp.mtr[0][1],tmp.mtr[0][2]
             ,tmp.mtr[1][0],tmp.mtr[1][1],tmp.mtr[1][2]
             ,tmp.mtr[2][0],tmp.mtr[2][1],tmp.mtr[2][2]));
             
   return 0;
}

// End of example implementational code

/*
 * Methods corresponding to IDL attributes and operations
 */
Frame ComFkSVC_impl::get_cur_frm()
{
  RTC_INFO(("get_cur_frm() was completed.\n"
             "    tool position:[%f, %f, %f ]\n"
             "    tool atitude :[%f, %f, %f \n"
             "                      %f, %f, %f \n"
             "                      %f, %f, %f] \n"
             ,current_frame.pos[0],current_frame.pos[1],current_frame.pos[2]
             ,current_frame.mat[0][0],current_frame.mat[0][1],current_frame.mat[0][2]
             ,current_frame.mat[1][0],current_frame.mat[1][1],current_frame.mat[1][2]
             ,current_frame.mat[2][0],current_frame.mat[2][1],current_frame.mat[2][2]));
             
  return current_frame;
}

// End of example implementational code

/*
 * Methods corresponding to IDL attributes and operations
 */
short ComFkSVC_impl::get_joint_num()
{
  RTC_INFO(("get_joint_num() was completed."
             "number of joint = %d", jnum));
             
  return jnum;
}

// End of example implementational code

/*
 * Example implementational code for IDL interface ComFrmCtrl
 */
ComFrmCtrlSVC_impl::ComFrmCtrlSVC_impl()
:rtclog("ComFrmCtrlSVC_impl")
{
  // Please add extra constructor code here.
}


ComFrmCtrlSVC_impl::~ComFrmCtrlSVC_impl()
{
  // Please add extra destructor code here.
}


/*
 * Methods corresponding to IDL attributes and operations
 */
void ComFrmCtrlSVC_impl::set_param(CORBA::Double k_pv, CORBA::Double v_m, CORBA::Double k_pr)
{
  k_gain = k_pv;
  v_max = v_m;
  k_pos_rot = k_pr;
  RTC_INFO(("set_param() was completed:k_gain = %f, v_max = %f, k_pos_rot = %f",k_gain, v_max, k_pos_rot));
}

int ComFrmCtrlSVC_impl::calc_vel(FRAME geo_ref, FRAME geo_cur, VECTOR p_vel, VECTOR r_vel)
{
  VECTOR dir, axis;
  MATRIX r_mat;
  REAL t_diff, p_diff, r_diff;
  REAL lim;
  REAL v_cmd,vp, vr;
  int i;

  lim = v_max / k_gain;
  v_sub(geo_ref.vtr,geo_cur.vtr,dir);
  p_diff = sqrt(v_dot(dir,dir));
  if(p_diff > settle*0.1) {
    sv_mul((1.0/p_diff),dir,dir);
  } else {
    for(i=0;i<3;i++) dir[i] = 0.0;
  }
  m_trans(geo_cur.mtr,r_mat);
  m_mul(r_mat,geo_ref.mtr,r_mat);
  r_diff = rot_axis(r_mat,axis);     // axis in geo_cur
  mv_mul(geo_cur.mtr,axis,axis);
  t_diff = sqrt(p_diff*p_diff + k_pos_rot * k_pos_rot * r_diff * r_diff);
  if(t_diff>=lim) {
    v_cmd = v_max;
  } else {
    v_cmd = k_gain * t_diff;
  }
  if(t_diff > settle) {
    state = 0;
    vp = v_cmd * p_diff/t_diff;
    vr = v_cmd * r_diff/t_diff;
  } else {
    state = 1;
    vp=0.0;
    vr=0.0;
  }
  for(i=0;i<3;i++) {
    p_vel[i] = vp * dir[i];
    r_vel[i] = vr * axis[i];
  }
  RTC_INFO(("calc_vel() was completed:p_vel = [%f, %f, %f][mm/sec], r_vel = [%f, %f, %f][rad/sec], state = %d",p_vel[0], p_vel[1], p_vel[2], r_vel[0], r_vel[1], r_vel[2], state));
  return 0;
}
// End of example implementational code

/*
 * Example implementational code for IDL interface ComMove
 */
ComMoveSVC_impl::ComMoveSVC_impl()
:rtclog("ComMoveSVC_impl")
{
  // Please add extra constructor code here.
}


ComMoveSVC_impl::~ComMoveSVC_impl()
{
  // Please add extra destructor code here.
}


/*
 * set grab position[mm] and attitude of tool(gripper)
 */
short ComMoveSVC_impl::move_to(const Frame& frm)
{/*
    int i,j;
    VECTOR tmp_vtr;
    VECTOR tmp_abc;
    MATRIX tmp_mtr;
    MATRIX tmp_amtr;
    MATRIX tmp_bmtr;
    MATRIX tmp_cmtr;
    
    for(i=0; i<3; i++) {
	    tmp_vtr[i] = xyz.pos[i];
	    tmp_abc[i] = abc.pos[i];	
    }
    ma(tmp_abc[0],tmp_amtr);
    mb(tmp_abc[1],tmp_bmtr);
    mc(tmp_abc[2],tmp_cmtr);
    m_mul(tmp_amtr,tmp_bmtr,tmp_mtr);
    m_mul(tmp_mtr,tmp_cmtr,tmp_mtr);
    */
    
    int i,j;
    
    for(i=0; i<3; i++) {
      for(j=0; j<3; j++) {
        ref_frm.mtr[i][j] = frm.mat[i][j];
       }
      ref_frm.vtr[i] = frm.pos[i];
    }
    ref_flag = 1;
    while(ref_flag){
       coil::usleep(10000);
     }
     RTC_INFO(("move_to() was completed.\n"
             "    reference position:[%f, %f, %f ]\n"
             "    reference atitude :[%f, %f, %f \n"
             "                      %f, %f, %f \n"
             "                      %f, %f, %f] \n"
             ,ref_frm.vtr[0],ref_frm.vtr[1],ref_frm.vtr[2]
             ,ref_frm.mtr[0][0],ref_frm.mtr[0][1],ref_frm.mtr[0][2]
             ,ref_frm.mtr[1][0],ref_frm.mtr[1][1],ref_frm.mtr[1][2]
             ,ref_frm.mtr[2][0],ref_frm.mtr[2][1],ref_frm.mtr[2][2]));
             
   return 0;
    
}


/*
 * 
 */
short ComMoveSVC_impl::set_max_joint_vel(const DoubleSeq& max_vel)
{
  int i;
  
  for(i=0;i<joint_num;i++) {
    max_joint_vel.data[i] = max_vel.data[i];  //[rad/sec]
  }
  
  RTC_INFO(("set_max_joint_vel() was completed."));
  for(i=0;i<joint_num;i++) {
    RTC_INFO(("max_joint_vel[%d] = %f",i, max_joint_vel.data[i]));
  }
    
  return 0;
}

/*
 * 
 */
short ComMoveSVC_impl::move_joint(const DoubleSeq& ref_angles)
{
  int i;
  double diff[joint_num]; //関節角度の差分

  
  mvjnt_flag = 1;
  k_pos = 1.0;
  lim = 0.001;            //[rad]
  
  while(mvjnt_flag){
    sum = 0.0;
    
    //目標値との差分から関節角速度を算出する
    for(i=0;i<joint_num;i++) {
      diff[i] = ref_angles.data[i] - joint_th.data[i];  
      if(fabs(diff[i]) < lim){
         joint_velocity.data[i] = 0.0;
      }else {
        joint_velocity.data[i] = k_pos * diff[i];    //[rad/sec]
         //最大動作速度を越えないよう調整
        if((fabs(joint_velocity.data[i])) > max_joint_vel.data[i]){
            //符号 正の場合
          if(joint_velocity.data[i] > 0){
            joint_velocity.data[i] =  max_joint_vel.data[i];
            //符号 負の場合
          }else {
            joint_velocity.data[i] =  -max_joint_vel.data[i];
            }
         }
         //動作下限値以下の速度にならないよう調整（速度ゼロの場合はそのままで良い）
        if((fabs(joint_velocity.data[i]) < min_joint_vel.data[i]) && (fabs(joint_velocity.data[i]) > 0)){
          //符号 正の場合
          if(joint_velocity.data[i] > 0){
            joint_velocity.data[i] =  min_joint_vel.data[i];
          //符号 負の場合
          }else {
            joint_velocity.data[i] =  -min_joint_vel.data[i];
           }
         }
       }
    }
    for(i=0;i<joint_num;i++){
      std::cout << "jointvel[" << i << "] = " << joint_velocity.data[i] << std::endl;
      sum = sum + joint_velocity.data[i];
    }
    //目標値到達ならば（全軸速度がゼロ）
    if(sum == 0.0){
      mvjnt_flag = 0;
    //最大動作速度 >= 関節動作速度 >= 最小動作速度になるよう調整
    }
    
    coil::usleep(10000);
  }
  return 0;
  
}

/*
 * 
 */
short ComMoveSVC_impl::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)
{
  x_max = x_maximum;
  y_max = y_maximum;
  z_max = z_maximum;
  x_min = x_minimum;
  y_min = y_minimum;
  z_min = z_minimum;
  
  RTC_INFO(("set_softlimit() was completed."));
  RTC_INFO(("max x_softlimit = %f",x_max));
  RTC_INFO(("max y_softlimit = %f",y_max));
  RTC_INFO(("max z_softlimit = %f",z_max));
  RTC_INFO(("min x_softlimit = %f",x_min));
  RTC_INFO(("min y_softlimit = %f",y_min));
  RTC_INFO(("min z_softlimit = %f",z_min));
  
  return 0;
}

/*
 * 
 */
short ComMoveSVC_impl::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)
{
  x_maximum = x_max;
  y_maximum = y_max;
  z_maximum = z_max;
  x_minimum = x_min;
  y_minimum = y_min;
  z_minimum = z_min;
  
  RTC_INFO(("set_softlimit() was completed."));
  RTC_INFO(("max x_softlimit = %f",x_maximum));
  RTC_INFO(("max y_softlimit = %f",y_maximum));
  RTC_INFO(("max z_softlimit = %f",z_maximum));
  RTC_INFO(("min x_softlimit = %f",x_minimum));
  RTC_INFO(("min y_softlimit = %f",y_minimum));
  RTC_INFO(("min z_softlimit = %f",z_minimum));
  
  return 0;
}

//-------------------------------------------------------------
// helper functions
//-------------------------------------------------------------
float calc_tm_dif(RTC::Time tm1, RTC::Time tm2) {
  return (float)(tm1.sec - tm2.sec)
    + (float)(1000000000 + tm1.nsec - tm2.nsec)/ 1000000000.0 - 1.0;
}
//
float calc_decay(double d_ftm, double l_ftm, double t_cnst) {
  if(d_ftm > l_ftm) {
    return exp((l_ftm - d_ftm)/ t_cnst);
  } else {
    return 1.0;
  }
}
//--------------------------------------------------------

// End of example implementational code



