#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_m port_type, void* port)
{
    protocol.port.data_port = port;
    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);
            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:
            msp_recv_buf(&protocol.msp, data, size);
            break;
        case PROTOCOL_MAVLINK:
            mavlink_recv_buf(&protocol.mavlink_device, data, size);
            break;
    }
}