#include "commons.h"
#include "proto.h"

#ifdef IS_MASTER
#include <Arduino.h>
#else
#include "mcc_generated_files/system/system.h"
#endif

static uint8_t in_index;
static bool is_started = false;
static bool led_set = false;
static bool sensor_set = false;
static bool error_set = false;
static bool next_in_should_be_end = false;

#ifndef IS_MASTER
extern bool tx_timed_out;
#endif


//==============================================================================
// Private Declarations
//==============================================================================

//==============================================================================
// Public
//==============================================================================
/*=****************************************************************************/
bool byte_in (uint8_t in, uint8_t* p_cmd, uint8_t* p_id, uint16_t* p_led1, 
                        uint16_t* p_led2, uint16_t* p_sensor, errors_t* p_error)
{
    if (!is_started)
    {
        if (in == START_DEL)
        {
            // Start at 1 since this 'in' is now useless
            in_index = 1;
            is_started = true;
            next_in_should_be_end = false;
        }
    }
    else
    {
        if (ID_INDEX == in_index)
        {
#ifdef MASTER_ID
            // Don't bother with messages not meant for us
            if (MY_ID != in)
            {
                is_started = false;
                return false;
            }
#else
            // Test if in valid in slave list
            *p_id = in;
#endif
        }

        // If should be done
        if (next_in_should_be_end)
        {
            is_started = false;
            return in == END_DEL;
        }
        
        // Save command for when finished
        if (CMD_INDEX == in_index)
        {
#ifdef IS_MASTER
            // Invalid inputs for MASTER
            if (GET_CMD_ID == in ||
                    SET_CMD_ID == in)
            {
                Serial.println("Ignored cmd");
                is_started = false;
                return false;
            }
#else
            // Invalid inputs for SLAVE
            if (ERROR_CMD_ID == in ||
                    DATA_CMD_ID == in)
            {
                is_started = false;
                return false;
            }
#endif
            *p_cmd = in;
            if (GET_CMD_ID == in)
                next_in_should_be_end = true;
        }
        else if (in_index > CMD_INDEX)
        {
            switch (*p_cmd)
            {
            case GET_CMD_ID:
                // Do nothing
                break;
            case SET_CMD_ID:
                // LED1
                if (LED1_INDEX == in_index)
                    *p_led1 = in << 8;
                else if (LED1_INDEX + 1 == in_index)
                    *p_led1 |= in;
                // LED2
                else if (LED2_INDEX == in_index)
                    *p_led2 = in << 8;
                else if (LED2_INDEX + 1 == in_index)
                {
                    *p_led2 |= in;
                    next_in_should_be_end = true;
                    led_set = true;
                }

                break;
            case DATA_CMD_ID:
                if (SENSOR_INDEX == in_index)
                    *p_sensor = in << 8;
                else if (SENSOR_INDEX + 1 == in_index)
                {
                    *p_sensor |= in;
                    next_in_should_be_end = true;
                    sensor_set = true;
                }

                break;
            case ERROR_CMD_ID:
                if (ERR_INDEX == in_index)
                {
                    *p_error = (errors_t) in;
                    next_in_should_be_end = true;
                    error_set = true;
                }

                break;
            default:
                is_started = false;
                break;
            }
        }

        ++in_index;
    }
    
    return false;
}

#ifdef IS_MASTER 
/*=****************************************************************************/
errors_t send_get (uint8_t peer_id)
{
    SET_WRITE();

    errors_t ret = uart_tx_byte(START_DEL);
    if (RET_SUCCESS(ret)) ret = uart_tx_byte(peer_id);
    if (RET_SUCCESS(ret)) ret = uart_tx_byte(GET_CMD_ID);
    if (RET_SUCCESS(ret)) ret = uart_tx_byte(END_DEL);

    Serial2.flush();

    SET_READ();
    return ret;
}

/*=****************************************************************************/
errors_t send_set (uint8_t peer_id, uint16_t led1, uint16_t led2)
{
    SET_WRITE();

    errors_t ret = uart_tx_byte(START_DEL);
    if (RET_SUCCESS(ret)) ret = uart_tx_byte(peer_id);
    if (RET_SUCCESS(ret)) ret = uart_tx_byte(SET_CMD_ID);

    // LED1
    if (RET_SUCCESS(ret)) ret = uart_tx_byte((led1 >> 8) & 0xFF);
    if (RET_SUCCESS(ret)) ret = uart_tx_byte(led1 & 0xFF);

    // LED2
    if (RET_SUCCESS(ret)) ret = uart_tx_byte((led2 >> 8) & 0xFF);
    if (RET_SUCCESS(ret)) ret = uart_tx_byte(led2 & 0xFF);

    if (RET_SUCCESS(ret)) ret = uart_tx_byte(END_DEL);

    Serial2.flush();

    SET_READ();
    return ret;
}

#else

/*=****************************************************************************/
errors_t send_data (uint8_t peer_id, uint16_t sensor)
{
    SET_WRITE();

    errors_t ret = uart_tx_byte(START_DEL);
    if (RET_SUCCESS(ret)) ret = uart_tx_byte(peer_id);
    if (RET_SUCCESS(ret)) ret = uart_tx_byte(DATA_CMD_ID);

    if (RET_SUCCESS(ret)) ret = uart_tx_byte((sensor >> 8) & 0xFF);
    if (RET_SUCCESS(ret)) ret = uart_tx_byte(sensor & 0xFF);

    if (RET_SUCCESS(ret)) ret = uart_tx_byte(END_DEL);

    SET_READ();
    return ret;
}

/*=****************************************************************************/
errors_t send_error (uint8_t peer_id, errors_t err)
{
    SET_WRITE();

    errors_t ret = uart_tx_byte(START_DEL);
    if (RET_SUCCESS(ret)) ret = uart_tx_byte(peer_id);
    if (RET_SUCCESS(ret)) ret = uart_tx_byte(ERROR_CMD_ID);

    if (RET_SUCCESS(ret)) ret = uart_tx_byte(err);

    if (RET_SUCCESS(ret)) ret = uart_tx_byte(END_DEL);

    SET_READ();
    return ret;
}
#endif

/*=****************************************************************************/
errors_t uart_rx_byte (uint8_t* p_byte)
{
    SET_READ();

#ifdef IS_MASTER
    if (Serial2.available() > 0)
        *p_byte = Serial2.read();
    else
        return NO_RX;
#else
    if(UART1.IsRxReady())
        *p_byte = UART1.Read();
    else
        return NO_RX;
#endif

    return SUCCESS;
}

/*=****************************************************************************/
errors_t uart_tx_byte (uint8_t byte)
{
#ifdef IS_MASTER
#ifdef DEBUG
    Serial.print(byte, HEX);
    Serial.print(' ');
#else
    Serial2.write(byte);
#endif
#else
    tx_timed_out = false;
    TMR_TX_Write(0);
    TMR_TX_Start();
    while (!UART1.IsTxReady() && !tx_timed_out)
        ;
    TMR_TX_Stop();
    if (tx_timed_out)
        return TX_READY_ERR;
    
    UART1.Write(byte);
    
    tx_timed_out = false;
    TMR_TX_Write(0);
    TMR_TX_Start();
    while (!UART1.IsTxDone() && !tx_timed_out)
        ;
    TMR_TX_Stop();
    if (tx_timed_out)
        return TX_DONE_ERR;
#endif

    return SUCCESS;
}