#include "nbus_slave.h"

#if MODULE_SLAVE == 1

void nbus_slave_unicastToSensorGet(nBus_TypeDef *nbus)
{
    nBus_sensorType_t sensor_type = nbus->interface->getType(nbus->rx_buffer[RX_SA]);

    if (sensor_type == TYPE_UNKNOWN) // handle bad sensor address
    {
        setErrorResponse(nbus, ILLEGAL_DEVICE_ADDRESS);
        return;
    }

    switch (nbus->function_code.function)
    {
    case CMD_PARAM: {

        switch (nbus->rx_length)
        {
        case RX_META: // get all params
        {
            for (uint8_t i = 0, j = 0; i < _NBUS_PARAM_COUNT; i++)
            {
                if (nbus->interface->hasParam(nbus->rx_buffer[RX_SA], i))
                {
                    int32_t param_value = nbus->interface->getParam(nbus->rx_buffer[RX_SA], i);
                    nbus->tx_buffer[4 + 5 * j] = i;                             // param id
                    nbus->tx_buffer[5 + 5 * j] = (uint8_t)(param_value & 0xFF); // param value 4B
                    nbus->tx_buffer[6 + 5 * j] = (uint8_t)((param_value >> 8) & 0xFF);
                    nbus->tx_buffer[7 + 5 * j] = (uint8_t)((param_value >> 16) & 0xFF);
                    nbus->tx_buffer[8 + 5 * j] = (uint8_t)((param_value >> 24) & 0xFF);

                    nbus->tx_length += 5;
                    j++;
                }
            }
        }
        break;

        case RX_META + 1: // get specified parameter: 4 meta + 1 param id
        {
            if (nbus->interface->hasParam(nbus->rx_buffer[RX_SA], (nBus_param_t)nbus->rx_buffer[RX_DT]) ==
                0) // handle non-existing param
            {
                setErrorResponse(nbus, PARAM_NOT_IMPLEMENTED);
                break;
            }

            nbus->tx_buffer[4] = nbus->rx_buffer[RX_DT]; // param id
            int32_t param_value = nbus->interface->getParam(nbus->rx_buffer[RX_SA],
                                                            (nBus_param_t)nbus->rx_buffer[RX_DT]); // param value 4B
            nbus->tx_buffer[5] = (uint8_t)(param_value & 0xFF);
            nbus->tx_buffer[6] = (uint8_t)((param_value >> 8) & 0xFF);
            nbus->tx_buffer[7] = (uint8_t)((param_value >> 16) & 0xFF);
            nbus->tx_buffer[8] = (uint8_t)((param_value >> 24) & 0xFF);
            nbus->tx_length += 5;
        }
        break;

        default:
            setErrorResponse(nbus, ILLEGAL_FUNCTION_PARAM);
            break;
        }
    }
    break;

    case CMD_DATA: {
        if (nbus->interface->device_ready())
        {
            uint8_t cnt = nbus->interface->getData(nbus->rx_buffer[RX_SA], &nbus->tx_buffer[TX_DT]);
            if (cnt == 0)
            {
                setErrorResponse(nbus, DEVICE_BUSY);
                break;
            }
            nbus->tx_length += cnt;
        }
        else
        {
            setErrorResponse(nbus, DEVICE_NOT_READY);
        }
    }
    break;

    case CMD_SENSOR_TYPE: {
        nbus->tx_buffer[TX_DT] = sensor_type;
        nbus->tx_length += 1;
        break;
    }

    case CMD_FORMAT: {
        nBus_sensorFormat_t format = nbus->interface->getSensorFormat(nbus->rx_buffer[RX_SA]);
        uint8_t *format_ptr = (uint8_t *)&format;
        nbus->tx_buffer[4] = nbus->rx_buffer[RX_SA]; // sensor address
        nbus->tx_buffer[5] = format_ptr[0];
        nbus->tx_buffer[6] = format_ptr[1];
        nbus->tx_buffer[7] = format_ptr[2];
        nbus->tx_length += 4;
    }
    break;

    default: {
        setErrorResponse(nbus, ILLEGAL_FUNCTION);
    }
    }
}

void nbus_slave_unicastToSensorSet(nBus_TypeDef *nbus)
{
    nBus_sensorType_t sensor_type =
        nbus->interface->getType(nbus->rx_buffer[RX_SA]); // nbus->rx_buffer[1] is whole address with type

    if (sensor_type == TYPE_UNKNOWN) // handle bad sensor address
    {
        setErrorResponse(nbus, ILLEGAL_DEVICE_ADDRESS);
        return;
    }

    switch (nbus->function_code.function)
    {
    case CMD_FIND: {
        if (nbus->rx_buffer[RX_DT] > 1) // if wrong parameter {0, 1}
        {
            setErrorResponse(nbus, ILLEGAL_FUNCTION_PARAM);
            break;
        }

        nbus->tx_buffer[TX_DT] = nbus->interface->find(nbus->rx_buffer[RX_DT]);
        nbus->tx_length += 1;
    }
    break;

    case CMD_PARAM: {

        uint8_t rx_payload = nbus->rx_length - RX_META;

        // empty or wrong number of parameters
        if (rx_payload == 0 || rx_payload % 5 != 0)
        {
            setErrorResponse(nbus, ILLEGAL_FUNCTION_PARAM);
            break;
        }
        else
        {
            uint8_t param_id;
            uint32_t param_value;

            uint8_t i = 0;

            for (; rx_payload > 0; rx_payload -= 5)
            {
                param_id = nbus->rx_buffer[3 + 5 * i];
                param_value = *(int32_t *)&nbus->rx_buffer[4 + 5 * i];

                nbus->tx_buffer[4 + 2 * i] = param_id;
                nbus->tx_buffer[5 + 2 * i] = nbus->interface->setParam(nbus->rx_buffer[RX_SA], param_id, param_value);
                nbus->tx_length += 2;
                i++;
            }
        }
    }
    break;

    case CMD_CALIBRATE: {
        nbus->hw_platform->led_on();
        nbus->tx_buffer[TX_DT] = nbus->interface->calibrate(nbus->rx_buffer[RX_SA]);
        nbus->tx_length += 1;
        nbus->hw_platform->led_off();
        break;
    }

    case CMD_DATA: {
        if (nbus->sensorInfo.type == 0) // if device is output only (sensor)
        {
            setErrorResponse(nbus, DEVICE_IS_READ_ONLY);
            break;
        }

        nbus->tx_length +=
            nbus->interface->setData(&nbus->rx_buffer[RX_DT], nbus->rx_length - RX_META, &nbus->tx_buffer[TX_DT]);
    }
    break;

    case CMD_STORE: {
        for (uint32_t i = 0; i < _NBUS_PARAM_COUNT; i++)
        {
            if (nbus->interface->hasParam(nbus->rx_buffer[RX_SA], i))
            {
                sensor_store_param(nbus, nbus->rx_buffer[RX_SA], i);
                // param_value = nbus->interface->getParam(nbus->sensorInfo.address,
                // all_params[i]);
                // nbus->memoryInterface->storeParam(nbus->sensorInfo.address,
                // all_params[i], param_value);
            }
        }

        nbus->tx_buffer[TX_DT] = 1;
        nbus->tx_length += 1;
    }
    break;

    default: {
        setErrorResponse(nbus, ILLEGAL_FUNCTION);
    }
    }
}

void sensor_store_param(nBus_TypeDef *nbus, uint8_t sensor_index, uint8_t param_name)
{
    uint32_t param_value = nbus->interface->getParam(sensor_index, param_name);
    nbus->memoryInterface->storeParam(sensor_index, param_name, param_value);
}

#endif