#include "nbus_slave.h"
#if MODULE_SLAVE == 1

void nbus_slave_unicastToModuleGet(nBus_TypeDef *nbus)
{
    switch (nbus->function_code.function)
    {
    case CMD_ECHO: {
        for (uint8_t i = 3; i < nbus->rx_length - 1; i++)
        {
            nbus->tx_buffer[i + 1] = nbus->rx_buffer[i];
        }
        nbus->tx_length += (nbus->rx_length - 4);
    }

    break;

    case CMD_PARAM: {
        if (nbus->rx_length >= 5) // get concrete param
        {
            if (nbus->interface->hasParam(0, (nBus_param_t)nbus->rx_buffer[3]) == 0) // handle non-existing param
            {
                setErrorResponse(nbus, PARAM_NOT_IMPLEMENTED);
                break;
            }

            nbus->tx_buffer[4] = nbus->rx_buffer[3]; // param id
            int32_t param_value = nbus->interface->getParam(0, (nBus_param_t)nbus->rx_buffer[3]);
            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;
        }
        else // get all params
        {
            nBus_param_t *params = nbus_interface_allParams();
            for (uint8_t i = 0; i < nbus_interface_allParamsCount(); i++)
            {
                if (nbus->interface->hasParam(0, params[i]))
                {
                    int32_t param_value = nbus->interface->getParam(0, params[i]);
                    nbus->tx_buffer[4 + 5 * i] = params[i]; // param id
                    // nbus->tx_buffer[5+2*i] =
                    // nbus->interface->getParam(0, params[i]);
                    nbus->tx_buffer[5 + 5 * i] = (uint8_t)(param_value & 0xFF);
                    nbus->tx_buffer[6 + 5 * i] = (uint8_t)((param_value >> 8) & 0xFF);
                    nbus->tx_buffer[7 + 5 * i] = (uint8_t)((param_value >> 16) & 0xFF);
                    nbus->tx_buffer[8 + 5 * i] = (uint8_t)((param_value >> 24) & 0xFF);

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

    case CMD_SENSOR_CNT: {
        nbus->tx_buffer[4] = nbus->interface->getSensorCount();
        nbus->tx_length += 1;
    }
    break;

    case CMD_DATA: {
        if (nbus->measure_active == MEASURE_RUNNING)
        {
            // response: sensor1_index:sensor1_data | sensor2_index:sensor2_data | ...
            uint8_t cnt = nbus->interface->getData(0, &nbus->tx_buffer[4]);
            if (cnt == 0)
            {
                setErrorResponse(nbus, DEVICE_BUSY);
            }
            nbus->tx_length += cnt;
        }
        else
        {
            setErrorResponse(nbus, DEVICE_NOT_READY);
        }
    }
    break;

    case CMD_SENSOR_TYPE: {
        // response: sensor1_index:sensor1_type | sensor2_index:sensor2_type | ...
        for (uint8_t i = 0; i < nbus->interface->getSensorCount(); i++)
        {
            nbus->tx_buffer[4 + 2 * i] = i;
            nbus->tx_buffer[5 + 2 * i] = nbus->interface->getType(i);
            nbus->tx_length += 2;
        }
    }
    break;

    case CMD_INFO: {
        switch (nbus->rx_buffer[3])
        {
        case INFO_GENERAL: {
            // name
            nbus->tx_buffer[4] = MODULE_NAME[0];
            nbus->tx_buffer[5] = MODULE_NAME[1];
            nbus->tx_buffer[6] = MODULE_NAME[2];
            nbus->tx_buffer[7] = MODULE_NAME[3];
            nbus->tx_buffer[8] = MODULE_NAME[4];
            nbus->tx_buffer[9] = MODULE_NAME[5];
            nbus->tx_buffer[10] = MODULE_NAME[6];
            nbus->tx_buffer[11] = MODULE_NAME[7];
            nbus->tx_length += 8;
            // type
            nbus->tx_buffer[12] = MODULE_TYPE[0];
            nbus->tx_buffer[13] = MODULE_TYPE[1];
            nbus->tx_buffer[14] = MODULE_TYPE[2];
            nbus->tx_length += 3;

            // Reference manual: Unique device ID registers
#if defined(STM32)
            uint32_t(*unique_id_3) = (uint32_t *)(0x1FF80064); // BASE address + 0x14 0ffset
#elif defined(ESP32) || defined(NRF)
            uint32_t unique_id_3 = 0x12345678;
#endif
            nbus->tx_buffer[15] = ((uint8_t *)(&unique_id_3))[0];
            nbus->tx_buffer[16] = ((uint8_t *)(&unique_id_3))[1];
            nbus->tx_buffer[17] = ((uint8_t *)(&unique_id_3))[2];
            nbus->tx_buffer[18] = ((uint8_t *)(&unique_id_3))[3];
            nbus->tx_length += 4;
            // fw version
            nbus->tx_buffer[19] = VERSION_FW[0];
            nbus->tx_buffer[20] = '.';
            nbus->tx_buffer[21] = VERSION_FW[1];
            nbus->tx_length += 3;
            // hw version
            nbus->tx_buffer[22] = VERSION_HW[0];
            nbus->tx_buffer[23] = '.';
            nbus->tx_buffer[24] = VERSION_HW[1];
            nbus->tx_length += 3;
            // module memory
            nbus->memoryInterface->getId(&nbus->tx_buffer[25]);
            nbus->tx_length += 8;
            break;
        }

        case INFO_FORMAT: {
            uint8_t sensor_cnt = nbus->interface->getSensorCount();

            for (int8_t i = 0; i < sensor_cnt; ++i)
            {
                nBus_sensorFormat_t format = nbus->interface->getSensorFormat(i + 1);
                uint8_t *format_ptr = (uint8_t *)&format;
                nbus->tx_buffer[4 * i + 4] = i + 1;
                nbus->tx_buffer[4 * i + 5] = format_ptr[0];
                nbus->tx_buffer[4 * i + 6] = format_ptr[1];
                nbus->tx_buffer[4 * i + 7] = format_ptr[2];
            }

            nbus->tx_length += 4 * sensor_cnt;
            break;
        }
        }
    }
    break;

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

void nbus_slave_unicastToModuleSet(nBus_TypeDef *nbus)
{
    switch (nbus->function_code.function)
    {
    case CMD_START: {
        nbus->measure_active = MEASURE_RUNNING;
        nbus->interface->start();
        nbus->hw_platform->led_on();
    }
    break;

    case CMD_STOP: {
        nbus->measure_active = MEASURE_STOPPED;
        nbus->interface->stop();
        nbus->hw_platform->led_off();
    }
    break;

    case CMD_PARAM: {
        // same as nbus_unicastToSensorSet
        int32_t param_value =
            nbus->rx_buffer[4] | nbus->rx_buffer[5] << 8 | nbus->rx_buffer[6] << 16 << nbus->rx_buffer[7] << 23;
        nBus_param_t p = nbus->interface->setParam(0, (nBus_param_t)nbus->rx_buffer[3], param_value);
        if (p == PARAM_NONE)
        {
            setErrorResponse(nbus, PARAM_NOT_IMPLEMENTED);
            break;
        }
        nbus->tx_buffer[4] = OK_CODE;
        nbus->tx_length += 1;
    }
    break;

    case CMD_DATA: {
        nbus->tx_buffer[4] = nbus->interface->setData(&nbus->rx_buffer[3]);
        if (nbus->tx_buffer[4] != OK_CODE)
        {
            nbus->function_code.error = 1;
        }

        nbus->tx_length += 1;
    }
    break;

    case CMD_SLEEP: {
        nbus->tx_buffer[4] = OK_CODE;
        nbus->tx_length += 1;
    }
    break;

    case CMD_WAKEUP: {
        nbus->tx_buffer[4] = OK_CODE;
        nbus->tx_length += 1;
    }
    break;

    case CMD_CALIBRATE: {
        nbus->hw_platform->led_on();
        nbus->tx_buffer[4] = nbus->interface->calibrate(0, 0, NULL);
        nbus->tx_length += 1;
        nbus->hw_platform->led_off();
    }
    break;

    case CMD_RESET: {
        // POZOR!  cas fomatovania: 0.3s (pri 1W pamati)
        memory_params_format();
        nbus->tx_buffer[4] = OK_CODE;
        nbus->tx_length += 1;
    }
    break;

        /*
          case CMD_STORE: {
            if (nbus->interface->store != NULL) {
              nbus->interface->store();
              nbus->tx_buffer[4] = OK_CODE;
            } else {
              nbus->tx_buffer[4] = ILLEGAL_FUNCTION;
            }

            nbus->tx_length += 1;
          } break;
        */
    default: {
        setErrorResponse(nbus, ILLEGAL_FUNCTION);
    }
    }
}

#endif