#include "nbus_slave.h"

#if MODULE_SLAVE == 1

void nbus_slave_unicastToSensorGet(nBus_TypeDef *nbus) {
  switch (nbus->function_code.function) {
  case CMD_SENSOR_TYPE: {
    nBus_sensorType_t t = nbus->interface->getType(nbus->sensorInfo.address);
    if (t == TYPE_UNKNOWN) {
      setErrorResponse(nbus, ILLEGAL_DEVICE_ADDRESS);
      break;
    }
    nbus->tx_buffer[4] = t;
    nbus->tx_length += 1;
  } break;

  case CMD_PARAM: {
    if (nbus->rx_length >= 5) {
      if (nbus->interface->hasParam(nbus->sensorInfo.address,
                                    (nBus_param_t)nbus->rx_buffer[3]) == 0) {
        setErrorResponse(nbus, PARAM_NOT_IMPLEMENTED);
        break;
      }
      nbus->tx_buffer[4] = nbus->rx_buffer[3];
      int32_t param_value = nbus->interface->getParam(
          nbus->sensorInfo.address, (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 {
      nBus_param_t *params = nbus_interface_allParams();
      for (uint8_t i = 0; i < nbus_interface_allParamsCount(); i++) {
        if (nbus->interface->hasParam(nbus->sensorInfo.address, params[i])) {
          int32_t param_value =
              nbus->interface->getParam(nbus->sensorInfo.address, params[i]);
          nbus->tx_buffer[4 + 5 * i] = params[i];
          // nbus->tx_buffer[5+2*i] =
          // nbus->interface->getParam(nbus->sensorInfo.address, 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_DATA: {
    if (nbus->measure_active == MEASURE_RUNNING) {
      uint8_t cnt = nbus->interface->getData(nbus->sensorInfo.address,
                                             &nbus->tx_buffer[4]);
      if (cnt == 0) {
        setErrorResponse(nbus, DEVICE_BUSY);
        // return;
      }
      nbus->tx_length += cnt;
    } else {
      setErrorResponse(nbus, DEVICE_NOT_READY);
    }

  } break;

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

void nbus_slave_unicastToSensorSet(nBus_TypeDef *nbus) {
  switch (nbus->function_code.function) {
  case CMD_PARAM: {
    if (!nbus->interface->hasParam(nbus->sensorInfo.address,
                                   (nBus_param_t)nbus->rx_buffer[3])) {
      setErrorResponse(nbus, PARAM_NOT_IMPLEMENTED);
      break;
    }

    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(nbus->sensorInfo.address,
                                               (nBus_param_t)nbus->rx_buffer[3],
                                               param_value);
    if (p == PARAM_NONE) {
      setErrorResponse(nbus, ILLEGAL_DATA_VALUE);
      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;

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

#endif