/******************************************************************************
 * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
 * Author: zhangyu,Altered: madesheng
 * Licensed under the Apache License, Version 1.0 (the "License");
 *****************************************************************************/
#ifndef ROBOT_ROBOT_PERCEPTION_H
#define ROBOT_ROBOT_PERCEPTION_H

#include "../common/robotapp.h"
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <canbus/canMsg.h>
#include "../canbus/candatatype.h"
#include "perception/aesMsg.h"
#include "perception/slMsg.h"
#include "perception/seMsg.h"
#include "perception/si2Msg.h"
#include "perception/cecMsg.h"
#include "perception/eiMsg.h"
#include "perception/eeMsg.h"
#include "perception/powerMsg.h"
#include "canbus/ActuatorEnableRequest.h"
#include "perception/pedLrnPosMsg.h"
#include "perception/strLrnPosMsg.h"
#include "perception/sfLrnPos1Msg.h"
#include "perception/sfLrnPos2Msg.h"
#include "perception/si1Msg.h"
#include "perception/smMsg.h"
#include "canbus/EstpCtrl.h"
#include "canbus/SysCtrlMode.h"
#include "perception/zsftStatsMsg.h"

namespace robot {
namespace perception {

#define ROBOT_SUCCESS     0
#define ROBOT_FAILTURE   -1

class Perception : public robot::common::RobotApp
{
public:
    Perception() {}

    /**
    * perception 节点名称
    */
    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:

    /**
    * @brief Can topic data callback function
    */

    void CanTopicCallback(const canbus::canMsg::ConstPtr& msg);

    /**
    * @brief Publish State_6E5 topic
    */

    bool PublishState_6E5(unsigned char* msg);

    /**
    * @brief Publish State_6E4 topic
    */

    bool PublishState_6E4(unsigned char* msg);

    /**
    * @brief Publish State_6ED topic
    */

    bool PublishState_6ED(unsigned char* msg);

    /**
    * @brief Publish State_6EE topic
    */

    bool PublishState_6EE(unsigned char* msg);

    /**
    * @brief Publish State_6D1 topic
    */

    bool PublishState_6D1(unsigned char* msg);

    /**
    * @brief Publish State_6D2~State_6D6 topic
    */

    bool PublishState_6D2_6(unsigned char* msg, int type, const ros::Publisher &pub);

    /**
    * @brief Publish State_2E0 topic
    */

    bool PublishState_2E0(unsigned char* msg);

    /**
    * @brief Publish State_2E1 topic
    */

    bool PublishState_2E1(unsigned char* msg);

    /**
    * @brief Publish State_6E1 topic
    */
    bool PublishState_6E1(unsigned char* msg);

    /**
    * @brief Publish State_6E2 topic
    */
    bool PublishState_6E2(unsigned char* msg);

    /**
    * @brief Publish State_6E3 topic
    */
    bool PublishState_6E3(unsigned char* msg);

    /**
    * @brief Publish State_6EF topic
    */
    bool PublishState_6EF(unsigned char* msg);

    /**
    * @brief Publish State_6EB topic
    */
    bool PublishState_6EB(unsigned char* msg);

    bool PublishState_6D0(unsigned char* msg);

    bool PublishState_6F5(unsigned char* msg);

    bool SoftStopControl();

    void OnTimer(const ros::TimerEvent &event);

    //字符串反转
    inline void CharReverse(unsigned char * s, int n)
    {
        for (int i = 0,j=n-1; i < j; i++,j--)
        {
            char c = s[i];
            s[i] = s[j];
            s[j] = c;
        }

    }


    //十六进制字符串转换成字节数组
    inline void HexStrToByte(const char* source, unsigned char* dest, int sourceLen)
    {
        short i;
        unsigned char highByte, lowByte;
        for (i = 0; i < sourceLen; i += 2)
        {
            highByte = toupper(source[i]);
            lowByte = toupper(source[i + 1]);

            if (highByte > 0x39)
                highByte -= 0x37;
            else
                highByte -= 0x30;

            if (lowByte > 0x39)
                lowByte -= 0x37;
            else
                lowByte -= 0x30;

            dest[i / 2] = (highByte << 4) | lowByte;
        }
    }

private:
ros::Subscriber sub;
     robot::canbusSpace::T_STATE_OUT_6E5 s_state_6E5;
     ::perception::aesMsg aes_msg;

     robot::canbusSpace::T_STATE_OUT_6E4 s_state_6E4;
     ::perception::powerMsg power_msg;

     robot::canbusSpace::T_ESTOP_2E1 s_state_2E1;
     ::perception::eeMsg ee_msg;

     robot::canbusSpace::T_ESTOP_2E0 s_state_2E0;
     ::perception::eiMsg ei_msg;

     robot::canbusSpace::T_STATE_OUT_6EE s_state_6EE;
     ::perception::slMsg sl_msg;

     robot::canbusSpace::T_STATE_OUT_6ED s_state_6ED;
     ::perception::si2Msg si2_msg;

     robot::canbusSpace::T_STATE_OUT_6D1 s_state_6D1;
    ::perception::seMsg se_msg;

     robot::canbusSpace::T_ACC_ERR s_state_6D2;
     ::perception::cecMsg cec_msg2;

     robot::canbusSpace::T_BRK_ERR s_state_6D3;
     ::perception::cecMsg cec_msg3;

     robot::canbusSpace::T_XSFT_ERR s_state_6D4;
     ::perception::cecMsg cec_msg4;

     robot::canbusSpace::T_YSFT_ERR s_state_6D5;
     ::perception::cecMsg cec_msg5;

     robot::canbusSpace::T_STR_ERR s_state_6D6;
     ::perception::cecMsg cec_msg6;

     robot::canbusSpace::T_STATE_OUT_6E1 s_state_6E1;
     ::perception::pedLrnPosMsg pedLrnPos_msg;

     robot::canbusSpace::T_STATE_OUT_6E2 s_state_6E2;
     ::perception::strLrnPosMsg strLrnPos_msg;

     robot::canbusSpace::T_STATE_OUT_6E3 s_state_6E3;
     ::perception::sfLrnPos1Msg sftmsg;

     robot::canbusSpace::T_STATE_OUT_6EF s_state_6EF;
     ::perception::sfLrnPos2Msg sft2msg;

     robot::canbusSpace::T_STATE_OUT_6EB s_state_6EB;
     ::perception::si1Msg si1_msg;

     robot::canbusSpace::T_STATE_OUT_6D0 s_state_6D0;
     ::perception::smMsg sm_msg;

     robot::canbusSpace::T_STATE_OUT_6F5 s_state_6F5;
     ::perception::zsftStatsMsg zsft_msg;


     ros::Publisher bus_pub_6E5;
     ros::Publisher bus_pub_6E4;
     ros::Publisher bus_pub_6ED;
     ros::Publisher bus_pub_6EE;

     ros::Publisher bus_pub_6D1;

     ros::Publisher bus_pub_6D2;
     ros::Publisher bus_pub_6D3;
     ros::Publisher bus_pub_6D4;
     ros::Publisher bus_pub_6D5;
     ros::Publisher bus_pub_6D6;

     ros::Publisher bus_pub_2E0;
     ros::Publisher bus_pub_2E1;

     ros::Publisher bus_pub_6E1;
     ros::Publisher bus_pub_6E2;
     ros::Publisher bus_pub_6E3;
     ros::Publisher bus_pub_6EF;
     ros::Publisher bus_pub_6EB;

     ros::Publisher bus_pub_6D0;
     ros::Publisher bus_pub_6F5;

     ros::ServiceClient clientActuatorEnableRequest;

     ros::Timer timer_;

};


}  // namespace perception
}  // namespace robot

#endif // ROBOT_ROBOT_PERCEPTION_H
