#include "protocol.h"

protocol_device_t protocol_device;
flash_settings_t protocol_settings;

void protocol_init(protocol_device_t device)
{
    protocol_device = device;
    flash_restore(&protocol_settings);
    led_set_brightness(protocol_settings.val.led_brightness);

    can_init(protocol_settings.val.can_id, DEFAULT_BROADCAST_ID);
}

flash_settings_t* protocol_get_settings(void)
{
    return &protocol_settings;
}

void protocol_save_settings(void)
{
    flash_save(&protocol_settings);
}

bool protocol_send_test()
{
    bool result = true;
    can_send_test(protocol_settings.val.can_id | 0x00000001);
    return result;
}

bool protocol_receive_message(CanRxMsgTypeDef* can_message)
{
    bool result = true;
    
    protocol_message_t message;
    message.command = can_message->Data[0] & 0x80;
    message.id = can_message->Data[0] & 0x7F;
    message.key = (can_message->Data[1] << 8) | can_message->Data[2];
    message.sensor = can_message->Data[3];
    message.data.byte_data[0] = can_message->Data[4];
    message.data.byte_data[1] = can_message->Data[5];
    message.data.byte_data[2] = can_message->Data[6];
    message.data.byte_data[3] = can_message->Data[7];
    
    protocol_process_message(&message);
    
    return result;
}

bool protocol_send_message(protocol_message_t* message)
{
    bool result = true;
    uint8_t data[8];
    
    data[0] = (message->command & 0x80) | (message->id & 0x7F);
    data[1] = (message->key >> 8) & 0xFF;
    data[2] = (message->key >> 0) & 0xFF;
    data[3] = message->sensor;
    data[4] = message->data.byte_data[0];
    data[5] = message->data.byte_data[1];
    data[6] = message->data.byte_data[2];
    data[7] = message->data.byte_data[3];
    
    can_send(protocol_settings.val.can_id | 0x00000001, CAN_ID_STD, 8, data);
    
    return result;
}

bool protocol_process_message(protocol_message_t* message)
{
    bool result = true;
    
    if (message->command)
    {
        switch(message->id)
        {
            case ESTOP:
                {
                    //call estop weak function
                    result = protocol_estop(message->data.float_data!=0.0f);
                }
                break;
            case SILENCE_BUS:
                {
                    //silence can bus
                    result = can_silence_bus(message->data.float_data!=0.0f);
                }
                break;
            case SET_OUTPUT:
                {
                    //call set output weak function
                    result = protocol_set_output(message);
                }
                break;
            case GET_DATA:
                {
                    //call get data weak function
                    result = protocol_get_data(message);
                }
                break;
            case CONFIG:
                {
                    result = _protocol_config(message);
                }
                break;
            default:
                result = false;
                break;
        }
    }
    
    return result;
}

bool protocol_send_data(protocol_data_key_t key, uint8_t sensor, float data)
{
    bool result = true;

    protocol_message_t message;
    message.command = false;
    message.id = protocol_device;
    message.key = key;
    message.sensor = sensor;
    message.data.float_data = data;

    result = protocol_send_message(&message);

    return result;
}

bool protocol_send_data_bytes(protocol_data_key_t key, uint8_t sensor, uint8_t* data)
{
    bool result = true;

    protocol_message_t message;
    message.command = false;
    message.id = protocol_device;
    message.key = key;
    message.sensor = sensor;
    message.data.byte_data[0] = data[0];
    message.data.byte_data[1] = data[1];
    message.data.byte_data[2] = data[2];
    message.data.byte_data[3] = data[3];

    result = protocol_send_message(&message);

    return result;
}

bool _protocol_get_data(protocol_message_t* message)
{
    bool result = false;

    if (message->key == CAN_ID)
    {
        protocol_send_data(message->key, message->sensor, protocol_settings.val.can_id);
    }
    else if (message->key == DATA_RATE)
    {
        protocol_send_data(message->key, message->sensor, ((float)protocol_settings.val.data_rate)/1000);
    }
    else if (message->key == LED_BRIGHTNESS)
    {
        protocol_send_data(message->key, message->sensor, protocol_settings.val.led_brightness);
    }
    else
    {
        //call config weak function
        result = protocol_get_data(message);
    }
    return result;
}

bool _protocol_config(protocol_message_t* message)
{
    bool result = false;

    if (message->key == CAN_ID)
    {
        uint32_t can_id = (uint16_t) message->data.float_data;
        protocol_settings.val.can_id = can_id;
        flash_save(&protocol_settings);
    }
    else if (message->key == DATA_RATE)
    {
        uint16_t data_rate = (uint16_t) message->data.float_data;
        protocol_settings.val.data_rate = data_rate * 1000;
        flash_save(&protocol_settings);
    }
    else if (message->key == LED_BRIGHTNESS)
    {
        uint8_t brightness = (uint8_t)message->data.float_data;
        result = led_set_brightness(brightness);
        protocol_settings.val.led_brightness = brightness;
        flash_save(&protocol_settings);
    }
    else
    {
        //call config weak function
        result = protocol_config(message);
    }
    return result;
}

__weak bool protocol_estop(bool value)
{
    return false;
}

__weak bool protocol_set_output(protocol_message_t* message)
{
    return false;
}

__weak bool protocol_get_data(protocol_message_t* message)
{
    return false;
}

__weak bool protocol_config(protocol_message_t* message)
{
    return false;
}