#include "car0105_control.h"
#include "modules_common.h"
#include "common.h"
#include "gpio_common.h"

static uint8_t forward_open_command[]={0x00,0xF1,0xFF};
static uint8_t forward_close_command[]={0x00,0x01,0xFF};
static uint8_t back_open_command[]={0x00,0xF2,0xFF};
static uint8_t back_close_command[]={0x00,0x02,0xFF};
static uint8_t left_open_command[]={0x00,0xF3,0xFF};
static uint8_t left_close_command[]={0x00,0x03,0xFF};
static uint8_t right_open_command[]={0x00,0xF4,0xFF};
static uint8_t right_close_command[]={0x00,0x04,0xFF};

static SerialPort car_serial;

 static bool serial_stop_index=0;

void car0105_serial_stop();

// 辅助函数：将整数波特率转换为 termios 定义的 speed_t
static speed_t get_baud_rate(int baud_rate) {
    switch (baud_rate) {
        case 9600: return B9600;
        case 19200: return B19200;
        case 38400: return B38400;
        case 57600: return B57600;
        case 115200: return B115200;
        case 230400: return B230400;
        case 460800: return B460800;
        case 921600: return B921600;
        default: return B9600; // 默认
    }
}


/**
 * 打开串口
 * 对应原 C++: bool SerialPort::open(speed_t baud_rate)
 */
static int serial_open(SerialPort *sp, const char *port_name, int baud_rate) {
    if (sp->is_open) {
        fprintf(stderr, "Warning: Port %s is already open.\n", sp->port_name);
        return 1; // 视为成功
    }

    // 保存端口名
    strncpy(sp->port_name, port_name, sizeof(sp->port_name) - 1);

    // 打开串口
    // O_RDWR: 读写模式
    // O_NOCTTY: 不作为控制终端
    // O_NDELAY: 非阻塞打开（防止DCD线不在线时在此阻塞）
    sp->fd = open(sp->port_name, O_RDWR | O_NOCTTY | O_NDELAY);
    if (sp->fd == -1) {
        fprintf(stderr, "Error: Failed to open serial port %s - %s\n", 
                sp->port_name, strerror(errno));
        return 0; // 失败
    }
    // 方法1：保持非阻塞标志
    //fcntl(sp->fd, F_SETFL, O_NONBLOCK);

    // 恢复为阻塞模式（对应原代码中的 fcntl(fd_, F_SETFL, 0)）
    fcntl(sp->fd, F_SETFL, 0);

    struct termios options;
    if (tcgetattr(sp->fd, &options) != 0) {
        fprintf(stderr, "Error: tcgetattr failed - %s\n", strerror(errno));
        close(sp->fd);
        sp->fd = -1;
        return 0;
    }

    // 设置波特率
    speed_t speed = get_baud_rate(baud_rate);
    cfsetispeed(&options, speed);
    cfsetospeed(&options, speed);

    // 设置控制标志 (8N1 配置)
    options.c_cflag &= ~PARENB;   // 无校验
    options.c_cflag &= ~CSTOPB;   // 1位停止位
    options.c_cflag &= ~CSIZE;    // 清除数据位掩码
    options.c_cflag |= CS8;       // 8位数据位
    
    // 启用接收器并设置本地模式
    options.c_cflag |= (CLOCAL | CREAD);

    // 设置输入标志 (禁用软件流控)
    options.c_iflag &= ~(IXON | IXOFF | IXANY);

    // 设置本地标志 (Raw模式: 无规范模式，无回显，无信号)
    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);

    // 设置输出标志 (Raw模式)
    options.c_oflag &= ~OPOST;

    // 设置控制字符
    // VMIN=0, VTIME=0: 纯非阻塞轮询读取（原代码逻辑）
    // 这意味着 read() 会立即返回，如果没有数据则返回0
    options.c_cc[VMIN] = 0;
    options.c_cc[VTIME] = 0;

    // 应用设置
    if (tcsetattr(sp->fd, TCSANOW, &options) != 0) {
        fprintf(stderr, "Error: tcsetattr failed - %s\n", strerror(errno));
        close(sp->fd);
        sp->fd = -1;
        return 0;
    }

    // 清空缓冲区
    tcflush(sp->fd, TCIOFLUSH);
    
    sp->is_open = 1;
   my_zlog_info("Serial port %s opened successfully.", sp->port_name);
    return 1; // 成功
}

/**
 * 发送数据
 * C语言中通常使用 write
 */
static int serial_send(SerialPort *sp, uint8_t *data, int len) {
    if (!sp || !sp->is_open) {
        my_zlog_error("Port not open or invalid pointer.");
        return -1;
    }

    int total_written = 0;
    int left = len;
    
    while (left > 0) {
        int written = write(sp->fd, data + total_written, left);
        
        if (written < 0) {
            if (errno == EINTR) {
                continue; // 被信号中断，重试
            }
            if (errno == EAGAIN || errno == EWOULDBLOCK) {
                // 如果是非阻塞模式，可能需要做 select/poll 或者稍后重试
                // 这里简单处理为错误，或者根据你的架构休眠一下
                my_zlog_error("Write buffer full.");
                return -1; 
            }
            my_zlog_error("Write failed - %s", strerror(errno));
            return -1;
        }
        
        total_written += written;
        left -= written;
    }
    delay_ms(10);
    return total_written;
}

/**
 * 关闭串口
 * 对应原 C++: void SerialPort::close()
 */
static void serial_close(SerialPort *sp) {
    // 原代码中有 stopListening(); C语言中如果没有线程需要手动停止，这里省略
    
    if (sp->is_open) {
        sp->is_open = 0;
        close(sp->fd);
        sp->fd = -1;
        my_zlog_info("Serial port %s closed.\n", sp->port_name);
    }
}

// 初始化结构体
static void serial_init(SerialPort *sp) {
    sp->fd = -1;
    sp->is_open = 0;
    memset(sp->port_name, 0, sizeof(sp->port_name));
}


int car0105_serial_init(){
    serial_init(&car_serial);
    // 1. 设置端口号
    // 香橙派/树莓派的USB转串口通常是 /dev/ttyUSB0 或 /dev/ttyACM0
    // 板载UART通常是 /dev/ttyS1 或 /dev/ttyS3 (取决于具体型号和DT配置)
    const char *dev_path = "/dev/ttyUSB0"; 
    if (!serial_open(&car_serial, dev_path, 9600)) {
        return -1;
    }
    car0105_serial_stop();
    //serial_send(&car_serial, start_continuous, sizeof(start_continuous));
    my_zlog_info("串口初始化成功");
    return 0;
}

// ==========================================
// 3. 核心处理函数 (对应 onSerialMessageReceived)
// ==========================================
/**
 * 解析并处理接收到的数据
 * @param buffer: 接收数据的缓冲区
 * @param len: 接收到的数据长度
 */
static int process_serial_data(const uint8_t *buffer, int len) {

    if(len<3){
        return -1;
    }
    if(buffer[2]==0xFF||buffer[3]==0xEF){
        my_zlog_info("收到串口数据");
        return 0;
    }else {
        my_zlog_info("串口数据大于3字节，但格式不正确");
        return -1;
    }
    
}

int car0105_serial_init_close(){
    serial_close(&car_serial);
    my_zlog_info("串口关闭成功");
    return 0; 
}

/**
 * @brief 等待串口连接成功
 * @param serial_port 串口文件描述符（由串口初始化函数返回）
 * @param max_retries 最大重试次数，0表示无限重试
 * @param retry_interval 重试间隔（秒）
 * @return 0: 连接成功, -1: 连接失败
 */
int wait_for_serial_connection(int (*connect_func)(void), int max_retries, int retry_interval)
{
    int retries = 0;
    
    while (max_retries == 0 || retries < max_retries) {
        my_zlog_info("尝试连接串口 (第 %d 次)...", retries + 1);
        
        int result = connect_func();
        
        if (result >= 0) {
            my_zlog_info("串口连接成功！");
            return 0;
        } else {
            my_zlog_warn("串口连接失败，%d秒后重试...", retry_interval);

            if (max_retries > 0) {
                retries++;
                if (retries >= max_retries) {
                    my_zlog_error("达到最大重试次数 (%d 次)", max_retries);
                    break;
                }
            }
            
            delay_s(retry_interval);
        }
    }
    
    return -1;
}

int car0105_serial_run(){
    uint8_t rx_buffer[1024];
    my_zlog_info("设备为串口打开设备");
    if(wait_for_serial_connection(car0105_serial_init, 500, 10)==0){
        my_zlog_info("串口启动成功");
    }else {
        my_zlog_info("串口启动失败");
        car0105_serial_init_close();
        return -1;
    }
     
    while(1){
        
         delay_ms(10);
        static int send_serial_count=0;
        int n = read(car_serial.fd, rx_buffer, sizeof(rx_buffer));
        int serial_index=process_serial_data(rx_buffer, n);
        send_serial_count++;
            
        if(send_serial_count>=55)send_serial_count=55;
        
       if(serial_index==0&&serial_stop_index==true) {
            send_serial_count=0;
        }
        if(send_serial_count==55){
            continue;
        }
       if(send_serial_count>50){
            car0105_serial_stop();
            if(send_serial_count>=55)send_serial_count=55;
            serial_stop_index=false;
       }    
        
    }
    car0105_serial_init_close();
    return 0;
    
}

void car0105_serial_stop(){
     serial_send(&car_serial, back_close_command, sizeof(back_close_command));
    serial_send(&car_serial, forward_close_command, sizeof(forward_close_command));
    serial_send(&car_serial, left_close_command, sizeof(left_close_command));
    serial_send(&car_serial, right_close_command, sizeof(right_close_command));

    my_zlog_info("串口超时，停止运行");
    
}


/*车的速度转向函数*/
void car0105_mode_1_flont(unsigned char gval) {
	if (gval < 50) {
        my_zlog_info("val:%d",gval);
		serial_send(&car_serial, forward_close_command, sizeof(forward_close_command));
        serial_send(&car_serial, back_close_command, sizeof(back_close_command));
	} else{
        serial_send(&car_serial, forward_open_command, sizeof(forward_open_command));
    }
}

void car0105_mode_2_back(unsigned char gval) {
   if (gval < 50) {
		serial_send(&car_serial, forward_close_command, sizeof(forward_close_command));
        serial_send(&car_serial, back_close_command, sizeof(back_close_command));
   }
	else {
        serial_send(&car_serial, back_open_command, sizeof(back_open_command));
    }
}


void car0105_mode_3_left(unsigned char gval) {
	if(gval<45){
		serial_send(&car_serial, left_close_command, sizeof(left_close_command));
        serial_send(&car_serial, right_close_command, sizeof(right_close_command));
	}else {
        serial_send(&car_serial, left_open_command, sizeof(left_open_command));
    }	
}

void car0105_mode_4_right(unsigned char gval) {
	if(gval<45){
        serial_send(&car_serial, left_close_command, sizeof(left_close_command));
		serial_send(&car_serial, right_close_command, sizeof(right_close_command));
	}else {
        serial_send(&car_serial, right_open_command, sizeof(right_open_command));
    }
}

void car0105_control_change(unsigned char *buf) {//车速度和转向引脚数值处理函数
    unsigned char mode=buf[1];
    unsigned char val=buf[2];
    serial_stop_index=true;
    switch(mode){
        case 1:			
			car0105_mode_1_flont(val);					 
			break;
		case 2:	
			car0105_mode_2_back(val);
			break;
		case 3:
			car0105_mode_3_left(val);
			break;
		case 4:
			car0105_mode_4_right(val);
			break; 
        default:
			break;
    } 
}