// -*- C++ -*-
/*!
 * @file  RefHardRh2.h
 * @brief RefHard VelocutyVersion RH2
 * @date  $Date$
 *
 * @author N.Takekawa (takekawa-777@aist.go.jp)
 *
 * Copyright (c) 2008, National Institute of Advanced Industrial Science and Tec
 * hnology (AIST). All rights reserved. This program is made available under the te
 * rms 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 REFHARDRH2_H
#define REFHARDRH2_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/IIS.h>

/*--------------------Addition---------------------*/
#include <termios.h>
#include <stdio.h>
#include <time.h>
#include <vector>
#include <iomanip>
#include <math.h>
#include "intellirobotStub.h"
//#include "IISStub.h"
//#include "IISSkel.h"
//#include "IIS.hh"
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<IIS::TimedVelocity>
{
	//ofstream ofsCallBack;
	public:
	Debug(void) {};
	IIS::TimedVelocity operator()(const IIS::TimedVelocity& value){
		IIS::TimedVelocity ret(value);
		double Time= double(value.tm.sec + 1.0e-9 * value.tm.nsec );
		struct timeval GetTime;
		if( gettimeofday(&GetTime, NULL) ) std::cout << "[ERROR] failed to gettimeofday" << std::endl;	
		double TimeNow= double(GetTime.tv_sec + 1.0e-9 * GetTime.tv_usec*1000 );
		printf("[Callback](Velocity) TimeNow(%f) | Time:%f  sec[%ld] nsec[%ld]\n", TimeNow, Time, value.tm.sec, value.tm.nsec );
		return ret;
	};
};	
#endif
/*--------------------Addition---------------------*/

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

// </rtc-template>

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

// </rtc-template>

using namespace RTC;
using namespace IIS;

/*!
 * @class RefHardRh2
 * @brief RefHard VelocutyVersion RH2
 *
 * Control of RH2
 *
 * in : Velocity  out : WheelAngle
 *
 * -
 *
 */
class RefHardRh2
  : public RTC::DataFlowComponentBase
{
 public:
  /*!
   * @brief constructor
   * @param manager Maneger Object
   */
  RefHardRh2(RTC::Manager* manager);

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

  // <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 none
   * @post none
   * 
   */
   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 none
   * @post none
   * 
   */
   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 none
   * @post none
   * 
   */
   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 none
   * @post none
   * 
   */
   virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);

  /***
   * The speed instruction is transmitted to the motor
   *
   * The execution action that is invoked periodically
   * former rtc_active_do()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * @pre none
   * @post none
   * 
   */
   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: -
   * - Range: -
   * - Constraint: -
   */
  std::string m_DeviceFileName;
  /*!
   * servo motor parameter [gain_P]
   * - Name: gain_P gain_P
   * - DefaultValue: 10
   * - Unit: -
   * - Constraint: -
   */
  int m_gain_P;
  /*!
   * servo motor parameter [gain_I]
   * - Name: gain_I gain_I
   * - DefaultValue: 0
   * - Unit: -
   * - Constraint: -
   */
  int m_gain_I;
  /*!
   * servo motor parameter [gain_D]
   * - Name: gain_D gain_D
   * - DefaultValue: 0
   * - Unit: -
   * - Constraint: -
   */
  int m_gain_D;
  /*!
   * servo motor parameter [gain_VP]
   * - Name: gain_VP gain_VP
   * - DefaultValue: 50
   * - Unit: -
   * - Constraint: -
   */
  int m_gain_VP;
  /*!
   * servo motor parameter [gain_VI]
   * - Name: gain_VI gain_VI
   * - DefaultValue: 0
   * - Unit: -
   * - Constraint: -
   */
  int m_gain_VI;
  /*!
   * servo motor parameter [gain_VD]
   * - Name: gain_VD gain_VD
   * - DefaultValue: 0
   * - Unit: -
   * - Constraint: -
   */
  int m_gain_VD;
  /*!
   * servo motor parameter [veloc_limit]
   * - Name: Velocity_limit Velocity_limit
   * - DefaultValue: 10.0
   * - Unit: [m/s]
   * - Constraint: -
   */
  double m_Velocity_limit;
  /*!
   * Value in which acceleration limitation is done
   * - Name: Acceleration_limit Acceleration_limit
   * - DefaultValue: 50.0
   * - Unit: [m/s^2]
   * - Constraint: -
   */
  double m_Acceleration_limit;
  /*!
   * Length of axle
   * - Name: WheelTread WheelTread
   * - DefaultValue: 0.441
   * - Unit: [m]
   * - Constraint: -
   */
  double m_WheelTread;
  /*!
   * Radius of Wheel(Left)
   * - Name: WheelRadiusLeft WheelRadiusLeft
   * - DefaultValue: 0.1
   * - Unit: [m]
   * - Constraint: -
   */
  double m_WheelRadiusLeft;
  /*!
   * Radius of Wheel(Right)
   * - Name: WheelRadiusRight WheelRadiusRight
   * - DefaultValue: 0.1
   * - Unit: [m]
   * - Constraint: -
   */
  double m_WheelRadiusRight;
  /*!
   * gear ratio
   * - Name: GearRatio GearRatio
   * - DefaultValue: 66.22
   * - Unit: -
   * - Constraint: -
   */
  double m_GearRatio;
  /*!
   * encoder count
   * - Name: EncoderCount EncoderCount
   * - DefaultValue: 2000
   * - Unit: -
   * - Constraint: -
   */
  int m_EncoderCount;
  /*!
   * Encoder Constant [L]
   * - Name: EncoderConstant_L EncoderConstant_L
   * - DefaultValue: 1.0
   * - Unit: -
   * - Constraint: -
   */
  double m_EncoderConstant_L;
  /*!
   * Encoder Constant [R]
   * - Name: EncoderConstant_R EncoderConstant_R
   * - DefaultValue: 1.0
   * - Unit: -
   * - Constraint: -
   */
  double m_EncoderConstant_R;
  // </rtc-template>

  // DataInPort declaration
  // <rtc-template block="inport_declare">
  IIS::TimedVelocity2D m_InTargetVelocity;
  /*!
   * target Trans/Rot Velocity of robot's center data for PD control
   * - Type: Trans/Rot velocity data: double,    timeStamp:tm
   * - Number: Trans velocity data:2, Rot velocity data:1,  timeStamp:1
   * - Semantics: target Trans/Rot Velocity of robot's center data for PD contro
   *              l
   *                [Trans Velocity]: translational velocity  of robot's center [m/sec]
   *                [Rot Velocity]: rotation velocity (swing velocity) of robot's center [rad/sec]
   * - Unit: Trans velocity [m/s],  Rot velocity [rad/s],  timeStamp: [sec],[nan
   *         osec]
   */
  InPort<IIS::TimedVelocity2D> m_InTargetVelocityIn;
  
  // </rtc-template>


  // DataOutPort declaration
  // <rtc-template block="outport_declare">
  TimedDoubleSeq m_OutWheelAngle;
  /*!
   * Wheel angle
   * - Type: angle data: double,    timeStamp:tm
   * - Number: angle data:2,  timeStamp:1
   * - Semantics: current Wheel Angle data for PD control
   *               [0]: left wheel's current data
   *               [1]: right wheel's current data
   * - Unit: angle data : [rad],  timeStamp: [sec],[nanosec]
   */
  OutPort<TimedDoubleSeq> m_OutWheelAngleOut;
  
  // </rtc-template>

  // CORBA Port declaration
  // <rtc-template block="corbaport_declare">
  /*!
   * Driving stop by Bump detection
   * Interface: Driving stop by Bump detection
   */
  RTC::CorbaPort m_BumpProvPort;
  /*!
   * Instruction of driving restart
   * Interface: Instruction of driving restart
   */
  RTC::CorbaPort m_InventGUIProvPort;
  
  // </rtc-template>

  // Service declaration
  // <rtc-template block="service_declare">
  /*!
   * Driving stop by Bump detection
   * - Argument:      void
   * - Return Value:  void
   */
  RefHardSVC_impl m_BumpRefHard;
  /*!
   * driving restart
   * - Argument:      Go(), Stop()
   * - Return Value:  void
   */
  RefHardSVC_impl m_InventGUIRefHard;
  
  // </rtc-template>

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

 private:

/*--------------------Addition---------------------*/

	// ========== Constant ========== //
	const static int BAUDRATE= 921600;  	//!< Transmission rate
	const static int DOFNUM= 2;						//!< Motor Num

	// ========== Flags ========== //
	bool isDev;			//!< FLAG (Is the device communication possible?)
	bool isStop;		//!< FLAG (Driving stop?)
	
	// ========== Device ========== //
 	serIF_t *serif;					//!< Device setting
	serIF_t *serif_old;			//!< Device setting (old)
	
	// ========== Target Velocity data ========== //
	double m_TransVel; 		//!< Velocity of target
	double m_RotVel;			//!< Whirl Velocity of target

	// ========== Wheel Velocity data ========== //
	double m_WheelAngularVel[DOFNUM];				//!< Angle rate for wheel
	double m_OldWheelAngularVel[DOFNUM];		//!< Angle rate for wheel (old)
	
	// ========== RefHw Parameter ========== //
	typedef struct RH_PARAM {
		double toAngle;			//!< Unit conversion [Encoder to Angle]
		double toVeloc;			//!< Unit conversion [Velocity to Veloc]
		double Direction;		//!< Direction of axis
		double WheelRadius;//!< Wheel Radius
		double BodyRadius;	//!< Body Radius
		double Correction;	//!< Encoder Correction value
	}Rh_Param;
	Rh_Param m_Rh[DOFNUM];		//!< RefHw Parameter
		
	// ========== Motor data ========== //
	int32_t m_posit[DOFNUM]; 	//!< Encoder Position data
	int16_t m_veloc[DOFNUM]; 	//!< Speed command data [Encoder/10ms]

	// ========== Drive Control ========== //
	int m_DriveCount;			//!< Drive count
	double m_TimeStep;		//!< time step of Acceleration limitation
	
	// ========== Time ========== //
	#if DEBUG_TIME
	double Time_Execute;				//!< onExecute time
	double Time_Execute_old;		//!< onExecute time [old]
	#endif
	
	// ========== CallBack ========== //
	#if CALLBACK	
	Debug m_debug;
	#endif
	
	// ========== Debug Files ========== //
	#if DEBUG
	ofstream VelocityLOG;				 		//!< for Log file. Velocity
	#endif

	// ========== Unit conversion ========== //
	// Conversion of Time
	double TM2TIME( RTC::Time tm ){ return double(tm.sec + 1.0e-9*tm.nsec);};		//!< conversion function from tm(structure) to time
	
	// ========== Function ========== //
	/*!
		@brief  Device Open
		@param Device : Device file name
		@param Baudrate : Baudrate
		@return bool				
	*/
	virtual bool OpenDev( std::string Device, int Baudrate );

	/*!
		@brief  set of motor Parameter
		@param Mode : Drive mode
		@param param : motor Parameter
		@return bool				
	*/
	virtual bool ParameterSet( svm_mode_t Mode, svm_param_t param[] );
	
	/*!
		@brief  Get Time
		@param void
		@return RTC::Time				
	*/
	virtual RTC::Time GetTime( void ){
		RTC::Time ingTime;
  	struct timeval GetTime;  		//!< structure of time [Present]
		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;
	};

/*--------------------Addition---------------------*/

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

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

};


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

#endif // REFHARDRH2_H
