#include "mavlink_control.h"


void mavlink_init(mavlink_device_t* mavlink_device)
{
    
}

int mavlink_message(mavlink_device_t* mavlink_device, mavlink_message_t *message, uint8_t cp)
{
	mavlink_status_t status;
	uint8_t          msgReceived = false;

	// --------------------------------------------------------------------------
	//   PARSE MESSAGE
	// --------------------------------------------------------------------------

		// the parsing
    msgReceived = mavlink_parse_char(MAVLINK_COMM_1, cp, message, &status);

    // check for dropped packets
    if ( (mavlink_device->lastStatus.packet_rx_drop_count != status.packet_rx_drop_count) && mavlink_device->debug )
    {
        printf("ERROR: DROPPED %d PACKETS\n", status.packet_rx_drop_count);
        unsigned char v=cp;
        printf("%02x ", v);
    }
    mavlink_device->lastStatus = status;


	// --------------------------------------------------------------------------
	//   mavlink_device->debugGING REPORTS
	// --------------------------------------------------------------------------
	if(msgReceived && mavlink_device->debug)
	{
		// Report info
		printf("Received message from serial with ID #%d (sys:%d|comp:%d):\n", message->msgid, message->sysid, message->compid);

		printf("Received serial data: ");
		unsigned int i;
		uint8_t buffer[MAVLINK_MAX_PACKET_LEN];

		// check message is write length
		unsigned int messageLength = mavlink_msg_to_send_buffer(buffer, message);

		// message length error
		if (messageLength > MAVLINK_MAX_PACKET_LEN)
		{
			printf("\nFATAL ERROR: MESSAGE LENGTH IS LARGER THAN BUFFER SIZE\n");
		}

		// print out the buffer
		else
		{
			for (i=0; i<messageLength; i++)
			{
				// unsigned char v=buffer[i];
				// printf("%02x ", v);
			}
			// printf("\n");
		}
	}

	// Done!
	return msgReceived;
}
int write_message(mavlink_device_t* mavlink_device, const mavlink_message_t *message)
{
	char buf[300];

	// Translate message to buffer
	unsigned len = mavlink_msg_to_send_buffer((uint8_t*)buf, message);

	// Write buffer to serial port, locks port while writing
	int bytesWritten = mavlink_device->write(buf,len,0);

	return bytesWritten;
}

void mavlik_packet_processing(mavlink_device_t* mavlink_device, mavlink_message_t* message)
{
    mavlink_device->current_messages.sysid  = message->sysid;
    mavlink_device->current_messages.compid = message->compid;

    switch(message->msgid){
        case MAVLINK_MSG_ID_HEARTBEAT:
        {
            //printf("MAVLINK_MSG_ID_HEARTBEAT\n");
            mavlink_msg_heartbeat_decode(message, &(mavlink_device->current_messages.heartbeat));
            
            break;
        }

        case MAVLINK_MSG_ID_SYS_STATUS:
        {
            //printf("MAVLINK_MSG_ID_SYS_STATUS\n");
            mavlink_msg_sys_status_decode(message, &(mavlink_device->current_messages.sys_status));
            break;
        }

        case MAVLINK_MSG_ID_BATTERY_STATUS:
        {
            //printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
            mavlink_msg_battery_status_decode(message, &(mavlink_device->current_messages.battery_status));
            break;
        }

        case MAVLINK_MSG_ID_RADIO_STATUS:
        {
            //printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
            mavlink_msg_radio_status_decode(message, &(mavlink_device->current_messages.radio_status));
            break;
        }

        case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
        {
            //printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
            mavlink_msg_local_position_ned_decode(message, &(mavlink_device->current_messages.local_position_ned));
            break;
        }

        case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
        {
            //printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
            mavlink_msg_global_position_int_decode(message, &(mavlink_device->current_messages.global_position_int));
            break;
        }

        case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
        {
            //printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
            mavlink_msg_position_target_local_ned_decode(message, &(mavlink_device->current_messages.position_target_local_ned));
            break;
        }

        case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
        {
            //printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
            mavlink_msg_position_target_global_int_decode(message, &(mavlink_device->current_messages.position_target_global_int));
            break;
        }

        case MAVLINK_MSG_ID_HIGHRES_IMU:
        {
            //printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
            mavlink_msg_highres_imu_decode(message, &(mavlink_device->current_messages.highres_imu));
            break;
        }

        case MAVLINK_MSG_ID_ATTITUDE:
        {
            //printf("MAVLINK_MSG_ID_ATTITUDE\n");
            mavlink_msg_attitude_decode(message, &(mavlink_device->current_messages.attitude));
            printf("pitch %f roll %f yaw %f\n", mavlink_device->current_messages.attitude.pitch * 180 / 3.1415926, mavlink_device->current_messages.attitude.roll * 180 / 3.1415926, mavlink_device->current_messages.attitude.yaw * 180 / 3.1415926);
            break;
        }

        default:
        {
            // printf("Warning, did not handle message id %i\n",message.msgid);
            break;
        }
    }
}
void mavlink_recv_loop(mavlink_device_t* mavlink_device)
{
    bool success = false; 

    mavlink_message_t message;
    uint8_t c;
    while(mavlink_device->read(&c,1,0))
    {
        success = mavlink_message(mavlink_device,&message,c);
        if(success){
            // printf("Received message from serial with ID #%d (sys:%d|comp:%d):\n", message.msgid, message.sysid, message.compid);
            mavlik_packet_processing(mavlink_device,&message);
            break;
        }
    }
}

int mavlink_recv_buf(mavlink_device_t* mavlink_device, void *data, uint32_t size)
{
    uint8_t res = 0;
    bool success = false; 
    
    mavlink_message_t message;
    
    uint32_t size_lat = size;
    uint8_t* data_char = (uint8_t*)data;

    while(size_lat --)    
    {
        uint8_t c = data_char[size - size_lat - 1];

        success = mavlink_message(mavlink_device,&message,c);
        if(success){
            // printf("Received message from serial with ID #%d (sys:%d|comp:%d):\n", message.msgid, message.sysid, message.compid);
            mavlik_packet_processing(mavlink_device,&message);
            success = false;
            res++;
        }
    }
    return res;
}