#include <OBSWConfig.h>
#include <devices/gpioIds.h>
#include <fsfw/datapool/PoolReadGuard.h>
#include <mission/devices/RadiationSensorHandler.h>
#include <mission/devices/devicedefinitions/GomspaceDefinitions.h>
#include <mission/devices/max1227.h>

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

RadiationSensorHandler::~RadiationSensorHandler() {}

void RadiationSensorHandler::doStartUp() {
  if (internalState == InternalState::OFF) {
    ReturnValue_t retval = stackHandler.deviceToOn(StackCommander::RAD_SENSOR);
    if (retval == BUSY) {
      return;
    }
    internalState = InternalState::POWER_SWITCHING;
  }
  if (internalState == InternalState::POWER_SWITCHING) {
    if (stackHandler.isSwitchOn()) {
      internalState = InternalState::SETUP;
    }
  }
  if (internalState == InternalState::CONFIGURED) {
    if (goToNormalMode) {
      setMode(MODE_NORMAL);
    } else {
      setMode(_MODE_TO_ON);
    }
  }
}

void RadiationSensorHandler::doShutDown() {
  ReturnValue_t retval = stackHandler.deviceToOff(StackCommander::RAD_SENSOR);
  if (retval == BUSY) {
    return;
  }
  internalState = InternalState::OFF;
  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 returnvalue::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 returnvalue::OK;
    }
    case (RAD_SENSOR::START_CONVERSION): {
      ReturnValue_t result = gpioIF->pullHigh(gpioIds::ENABLE_RADFET);
      if (result != returnvalue::OK) {
#if OBSW_VERBOSE_LEVEL >= 1
        sif::warning
            << "RadiationSensorHandler::buildCommandFromCommand: Pulling RADFET Enable pin "
               "high failed"
            << std::endl;
#endif
      }
      /* First the fifo will be reset here */
      cmdBuffer[0] = RAD_SENSOR::RESET_DEFINITION;
      cmdBuffer[1] = RAD_SENSOR::CONVERSION_DEFINITION;
      rawPacket = cmdBuffer;
      rawPacketLen = 2;
      return returnvalue::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 returnvalue::OK;
    }
    case RAD_SENSOR::ENABLE_DEBUG_OUTPUT: {
      printPeriodicData = true;
      rawPacketLen = 0;
      return returnvalue::OK;
    }
    case RAD_SENSOR::DISABLE_DEBUG_OUTPUT: {
      rawPacketLen = 0;
      printPeriodicData = false;
      return returnvalue::OK;
    }
    default:
      return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
  }
  return returnvalue::FAILED;
}

void RadiationSensorHandler::fillCommandAndReplyMap() {
  this->insertInCommandMap(RAD_SENSOR::WRITE_SETUP);
  this->insertInCommandMap(RAD_SENSOR::START_CONVERSION);
  this->insertInCommandMap(RAD_SENSOR::ENABLE_DEBUG_OUTPUT);
  this->insertInCommandMap(RAD_SENSOR::DISABLE_DEBUG_OUTPUT);
  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:
      *foundLen = remainingSize;
      return IGNORE_REPLY_DATA;
    case RAD_SENSOR::READ_CONVERSIONS: {
      ReturnValue_t result = gpioIF->pullLow(gpioIds::ENABLE_RADFET);
      if (result != returnvalue::OK) {
#if OBSW_VERBOSE_LEVEL >= 1
        sif::warning << "RadiationSensorHandler::scanForReply; Pulling RADFET Enale pin "
                        "low failed"
                     << std::endl;
#endif
      }
      break;
    }
    case RAD_SENSOR::ENABLE_DEBUG_OUTPUT:
    case RAD_SENSOR::DISABLE_DEBUG_OUTPUT:
      sif::info << "RadiationSensorHandler::scanForReply: " << remainingSize << std::endl;
      break;
    default:
      break;
  }

  *foundLen = remainingSize;

  return returnvalue::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);
      uint16_t tempRaw = ((packet[offset] & 0x0f) << 8) | packet[offset + 1];
      dataset.temperatureCelcius = max1227::getTemperature(tempRaw);
      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);
      dataset.setValidity(true, true);
      if (printPeriodicData) {
        sif::info << "Radiation sensor temperature: " << dataset.temperatureCelcius << " °C"
                  << std::dec << 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;
      }
      break;
    }
    default: {
      sif::debug << "RadiationSensorHandler::interpretDeviceReply: Unknown reply id" << std::endl;
      return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
    }
  }
  return returnvalue::OK;
}

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}));
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(dataset.getSid(), false, 20.0));
  return returnvalue::OK;
}

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

void RadiationSensorHandler::enablePeriodicDataPrint(bool enable) {
  this->printPeriodicData = enable;
}