#include "rclcpp/rclcpp.hpp"
#include "zxwl_log.hpp"
#include "interface/msg/mc.hpp"
#include "interface/msg/fu.hpp"
#include "interface/msg/ctrl.hpp"
#include "controller.hpp"
#include "g_variable.h"
#include "CAN_recv.h"
#include "CAN_send.h"

class mc_node : public rclcpp::Node
{
public:
    mc_node(std::string name) : Node(name)
    {
        ZXWL_TITLE << "------------------------------------------------------" << ZXWL_REND;
        ZXWL_TITLE << "                 mc_node 节点已经启动" << ZXWL_REND;
        ZXWL_TITLE << "------------------------------------------------------" << ZXWL_REND;

        memset(&read_frame, 0, sizeof(struct can_frame));
        memset(&write_frame, 0, sizeof(struct can_frame));

        // 创建发布者
        msg_publisher_ = this->create_publisher<interface::msg::MC>("mc_message", 10);
        // 创建订阅者订阅话题
        command_subscribe_fu = this->create_subscription<interface::msg::FU>("fu_message",
                                                                             10,
                                                                             std::bind(&mc_node::command_callback_fu,
                                                                                       this,
                                                                                       std::placeholders::_1));
        command_subscribe_ctrl = this->create_subscription<interface::msg::CTRL>("ctrl_command",
                                                                                 10,
                                                                                 std::bind(&mc_node::command_callback_ctrl,
                                                                                           this,
                                                                                           std::placeholders::_1));
        // 创建定时器,100ms为周期,定时发布
        timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&mc_node::timer_callback, this));
    }
    ~mc_node()
    {
        // 在节点关闭时调用 Exit() 函数
        Exit();
        ZXWL_DEBUG << "exit!!!" << ZXWL_REND;
    }

private:
    void timer_callback()
    {
        // 创建消息、发布
        interface::msg::MC message;
        if (g_driving_state.ebp_st == 1)
            message.ebp = 1;
        else if (g_driving_state.ebp_st == 2)
            message.ebp = 0;

        message.gear = g_driving_state.gear_st;
        message.door = g_vcu_vstate1.door;
        message.warning = g_vcu_vstate1.warning_light;

        if (!g_vcu_vstate1.left_blinker && !g_vcu_vstate1.right_blinker)
            message.turn_light = 0;
        else if (g_vcu_vstate1.left_blinker)
            message.turn_light = 1;
        else if (g_vcu_vstate1.right_blinker)
            message.turn_light = 2;

        if (g_vcu_vstate1.headlamp == 3)
            message.headlamp = 0;
        else
            message.headlamp = g_vcu_vstate1.headlamp;

        if (g_vcu_vstate1.rear_foglight || g_vcu_vstate1.rear_foglight || g_vcu_vstate1.rear_foglight)
            message.fog_light = 1;
        else
            message.fog_light = 0;

        if (g_vcu_vstate4.aircondition == 3)
            message.ac_mode = 0;
        else
            message.ac_mode = g_vcu_vstate4.aircondition;

        message.power = g_vcu_vstate1.SOC / 2;
        message.mileage = g_vcu_vstate1.ODO * 2;
        message.speed = g_vcu_vstate1.speed - 50;
        ZXWL_DEBUG << "当前速度 : " << g_vcu_vstate1.speed - 50 << ZXWL_REND;
        message.horn = g_vcu_vstate8.horn;

        float angle_temp = (float)(g_eps_state.sw_angle) * 0.1 - 1080; // -520~520
        message.steering_angle = angle_temp / 1080.0 * 450.0;
        // ZXWL_DEBUG << "steering  : " << message.steering_angle << ZXWL_REND;

        msg_publisher_->publish(message);
    }

    // 收到话题fu_message数据的回调函数 --> 自动驾驶模式
    void command_callback_fu(const interface::msg::FU::SharedPtr msg)
    {
        speed_fu = ((float)(msg->speed)) / 10;
        drive_mode = msg->drive_mode; // 判断接下来是否减速
        ZXWL_INFOL << "车轮角度 : " << msg->angle << ZXWL_REND;
        ZXWL_INFOL << "驾驶模式 : " << msg->drive_mode << ZXWL_REND;
        ZXWL_INFOL << "FU限速 : " << speed_fu << ZXWL_REND;

        if (mode == 1)
        {
            ZXWL_INFO << "------------------------------------------------------" << ZXWL_REND;
            ZXWL_INFO << "                  自动驾驶模式" << ZXWL_REND;
            ZXWL_INFO << "------------------------------------------------------" << ZXWL_REND;
            if (msg->speed > 0)
            {
                if (msg->ems == 0) // 急停复位，恢复到正常自动驾驶
                {
                    g_ecu_speed_cmd.epb_state = 2; // 释放电子手刹
                    ZXWL_DEBUG << "前进档！" << ZXWL_REND;
                    Drive();                    // 挂前进挡
                    Speed_Limit(speed_fu, 0.2); // 限速   km/h 加速度设置为   m/s²;
                }
                else if (msg->ems == 1) // 急停
                {
                    ZXWL_ERROR << "急停!" << ZXWL_REND;
                    Drive();     // 挂前进挡
                    Brake(-5.4); // brake设置为 5.4 m/s²
                    sleep(5);
                    Neutral();                     // 挂空挡
                    g_ecu_speed_cmd.epb_state = 1; // 拉起电子手刹
                }
            }
            else if (msg->speed < 0)
            {
                g_ecu_speed_cmd.epb_state = 2; // 释放电子手刹
                ZXWL_DEBUG << "后退档!" << ZXWL_REND;
                Reverse();                   // 挂后退挡
                Speed_Limit(speed_fu, -0.2); // 限速   km/h 加速度设置为   m/s²;
            }
            else if (msg->speed == 0)
            {
                Drive();     // 挂前进挡
                Brake(-0.5); // brake设置为 1 m/s²
                sleep(5);
                Neutral();                     // 挂空挡
                g_ecu_speed_cmd.epb_state = 1; // 拉起电子手刹
                ZXWL_DEBUG << "停车!" << ZXWL_REND;
            }

            if (msg->action == 50)
            {
                Drive();     // 挂前进挡
                Brake(-0.5); // brake设置为  m/s²
                ZXWL_DEBUG << "刹车!" << ZXWL_REND;
            }
            g_ecu_control_cmd.door = msg->door;

            Steering(msg->angle);
        }
    }

    // 收到话题ctrl_command数据的回调函数 --> 远程驾驶模式
    void command_callback_ctrl(const interface::msg::CTRL::SharedPtr message)
    {
        mode = message->mode;
        // ZXWL_INFOL << "驾驶模式 : " << message->mode << ZXWL_REND;

        if (mode == 0)
        {
            ZXWL_INFO << "------------------------------------------------------" << ZXWL_REND;
            ZXWL_INFO << "                  手动模式" << ZXWL_REND;
            ZXWL_INFO << "------------------------------------------------------" << ZXWL_REND;

            Drive();     // 挂前进挡
            Brake(-0.5); // brake设置为 -0.5 m/s²
            sleep(3);
            Neutral(); // 挂空挡
            // g_ecu_speed_cmd.epb_state = 1; // 拉起电子手刹
        }
    }

    // 参数：实际物理值
    // 实际发送协议值 =（ 实际物理值 - 偏移值 ）/ 比例尺
    void Neutral() // 20ms/50ms
    {
        // ZXWL_DEBUG << "Neutral" << ZXWL_REND;
        g_ecu_speed_cmd.gear_state = GEAR_N; // 空挡
        g_ecu_speed_cmd.acceleration = 0x80; // 加速度置零
    }

    void Drive() // 20ms
    {
        // ZXWL_DEBUG << "Drive" << ZXWL_REND;
        g_ecu_speed_cmd.gear_state = GEAR_D; // 前进挡
    }

    void Reverse() // 20ms
    {
        // ZXWL_DEBUG << "Reverse" << ZXWL_REND;
        g_ecu_speed_cmd.gear_state = GEAR_R; // 倒车档
    }

    void Brake(float brake) // 20ms -5.4~0
    {
        g_ecu_speed_cmd.acceleration = (brake + 12.8) / 0.1; // 加速度 -
    }

    void Steering(float angle) // 20ms ±520
    {
        float can_angle;
        can_angle = (angle + 1080) * 10;
        if (can_angle > 16200)
            can_angle = 16200;
        if (can_angle < 5400)
            can_angle = 5400;

        g_ecu_eps_cmd.ang_velocity_max = 0xFA; // 方向盘角速度命令 250 * 2 - 0 = 500
        g_ecu_eps_cmd.steering_wheel_angle = (unsigned short)can_angle;

        if (angle < -50)
        {
            g_ecu_control_cmd.right_blinker = 2;
            g_ecu_control_cmd.left_blinker = 1;
        }
        else if (angle > 50)
        {
            g_ecu_control_cmd.right_blinker = 1;
            g_ecu_control_cmd.left_blinker = 2;
        }
        else
        {
            g_ecu_control_cmd.right_blinker = 0;
            g_ecu_control_cmd.left_blinker = 0;
        }
    }

    // 加速
    void Accelerate(float acc) // 20ms -5.4~1.6
    {
        g_ecu_speed_cmd.acceleration = (unsigned char)((acc + 12.8) / 0.1);
    }

    void Exit()
    {
        g_ecu_speed_cmd.gear_state = GEAR_N; // 空挡
        g_ecu_speed_cmd.acceleration = 0x80; // 加速度置零
        g_ecu_speed_cmd.epb_state = 1;       // 拉起电子手刹
        g_ecu_speed_cmd.emergency_brake = 0; // 紧急制动无状态

        g_ecu_eps_cmd.steering_wheel_angle = 10800; // 方向置零
        g_ecu_eps_cmd.eps_state = 0;                // 停止控制
        g_ecu_eps_cmd.ang_velocity_max = 0;

        g_ecu_control_cmd.autopilot = 0; // 不请求无人驾驶模式请求
    }

    // 设置限速
    void Speed_Limit(float threshold, float acc)
    {
        // 加速
        if ((g_vcu_vstate1.speed - 50) < threshold) // 限速 threshold km/h
            Accelerate(acc);                        // 加速度设置为 acc m/s²

        // 刹车
        else if ((g_vcu_vstate1.speed - 50) > ((float)(threshold) * 1.5))
            Accelerate(-0.24);
        else if ((g_vcu_vstate1.speed - 50) > ((float)(threshold) * 1.3))
            Accelerate(-0.22);
        else if ((g_vcu_vstate1.speed - 50) > ((float)(threshold) * 1.1))
            Accelerate(-0.2);
        else
            Accelerate(0);

        if (drive_mode != 0)
            ZXWL_DEBUG << "====限速====" << ZXWL_REND;
    }

    // int count = 0;
    float speed_fu;

    /*0：手动驾驶，1：自动驾驶，2：遥控驾驶，3：远程驾驶*/
    int mode = 0;

    /*0：直行，1：转弯 ，2：站内*/
    int drive_mode = 0;

    /*1:开，2：关*/
    int door = 0;

    // 声名定时器指针
    rclcpp::TimerBase::SharedPtr timer_;
    // 声明一个发布者
    rclcpp::Publisher<interface::msg::MC>::SharedPtr msg_publisher_;
    // 声明一个订阅者
    rclcpp::Subscription<interface::msg::FU>::SharedPtr command_subscribe_fu;
    rclcpp::Subscription<interface::msg::CTRL>::SharedPtr command_subscribe_ctrl;
};

void RecvFromCAN_thread(void)
{
    // ZXWL_DEBUG << "RecvFromCAN_pthread" << ZXWL_REND;
    memset(&g_vcu_vstate1, 0, sizeof(vcu_vstate1_st));
    memset(&g_vcu_vstate4, 0, sizeof(vcu_vstate4_st));
    memset(&g_driving_state, 0, sizeof(driving_state));
    memset(&g_eps_state, 0, sizeof(eps_state));
    // 声明can类指针
    Can_socket *can_socket_recv;
    // 初始化can类
    can_socket_recv = new Can_socket();
    can_socket_recv->init("can0");
    int fd_r = can_socket_recv->can_fd;

    // 过滤报文
    canid_t can_id_1 = DRIVING_STATE | CAN_EFF_FLAG;
    canid_t can_id_2 = VCU_VEHICLE_STATE1 | CAN_EFF_FLAG;
    canid_t can_id_3 = VCU_VEHICLE_STATE4 | CAN_EFF_FLAG;
    canid_t can_id_4 = EPS_STATE | CAN_EFF_FLAG;
    canid_t can_mask = CAN_EFF_MASK;
    can_socket_recv->set_rfilter(can_mask, can_id_1, can_id_2, can_id_3, can_id_4);

    while (1)
    {
        can_recv(fd_r);
    }
}

void SendToCAN_thread(void)
{
    // ZXWL_DEBUG << "SendToCAN_pthread" << ZXWL_REND;
    memset(&g_ecu_eps_cmd, 0, sizeof(struct ecu_eps_st));
    memset(&g_ecu_speed_cmd, 0, sizeof(struct ecu_speed_st));
    memset(&g_ecu_control_cmd, 0, sizeof(struct ecu_control_st));
    ecu_struct_init();
    Timer_Set(&t1, EPS_TIMER);
    Timer_Set(&t2, SPEED_TIMER);
    Timer_Set(&t3, CONTROL_TIMER);
    // 声明can类指针
    Can_socket *can_socket_send;
    // 初始化can类
    can_socket_send = new Can_socket();
    can_socket_send->init("can0");
    int fd_s = can_socket_send->can_fd;
    // InitialSend = true;
    while (1)
    {
        ecu_send_packet(fd_s);
    }
}

int main(int argc, char **argv)
{
    // ZXWL_DEBUG << "mc_node -> main()" << ZXWL_REND;
    std::thread CANRecvThread(RecvFromCAN_thread);
    std::thread CANSendThread(SendToCAN_thread);

    rclcpp::init(argc, argv);
    /*创建对应节点的共享指针对象*/
    auto node = std::make_shared<mc_node>("mc_node");
    /* 运行节点，并检测退出信号*/
    rclcpp::spin(node);
    rclcpp::shutdown();

    CANRecvThread.join();
    CANSendThread.join();

    return 0;
}

// if (drive_mode == 0) //  直行 不另外限速
// {
//     if ((g_vcu_vstate1.speed - 50) < threshold) // 限速 threshold km/h
//         Accelerate(acc);                        // 加速度设置为 acc m/s²
//     else if ((g_vcu_vstate1.speed - 50) > ((float)(threshold) * 1.2))
//         Accelerate(-0.8);
//     else if ((g_vcu_vstate1.speed - 50) > ((float)(threshold) * 1.1))
//         Accelerate(-0.4);
//     else
//         Accelerate(0);
// }
// else // 转弯或者站内 进一步限速到  km/h
// {
//     ZXWL_DEBUG << "====限速====" << ZXWL_REND;
//     if ((g_vcu_vstate1.speed - 50) < threshold) // 限速 threshold km/h
//         Accelerate(acc);                        // 加速度设置为 acc m/s²
//     else if ((g_vcu_vstate1.speed - 50) > ((float)(threshold) * 1.2))
//         Accelerate(-0.4);
//     else if ((g_vcu_vstate1.speed - 50) > ((float)(threshold) * 1.1))
//         Accelerate(-0.2);
//     else
//         Accelerate(0);
// }