#include "p_protocol.h"

protocol_t protocol;

int Protocol_read(void *data, uint32_t size, uint32_t wait_ms)
{
    switch (protocol.port_type)
    {
    case PORT_LINUX_UART:
        break;
    case PORT_LINUX_SBDATA:
        return sb_data_port_read(protocol.port.data_port, data, size, 0);
        break;
    default:
        break;
    }
    return -1;
}
int Protocol_write(const void *data, uint32_t size, uint32_t wait_ms)
{
    switch (protocol.port_type)
    {
    case PORT_LINUX_UART:
        break;
    case PORT_LINUX_SBDATA:
        return sb_data_port_write(protocol.port.data_port, data, size, 0);
        break;
    default:
        break;
    }
    return -1;
}

void Protocol_init(port_type_e port_type, void* port)
{
    protocol.port.data_port = port;
    protocol.port_type = port_type;
    switch (port_type)
    {
    case PORT_LINUX_UART:
        protocol.port.uart_fd = (int*)port;
        break;
    case PORT_LINUX_SBDATA:
        protocol.port.data_port = (sb_data_port_t*)port;
        break;
    default:
        break;
    }

    protocol.msp.read = protocol.mavlink_device.read = Protocol_read;
    protocol.msp.write = protocol.mavlink_device.write = Protocol_write;

    msp_init(&protocol.msp);
    mavlink_init(&protocol.mavlink_device);

}
int Protocol_search()
{
    uint8_t c;
    while(Protocol_read(&c,1,0))
    {
        // MAVLINK 解析
        mavlink_message_t message;
        if(mavlink_message(&protocol.mavlink_device, &message,c)){
            // printf("Received message from serial with ID #%d (sys:%d|comp:%d):\n", message.msgid, message.sysid, message.compid);
            mavlik_packet_processing(&protocol.mavlink_device, &message);
            return PROTOCOL_MAVLINK;
        }
        // MSP 解析
        if (msp_parse_received_data(&protocol.msp, c))
        {
            if (protocol.msp.c_state == MSP_COMMAND_RECEIVED)
            {
                msp_process_received_command(&protocol.msp);
                return PROTOCOL_MSP;
            }
        }
    }
    return PROTOCOL_IDLE;
}

void Protocol_loop(void* arg)
{
    switch(protocol.pro_type)
    {
        case PROTOCOL_IDLE:
            protocol.pro_type = Protocol_search();
            break;
        case PROTOCOL_MSP:
            msp_recv_loop(&protocol.msp);
            break;
        case PROTOCOL_MAVLINK:
            mavlink_recv_loop(&protocol.mavlink_device);
            break;
    }
}

int Protocol_buf_search(void* data, uint32_t size)
{
    uint32_t size_lat = size;
    uint8_t* data_char = (uint8_t*)data;
    while (size_lat --)
    {
        uint8_t c = data_char[size - size_lat - 1];
        // MAVLINK 解析
        mavlink_message_t message;
        if(mavlink_message(&protocol.mavlink_device, &message,c)){
            mavlik_packet_processing(&protocol.mavlink_device, &message);
            memcpy(&protocol.message,&message,sizeof(mavlink_message_t));
            return PROTOCOL_MAVLINK;
        }
        // MSP 解析
        if (msp_parse_received_data(&protocol.msp, c))
        {
            if (protocol.msp.c_state == MSP_COMMAND_RECEIVED)
            {
                msp_process_received_command(&protocol.msp);
                return PROTOCOL_MSP;
            }
        }
    }
    // printf("search %c %c\r\n", data_char[0], data_char[1]);
    return PROTOCOL_IDLE;
}
void Protocol_buf_decode(void* data, uint32_t size)
{
    switch(protocol.pro_type)
    {
        case PROTOCOL_IDLE:
            protocol.pro_type = Protocol_buf_search(data, size);
            break;
        case PROTOCOL_MSP:
            protocol.analysis_sussess_count += msp_recv_buf(&protocol.msp, data, size);
            break;
        case PROTOCOL_MAVLINK:
            protocol.analysis_sussess_count += mavlink_recv_buf(&protocol.mavlink_device, data, size);
            break;
    }
}
void protocol_set_message_status(message_status_e status)
{
    protocol.message_status = status;
}
protocol_status_e get_protocol_status(void)
{
    protocol.protocol_status = PROTOCOL_STATUS_OK;

    // 先OTA升级、未知协议、未收到飞控数据
    if(protocol.message_status == MESSAGE_OTA || protocol.message_status == MESSAGE_FC_ISP)
    {
        protocol.protocol_status = PROTOCOL_STATUS_IN_OTA;
    }
    else if(protocol.pro_type == PROTOCOL_IDLE)
    {
        protocol.protocol_status = PROTOCOL_STATUS_TYPE_IDLE;
    } else if(protocol.analysis_sussess_count == 0)
    {   
        protocol.protocol_status = PROTOCOL_STATUS_NO_DATA;
    }

    protocol.analysis_sussess_count = 0;

    return protocol.protocol_status;
}

int fc_reboot(void)
{
    switch(protocol.pro_type)
    {
        case PROTOCOL_IDLE:
            printf("The protocol cannot be restarted as it has not been recognized\n");
            break;
        case PROTOCOL_MSP:
        {
            msp_send_reboot(&protocol.msp);
        }
            break;
        case PROTOCOL_MAVLINK:
        {
            mavlink_message_t msg;
            uint8_t buf[MAVLINK_MAX_PACKET_LEN];
            mavlink_msg_command_long_pack(
                255, 0, &msg,
                /* target_system */ protocol.message.sysid,
                /* target_component */ protocol.message.compid,
                /* command */ MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN,
                /* confirmation */ 0,
                /* param1 */ 1,  // 1 = reboot
                /* param2 */ 1,
                /* param3 */ 0,
                /* param4 */ 0,
                /* param5 */ 0,
                /* param6 */ 0,
                /* param7 */ 0
            );

            // 打包成字节流发送
            uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);

            int size =  Protocol_write(buf, len, 0);
            printf("reboot sys:%d com:%d size:%d\n", protocol.message.sysid, protocol.message.compid,size);
        }
            break;
    }
    return 1;
}