#include "can_ros_package/can_ros_node.h"
#include <iostream>
#include <thread>
#include <chrono>

namespace can_ros_package {

CanDriver::CanDriver() 
    : is_running_(false),
      device_type_(VCI_USBCAN2), 
      device_ind_(0),
      can1_connected_(false),
      can2_connected_(false),
      reconnect_interval_ms_(600),
      max_reconnect_attempts_(5),
      new_data_available_(false) {
    // 初始化互斥锁和原子变量
}

CanDriver::~CanDriver() {
    stop();
}

bool CanDriver::init() {
    // 打开设备
    if (VCI_OpenDevice(device_type_, device_ind_, 0) != STATUS_OK) {
        std::cerr << "Failed to open CAN device" << std::endl;
        return false;
    }
    
    // 读取设备信息
    if (VCI_ReadBoardInfo(device_type_, device_ind_, &board_info_) != STATUS_OK) {
        std::cerr << "Failed to read board info" << std::endl;
        VCI_CloseDevice(device_type_, device_ind_);
        return false;
    }
    
    // 分别初始化两路CAN通道
    bool can1_init = initCanChannel(0);
    bool can2_init = initCanChannel(1);
    
    can1_connected_ = can1_init;
    can2_connected_ = can2_init;
    
    if (!can1_init && !can2_init) {
        VCI_CloseDevice(device_type_, device_ind_);
        return false;
    }
    
    is_running_ = true;
    new_data_available_ = false;
    
    // 启动接收线程
    receive_thread_ = std::thread(&CanDriver::receiveLoop, this);
    
    return true;
}

// 波特率	Timing0 (BTR0)	Timing1 (BTR1)	说明
// 1 Mbps	0x00	0x14	高速通信
// 500 Kbps	0x00	0x1C	最常用配置
// 250 Kbps	0x01	0x1C	中等速度通信
// 125 Kbps	0x03	0x1C	低速通信，你的原始配置
// 50 Kbps	0x09	0x1C	长距离或低干扰环境
// 10 Kbps	0x13	0x22	非常低速，特殊场景
bool CanDriver::initCanChannel(int channel) {
    VCI_INIT_CONFIG config;
    config.AccCode = 0;
    config.AccMask = 0xFFFFFFFF;
    config.Filter = 1;  // 接收所有帧
    
    if(channel == 0){
        // 设置1000Kbps波特率
        config.Timing0 = 0x00;
        config.Timing1 = 0x14;
    } else if(channel == 1) {
        // 设置250Kbps波特率
        config.Timing0 = 0x01;
        config.Timing1 = 0x1C;
    }
    
    config.Mode = 0;  // 正常模式
    
    if (VCI_InitCAN(device_type_, device_ind_, channel, &config) != STATUS_OK) {
        std::cerr << "Failed to initialize CAN channel " << channel << std::endl;
        return false;
    }
    
    // 启动前短暂延迟
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
    
    if (VCI_StartCAN(device_type_, device_ind_, channel) != STATUS_OK) {
        std::cerr << "Failed to start CAN channel " << channel << std::endl;
        return false;
    }
    
    return true;
}

bool CanDriver::sendToCan1(VCI_CAN_OBJ& msg) {
    if (!can1_connected_) {
        return false;
    }
    
    std::lock_guard<std::mutex> send_lock(send_mutex_);
    std::lock_guard<std::mutex> reconnect_lock(reconnect_mutex_);
    
    int result = VCI_Transmit(device_type_, device_ind_, 0, &msg, 1);
    
    if (result != STATUS_OK) {
        std::cerr << "Failed to send to CAN1, error code: " << result << std::endl;
        can1_connected_ = false;
        std::thread(&CanDriver::reconnect, this, 0).detach();
        return false;
    }
    
    return true;
}

bool CanDriver::sendToCan2(VCI_CAN_OBJ& msg) {
    if (!can2_connected_) {
        return false;
    }
    
    std::lock_guard<std::mutex> send_lock(send_mutex_);
    std::lock_guard<std::mutex> reconnect_lock(reconnect_mutex_);
    
    int result = VCI_Transmit(device_type_, device_ind_, 1, &msg, 1);
    
    if (result != STATUS_OK) {
        std::cerr << "Failed to send to CAN2, error code: " << result << std::endl;
        can2_connected_ = false;
        std::thread(&CanDriver::reconnect, this, 1).detach();
        return false;
    }
    
    return true;
}

void CanDriver::receiveLoop() {
    VCI_CAN_OBJ rec[3000];
    int reclen = 0;
    
    while (is_running_) {
        // 接收CAN1数据
        if (can1_connected_) {
            {
                std::lock_guard<std::mutex> reconnect_lock(reconnect_mutex_);
                reclen = VCI_Receive(device_type_, device_ind_, 0, rec, 3000, 0); // 0ms超时实现非阻塞读取
            }
            
            if (reclen < 0) {
                std::cerr << "Error receiving from CAN1, error code: " << reclen << std::endl;
                can1_connected_ = false;
                std::thread(&CanDriver::reconnect, this, 0).detach();
            } else if (reclen > 0) {
                std::lock_guard<std::mutex> receive_lock(receive_mutex_);
                for (int j = 0; j < reclen; j++) {
                    can_msgs_.push_back(rec[j]);
                }
                new_data_available_ = true;
                receive_cv_.notify_one();
            }
        }
        
        // 接收CAN2数据
        if (can2_connected_) {
            {
                std::lock_guard<std::mutex> reconnect_lock(reconnect_mutex_);
                reclen = VCI_Receive(device_type_, device_ind_, 1, rec, 3000, 0); // 0ms超时实现非阻塞读取
            }
            
            if (reclen < 0) {
                std::cerr << "Error receiving from CAN2, error code: " << reclen << std::endl;
                can2_connected_ = false;
                std::thread(&CanDriver::reconnect, this, 1).detach();
            } else if (reclen > 0) {
                std::lock_guard<std::mutex> receive_lock(receive_mutex_);
                for (int j = 0; j < reclen; j++) {
                    can_msgs_.push_back(rec[j]);
                }
                new_data_available_ = true;
                receive_cv_.notify_one();
            }
        }
        
        // 短暂休眠，避免CPU占用过高
        std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
}

std::vector<VCI_CAN_OBJ> CanDriver::getReceivedMessages() {
    std::unique_lock<std::mutex> lock(receive_mutex_);
    
    // 等待新数据或超时
    if (!new_data_available_) {
        receive_cv_.wait_for(lock, std::chrono::milliseconds(10));
    }
    
    std::vector<VCI_CAN_OBJ> result = can_msgs_;
    can_msgs_.clear();
    new_data_available_ = false;
    return result;
}

void CanDriver::reconnect(int channel) {
    std::string channel_name = (channel == 0) ? "CAN1" : "CAN2";
    int attempt = 0;
    
    while (is_running_ && attempt < max_reconnect_attempts_ && 
          !(channel == 0 ? can1_connected_ : can2_connected_)) 
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(reconnect_interval_ms_));
        
        if (tryReconnectChannel(channel)) {
            ROS_INFO_STREAM("Successfully reconnected to " << channel_name);
            break;
        } else {
            ROS_WARN_STREAM("Reconnect attempt " << (attempt+1) 
                          << "/" << max_reconnect_attempts_ 
                          << " failed for " << channel_name);
            attempt++;
        }
    }
    
    if (attempt >= max_reconnect_attempts_) {
        ROS_ERROR_STREAM("Giving up on reconnecting " << channel_name 
                      << " after " << max_reconnect_attempts_ << " attempts");
    }
}

bool CanDriver::tryReconnectChannel(int channel) {
    std::lock_guard<std::mutex> reconnect_lock(reconnect_mutex_);
    std::string channel_name = (channel == 0) ? "CAN1" : "CAN2";
    
    // 1. 重置通道
    if (is_running_) {
        VCI_ResetCAN(device_type_, device_ind_, channel);
        VCI_ClearBuffer(device_type_, device_ind_, channel);
    }
    
    // 2. 重新初始化通道
    VCI_INIT_CONFIG config;
    config.AccCode = 0;
    config.AccMask = 0xFFFFFFFF;
    config.Filter = 1;  // 接收所有帧
    
    if(channel == 0){
        // 设置1000Kbps波特率
        config.Timing0 = 0x00;
        config.Timing1 = 0x1C;
    } else if(channel == 1) {
        // 设置250Kbps波特率
        config.Timing0 = 0x01;
        config.Timing1 = 0x1C;
    }
    
    config.Mode = 0;  // 正常模式
    
    if (VCI_InitCAN(device_type_, device_ind_, channel, &config) != STATUS_OK) {
        return false;
    }
    
    // 启动前短暂延迟
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
    
    if (VCI_StartCAN(device_type_, device_ind_, channel) != STATUS_OK) {
        return false;
    }
    
    // 3. 更新连接状态
    if (channel == 0) {
        can1_connected_ = true;
    } else {
        can2_connected_ = true;
    }
    
    // 启动后延迟确保稳定
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
    return true;
}

void CanDriver::stop() {
    if (is_running_) {
        is_running_ = false;
        can1_connected_ = false;
        can2_connected_ = false;
        
        // 通知接收线程退出
        {
            std::lock_guard<std::mutex> lock(receive_mutex_);
            receive_cv_.notify_all();
        }
        
        if (receive_thread_.joinable()) {
            receive_thread_.join();
        }
        
        // 复位并关闭两路CAN通道
        VCI_ResetCAN(device_type_, device_ind_, 0);
        VCI_ResetCAN(device_type_, device_ind_, 1);
        VCI_CloseDevice(device_type_, device_ind_);
    }
}

bool CanDriver::isCan1Connected() {
    return can1_connected_;
}

bool CanDriver::isCan2Connected() {
    return can2_connected_;
}

} // namespace can_ros_package