#include <fsfw/datapool/PoolReadGuard.h>
#include <mission/controller/PowerController.h>

PowerController::PowerController(object_id_t objectId, bool enableHkSets)
    : ExtendedControllerBase(objectId), enableHkSets(enableHkSets), parameterHelper(this) {}

ReturnValue_t PowerController::initialize() {
  ReturnValue_t result = parameterHelper.initialize();
  if (result != returnvalue::OK) {
    return result;
  }
  sif::debug << "Rush B, no stop" << std::endl;
  return ExtendedControllerBase::initialize();
}

ReturnValue_t PowerController::handleCommandMessage(CommandMessage *message) {
  ReturnValue_t result = actionHelper.handleActionMessage(message);
  if (result == returnvalue::OK) {
    return result;
  }
  result = parameterHelper.handleParameterMessage(message);
  if (result == returnvalue::OK) {
    return result;
  }
  return result;
}

MessageQueueId_t PowerController::getCommandQueue() const { return commandQueue->getId(); }

ReturnValue_t PowerController::getParameter(uint8_t domainId, uint8_t parameterId,
                                            ParameterWrapper *parameterWrapper,
                                            const ParameterWrapper *newValues,
                                            uint16_t startAtIndex) {
  return returnvalue::FAILED;
}

void PowerController::performControlOperation() {
  switch (internalState) {
    case InternalState::STARTUP: {
      initialCountdown.resetTimer();
      internalState = InternalState::INITIAL_DELAY;
      return;
    }
    case InternalState::INITIAL_DELAY: {
      if (initialCountdown.hasTimedOut()) {
        internalState = InternalState::READY;
      }
      return;
    }
    case InternalState::READY: {
      if (mode != MODE_OFF) {
        // do something
      }
      break;
    }
    default:
      break;
  }
}

ReturnValue_t PowerController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                       LocalDataPoolManager &poolManager) {
  return returnvalue::OK;
}

LocalPoolDataSetBase *PowerController::getDataSetHandle(sid_t sid) {
  switch (sid.ownerSetId) {
    default:
      return nullptr;
  }
  return nullptr;
}

ReturnValue_t PowerController::checkModeCommand(Mode_t mode, Submode_t submode,
                                                uint32_t *msToReachTheMode) {
  if (mode == MODE_OFF) {
    if (submode == SUBMODE_NONE) {
      return returnvalue::OK;
    } else {
      return INVALID_SUBMODE;
    }
  }
  return INVALID_MODE;
}