/******************************************************************************
 * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
 * Author: limengqi
 * Licensed under the Apache License, Version 1.0 (the "License");
 *****************************************************************************/

/**
 * @file
 */

#ifndef ROBOT_ROBOT_CANBUS_H_
#define ROBOT_ROBOT_CANBUS_H_

#include <memory>
#include <string>
#include <utility>
#include <vector>
#include <stdio.h>
#include <dlfcn.h>
#include <libpcan.h>
#include <fcntl.h>
#include <errno.h>
#include <signal.h>
#include "../common/robotapp.h"
#include "ros/ros.h"
#include "candatatype.h"
#include <canbus/obdMsg.h>
#include <canbus/ActuatorEnableRequest.h>
#include <canbus/LearnModeRequest.h>
#include <canbus/AccPedalActCtrReq.h>
#include <canbus/SftActCtrReq.h>
#include <canbus/StrActCtrReq.h>
#include <canbus/RBTManager.h>
#include <canbus/PowerOnRequest.h>
#include <canbus/ActuatorCtrLimit.h>
#include <canbus/VehicleStart.h>
#include <canbus/SlopeMode.h>
#include <canbus/StrLoopReq.h>
#include <canbus/AccLoopReq.h>
#include <canbus/BrkLoopReq.h>
#include <canbus/StrTxtReq.h>
#include <canbus/BrkTxtReq.h>
#include <canbus/AccTxtReq.h>
#include <canbus/StrTxtLineReq.h>
#include <canbus/VehicleStatSetReq.h>
#include <canbus/RbtDriverCtrl.h>
#include <canbus/BrkPedalActCtrReq.h>
#include <canbus/StrTangLineCtrReq.h>
#include <canbus/StrSinCtrReq.h>
#include <canbus/StrPosTimCtrReq.h>
#include <canbus/EstpCtrl.h>
#include <canbus/RobotWorkMd.h>
#include <canbus/SysCtrlMode.h>
#include <canbus/BrkTimCtrlStp.h>
#include <canbus/AccPosCtrlStp.h>
#include <canbus/StrAccCtrlReq.h>
#include <canbus/RbtSlfChckCtrlReq.h>
#include <canbus/IOMode.h>
#include "std_msgs/String.h"
#include <canbus/SftTxtReq.h>

using namespace std;

/**
 * @namespace robot::canbus
 * @brief robot::canbus
 */
namespace robot {
namespace canbusSpace {

//define mapping function according to target function in libpcan.h
typedef DWORD   (*funCAN_Init_TYPE)(HANDLE hHandle, WORD wBTR0BTR1, int nCANMsgType);
typedef HANDLE  (*funLINUX_CAN_Open_TYPE)(const char *szDeviceName, int nFlag);
typedef DWORD   (*funCAN_VersionInfo_TYPE)(HANDLE hHandle, LPSTR lpszTextBuff);
typedef DWORD   (*funCAN_Close_TYPE)(HANDLE hHandle);
typedef DWORD   (*funLINUX_CAN_Read_TYPE)(HANDLE hHandle, TPCANRdMsg* pMsgBuff);
typedef DWORD   (*funCAN_Status_TYPE)(HANDLE hHandle);
typedef DWORD   (*funCAN_Write_TYPE)(HANDLE hHandle, TPCANMsg* pMsgBuff);
typedef int     (*funnGetLastError_TYPE)(void);

//the target device name
#define DEFAULT_NODE "/dev/pcanusb32"
#define ROBOT_SUCCESS     0
#define ROBOT_FAILTURE   -1


/**
* @class Canbus
*
* @brief canbus module main class.
* It processes the control data to send protocol messages to can bus.
*/
class Canbus : public robot::common::RobotApp{
 public:
  Canbus()  {}

  /**
  * canbus总线节点名称
  */
  std::string Name() const override;

  /**
  * @brief module initialization function
  * @return initialization status
  */
  int Init() override;

  /**
  * @brief module start function
  * @return start status
  */
  int Start() override;

  /**
  * @brief module stop function
  */
  void Stop() override;


 private:

  void InitServiceAndMsg();
  /**
  * @brief timer callback function
  */
  void OnTimer(const ros::TimerEvent &event);

  void StrLoopOnTimer(const ros::TimerEvent &event);  //转向机构循环指令定时器回调函数

  void AccLoopOnTimer(const ros::TimerEvent &event);  //油门踏板机构循环指令定时器回调函数

  void BrkLoopOnTimer(const ros::TimerEvent &event);  //制动踏板机构循环指令定时器回调函数

  void StrTxtOnTimer(const ros::TimerEvent &event);  //转向文件定时器回调函数

  void BrkTxtOnTimer(const ros::TimerEvent &event);  //制动文件定时器回调函数

  void AccTxtOnTimer(const ros::TimerEvent &event);  //油门文件定时器回调函数

  void StrTxtLineOnTimer(const ros::TimerEvent &event);  //转向直线文件定时器回调函数

  void SftTxtOnTimer(const ros::TimerEvent &event); //挡位文件定时器回调函数

  //define function pointer,there is a one-to-one mapping between target function and your defined function
  funCAN_Init_TYPE        funCAN_Init;
  funLINUX_CAN_Open_TYPE  funLINUX_CAN_Open;
  funCAN_VersionInfo_TYPE funCAN_VersionInfo;
  funCAN_Close_TYPE       funCAN_Close;
  funLINUX_CAN_Read_TYPE  funLINUX_CAN_Read;
  funCAN_Status_TYPE      funCAN_Status;
  funnGetLastError_TYPE   funnGetLastError;
  funCAN_Write_TYPE       funCAN_Write;

  /**
  * @brief can bus data read loop
  */
  int read_loop();

  /**
  * @brief read data from can bus
  */
  int read_data(TPCANMsg *pMsgBuff);

  /**
  * @brief send data to can bus
  */
  int write_data(TPCANMsg* pMsgBuff, bool print=true);

  /**
  * @brief publish data from can bus
  */
  void publish_data(TPCANMsg *pMsgBuff);

  int read_txt(string &path,int flag);  //读取TXT文件

  /**
  * @brief create services
  */
  bool ActuatorEnableRequest(canbus::ActuatorEnableRequest::Request &req,
                             canbus::ActuatorEnableRequest::Response &res);

  bool ExcuteLearnModeRequest(canbus::LearnModeRequest::Request &req,
                                canbus::LearnModeRequest::Response &res);

  bool AccPedalActCtrRequest(canbus::AccPedalActCtrReq::Request &req,
                             canbus::AccPedalActCtrReq::Response &res);

  bool BrkPedalActCtrRequest(canbus::BrkPedalActCtrReq::Request &req,
                             canbus::BrkPedalActCtrReq::Response &res);

  bool SftActCtrRequest(canbus::SftActCtrReq::Request &req,
                        canbus::SftActCtrReq::Response &res);

  bool StrActCtrRequest(canbus::StrActCtrReq::Request &req,
                        canbus::StrActCtrReq::Response &res);

  bool RBTManager(canbus::RBTManager::Request &req,
                        canbus::RBTManager::Response &res);

  bool PowerOnRequest(canbus::PowerOnRequest::Request &req,
                      canbus::PowerOnRequest::Response &res);

  bool ActuatorCtrLimit(canbus::ActuatorCtrLimit::Request &req,
                        canbus::ActuatorCtrLimit::Response &res);

  bool VehicleStart(canbus::VehicleStart::Request &req,
                     canbus::VehicleStart::Response &res);

  bool SlopeMode(canbus::SlopeMode::Request &req,
                 canbus::SlopeMode::Response &res);

  bool StrLoopReq(canbus::StrLoopReq::Request &req,
                 canbus::StrLoopReq::Response &res);

  bool AccLoopReq(canbus::AccLoopReq::Request &req,
                 canbus::AccLoopReq::Response &res);


  bool BrkLoopReq(canbus::BrkLoopReq::Request &req,
                 canbus::BrkLoopReq::Response &res);

  bool StrTxtReq(canbus::StrTxtReq::Request &req,
                 canbus::StrTxtReq::Response &res);

  bool BrkTxtReq(canbus::BrkTxtReq::Request &req,
                 canbus::BrkTxtReq::Response &res);

  bool AccTxtReq(canbus::AccTxtReq::Request &req,
                 canbus::AccTxtReq::Response &res);

  bool StrTxtLineReq(canbus::StrTxtLineReq::Request &req,
                 canbus::StrTxtLineReq::Response &res);

  bool VehicleStatSetReq(canbus::VehicleStatSetReq::Request &req,
                         canbus::VehicleStatSetReq::Response &res);

  bool RbtDriverControl(canbus::RbtDriverCtrl::Request &req,
                         canbus::RbtDriverCtrl::Response &res);

  bool StrTangLineCtrReq(canbus::StrTangLineCtrReq::Request &req,
                         canbus::StrTangLineCtrReq::Response &res);

  bool StrSinCtrReq(canbus::StrSinCtrReq::Request &req,
                    canbus::StrSinCtrReq::Response &res);

  bool StrPosTimCtrReq(canbus::StrPosTimCtrReq::Request &req,
                       canbus::StrPosTimCtrReq::Response &res);

  bool EstpCtrl(canbus::EstpCtrl::Request &req,canbus::EstpCtrl::Response &res);

  bool RobotWorkMd(canbus::RobotWorkMd::Request &req,canbus::RobotWorkMd::Response &res);

  bool SysCtrlMode(canbus::SysCtrlMode::Request &req,canbus::SysCtrlMode::Response &res);

  bool BrkTimCtrlStp(canbus::BrkTimCtrlStp::Request &req,canbus::BrkTimCtrlStp::Response &res);

  bool AccPosCtrlStp(canbus::AccPosCtrlStp::Request &req,canbus::AccPosCtrlStp::Response &res);

  bool StrAccCtrlReq(canbus::StrAccCtrlReq::Request &req,canbus::StrAccCtrlReq::Response &res);

  bool RbtSlfChckCtrlReq(canbus::RbtSlfChckCtrlReq::Request &req,canbus::RbtSlfChckCtrlReq::Response &res);

  bool IOMode(canbus::IOMode::Request &req,canbus::IOMode::Response &res);

  bool SftTxtReq(canbus::SftTxtReq::Request &req,
                 canbus::SftTxtReq::Response &res);

private:

  int64_t last_timestamp_ = 0;
  ros::Timer timer_;

  ros::Timer str_timer_;    //转向机构循环指令定时器
  bool strTimerState = false;  //转向机构循环指令定时器标志位
  int strLoopNum = 0;   //转向机构循环指令一套动作执行次数
  int strAngle ;         //方向盘转角
  bool strIncrease;      //递增/递减
  canbus::StrLoopReq::Request strLpReq;  //转向机构循环指令
  T_STR_ACT_CTR_REQ tStrActCtrReq ={0};   //转向机构循环指令中发送的转向执行机构请求


  ros::Timer acc_timer_;      //油门踏板机构循环指令定时器
  bool accTimerState = false;  //油门踏板机构循环指令定时器标志位
  int accLoopNum = 0;          //油门踏板机构循环指令一套动作执行次数
  uint accPsng ;         //油门踏板位置
  bool accIncrease;      //递增/递减
  canbus::AccLoopReq::Request accLpReq;   //油门踏板机构循环指令
  T_ACC_PEDAL_ACT_CTR_REQ tAccCtrReq = {0};  //油门踏板机构循环指令中发送的踏板执行机构控制命令


  ros::Timer brk_timer_;    //制动踏板机构循环指令定时器
  bool brkTimerState = false;  //制动踏板机构循环指令定时器标志位
  int brkLoopNum = 0;          //制动踏板机构循环指令一套动作执行次数
  uint brkPsng ;         //制动踏板位置
  bool brkIncrease;      //递增/递减
  canbus::BrkLoopReq::Request brkLpReq;  //制动踏板机构循环指令
  T_BRK_PEDAL_ACT_CTR_REQ tBrkCtrReq = {0};  //制动踏板机构循环指令中发送的踏板执行机构控制命令

   const double duration_txt = 0.01;//10ms//转向制动发送时间

   const double duration_txt_acc = 0.01; //10ms//油门发送时间

   ros::Timer str_txt_timer_;    //转向文件定时器
   bool strTxtTimerState = false;  //转向文件定时器标志位
   double angles[70000] = {0};  //TXT文件中的角度值
   bool hasSpeedFile = false; //是否存在角度文件对应的转速文件
   double angle_speeds[70000] = {0};  //TXT文件中的转速值
   int angles_count = 0;   //TXT文件中的角度值的编号
   int angles_number = 0; //TXT文件中的角度值的数量
   T_STR_ACT_CTR_REQ tStrActCtrReqTxt = {0};

   ros::Timer brk_txt_timer_;    //制动文件定时器
   bool brkTxtTimerState = false;  //制动文件定时器标志位
   double psngs[50000] = {0};  //TXT文件中的位置值
   int psng_count = 0;   //TXT文件中的位置值的编号
   int psng_number = 0; //TXT文件中的位置值的数量
   T_BRK_PEDAL_ACT_CTR_REQ tBrkCtrReqTxt = {0};

   ros::Timer acc_txt_timer_;    //油门文件定时器
   bool accTxtTimerState = false;  //油门文件定时器标志位
   double acc_psngs[69910] = {0};  //TXT文件中的位置值
   int acc_psng_count = 0;   //TXT文件中的位置值的编号
   int acc_psng_number = 0; //TXT文件中的位置值的数量
   T_ACC_PEDAL_ACT_CTR_REQ tAccCtrReqTxt = {0};

   ros::Timer str_txt_line_timer_;    //转向直线文件定时器
   bool strTxtLineTimerState = false;  //转向直线文件定时器标志位
   double angles_line[5000] = {0};  //转向直线TXT文件中的角度值
   int angles_line_count = 0;   //转向直线TXT文件中的角度值的编号
   int angles_line_number = 0; //转向直线TXT文件中的角度值的数量
   T_STR_ACT_CTR_REQ tStrActCtrReqTxtLine = {0};

   ros::Timer sft_txt_timer_;    //挡位文件定时器
   bool sftTxtTimerState = false;  //挡位文件定时器标志位
   int sft_psngs[69910] = {0};  //TXT文件中的位置值
   int sft_psng_count = 0;   //TXT文件中的位置值的编号
   int sft_psng_number = 0; //TXT文件中的位置值的数量
   T_SFT_ACT_CTR_REQ tSftCtrReqTxt = {0};


  const double duration = 0.01;

  const double rate = 0.0001;  //0.0001s
  //GLOBALS
  void *libm_handle = NULL;//define pointer used for file acess of libpcan.so
  HANDLE pcan_handle =NULL;//void *pcan_handle
  unsigned short unBaudrate = CAN_BAUD_1M; //set the communicate baud rate of can bus
  int nExtended = CAN_INIT_TYPE_ST;       //set can message int standard model   CAN_INIT_TYPE_EX/CAN_INIT_TYPE_ST

  T_VEHICLE_STATUS_1 status1 = {0};    //VehicleStaus1


  T_VEHICLE_STATUS_2 status2 = {0};     //VehicleStatus2


  T_VEHICLE_STATUS_3 status3 = {0};     //VehicleStatus3


  T_ICU_MONITOR icuMonitor = {0}; //ICUMonitor

  T_WORKING_MODE_CONFING workModeCnfg = {0};   //机器人工作模式配置


  int heatbeat = 0;  //ICUAlive


  ros::Publisher canRCUData_pub; // 发布ＲＣＵ数据包

  ros::Publisher canObdData_pub; // 发布OBD数据包

  canbus::obdMsg obd; // obd data

};

}  // namespace canbus
}  // namespace robot

#endif  // ROBOT_ROBOT_CANBUS_H_
