#include <OBSWConfig.h>
#include <fsfw/datapool/PoolReadGuard.h>
#include <mission/tcs/Tmp1075Definitions.h>
#include <mission/tcs/Tmp1075Handler.h>

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

Tmp1075Handler::~Tmp1075Handler() {}

void Tmp1075Handler::doStartUp() { setMode(MODE_ON); }

void Tmp1075Handler::doShutDown() {
  communicationStep = CommunicationStep::START_ADC_CONVERSION;
  PoolReadGuard pg(&dataset);
  dataset.setValidity(false, true);
  setMode(_MODE_POWER_DOWN);
}

ReturnValue_t Tmp1075Handler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
  if (communicationStep == CommunicationStep::START_ADC_CONVERSION) {
    *id = TMP1075::START_ADC_CONVERSION;
    communicationStep = CommunicationStep::GET_TEMPERATURE;
    return buildCommandFromCommand(*id, NULL, 0);
  } else {
    *id = TMP1075::GET_TEMP;
    communicationStep = CommunicationStep::START_ADC_CONVERSION;
    return buildCommandFromCommand(*id, NULL, 0);
  }
  return returnvalue::OK;
}

ReturnValue_t Tmp1075Handler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
  return NOTHING_TO_SEND;
}

ReturnValue_t Tmp1075Handler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
                                                      const uint8_t *commandData,
                                                      size_t commandDataLen) {
  switch (deviceCommand) {
    case (TMP1075::START_ADC_CONVERSION): {
      std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
      prepareAdcConversionCommand();
      rawPacket = cmdBuffer;
      rawPacketLen = TMP1075::CFGR_CMD_SIZE;
      return returnvalue::OK;
    }
    case (TMP1075::GET_TEMP): {
      std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
      prepareGetTempCommand();
      rawPacket = cmdBuffer;
      rawPacketLen = TMP1075::POINTER_REG_SIZE;
      rememberCommandId = TMP1075::GET_TEMP;
      return returnvalue::OK;
    }
    default:
      return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
  }
  return returnvalue::FAILED;
}

void Tmp1075Handler::fillCommandAndReplyMap() {
  this->insertInCommandMap(TMP1075::START_ADC_CONVERSION);
  this->insertInCommandAndReplyMap(TMP1075::GET_TEMP, 1, &dataset, TMP1075::GET_TEMP_REPLY_SIZE);
}

ReturnValue_t Tmp1075Handler::scanForReply(const uint8_t *start, size_t remainingSize,
                                           DeviceCommandId_t *foundId, size_t *foundLen) {
  switch (rememberCommandId) {
    case (TMP1075::GET_TEMP):
      *foundId = TMP1075::GET_TEMP;
      *foundLen = TMP1075::GET_TEMP_REPLY_SIZE;
      rememberCommandId = TMP1075::NONE;
      break;
    default:
      return IGNORE_REPLY_DATA;
  }
  return returnvalue::OK;
}

ReturnValue_t Tmp1075Handler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {
  switch (id) {
    case TMP1075::GET_TEMP: {
      // Convert 12 bit MSB first raw temperature to 16 bit first.
      int16_t tempValueRaw = static_cast<uint16_t>((packet[0] << 8) | packet[1]) >> 4;
      // Sign extension to 16 bits: If the sign bit is set, fill up with ones on the left.
      tempValueRaw = (packet[0] & 0x80) ? (tempValueRaw | 0xF000) : tempValueRaw;
      // 0.0625 is the sensor sensitivity.
      float tempValue = ((static_cast<float>(tempValueRaw)) * 0.0625);
#if OBSW_DEBUG_TMP1075 == 1
      sif::info << "Tmp1075 with object id: 0x" << std::hex << getObjectId()
                << ": Temperature: " << tempValue << " °C" << std::endl;
#endif
      ReturnValue_t result = dataset.read();
      if (result == returnvalue::OK) {
        dataset.temperatureCelcius = tempValue;
        dataset.setValidity(true, true);
        dataset.commit();
      } else {
        dataset.setValidity(false, true);
      }
      break;
    }

    default: {
      return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
    }
  }
  return returnvalue::OK;
}

void Tmp1075Handler::setNormalDatapoolEntriesInvalid() {}

void Tmp1075Handler::prepareAdcConversionCommand() {
  cmdBuffer[0] = TMP1075::CFGR_ADDR;
  cmdBuffer[1] = TMP1075::ONE_SHOT_MODE >> 8;
  cmdBuffer[2] = TMP1075::ONE_SHOT_MODE & 0xFF;
}

void Tmp1075Handler::prepareGetTempCommand() { cmdBuffer[0] = TMP1075::TEMP_REG_ADDR; }

uint32_t Tmp1075Handler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 500; }

ReturnValue_t Tmp1075Handler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                      LocalDataPoolManager &poolManager) {
  localDataPoolMap.emplace(TMP1075::TEMPERATURE_C_TMP1075, new PoolEntry<float>({0.0}));
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(dataset.getSid(), false, 30.0));
  return returnvalue::OK;
}

void Tmp1075Handler::setModeNormal() { setMode(_MODE_TO_NORMAL); }

ReturnValue_t Tmp1075Handler::setHealth(HealthState health) {
  if (health != FAULTY and health != PERMANENT_FAULTY and health != HEALTHY and
      health != EXTERNAL_CONTROL) {
    return returnvalue::FAILED;
  }
  return returnvalue::OK;
}