#include <OBSWConfig.h>
#include <mission/devices/Tmp1075Handler.h>
#include <mission/devices/devicedefinitions/Tmp1075Definitions.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() {
  if (getMode() == _MODE_START_UP) {
    setMode(MODE_ON);
  }
}

void Tmp1075Handler::doShutDown() {
  communicationStep = CommunicationStep::START_ADC_CONVERSION;
  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: {
      int16_t tempValueRaw = 0;
      tempValueRaw = packet[0] << 4 | packet[1] >> 4;
      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); }