#include <stdio.h>
#include "laser.h"
extern size_t strlen(const char *);

const char FRAME_HEAD_RSP = '$';
const char FRAME_TAIL = '\r';

const char FRAME_HEAD[] = "%01#";
const char FRAME_TRUE[] = "+00001";
const char FRAME_FALSE[] = "+00000";
const char FRAME_BCC[] = "**";
const char REQ_DEVICE_ONLINE[] = "WCSR03001";
const char REQ_OPEN_STATUS[] = "RLR";       // 激光开启状态查询
const char REQ_OPEN_STATUS_WRITE[] = "WLR"; // 激光开启状态设置
const char REQ_DISTANCE_READ[] = "RMD";     // 测定值读出
const char REQ_ZERO_STATUS[] = "RZS";       // 调零状态查询
const char REQ_ZERO_STATUS_WRITE[] = "WZS"; // 调零状态设置

//static uart_t *handle;
static char request_buf[RS485_REC_LEN];
static uint8_t uart_buf[RS485_REC_LEN];
static uint8_t *response_ptr;
static uint8_t response_ptr_offset = 0;
laser_t laser_handle;
uint8_t res; 
int rx_count = 0;

int rec_flag = 0;


static void laser_send(uint8_t *data, uint8_t length)
{
    laser_handle.send_data_cb(&huart3, data, length);
}

// 是否在线
static void command_device_connect_req(void)
{
    snprintf(request_buf, sizeof(request_buf), "%s%s%s%c", FRAME_HEAD, REQ_DEVICE_ONLINE, FRAME_BCC, FRAME_TAIL);

    laser_send((uint8_t *)request_buf, strlen(request_buf));
}

// 激光开启状态查询
static void command_open_status_req(void)
{
    snprintf(request_buf, sizeof(request_buf), "%s%s%s%c", FRAME_HEAD, REQ_OPEN_STATUS, FRAME_BCC, FRAME_TAIL);

    laser_send((uint8_t *)request_buf, strlen(request_buf));
}

// 激光开启状态设置
static void command_open_status_write_req(bool open)
{
    if (open)
    {
        snprintf(request_buf, sizeof(request_buf), "%s%s%s%s%c", FRAME_HEAD, REQ_OPEN_STATUS_WRITE, FRAME_TRUE, FRAME_BCC, FRAME_TAIL);
    }
    else
    {
        snprintf(request_buf, sizeof(request_buf), "%s%s%s%s%c", FRAME_HEAD, REQ_OPEN_STATUS_WRITE, FRAME_FALSE, FRAME_BCC, FRAME_TAIL);
    }

    laser_send((uint8_t *)request_buf, strlen(request_buf));
}

// 测定值读出
static void command_distance_read_req(void)
{
    snprintf(request_buf, sizeof(request_buf), "%s%s%s%c", FRAME_HEAD, REQ_DISTANCE_READ, FRAME_BCC, FRAME_TAIL);

    laser_send((uint8_t *)request_buf, strlen(request_buf));
}

// 调零状态查询
static void command_zero_status_req(void)
{
    snprintf(request_buf, sizeof(request_buf), "%s%s%s%c", FRAME_HEAD, REQ_ZERO_STATUS, FRAME_BCC, FRAME_TAIL);

    laser_send((uint8_t *)request_buf, strlen(request_buf));
}

// 调零状态开启
static void command_zero_status_write_req(bool open)
{
    if (open)
    {
        snprintf(request_buf, sizeof(request_buf), "%s%s%s%s%c", FRAME_HEAD, REQ_ZERO_STATUS_WRITE, FRAME_TRUE, FRAME_BCC, FRAME_TAIL);
    }
    else
    {
        snprintf(request_buf, sizeof(request_buf), "%s%s%s%s%c", FRAME_HEAD, REQ_ZERO_STATUS_WRITE, FRAME_FALSE, FRAME_BCC, FRAME_TAIL);
    }

    laser_send((uint8_t *)request_buf, strlen(request_buf));
}

static void command_zero_status_close_req(void)
{
    command_zero_status_write_req(false);
}

static void command_zero_status_open_req(void)
{
    command_zero_status_write_req(true);
}

// 处理数据
static void laser_data_process(uint8_t *data, uint8_t length)
{
    char cmd[3] = {0};
    // 向右偏移4个位置获取指令部分
    response_ptr_offset = 0;
    response_ptr = data;
    response_ptr_offset += 4;
    if (!laser_handle.connect)
    {
        osel_memcpy((uint8_t *)cmd, response_ptr + response_ptr_offset, 2);
        response_ptr_offset += 2;
    }
    else
    {
        osel_memcpy((uint8_t *)cmd, response_ptr + response_ptr_offset, 3);
        response_ptr_offset += 3;
    }

    cmd_parsing(cmd);
}

static void command_device_connect_rsp(void)
{
    laser_handle.connect = true;
    if (laser_handle.state != LASER_READY)
    {
        laser_handle.state = LASER_CONNECT;
    }
}

static void command_open_status_rsp(void)
{
    uint8_t status[5] = {0};
    response_ptr_offset++; // 跳过符号
    osel_memcpy(status, response_ptr + response_ptr_offset, 5);
    const char *cs = FRAME_TRUE;
	const char *o_status = (char *)status;
    if(!strncmp(o_status, (cs + 1), 5))
    {
        laser_handle.open_status = true;
        if (laser_handle.state != LASER_READY)
        {
            laser_handle.state = LASER_OPEN_STATUS;
        }
    }
    else
    {
        laser_handle.open_status = false;
    }

#if STATUS_DEFAULT == 1
    if (laser_handle.state == LASER_CONNECT)
    {
        if (!laser_handle.open_status)
        {
            command_open_status_write_req(true);
        }
    }

#endif
}

static void command_open_status_write_rsp(void)
{
}

#define PRECISION 100000 // 精度，即小数点后的位数
static float to_float(uint8_t *arr)
{
    int num = arr[0] * 1000000 + arr[1] * 100000 + arr[2] * 10000 + arr[3] * 1000 + arr[4] * 100 + arr[5] * 10 + arr[6]; // 数字部分的值
    float result = (float)num / PRECISION;  // 将数字部分和小数部分合并成浮点数

    return result;
}
static void command_distance_read_rsp(void)
{
    uint8_t distance[10] = {0};
    float f = 0;
    osel_memcpy(distance, response_ptr + response_ptr_offset, 10);
    for (uint8_t i = 1; i <= 7; i++)
    {
        CHAR_TO_NUM(distance[i], distance[i]);
    }
    f = to_float(&distance[1]);
    if (distance[0] == '-')
    {
        f = -f;
    }

    if (laser_handle.state != LASER_READY)
    {
        laser_handle.state = LASER_DISTANCE;
    }
    laser_handle.distance = f * 1000;
}

static void command_zero_status_rsp(void)
{
    uint8_t status[5] = {0};
    response_ptr_offset++; // 跳过符号
    osel_memcpy(status, response_ptr + response_ptr_offset, 5);
    const char *cs = FRAME_TRUE;
	const char *z_status = (char *)status;
    if(!strncmp(z_status, (cs + 1), 5))
    {
        laser_handle.zero_status = true;
    }
    else
    {
        laser_handle.zero_status = false;
    }
}

static void command_zero_status_write_rsp(void)
{
#if STATUS_DEFAULT == 1
    if (laser_handle.state == LASER_OPEN_STATUS)
    {
        laser_handle.zero_status = false;
        laser_handle.state = LASER_ZERO_STATUS_CLOSE;
    }
    else if (laser_handle.state == LASER_ZERO_STATUS_CLOSE)
    {
        laser_handle.zero_status = true;
        laser_handle.state = LASER_ZERO_STATUS_OPEN;
    }
    else
    {
        laser_handle.zero_status = false;
        laser_handle.state = LASER_CONNECT;
    }

#endif
}

static void _laser_ready(void)
{
    laser_handle.state = LASER_READY;
}
//REGISTER_CMD(WC, command_device_connect_rsp, "");
//REGISTER_CMD(RLR, command_open_status_rsp, "");
//REGISTER_CMD(WLR, command_open_status_write_rsp, "");
//REGISTER_CMD(RMD, command_distance_read_rsp, "");
//REGISTER_CMD(RZS, command_zero_status_rsp, "");
//REGISTER_CMD(WZS, command_zero_status_write_rsp, "");

laser_event_t laser_event[] = {
    {LASER_IDEL, command_device_connect_req},
    {LASER_CONNECT, command_open_status_req},
    {LASER_OPEN_STATUS, command_zero_status_close_req},
    {LASER_ZERO_STATUS_CLOSE, command_zero_status_open_req},
    {LASER_ZERO_STATUS_OPEN, command_distance_read_req},
    {LASER_DISTANCE, _laser_ready},
};

void uart3_it_init(void)
{
    /*接收配置*/
	HAL_UART_Receive_IT(&huart3, (uint8_t *)&res, 1);

}

void usart3_laser_send(UART_HandleTypeDef *huart,uint8_t *Tx_Buf,uint16_t Size)
{
	HAL_UART_Transmit_DMA(huart,Tx_Buf,Size);
}

void laser_init(send_data_cb_t cb)
{
	uart3_it_init();
	
    laser_handle.state = LASER_IDEL;
    laser_handle.send_data_cb = cb;
    laser_handle.command_device_connect_req = command_device_connect_req;
    laser_handle.command_open_status_req = command_open_status_req;
    laser_handle.command_open_status_write_req = command_open_status_write_req;
    laser_handle.command_distance_read_req = command_distance_read_req;
    laser_handle.command_zero_status_req = command_zero_status_req;
    laser_handle.command_zero_status_write_req = command_zero_status_write_req;
}

 //串口接收中断回调函数，激光
void laser_rx_cb(void)
{
	if(res == 0x25) rec_flag = 1;
	
	if(rec_flag == 1)
	{
		uart_buf[rx_count] = res;
		rx_count ++;
		if(uart_buf[rx_count - 1] == 0x2A && uart_buf[rx_count - 2] == 0x2A)
		{
			laser_data_process(uart_buf,rx_count);
			rx_count = 0;
			rec_flag = 0;
		}
	}
	HAL_UART_Receive_IT(&huart3, (uint8_t *)&res, 1);

}


//根据cmd指令执行相应程序
void cmd_parsing(char *str)
{
	//if(strcmp(str, "WC") == 0)
	if(!strncmp(str, REQ_DEVICE_ONLINE,2))
		command_device_connect_rsp();
	else if(!strncmp(str, REQ_OPEN_STATUS,3))
		command_open_status_rsp();
	else if(!strncmp(str, REQ_OPEN_STATUS_WRITE,3))
		command_open_status_write_rsp();
	else if(!strncmp(str, REQ_DISTANCE_READ,3))
		command_distance_read_rsp();
	else if(!strncmp(str, REQ_ZERO_STATUS,3))
		command_zero_status_rsp();
	else if(!strncmp(str, REQ_ZERO_STATUS_WRITE,3))
		command_zero_status_write_rsp();

}

//激光传感器任务流程
void laser_task(void)
{
	//command_zero_status_open_req();
        if (laser_handle.state != LASER_READY)
        {
			if(it_500ms_flag == 1)
			{
				if(laser_handle.state == LASER_IDEL)
					command_device_connect_req();
				else if(laser_handle.state == LASER_CONNECT)
					command_open_status_req();
				else if(laser_handle.state == LASER_OPEN_STATUS)
					command_zero_status_close_req();
				else if(laser_handle.state == LASER_ZERO_STATUS_CLOSE)
					command_zero_status_open_req();
				else if(laser_handle.state == LASER_ZERO_STATUS_OPEN)
					command_distance_read_req();
				else if(laser_handle.state == LASER_DISTANCE)
					_laser_ready();
				it_500ms_flag = 0;
			}
        }
        else
        {
			if(it_25ms_flag == 1)
			{
				laser_handle.command_distance_read_req();
				if(laser_handle.distance >= 0)
				{
					InputReg[24] = laser_handle.distance;
				}
				else
				{
					int laser_num = -laser_handle.distance;
					
					InputReg[24] = (uint16_t)((0xFFFF - laser_num) + 1);
				}
				it_25ms_flag = 0;
				//laser_handle.connect = true;
			}
        }
}