#include <OBSWConfig.h>
#include <fsfw/datapool/PoolReadGuard.h>
#include <mission/devices/RadiationSensorHandler.h>

RadiationSensorHandler::RadiationSensorHandler(object_id_t objectId, object_id_t comIF,
                                               CookieIF *comCookie)
    : DeviceHandlerBase(objectId, comIF, comCookie), dataset(this) {
  if (comCookie == NULL) {
    sif::error << "RadiationSensorHandler: Invalid com cookie" << std::endl;
  }
}

RadiationSensorHandler::~RadiationSensorHandler() {}

void RadiationSensorHandler::doStartUp() {
  if (internalState == InternalState::CONFIGURED) {
    if (goToNormalMode) {
      setMode(MODE_NORMAL);
    }

    else {
      setMode(_MODE_TO_ON);
    }
  }
}

void RadiationSensorHandler::doShutDown() { setMode(_MODE_POWER_DOWN); }

ReturnValue_t RadiationSensorHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
  switch (communicationStep) {
    case CommunicationStep::START_CONVERSION: {
      *id = RAD_SENSOR::START_CONVERSION;
      communicationStep = CommunicationStep::READ_CONVERSIONS;
      break;
    }
    case CommunicationStep::READ_CONVERSIONS: {
      *id = RAD_SENSOR::READ_CONVERSIONS;
      communicationStep = CommunicationStep::START_CONVERSION;
      break;
    }
    default: {
      sif::debug << "RadiationSensorHandler::buildNormalDeviceCommand: Unknown communication "
                 << "step" << std::endl;
      return HasReturnvaluesIF::RETURN_OK;
    }
  }
  return buildCommandFromCommand(*id, nullptr, 0);
}

ReturnValue_t RadiationSensorHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
  if (internalState == InternalState::SETUP) {
    *id = RAD_SENSOR::WRITE_SETUP;
  } else {
    return NOTHING_TO_SEND;
  }
  return buildCommandFromCommand(*id, nullptr, 0);
}

ReturnValue_t RadiationSensorHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
                                                              const uint8_t *commandData,
                                                              size_t commandDataLen) {
  switch (deviceCommand) {
    case (RAD_SENSOR::WRITE_SETUP): {
      cmdBuffer[0] = RAD_SENSOR::SETUP_DEFINITION;
      rawPacket = cmdBuffer;
      rawPacketLen = 1;
      internalState = InternalState::CONFIGURED;
      return RETURN_OK;
    }
    case (RAD_SENSOR::START_CONVERSION): {
      /* First the fifo will be reset here */
      cmdBuffer[0] = RAD_SENSOR::RESET_DEFINITION;
      cmdBuffer[1] = RAD_SENSOR::CONVERSION_DEFINITION;
      rawPacket = cmdBuffer;
      rawPacketLen = 2;
      return RETURN_OK;
    }
    case (RAD_SENSOR::READ_CONVERSIONS): {
      cmdBuffer[0] = RAD_SENSOR::DUMMY_BYTE;
      std::memset(cmdBuffer, RAD_SENSOR::DUMMY_BYTE, RAD_SENSOR::READ_SIZE);
      rawPacket = cmdBuffer;
      rawPacketLen = RAD_SENSOR::READ_SIZE;
      return RETURN_OK;
    }
      //    case(RAD_SENSOR::AIN0_AND_TMP_CONVERSION): {
      //        /* First the fifo will be reset here */
      //        cmdBuffer[0] = RAD_SENSOR::RESET_DEFINITION;
      //        cmdBuffer[1] = RAD_SENSOR::CONVERSION_DEFINITION;
      //        rawPacket = cmdBuffer;
      //        rawPacketLen = 2;
      //        return RETURN_OK;
      //    }
    default:
      return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
  }
  return HasReturnvaluesIF::RETURN_FAILED;
}

void RadiationSensorHandler::fillCommandAndReplyMap() {
  this->insertInCommandMap(RAD_SENSOR::WRITE_SETUP);
  this->insertInCommandMap(RAD_SENSOR::START_CONVERSION);
  this->insertInCommandAndReplyMap(RAD_SENSOR::READ_CONVERSIONS, 1, &dataset,
                                   RAD_SENSOR::READ_SIZE);
}

ReturnValue_t RadiationSensorHandler::scanForReply(const uint8_t *start, size_t remainingSize,
                                                   DeviceCommandId_t *foundId, size_t *foundLen) {
  *foundId = this->getPendingCommand();

  switch (*foundId) {
    case RAD_SENSOR::START_CONVERSION:
    case RAD_SENSOR::WRITE_SETUP:
      return IGNORE_REPLY_DATA;
    default:
      break;
  }

  *foundLen = remainingSize;

  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t RadiationSensorHandler::interpretDeviceReply(DeviceCommandId_t id,
                                                           const uint8_t *packet) {
  switch (id) {
    case RAD_SENSOR::READ_CONVERSIONS: {
      uint8_t offset = 0;
      PoolReadGuard readSet(&dataset);
      dataset.temperatureCelcius = (*(packet + offset) << 8 | *(packet + offset + 1)) * 0.125;
      offset += 2;
      dataset.ain0 = (*(packet + offset) << 8 | *(packet + offset + 1));
      offset += 2;
      dataset.ain1 = (*(packet + offset) << 8 | *(packet + offset + 1));
      offset += 6;
      dataset.ain4 = (*(packet + offset) << 8 | *(packet + offset + 1));
      offset += 2;
      dataset.ain5 = (*(packet + offset) << 8 | *(packet + offset + 1));
      offset += 2;
      dataset.ain6 = (*(packet + offset) << 8 | *(packet + offset + 1));
      offset += 2;
      dataset.ain7 = (*(packet + offset) << 8 | *(packet + offset + 1));

#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_RAD_SENSOR
      sif::info << "Radiation sensor temperature: " << dataset.temperatureCelcius << " °C"
                << std::endl;
      sif::info << "Radiation sensor ADC value channel 0: " << dataset.ain0 << std::endl;
      sif::info << "Radiation sensor ADC value channel 1: " << dataset.ain1 << std::endl;
      sif::info << "Radiation sensor ADC value channel 4: " << dataset.ain4 << std::endl;
      sif::info << "Radiation sensor ADC value channel 5: " << dataset.ain5 << std::endl;
      sif::info << "Radiation sensor ADC value channel 6: " << dataset.ain6 << std::endl;
      sif::info << "Radiation sensor ADC value channel 7: " << dataset.ain7 << std::endl;
#endif
      break;
    }
    default: {
      sif::debug << "RadiationSensorHandler::interpretDeviceReply: Unknown reply id" << std::endl;
      return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
    }
  }
  return HasReturnvaluesIF::RETURN_OK;
}

void RadiationSensorHandler::setNormalDatapoolEntriesInvalid() {}

uint32_t RadiationSensorHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
  return 5000;
}

ReturnValue_t RadiationSensorHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                              LocalDataPoolManager &poolManager) {
  localDataPoolMap.emplace(RAD_SENSOR::TEMPERATURE_C, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(RAD_SENSOR::AIN0, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(RAD_SENSOR::AIN1, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(RAD_SENSOR::AIN4, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(RAD_SENSOR::AIN5, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(RAD_SENSOR::AIN6, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(RAD_SENSOR::AIN7, new PoolEntry<uint16_t>({0}));
  return HasReturnvaluesIF::RETURN_OK;
}

void RadiationSensorHandler::setToGoToNormalModeImmediately() { this->goToNormalMode = true; }