#define F_CPU 16000000
#include <avr/io.h>
#include <avr/interrupt.h>
#include "common/mavlink.h"
#include "ardupilotmega/ardupilotmega.h"

#define rxEN 1
#define txIEN 1
#define drIEN 1
#define txEN 1
#define async 0x00
#define noParity 0x00
#define oneStop 0x00
#define chSize8 0x03
#define BAUD 57600
#define BAUDPRESCALAR ((F_CPU/(BAUD))/16 - 1) //Register Values

typedef struct{

        uint8_t sysid;
        uint8_t compid;
        uint16_t len;
        uint8_t bytes;
        float data[2];
        uint8_t* pData;
        uint8_t dataBuffer[MAVLINK_MAX_PACKET_LEN];
        uint8_t lenCounter;
        mavlink_message_t* msg;

}mavlink_data_t;

mavlink_data_t mavlink_data;


void sendData_UART();
void initUART(uint8_t umsel, uint8_t upm, uint8_t usbs, uint8_t ucsz); //Prototypes


int main(void)
{
    mavlink_data.sysid      = 1;
    mavlink_data.compid     = 100;
    mavlink_data.bytes      = 8;
    mavlink_data.data[0]    = 566;
    mavlink_data.data[1]    = 123;
    *mavlink_data.pData = mavlink_data.data[0];
    mavlink_data.lenCounter = 0;

    initUART(async, noParity, oneStop, chSize8); //Initiates UART 8-N-1
    sei();

    mavlink_msg_data64_pack(mavlink_data.sysid, mavlink_data.compid, mavlink_data.msg, mavlink_data.data[0], mavlink_data.bytes, mavlink_data.pData);

    mavlink_data.len = mavlink_msg_to_send_buffer(mavlink_data.dataBuffer, mavlink_data.msg);

    sendData_UART();

    while(1){};
}

void initUART(uint8_t umsel, uint8_t upm, uint8_t usbs, uint8_t ucsz){
    //Initiates UART Baud and sets it to a 8-N-1 setting
    UBRR0 = (uint8_t)(BAUDPRESCALAR);

    UCSR0C = (umsel << UMSEL00) |
             (upm   << UPM00)   |
             (usbs  << USBS0)   |
             (ucsz  << UCSZ00);
}

void sendData_UART(){
    UCSR0B |= ((txIEN << TXCIE0) |
               (drIEN << UDRIE0) |
               (txEN  << TXEN0));
}

ISR(USART0_UDRE_vect){
    //If lengthCounter != len
    //Else send disable interrupt
    if(mavlink_data.lenCounter != mavlink_data.len){
        UDR0 = mavlink_data.dataBuffer[mavlink_data.lenCounter];
        mavlink_data.lenCounter++;
    }
    else{
        UCSR0B &= ~(drIEN << UDRIE0);
        mavlink_data.lenCounter = 0;
    }
}

ISR(USART0_TX_vect){
    //Disables TX and interrupt for power consumption
    UCSR0B &= ~((txIEN << TXCIE0) |
                (txEN  << TXEN0));
}