#include "SusHandler.h"

#include <mission/tcs/max1227.h>

#include <cmath>

#include "fsfw/datapool/PoolReadGuard.h"

SusHandler::SusHandler(object_id_t objectId, uint8_t susIdx, object_id_t deviceCommunication,
                       CookieIF *comCookie)
    : DeviceHandlerBase(objectId, deviceCommunication, comCookie), dataset(this), susIdx(susIdx) {}

SusHandler::~SusHandler() = default;

void SusHandler::doStartUp() {
  if (internalState != InternalState::STARTUP) {
    commandExecuted = false;
    updatePeriodicReply(true, REPLY);
    internalState = InternalState::STARTUP;
  }
  if (internalState == InternalState::STARTUP) {
    if (commandExecuted) {
      if (waitingForRecovery) {
        waitingForRecovery = false;
      }
      setMode(MODE_ON);
      internalState = InternalState::NONE;
      commandExecuted = false;
    }
  }
}

void SusHandler::doShutDown() {
  if (internalState != InternalState::SHUTDOWN) {
    PoolReadGuard pg(&dataset);
    dataset.tempC = thermal::INVALID_TEMPERATURE;
    dataset.setValidity(false, true);
    internalState = InternalState::SHUTDOWN;
    commandExecuted = false;
  }
  if (internalState == InternalState::SHUTDOWN and commandExecuted) {
    updatePeriodicReply(false, REPLY);
    commandExecuted = false;
    internalState = InternalState::NONE;
    setMode(MODE_OFF);
  }
}

ReturnValue_t SusHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
  if (internalState == InternalState::STARTUP) {
    *id = REQUEST;
    return prepareRequest(acs::SimpleSensorMode::NORMAL);
  } else if (internalState == InternalState::SHUTDOWN) {
    *id = REQUEST;
    return prepareRequest(acs::SimpleSensorMode::OFF);
  }
  return NOTHING_TO_SEND;
}

ReturnValue_t SusHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
  *id = REQUEST;
  return prepareRequest(acs::SimpleSensorMode::NORMAL);
}

ReturnValue_t SusHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
                                                  const uint8_t *commandData,
                                                  size_t commandDataLen) {
  return NOTHING_TO_SEND;
}

ReturnValue_t SusHandler::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId,
                                       size_t *foundLen) {
  if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) {
    return IGNORE_FULL_PACKET;
  }
  if (len != sizeof(acs::SusReply)) {
    *foundLen = len;
    return returnvalue::FAILED;
  }
  *foundId = REPLY;
  *foundLen = len;
  if (internalState == InternalState::SHUTDOWN) {
    commandExecuted = true;
  }
  return returnvalue::OK;
}
ReturnValue_t SusHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {
  const auto *reply = reinterpret_cast<const acs::SusReply *>(packet);
  if (reply->dataWasSet) {
    if (internalState == InternalState::STARTUP) {
      commandExecuted = true;
    }
    PoolReadGuard pg(&dataset);
    // Simple FDIR variant to make the handler more robust to invalid messages which
    // appear sometimes for the SUS device: Allow invalid message up to a certain threshold
    // before triggering FDIR reactions.
    if (reply->tempRaw == 0xfff and not waitingForRecovery) {
      if (invalidMsgCounter == 0) {
        triggerEvent(TEMPERATURE_ALL_ONES_START);
      } else if (invalidMsgCounter == susMax1227::MAX_INVALID_MSG_COUNT) {
        triggerEvent(DeviceHandlerIF::DEVICE_WANTS_HARD_REBOOT);
        waitingForRecovery = true;
      } else {
        invalidMsgCounter++;
      }
      dataset.setValidity(false, true);
      dataset.tempC = thermal::INVALID_TEMPERATURE;
      std::memset(dataset.channels.value, 0, sizeof(dataset.channels.value));
    } else {
      dataset.setValidity(true, true);
      dataset.tempC = max1227::getTemperature(reply->tempRaw);
      std::memcpy(dataset.channels.value, reply->channelsRaw, sizeof(reply->channelsRaw));
    }
  }
  return returnvalue::OK;
}

void SusHandler::fillCommandAndReplyMap() {
  insertInCommandMap(REQUEST);
  insertInReplyMap(REPLY, 5, nullptr, 0, true);
}

void SusHandler::setToGoToNormalMode(bool enable) { this->goToNormalMode = enable; }

uint32_t SusHandler::getTransitionDelayMs(Mode_t from, Mode_t to) { return transitionDelay; }

void SusHandler::modeChanged(void) { internalState = InternalState::NONE; }

ReturnValue_t SusHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                  LocalDataPoolManager &poolManager) {
  localDataPoolMap.emplace(susMax1227::TEMPERATURE_C, &tempC);
  localDataPoolMap.emplace(susMax1227::CHANNEL_VEC, &channelVec);
  poolManager.subscribeForDiagPeriodicPacket(
      subdp::DiagnosticsHkPeriodicParams(dataset.getSid(), false, 5.0));
  return returnvalue::OK;
}

ReturnValue_t SusHandler::prepareRequest(acs::SimpleSensorMode mode) {
  request.mode = mode;
  rawPacket = reinterpret_cast<uint8_t *>(&request);
  rawPacketLen = sizeof(acs::SusRequest);
  return returnvalue::OK;
}

LocalPoolDataSetBase *SusHandler::getDataSetHandle(sid_t sid) {
  if (sid == dataset.getSid()) {
    return &dataset;
  }
  return nullptr;
}