// -*- C++ -*-
/*!
 * @file  RH2ArmControl.h
 * @brief ArmController
 * @date  $Date$
 *
 * @author N.Takekawa (takekawa-777@aist.go.jp)
 *
 * Copyright (c) 2008, National Institute
 * of Advanced Industrial Science and Technology (AIST).
 * All rights reserved. This program is made available under the terms 
 * of the Eclipse Public License v1.0 which accompanies this distribution, 
 * and is available at http://www.eclipse.org/legal/epl-v10.html
 *
 * $Id$
 */

#ifndef RH2ARMCONTROL_H
#define RH2ARMCONTROL_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>

/*--------------------Addition---------------------*/
#include <termios.h>
#include <stdio.h>
#include <vector>
#include <math.h>
#include <iomanip>
#include <signal.h>
#include <stdlib.h>
#include <iostream>
extern "C" {
#include "svmlib.h"
}

using namespace std;
#define DEBUG 0
#define DEBUG_TIME 0
#define CALLBACK 0

#if CALLBACK
#include <rtm/RingBuffer.h>
class Debug
	: public RTC::OnWriteConvert<RTC::TimedDoubleSeq>
{
	ofstream ofsCallBack;
	public:
		Debug(void) { 
		};
		RTC::TimedDoubleSeq operator()(const RTC::TimedDoubleSeq& value)
		{
			RTC::TimedDoubleSeq ret(value);
			double dummy_sec = value.tm.sec + 1.0e-9*value.tm.nsec;
			printf("[CallBack](InVelocity) sec:%ld  nsec:%ld\n",value.tm.sec, value.tm.nsec);
			return ret;
		};
};	
#endif
/*--------------------Addition---------------------*/

// Service implementation headers
// <rtc-template block="service_impl_h">

// </rtc-template>

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

// </rtc-template>

using namespace RTC;

/*!
 * @class RH2ArmControl
 * @brief ArmController
 *
 * The arm is driven. 
 *
 * in : ArmVelocity  out : ArmAngle
 *
 * NA
 *
 * NA
 *
 */
class RH2ArmControl
  : public RTC::DataFlowComponentBase
{
 public:
  /*!
   * @brief constructor
   * @param manager Maneger Object
   */
  RH2ArmControl(RTC::Manager* manager);

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

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

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

  /*!
   * The device port is opened
   *
   * The initialize action (on CREATED->ALIVE transition)
   * formaer rtc_init_entry() 
   *
   * @return RTC::ReturnCode_t
   * 
   * @pre NA
   * @post NA
   * 
   */
   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);

  /***
   * close of device port
   *
   * The shutdown action when ExecutionContext stop
   * former rtc_stopping_entry()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * @pre NA
   * @post NA
   * 
   */
   virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id);

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

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

  /***
   * Instruction transmission to motor
   *
   * The execution action that is invoked periodically
   * former rtc_active_do()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * @pre NA
   * @post NA
   * 
   */
   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">
  /*!
   * Device File Name
   * - Name: DeviceFileName DeviceFileName
   * - DefaultValue: /dev/ttyUSB0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  std::string m_DeviceFileName;
  /*!
   * P-Gain of the [No.1] axis motor
   * - Name: Pgain_1 Pgain_1
   * - DefaultValue: 10
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_Pgain_1;
  /*!
   * P-Gain of the [No.2] axis motor
   * - Name: Pgain_2 Pgain_2
   * - DefaultValue: 10
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_Pgain_2;
  /*!
   * P-Gain of the [No.3] axis motor
   * - Name: Pgain_3 Pgain_3
   * - DefaultValue: 14
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_Pgain_3;
  /*!
   * P-Gain of the [No.4] axis motor
   * - Name: Pgain_4 Pgain_4
   * - DefaultValue: 14
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_Pgain_4;
  /*!
   * P-Gain of the [No.5] axis motor
   * - Name: Pgain_5 Pgain_5
   * - DefaultValue: 10
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_Pgain_5;
  /*!
   * P-Gain of the [No.6] axis motor
   * - Name: Pgain_6 Pgain_6
   * - DefaultValue: 50
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_Pgain_6;
  /*!
   * P-Gain of the [No.7] axis motor
   * - Name: Pgain_7 Pgain_7
   * - DefaultValue: 50
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_Pgain_7;
  /*!
   * I-Gain of the [No.1] axis motor
   * - Name: Igain_1 Igain_1
   * - DefaultValue: 0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_Igain_1;
  /*!
   * I-Gain of the [No.2] axis motor
   * - Name: Igain_2 Igain_2
   * - DefaultValue: 0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_Igain_2;
  /*!
   * I-Gain of the [No.3] axis motor
   * - Name: Igain_3 Igain_3
   * - DefaultValue: 0

   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_Igain_3;
  /*!
   * I-Gain of the [No.4] axis motor
   * - Name: Igain_4 Igain_4
   * - DefaultValue: 0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_Igain_4;
  /*!
   * I-Gain of the [No.5] axis motor
   * - Name: Igain_5 Igain_5
   * - DefaultValue: 0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_Igain_5;
  /*!
   * I-Gain of the [No.6] axis motor
   * - Name: Igain_6 Igain_6
   * - DefaultValue: 0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_Igain_6;
  /*!
   * I-Gain of the [No.7] axis motor
   * - Name: Igain_7 Igain_7
   * - DefaultValue: 0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_Igain_7;
  /*!
   * D-Gain of the [No.1] axis motor
   * - Name: Dgain_1 Dgain_1
   * - DefaultValue: 0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_Dgain_1;
  /*!
   * D-Gain of the [No.2] axis motor
   * - Name: Dgain_2 Dgain_2
   * - DefaultValue: 0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_Dgain_2;
  /*!
   * D-Gain of the [No.3] axis motor
   * - Name: Dgain_3 Dgain_3
   * - DefaultValue: 0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_Dgain_3;
  /*!
   * D-Gain of the [No.4] axis motor
   * - Name: Dgain_4 Dgain_4
   * - DefaultValue: 0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_Dgain_4;
  /*!
   * D-Gain of the [No.5] axis motor
   * - Name: Dgain_5 Dgain_5
   * - DefaultValue: 0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_Dgain_5;
  /*!
   * D-Gain of the [No.6] axis motor
   * - Name: Dgain_6 Dgain_6
   * - DefaultValue: 0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_Dgain_6;
  /*!
   * D-Gain of the [No.7] axis motor
   * - Name: Dgain_7 Dgain_7
   * - DefaultValue: 0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_Dgain_7;
  /*!
   * P-Gain of the [No.1] axis motor
   * - Name: VPgain_1 VPgain_1
   * - DefaultValue: 20
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_VPgain_1;
  /*!
   * P-Gain of the [No.2] axis motor
   * - Name: VPgain_2 VPgain_2
   * - DefaultValue: 50
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_VPgain_2;
  /*!
   * P-Gain of the [No.3] axis motor
   * - Name: VPgain_3 VPgain_3
   * - DefaultValue: 100
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_VPgain_3;
  /*!
   * P-Gain of the [No.4] axis motor
   * - Name: VPgain_4 VPgain_4
   * - DefaultValue: 150
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_VPgain_4;
  /*!
   * P-Gain of the [No.5] axis motor
   * - Name: VPgain_5 VPgain_5
   * - DefaultValue: 50
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_VPgain_5;
  /*!
   * P-Gain of the [No.6] axis motor
   * - Name: VPgain_6 VPgain_6
   * - DefaultValue: 150
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_VPgain_6;
  /*!
   * P-Gain of the [No.7] axis motor
   * - Name: VPgain_7 VPgain_7
   * - DefaultValue: 250
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_VPgain_7;
  /*!
   * I-Gain of the [No.1] axis motor
   * - Name: VIgain_1 VIgain_1
   * - DefaultValue: 0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_VIgain_1;
  /*!
   * I-Gain of the [No.2] axis motor
   * - Name: VIgain_2 VIgain_2
   * - DefaultValue: 0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_VIgain_2;
  /*!
   * I-Gain of the [No.3] axis motor
   * - Name: VIgain_3 VIgain_3
   * - DefaultValue: 0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_VIgain_3;
  /*!
   * I-Gain of the [No.4] axis motor
   * - Name: VIgain_4 VIgain_4
   * - DefaultValue: 0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_VIgain_4;
  /*!
   * I-Gain of the [No.5] axis motor
   * - Name: VIgain_5 VIgain_5
   * - DefaultValue: 0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_VIgain_5;
  /*!
   * I-Gain of the [No.6] axis motor
   * - Name: VIgain_6 VIgain_6
   * - DefaultValue: 0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_VIgain_6;
  /*!
   * I-Gain of the [No.7] axis motor
   * - Name: VIgain_7 VIgain_7
   * - DefaultValue: 0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_VIgain_7;
  /*!
   * D-Gain of the [No.1] axis motor
   * - Name: VDgain_1 VDgain_1
   * - DefaultValue: 0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_VDgain_1;
  /*!
   * D-Gain of the [No.2] axis motor
   * - Name: VDgain_2 VDgain_2
   * - DefaultValue: 0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_VDgain_2;
  /*!
   * D-Gain of the [No.3] axis motor
   * - Name: VDgain_3 VDgain_3
   * - DefaultValue: 0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_VDgain_3;
  /*!
   * D-Gain of the [No.4] axis motor
   * - Name: VDgain_4 VDgain_4
   * - DefaultValue: 0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_VDgain_4;
  /*!
   * D-Gain of the [No.5] axis motor
   * - Name: VDgain_5 VDgain_5
   * - DefaultValue: 0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_VDgain_5;
  /*!
   * D-Gain of the [No.6] axis motor
   * - Name: VDgain_6 VDgain_6
   * - DefaultValue: 0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_VDgain_6;
  /*!
   * D-Gain of the [No.7] axis motor
   * - Name: VDgain_7 VDgain_7
   * - DefaultValue: 0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_VDgain_7;
  /*!
   * Velocity Limit of the [No.1] axis motor
   * - Name: VelocLimit_1 VelocLimit_1
   * - DefaultValue: 1.04
   * - Unit: rad/sec
   * - Range: NA
   * - Constraint: NA
   */
  double m_VelocLimit_1;
  /*!
   * Velocity Limit of the [No.2] axis motor
   * - Name: VelocLimit_2 VelocLimit_2
   * - DefaultValue: 0.52
   * - Unit: rad/sec
   * - Range: NA
   * - Constraint: NA
   */
  double m_VelocLimit_2;
  /*!
   * Velocity Limit of the [No.3] axis motor
   * - Name: VelocLimit_3 VelocLimit_3
   * - DefaultValue: 0.4
   * - Unit: rad/sec
   * - Range: NA
   * - Constraint: NA
   */
  double m_VelocLimit_3;
  /*!
   * Velocity Limit of the [No.4] axis motor
   * - Name: VelocLimit_4 VelocLimit_4
   * - DefaultValue: 0.88
   * - Unit: rad/sec
   * - Range: NA
   * - Constraint: NA
   */
  double m_VelocLimit_4;
  /*!
   * Velocity Limit of the [No.5] axis motor
   * - Name: VelocLimit_5 VelocLimit_5
   * - DefaultValue: 1.11
   * - Unit: rad/sec
   * - Range: NA
   * - Constraint: NA
   */
  double m_VelocLimit_5;
  /*!
   * Velocity Limit of the [No.6] axis motor
   * - Name: VelocLimit_6 VelocLimit_6
   * - DefaultValue: 2.45
   * - Unit: rad/sec
   * - Range: NA
   * - Constraint: NA
   */
  double m_VelocLimit_6;
  /*!
   * Velocity Limit of the [No.7] axis motor
   * - Name: VelocLimit_7 VelocLimit_7
   * - DefaultValue: 0.013
   * - Unit: m/sec
   * - Range: NA
   * - Constraint: NA
   */
  double m_VelocLimit_7;
  /*!
   * acceleration limit of the [No.1] axis motor
   * - Name: AccelLimit_1 AccelLimit_1
   * - DefaultValue: 2.09
   * - Unit: rad/sec^2
   * - Range: NA
   * - Constraint: NA
   */
  double m_AccelLimit_1;
  /*!
   * acceleration limit of the [No.2] axis motor
   * - Name: AccelLimit_2 AccelLimit_2
   * - DefaultValue: 1.84
   * - Unit: rad/sec^2
   * - Range: NA
   * - Constraint: NA
   */
  double m_AccelLimit_2;
  /*!
   * acceleration limit of the [No.3] axis motor
   * - Name: AccelLimit_3 AccelLimit_3
   * - DefaultValue: 2.04
   * - Unit: rad/sec^2
   * - Range: NA
   * - Constraint: NA
   */
  double m_AccelLimit_3;
  /*!
   * acceleration limit of the [No.4] axis motor
   * - Name: AccelLimit_4 AccelLimit_4
   * - DefaultValue: 4.41
   * - Unit: rad/sec^2
   * - Range: NA
   * - Constraint: NA
   */
  double m_AccelLimit_4;
  /*!
   * acceleration limit of the [No.5] axis motor
   * - Name: AccelLimit_5 AccelLimit_5
   * - DefaultValue: 5.57
   * - Unit: rad/sec^2
   * - Range: NA
   * - Constraint: NA
   */
  double m_AccelLimit_5;
  /*!
   * acceleration limit of the [No.6] axis motor
   * - Name: AccelLimit_6 AccelLimit_6
   * - DefaultValue: 12.27
   * - Unit: rad/sec^2
   * - Range: NA
   * - Constraint: NA
   */
  double m_AccelLimit_6;
  /*!
   * acceleration limit of the [No.7] axis motor
   * - Name: AccelLimit_7 AccelLimit_7
   * - DefaultValue: 0.08
   * - Unit: m/sec^2
   * - Range: NA
   * - Constraint: NA
   */
  double m_AccelLimit_7;
  /*!
   * gear ratio of the [No.1] axis motor
   * - Name: GearRatio_1 GearRatio_1
   * - DefaultValue: 300.0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  double m_GearRatio_1;
  /*!
   * gear ratio of the [No.2] axis motor
   * - Name: GearRatio_2 GearRatio_2
   * - DefaultValue: 333.0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  double m_GearRatio_2;
  /*!
   * gear ratio of the [No.3] axis motor
   * - Name: GearRatio_3 GearRatio_3
   * - DefaultValue: 300.0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  double m_GearRatio_3;
  /*!
   * gear ratio of the [No.4] axis motor
   * - Name: GearRatio_4 GearRatio_4
   * - DefaultValue: 360.0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  double m_GearRatio_4;
  /*!
   * gear ratio of the [No.5] axis motor
   * - Name: GearRatio_5 GearRatio_5
   * - DefaultValue: 300.0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  double m_GearRatio_5;
  /*!
   * gear ratio of the [No.6] axis motor
   * - Name: GearRatio_6 GearRatio_6
   * - DefaultValue: 100.0
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  double m_GearRatio_6;
  /*!
   * gear ratio of the [No.7] axis motor
   * - Name: GearRatio_7 GearRatio_7
   * - DefaultValue: 4.4
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  double m_GearRatio_7;
  /*!
   * Encoder count of the [No.1] axis motor
   * - Name: EncoderCount_1 EncoderCount_1
   * - DefaultValue: 1000
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_EncoderCount_1;
  /*!
   * Encoder count of the [No.2] axis motor
   * - Name: EncoderCount_2 EncoderCount_2
   * - DefaultValue: 1024
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_EncoderCount_2;
  /*!
   * Encoder count of the [No.3] axis motor
   * - Name: EncoderCount_3 EncoderCount_3
   * - DefaultValue: 1024
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_EncoderCount_3;
  /*!
   * Encoder count of the [No.4] axis motor
   * - Name: EncoderCount_4 EncoderCount_4
   * - DefaultValue: 512
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_EncoderCount_4;
  /*!
   * Encoder count of the [No.5] axis motor
   * - Name: EncoderCount_5 EncoderCount_5
   * - DefaultValue: 512
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_EncoderCount_5;
  /*!
   * Encoder count of the [No.6] axis motor
   * - Name: EncoderCount_6 EncoderCount_6
   * - DefaultValue: 512
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_EncoderCount_6;
  /*!
   * Encoder count of the [No.7] axis motor
   * - Name: EncoderCount_7 EncoderCount_7
   * - DefaultValue: 512
   * - Unit: NA
   * - Range: NA
   * - Constraint: NA
   */
  int m_EncoderCount_7;
  // </rtc-template>

  // DataInPort declaration
  // <rtc-template block="inport_declare">
  TimedDoubleSeq m_InArmVelocity;
  /*!
   * Angle of target
   * - Type: TimedDoubleSeq
   * - Number: 7
   * - Semantics: [0] : No1.axis [rad/sec]
   *              [1] : No2.axis [rad/sec]
   *              [2] : No3.axis [rad/sec]
   *              [3] : No4.axis [rad/sec]
   *              [4] : No5.axis [rad/sec]
   *              [5] : No6.axis [rad/sec]
   *              [6] : No7.axis [m/sec]
   * - Unit: NA
   * - Frequency: NA
   * - Operation Cycle: NA
   */
  InPort<TimedDoubleSeq> m_InArmVelocityIn;
  
  // </rtc-template>


  // DataOutPort declaration
  // <rtc-template block="outport_declare">
  TimedDoubleSeq m_OutArmAngle;
  /*!
   * Present angle
   * - Type: TimedDoubleSeq
   * - Number: 7
   * - Semantics: [0] : No1.axis [rad]
   *              [1] : No2.axis [rad]
   *              [2] : No3.axis [rad]
   *              [3] : No4.axis [rad]
   *              [4] : No5.axis [rad]
   *              [5] : No6.axis [rad]
   *              [6] : No7.axis [m]
   * - Unit: NA
   * - Frequency: NA
   * - Operation Cycle: NA
   */
  OutPort<TimedDoubleSeq> m_OutArmAngleOut;
  
  // </rtc-template>

  // CORBA Port declaration
  // <rtc-template block="corbaport_declare">
  
  // </rtc-template>

  // Service declaration
  // <rtc-template block="service_declare">
  
  // </rtc-template>

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

 private:
  int dummy;
  
/*--------------------Addition---------------------*/
   
	// ========== Constant ========== //
	const static int BAUDRATE= 921600;  //!< Transmission rate
	const static int ARMNUM= 7;					//!< Motor Num

	// ========== Flags ========== //
	bool isDev;			//!< FLAG (Is the device communication possible?)
	
	// ========== Device ========== //
 	serIF_t *serif;					//!< Device setting
	serIF_t *serif_old;			//!< Device setting (old)
		
	// ========== Angle data ========== //
	double m_OldAngle[ARMNUM];			//!< Old Angle data
	
	// ========== Velocity data ========== //
	double m_TargetVelocity[ARMNUM];		//!< Target Velocity data
	double m_CurrentVelocity[ARMNUM];		//!< Current Velocity data

	// ========== Motor Parameter ========== //
	svm_param_t m_param[ARMNUM];		//!< Motor Parameter 
	svm_config_t m_config[ARMNUM];	//!< Motor Parameter 
		
	// ========== Motor data ========== //
	int32_t m_posit[ARMNUM]; 	//!< Encoder Position data
	int16_t m_veloc[ARMNUM]; 	//!< Speed command data [Encoder/10ms]
	
	// ========== Arm Parameter ========== //
	typedef struct ARM_PARAM {
		double toAngle;		//!< Unit conversion [Encoder to Angle]
		double toVeloc;		//!< Unit conversion [Velocity to Veloc]
		double Direction;//!< Direction of arm axis
	}Arm_Param;
	Arm_Param m_Arm[ARMNUM];		//!< Arm Parameter
	
	// ========== Time ========== //
	RTC::Time m_Time;			//!< Output time [tm]
	double m_getTime;			//!< Output time [time]
	double m_oldTime;			//!< old time [time]
	#if DEBUG_TIME
	double Time_Execute;				//!< onExecute time
	double Time_Execute_old;		//!< onExecute time [old]
	#endif
	
	// ========== Debug Files ========== //
	#if DEBUG
	ofstream VelocityLOG;			//!< for Log file. Velocity
	#endif
	
	// ========== Conversion ========== //
	double TM2TIME( RTC::Time tm ){ return double(tm.sec + 1.0e-9*tm.nsec);};		//!< conversion function from tm(structure) to time
	double DEG2RAD( double degree ){ return degree*M_PI/180.0; };	//!< conversion function from degree to radian
	double RAD2DEG( double radian ){ return radian*180.0/M_PI; };	//!< conversion function from radian ti degree
	
	// ========== Function ========== //
	/*!
		@brief  Device Open
		@param Device : Device file name
		@param Baudrate : Baudrate
		@return bool				
	*/
	virtual bool OpenDev( std::string Device, int Baudrate );

	/*!
		@brief  Arm Calibration
		@return bool				
	*/
  virtual bool Calibration( void );
  
	/*!
		@brief  Get Time
		@param void
		@return RTC::Time				
	*/
	virtual RTC::Time GetTime( void ){
		RTC::Time ingTime;
  	struct timeval GetTime;
		if( gettimeofday(&GetTime, NULL) )
			std::cout << "[ERROR] failed to gettimeofday" << std::endl;	
		ingTime.sec= GetTime.tv_sec;
		ingTime.nsec= GetTime.tv_usec*1000;
		return ingTime;
	};

	/*!
		@brief  motor Calibration
		@param timeout_count
		@return bool				
	*/
  virtual void calib( int timeout_count );
  
	/*!
		@brief  motor Calibration
		@param void
		@return void				
	*/
  virtual int rh_posEnc( int *goalEnc );
  
/*--------------------Addition---------------------*/

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

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

};


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

#endif // RH2ARMCONTROL_H
