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

#include "com_handSVC_impl.h"

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


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

/*
 * Methods corresponding to IDL attributes and operations
 */
CORBA::Short ComHandSVC_impl::InitHand()
{

  std::cout << "not supported." << std::endl;
  
  return 0;
}

/*
 * Methods corresponding to IDL attributes and operations
 */
CORBA::Short ComHandSVC_impl::OpenHand(CORBA::Short slow)
{

  coil::TimeValue tv;
  action_mode = 1;  //開口モード
  condition = 2;    //動作中
  RTC_INFO(("OpenHand() was called."));
  
  return 0;
}

CORBA::Short ComHandSVC_impl::CloseHand(CORBA::Short slow)
{

  coil::TimeValue tv;
  action_mode = 2;  //閉口モード
  condition = 2;    //動作中
  tv = coil::gettimeofday();
  s_tm.sec = tv.sec();
  RTC_INFO(("CloseHand() was called."));

  return 0;
}

CORBA::Short ComHandSVC_impl::GetHandCond()
{

  if(condition == 0) {
    RTC_INFO(("Close completed!"));
  }else if(condition == 1) {
    RTC_INFO(("Open completed!"));
  }else if(condition == 2) {
    RTC_INFO(("Now moving!"));
  }else if(condition == 3) {
    RTC_INFO(("Interrupted!"));
  }else {
    RTC_INFO(("Input error!"));
    return -1;
  }
  RTC_INFO(("GetHandCond() was called."));
  
  return condition;
}

CORBA::Short ComHandSVC_impl::GetHandMode()
{

  if(action_mode == 0) {
    RTC_INFO(("Mode : READY"));
  }else if(action_mode == 1) {
    RTC_INFO(("Mode : OPEN"));
  }else if(action_mode == 2) {
    RTC_INFO(("Mode : CLOSE"));
  }else {
    RTC_INFO(("Mode error!"));
    return -1;
  }
  RTC_INFO(("GetHandMode() was called."));
  
  return action_mode;
}

CORBA::Short ComHandSVC_impl::StopHand()
{

  action_mode = 0;  //待機中
  condition = 3;    //動作中断
  RTC_INFO(("StopHand() was called."));
  
  return 0;
}

//-------------------------------------------------------------
// helper functions
//-------------------------------------------------------------
double calc_tm_dif(RTC::Time tm1, RTC::Time tm2) {
  return (double)(tm1.sec - tm2.sec)
    + (double)(1000000000 + tm1.nsec - tm2.nsec)/ 1000000000.0 - 1.0;
}
//
int diff_check(double num1, double num2, double diff) {


  if((num1 - num2) < diff){
    return 1;
  }else {
    return 0;
  }
}


// End of example implementational code



