#include <rclcpp/rclcpp.hpp>
#include "sweeper_interfaces/msg/can_frame.hpp"
#include "mqtt_report/mqtt_client.hpp"
#include "mqtt_report/get_config.h"
#include "mqtt_report/report_struct.h"
#include "mqtt_report/fault_codes.h"
#include <algorithm>
#include <bitset>
#include <iostream>

namespace sweeperMsg = sweeper_interfaces::msg;

Config config; // 清扫车配置文件

GeneralMsg info_report; // 常规消息上报

// 解析can报文，做消息上报
void Msg_Handler(const sweeperMsg::CanFrame::SharedPtr msg)
{
    switch (msg->id)
    {
    // BMS_Status
    case 0x1821E5F1:
    {
        info_report.power = msg->data[4] * 0.5f;
        info_report.chargeStatus = (msg->data[3] == 0x61);
        // std::cout << "[0x1821E5F1] power : " << info_report.power << std::endl;
        // std::cout << "[0x1821E5F1] chargeStatus : " << info_report.chargeStatus << std::endl;
        break;
    }

    // BMS_Fault
    case 0x1825E5F1:
    {
        for (int i = 0; i < 28; ++i)
        {
            int byte_index = 1 + (i / 4);       // 从 data[1] 开始，所以 +1
            int bit_offset = (3 - (i % 4)) * 2; // 每 2bit 表示一个错误
            uint8_t level = (msg->data[byte_index] >> bit_offset) & 0x03;

            int error_code = 1001 + i;

            if (level != 0)
            {
                vehicle_error_code.addErrorCode(error_code);
                updateFaultLevel(error_code, level);
            }
            else
            {
                vehicle_error_code.removeErrorCode(error_code);
            }
        }
        break;
    }

    // MCU_Status_1 和 MCU_Fault（基础ID：0x0CF11E00，叠加设备ID）
    case 0x0CF11E05:
    {
        // 解析电机实际转速（data[0]：低字节，data[1]：高字节）
        uint16_t raw_speed = (static_cast<uint16_t>(msg->data[1]) << 8) | msg->data[0];
        float actual_speed_rpm = static_cast<float>(raw_speed); // 量程0-6000rpm，1rpm/bit
        // 转换为km/h
        float gear_ratio = 32.0;  // 电机速比
        float wheel_radius = 0.3; // 轮胎直径(m)
        info_report.speed = (actual_speed_rpm * 60 * 3.1416 * wheel_radius) / (1000 * gear_ratio);
        std::cout << "[0x0CF11E00] speed : " << info_report.speed << std::endl;

        // 故障码（data[6]：低字节，data[7]：高字节）
        uint16_t mcu_fault_code = (static_cast<uint16_t>(msg->data[7]) << 8) | msg->data[6];
        // 清空原有MCU故障码（1101-1116，对应ERR0-ERR15）
        for (int code = 1101; code <= 1116; ++code)
        {
            vehicle_error_code.removeErrorCode(code);
        }
        // 遍历16个故障位（ERR0-ERR15）
        for (int i = 0; i < 16; ++i)
        {
            bool is_fault = (mcu_fault_code >> i) & 0x01; // 提取第i位故障状态
            int error_code = 1101 + i;                    // 自定义故障码（1101=ERR0，1116=ERR15）

            if (is_fault && faultMap.count(error_code)) // 故障存在且在映射表中
            {
                vehicle_error_code.addErrorCode(error_code);
                updateFaultLevel(error_code, 1); // 协议未定义故障等级，默认1级（可根据需求调整）
            }
            else // 故障不存在
            {
                vehicle_error_code.removeErrorCode(error_code);
            }
        }

        break;
    }

    // MCU_Status_2（基础ID：0x0CF11F00，叠加设备ID）
    case 0x0CF11F05:
    {
        // 解析控制器温度（data[2]，偏移量40℃，1℃/bit）
        int raw_ctrl_temp = static_cast<int>(msg->data[2]);
        info_report.controllerTemp = raw_ctrl_temp - 40; // 实际温度=原始值-40，量程：-40~215℃

        // 解析电机温度（data[3]，偏移量30℃，1℃/bit）
        int raw_motor_temp = static_cast<int>(msg->data[3]);
        info_report.motorTemp = raw_motor_temp - 30; // 实际温度=原始值-30，量程：-30~225℃

        // 解析控制器状态（data[4]，BIT1-BIT0：命令状态；BIT3-BIT2：反馈状态）
        uint8_t ctrl_status = msg->data[4];
        // 反馈状态（BIT3-BIT2）：0=空挡，1=前进，2=后退，3=保留
        int feedback_gear = (ctrl_status >> 2) & 0x03;
        // 档位优先级：命令状态优先（空挡时以反馈状态为准，可根据需求调整）
        info_report.gear = feedback_gear;

        std::cout << "[0x0CF11F00] controllerTemp : " << info_report.controllerTemp << std::endl;
        std::cout << "[0x0CF11F00] motorTemp : " << info_report.motorTemp << std::endl;
        std::cout << "[0x0CF11F00] gear : " << info_report.gear << std::endl;

        break;
    }

    // EPS_Status and EPS_Fault
    case 0x401:
    {
        // 第3、4字节拼接为原始角度数据
        uint16_t raw_value = static_cast<uint16_t>((msg->data[3] << 8) | msg->data[4]);
        info_report.steeringAngle = (raw_value - 1024) / 7.0f;
        // std::cout << "[0x401] angle : " << info_report.angle << std::endl;

        // 清空所有 EPS 错误码
        for (int code = 1201; code <= 1218; ++code)
        {
            vehicle_error_code.removeErrorCode(code);
        }

        // 添加当前报文中的错误码(最多2个)
        uint8_t data_bytes[2] = {msg->data[2], msg->data[6]};
        for (uint8_t byte : data_bytes)
        {
            for (int i = 0; i < 18; ++i)
            {
                if (byte == epsErrorCode[i])
                {
                    vehicle_error_code.addErrorCode(1201 + i);
                    break; // 每个 byte 最多匹配一个错误码
                }
            }
        }
        break;
    }

    // VCU_Fault_1和Fault_2报文解析 (ID: 0x216)
    case 0x216:
    {
        // 提取故障1 (32位): msg->data[0] ~ msg->data[3]
        uint32_t fault1_bits = (static_cast<uint32_t>(msg->data[0]) << 0) |
                               (static_cast<uint32_t>(msg->data[1]) << 8) |
                               (static_cast<uint32_t>(msg->data[2]) << 16) |
                               (static_cast<uint32_t>(msg->data[3]) << 24);

        // 提取故障2 (32位): msg->data[4] ~ msg->data[7]
        uint32_t fault2_bits = (static_cast<uint32_t>(msg->data[4]) << 0) |
                               (static_cast<uint32_t>(msg->data[5]) << 8) |
                               (static_cast<uint32_t>(msg->data[6]) << 16) |
                               (static_cast<uint32_t>(msg->data[7]) << 24);

        // 清空VCU故障码（1701 ~ 1764)
        for (int code = 1701; code <= 1764; ++code)
        {
            vehicle_error_code.removeErrorCode(code);
        }

        // 处理故障1的bit0 ~ bit31 → 1701 ~ 1732
        for (uint8_t bit = 0; bit <= 31; ++bit)
        {
            if ((fault1_bits >> bit) & 0x01)
            {
                vehicle_error_code.addErrorCode(1701 + bit);
            }
        }

        // 处理故障2的bit0 ~ bit31 → 1733 ~ 1764
        for (uint8_t bit = 0; bit <= 31; ++bit)
        {
            if ((fault2_bits >> bit) & 0x01)
            {
                vehicle_error_code.addErrorCode(1733 + bit);
            }
        }

        break;
    }

    // VCU_Fault_3和Fault_4报文解析 (ID: 0x217)
    case 0x217:
    {
        // 提取故障3 (32位): msg->data[0] ~ msg->data[3]
        uint32_t fault3_bits = (static_cast<uint32_t>(msg->data[0]) << 0) |
                               (static_cast<uint32_t>(msg->data[1]) << 8) |
                               (static_cast<uint32_t>(msg->data[2]) << 16) |
                               (static_cast<uint32_t>(msg->data[3]) << 24);

        // 提取故障4 (32位): msg->data[4] ~ msg->data[7]
        uint32_t fault4_bits = (static_cast<uint32_t>(msg->data[4]) << 0) |
                               (static_cast<uint32_t>(msg->data[5]) << 8) |
                               (static_cast<uint32_t>(msg->data[6]) << 16) |
                               (static_cast<uint32_t>(msg->data[7]) << 24);

        // 清空VCU故障码（1765 ~ 1814）
        for (int code = 1765; code <= 1814; ++code)
        {
            vehicle_error_code.removeErrorCode(code);
        }

        // 处理故障3的bit0 ~ bit31 → 1765 ~ 1796
        for (uint8_t bit = 0; bit <= 31; ++bit)
        {
            if ((fault3_bits >> bit) & 0x01)
            {
                vehicle_error_code.addErrorCode(1765 + bit);
            }
        }

        // 处理故障4的bit0 ~ bit18 → 1797 ~ 1814
        for (uint8_t bit = 0; bit <= 18; ++bit)
        {
            if ((fault4_bits >> bit) & 0x01)
            {
                vehicle_error_code.addErrorCode(1797 + bit);
            }
        }

        break;
    }
    }
}

std::string pack_general_info_to_json(const GeneralMsg &msg)
{
    ordered_json j;

    j["power"] = msg.power;
    j["chargeStatus"] = msg.chargeStatus;
    j["gear"] = msg.gear;
    j["speed"] = msg.speed;
    j["steeringAngle"] = msg.steeringAngle;
    j["motorTemp"] = msg.motorTemp;
    j["controllerTemp"] = msg.controllerTemp;
    j["timestamp"] = msg.timestamp;

    return j.dump(4); // 输出有序 JSON 字符串
}

class CanDataSubscriber : public rclcpp::Node
{
public:
    CanDataSubscriber(const Config &config)
        : Node("can_data_subscriber"),
          mqtt_client_(config.mqtt_ip + ":" + std::to_string(config.mqtt_port), generate_mqtt_client_id(), "zxwl", "zxwl1234@"),
          info_topic_(config.info_topic),
          fault_topic_(config.fault_topic)
    {
        if (!mqtt_client_.connect())
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to connect to MQTT server");
        }

        subscription_ = this->create_subscription<sweeperMsg::CanFrame>(
            "can_data", 10, std::bind(&CanDataSubscriber::topic_callback, this, std::placeholders::_1));

        info_timer_ = this->create_wall_timer(
            std::chrono::milliseconds(100),
            std::bind(&CanDataSubscriber::info_timer_callback, this));

        fault_timer_ = this->create_wall_timer(
            std::chrono::milliseconds(500),
            std::bind(&CanDataSubscriber::fault_timer_callback, this));
    }

private:
    void topic_callback(const sweeperMsg::CanFrame::SharedPtr msg)
    {
        // RCLCPP_INFO(this->get_logger(), "Received CAN frame ID: %u, DLC: %u", msg->id, msg->dlc);
        Msg_Handler(msg);
    }

    void info_timer_callback()
    {
        info_report.timestamp = getCurrentTimestampMs();
        std::string json_msg = pack_general_info_to_json(info_report);
        mqtt_client_.publish(info_topic_, json_msg);
        // std::cout << "topic : " << info_topic_ << std::endl;
        // std::cout << "json_msg : " << json_msg << std::endl;
    }

    void fault_timer_callback()
    {
        for (int code : vehicle_error_code.getAllErrorCodes())
        {
            std::string fault_json = generateFaultJson(code, config.vid, getCurrentTimestampMs());
            mqtt_client_.publish(fault_topic_, fault_json);
        }
    }

    rclcpp::Subscription<sweeperMsg::CanFrame>::SharedPtr subscription_;
    MQTTClientWrapper mqtt_client_;
    std::string info_topic_;
    std::string fault_topic_;
    rclcpp::TimerBase::SharedPtr info_timer_;
    rclcpp::TimerBase::SharedPtr fault_timer_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);

    if (!load_config(config))
    {
        RCLCPP_ERROR(rclcpp::get_logger("main"), "Failed to load MQTT config.");
        return 1;
    }

    auto node = std::make_shared<CanDataSubscriber>(config);

    RCLCPP_INFO(node->get_logger(), "info_topic : %s", config.info_topic.c_str());
    RCLCPP_INFO(node->get_logger(), "fault_topic : %s", config.fault_topic.c_str());

    RCLCPP_INFO(node->get_logger(), "MqttReport node started.");

    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
