#include <rclcpp/rclcpp.hpp>
#include "mc/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"

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

GeneralMsg info_report; // 常规消息上报

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

    case 0x1A2:
    {
        const uint8_t gearByte = msg->data[0];
        info_report.gear = (gearByte & 0x01) ? 3 : ((gearByte >> 1) & 0x03);

        const int tempRaw = (msg->data[5] << 8) | msg->data[6];
        info_report.motorTemp = static_cast<int>(tempRaw * 0.1f - 100.0f);
        break;
    }

    case 0x1A3:
    {
        uint16_t motorspeed = msg->data[4] << 8 | msg->data[5]; // 电机RPM
        float gear_ratio = 32.0;
        float wheel_radius = 0.15; // 轮胎半径，单位米
        float vehicle_speed_kmh = (2 * 3.1416 * wheel_radius * (motorspeed / gear_ratio) * 60.0) / 1000.0;

        info_report.speed = vehicle_speed_kmh;
        break;
    }

    case 0x18FF0121:
    {
        const uint16_t WaterValue = (uint16_t)(((msg->data[3] & 0x3F) << 4) | ((msg->data[2] & 0xF0) >> 4));
        float WaterLevel = WaterValue * (-0.235) + 199.75;
        info_report.waterLevel = static_cast<int>(std::round(WaterLevel / 1.8f)); // 液位百分比
        break;
    }

    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;
    }

    case 0x1A4:
    {
        // 清空所有 MCU 错误码
        for (int code = 1101; code <= 1125; ++code)
        {
            vehicle_error_code.removeErrorCode(code);
        }

        // 添加当前报文中的错误码（最多6个）
        for (uint8_t i = 0; i < 6; ++i)
        {
            for (uint8_t j = 0; j < 25; ++j)
            {
                if (msg->data[i] == mcuErrorCode[j])
                {
                    int error_code = 1101 + j;
                    vehicle_error_code.addErrorCode(error_code);
                    break;
                }
            }
        }
        break;
    }

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

        // 添加当前报文中的错误码（最多2个）
        uint8_t data_bytes[2] = {msg->data[3], msg->data[7]};
        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;
    }

    case 0x0CFF26EF:
    {
        uint8_t byte = msg->data[7]; // 主芯片

        for (int i = 0; i < 8; ++i)
        {
            uint8_t bit_flag = (byte >> (7 - i)) & 0x01;
            if (bit_flag)
            {
                vehicle_error_code.addErrorCode(1401 + i);
                break; // 每个 byte 最多匹配一个错误码
            }
        }

        byte = msg->data[5];
        for (int i = 0; i < 8; ++i)
        {
            uint8_t bit_flag = (byte >> (7 - i)) & 0x01;
            if (bit_flag)
            {
                vehicle_error_code.addErrorCode(1409 + i);
                break; // 每个 byte 最多匹配一个错误码
            }
        }
        break;
    }

    case 0x0CFF36EF:
    {
        uint8_t byte = msg->data[7]; // 主芯片

        for (int i = 0; i < 8; ++i)
        {
            uint8_t bit_flag = (byte >> (7 - i)) & 0x01;
            if (bit_flag)
            {
                vehicle_error_code.addErrorCode(1301 + i);
                break; // 每个 byte 最多匹配一个错误码
            }
        }

        byte = msg->data[5];
        for (int i = 0; i < 8; ++i)
        {
            uint8_t bit_flag = (byte >> (7 - i)) & 0x01;
            if (bit_flag)
            {
                vehicle_error_code.addErrorCode(1309 + i);
                break; // 每个 byte 最多匹配一个错误码
            }
        }
        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["waterLevel"] = msg.waterLevel;
    j["motorTemp"] = msg.motorTemp;
    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<mc::msg::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 mc::msg::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);
    }

    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<mc::msg::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))
    {
        std::cerr << "Failed to load MQTT config." << std::endl;
        return 1;
    }

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

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