#include "FreshDeviceHandlerBase.h"

#include "fsfw/devicehandlers/DeviceHandlerFailureIsolation.h"
#include "fsfw/ipc/QueueFactory.h"
#include "fsfw/subsystem/helper.h"

FreshDeviceHandlerBase::FreshDeviceHandlerBase(DhbConfig config)
    : SystemObject(config.objectId),
      actionHelper(this, nullptr),
      modeHelper(this),
      healthHelper(this, getObjectId()),
      paramHelper(this),
      poolManager(this, nullptr),
      defaultFdirParent(config.defaultFdirParent) {
  auto mqArgs = MqArgs(config.objectId, static_cast<void*>(this));
  messageQueue = QueueFactory::instance()->createMessageQueue(
      config.msgQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
}
FreshDeviceHandlerBase::~FreshDeviceHandlerBase() {
  QueueFactory::instance()->deleteMessageQueue(messageQueue);
  if (not hasCustomFdir) {
    delete fdirInstance;
  }
}

[[nodiscard]] object_id_t FreshDeviceHandlerBase::getObjectId() const {
  return SystemObject::getObjectId();
}

ReturnValue_t FreshDeviceHandlerBase::performOperation(uint8_t opCode) {
  performDeviceOperationPreQueueHandling(opCode);
  handleQueue();
  performDeviceOperation(opCode);
  poolManager.performHkOperation();
  return returnvalue::OK;
}

ReturnValue_t FreshDeviceHandlerBase::performDeviceOperationPreQueueHandling(uint8_t opCode) {
  return returnvalue::OK;
}

void FreshDeviceHandlerBase::startTransition(Mode_t mode_, Submode_t submode_) {
  triggerEvent(CHANGING_MODE, mode_, submode_);
  // Complete mode transition immediately by default.
  setMode(mode_, submode_);
}

void FreshDeviceHandlerBase::setMode(Mode_t newMode, Submode_t newSubmode) {
  mode = newMode;
  submode = newSubmode;
  modeHelper.modeChanged(mode, submode);
  modeChanged(mode, submode);
  announceMode(false);
}

void FreshDeviceHandlerBase::setMode(Mode_t newMode) { setMode(newMode, submode); }

void FreshDeviceHandlerBase::getMode(Mode_t* mode_, Submode_t* submode_) {
  *mode_ = this->mode;
  *submode_ = this->submode;
}

void FreshDeviceHandlerBase::announceMode(bool recursive) {
  triggerEvent(MODE_INFO, mode, submode);
}

void FreshDeviceHandlerBase::modeChanged(Mode_t mode_, Submode_t submode_) {}

[[nodiscard]] MessageQueueId_t FreshDeviceHandlerBase::getCommandQueue() const {
  return messageQueue->getId();
}

ReturnValue_t FreshDeviceHandlerBase::handleQueue() {
  CommandMessage command;
  ReturnValue_t result;
  for (result = messageQueue->receiveMessage(&command); result == returnvalue::OK;
       result = messageQueue->receiveMessage(&command)) {
    result = actionHelper.handleActionMessage(&command);
    if (result == returnvalue::OK) {
      continue;
    }

    result = modeHelper.handleModeCommand(&command);
    if (result == returnvalue::OK) {
      continue;
    }

    result = healthHelper.handleHealthCommand(&command);
    if (result == returnvalue::OK) {
      continue;
    }

    result = paramHelper.handleParameterMessage(&command);
    if (result == returnvalue::OK) {
      continue;
    }

    result = poolManager.handleHousekeepingMessage(&command);
    if (result == returnvalue::OK) {
      continue;
    }

    result = handleCommandMessage(&command);
    if (result == returnvalue::OK) {
      continue;
    }
    command.setToUnknownCommand();
    messageQueue->reply(&command);
  }
  return result;
}

HasHealthIF::HealthState FreshDeviceHandlerBase::getHealth() { return healthHelper.getHealth(); }

const HasHealthIF* FreshDeviceHandlerBase::getOptHealthIF() const { return this; }

const HasModesIF& FreshDeviceHandlerBase::getModeIF() const { return *this; }

ModeTreeChildIF& FreshDeviceHandlerBase::getModeTreeChildIF() { return *this; }

ReturnValue_t FreshDeviceHandlerBase::connectModeTreeParent(HasModeTreeChildrenIF& parent) {
  return modetree::connectModeTreeParent(parent, *this, &healthHelper, modeHelper);
}

// Executable Overrides.
void FreshDeviceHandlerBase::setTaskIF(PeriodicTaskIF* task_) { executingTask = task_; }

// Pool Manager overrides.
LocalDataPoolManager* FreshDeviceHandlerBase::getHkManagerHandle() { return &poolManager; }

[[nodiscard]] uint32_t FreshDeviceHandlerBase::getPeriodicOperationFrequency() const {
  return this->executingTask->getPeriodMs();
}

ReturnValue_t FreshDeviceHandlerBase::initializeAfterTaskCreation() {
  return poolManager.initializeAfterTaskCreation();
}

ReturnValue_t FreshDeviceHandlerBase::setHealth(HasHealthIF::HealthState health) {
  // Assembly should handle commanding to OFF.
  healthHelper.setHealth(health);
  return returnvalue::OK;
}

void FreshDeviceHandlerBase::triggerEvent(Event event, uint32_t parameter1, uint32_t parameter2) {
  fdirInstance->triggerEvent(event, parameter1, parameter2);
}

void FreshDeviceHandlerBase::forwardEvent(Event event, uint32_t parameter1,
                                          uint32_t parameter2) const {
  fdirInstance->triggerEvent(event, parameter1, parameter2);
}

void FreshDeviceHandlerBase::setToExternalControl() { setHealth(HealthState::EXTERNAL_CONTROL); }

// System Object overrides.
ReturnValue_t FreshDeviceHandlerBase::initialize() {
  ReturnValue_t result = modeHelper.initialize();
  if (result != returnvalue::OK) {
    return result;
  }
  result = healthHelper.initialize();
  if (result != returnvalue::OK) {
    return result;
  }
  result = actionHelper.initialize(messageQueue);
  if (result != returnvalue::OK) {
    return result;
  }
  result = paramHelper.initialize();
  if (result != returnvalue::OK) {
    return result;
  }

  result = poolManager.initialize(messageQueue);
  if (result != returnvalue::OK) {
    return result;
  }
  if (fdirInstance == nullptr) {
    hasCustomFdir = false;
    fdirInstance = new DeviceHandlerFailureIsolation(getObjectId(), defaultFdirParent);
  }

  result = fdirInstance->initialize();
  if (result != returnvalue::OK) {
    return result;
  }
  return SystemObject::initialize();
}

ReturnValue_t FreshDeviceHandlerBase::getParameter(uint8_t domainId, uint8_t uniqueId,
                                                   ParameterWrapper* parameterWrapper,
                                                   const ParameterWrapper* newValues,
                                                   uint16_t startAtIndex) {
  ReturnValue_t result =
      fdirInstance->getParameter(domainId, uniqueId, parameterWrapper, newValues, startAtIndex);
  if (result != INVALID_DOMAIN_ID) {
    return result;
  }
  return INVALID_DOMAIN_ID;
}