#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sstream>
#include <unistd.h>
#include <algorithm>
#include <fcntl.h>
#include <errno.h>
#include <iostream>
#include <iomanip>
#include <net/if.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <sys/socket.h>
#include <sys/ioctl.h>

#include "CanMsgHandler.hpp"

CanMsgHandler::CanMsgHandler() {}

CanMsgHandler::~CanMsgHandler()
{
    ClosePort();
}

// 打开CAN
bool CanMsgHandler::OpenPort(const std::string& port, const std::string& baudrate)
{
    ClosePort();

    // 创建SocketCAN套接字
    fd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
    if (fd < 0) {
        perror("Failed to create CAN socket");
        return false;
    }
    
    // 获取接口索引
    struct ifreq ifr;
    strncpy(ifr.ifr_name, port.c_str(), IFNAMSIZ - 1);
    ifr.ifr_name[IFNAMSIZ - 1] = '\0';
    
    if (ioctl(fd, SIOCGIFINDEX, &ifr) < 0) {
        perror("Failed to get interface index");
        close(fd);
        fd = -1;
        return false;
    }
    
    // 绑定套接字到CAN接口
    struct sockaddr_can addr;
    memset(&addr, 0, sizeof(addr));
    addr.can_family = AF_CAN;
    addr.can_ifindex = ifr.ifr_ifindex;
    
    if (bind(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
        perror("Failed to bind CAN socket");
        close(fd);
        fd = -1;
        return false;
    }

    // // 设置非阻塞模式
    // int flags = fcntl(fd, F_GETFL, 0);
    // fcntl(fd, F_SETFL, flags | O_NONBLOCK);

    // 设置CAN波特率
    // std::string cmd = "ip link set " + port + " down";
    // std::cout << cmd << std::endl;
    // if (system(cmd.c_str()) != 0) {
    //     return false;
    // }

    // cmd = "ip link set " + port + " type can bitrate " + baudrate + " dbitrate 1000000 fd on";
    // std::cout << cmd << std::endl;
    // if (system(cmd.c_str()) != 0) {
    //     return false;
    // }
    
    // cmd = "ip link set " + port + " up";
    // std::cout << cmd << std::endl;
    // if (system(cmd.c_str()) != 0) {
    //     return false;
    // }

    return true;
}

// 发送数据
int CanMsgHandler::SendDeviceMsg(std::vector<uint8_t> &data)
{
    struct can_frame frame;

    frame.can_id = data[0];
    frame.can_id = frame.can_id << 12;
    frame.can_id += data[1];
    frame.can_id = frame.can_id << 8;
    frame.can_id += data[2];
    frame.can_id |= CAN_EFF_FLAG;

    frame.can_dlc = data.size() - 3;

    for (int i = 0; i < frame.can_dlc; i++)
    {
        frame.data[i] = data[i + 3];
    }
    for (int num : data) {
        std::cout << std::hex << std::setw(2) << std::setfill('0')
                  << num << " ";
    }
    std::cout << std::endl;
    
    int bytesWritten = SendMsg(&frame, sizeof(struct can_frame));
    if (bytesWritten < 0) {
        perror("发送数据失败");
    }

    return bytesWritten;
}

int CanMsgHandler::RecvDeviceMsg(std::vector<uint8_t>& data, int timeoutMs)
{
    int bytesRead;
    struct can_frame frame;

    bytesRead = RecvMsg(&frame, sizeof(struct can_frame), timeoutMs);

    if (bytesRead > 0)
    {
        data.insert(data.end(), (frame.can_id >> 20) & 0xFF);
        data.insert(data.end(), (frame.can_id >> 8) & 0xFF);
        data.insert(data.end(), frame.can_id & 0xFF);
        data.insert(data.end(), frame.data, frame.data + frame.can_dlc);
    }

    return bytesRead;
}

uint32_t CanMsgHandler::GetCanId(uint32_t DeviceID, uint32_t CommandID, uint32_t BlockID)
{
    uint32_t Canid = DeviceID << 20 + CommandID << 8 + BlockID;

    return Canid;
}