#include "SolarArrayDeploymentHandler.h"

#include <devices/gpioIds.h>
#include <fsfw/ipc/QueueFactory.h>
#include <fsfw/objectmanager/ObjectManager.h>
#include <fsfw_hal/common/gpio/GpioCookie.h>

SolarArrayDeploymentHandler::SolarArrayDeploymentHandler(
    object_id_t setObjectId_, object_id_t gpioDriverId_, CookieIF* gpioCookie_,
    object_id_t mainLineSwitcherObjectId_, pcdu::Switches mainLineSwitch_, gpioId_t deplSA1,
    gpioId_t deplSA2, uint32_t burnTimeMs)
    : SystemObject(setObjectId_),
      gpioDriverId(gpioDriverId_),
      gpioCookie(gpioCookie_),
      mainLineSwitcherObjectId(mainLineSwitcherObjectId_),
      mainLineSwitch(mainLineSwitch_),
      deplSA1(deplSA1),
      deplSA2(deplSA2),
      burnTimeMs(burnTimeMs),
      actionHelper(this, nullptr) {
  auto mqArgs = MqArgs(setObjectId_, static_cast<void*>(this));
  commandQueue = QueueFactory::instance()->createMessageQueue(
      cmdQueueSize, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
}

SolarArrayDeploymentHandler::~SolarArrayDeploymentHandler() {}

ReturnValue_t SolarArrayDeploymentHandler::performOperation(uint8_t operationCode) {
  if (operationCode == DeviceHandlerIF::PERFORM_OPERATION) {
    handleStateMachine();
    return RETURN_OK;
  }
  return RETURN_OK;
}

ReturnValue_t SolarArrayDeploymentHandler::initialize() {
  ReturnValue_t result = SystemObject::initialize();
  if (result != RETURN_OK) {
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }

  gpioInterface = ObjectManager::instance()->get<GpioIF>(gpioDriverId);
  if (gpioInterface == nullptr) {
    sif::error << "SolarArrayDeploymentHandler::initialize: Invalid Gpio interface." << std::endl;
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }

  result = gpioInterface->addGpios(dynamic_cast<GpioCookie*>(gpioCookie));
  if (result != RETURN_OK) {
    sif::error << "SolarArrayDeploymentHandler::initialize: Failed to initialize Gpio interface"
               << std::endl;
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }

  if (mainLineSwitcherObjectId != objects::NO_OBJECT) {
    mainLineSwitcher = ObjectManager::instance()->get<PowerSwitchIF>(mainLineSwitcherObjectId);
    if (mainLineSwitcher == nullptr) {
      sif::error
          << "SolarArrayDeploymentHandler::initialize: Main line switcher failed to fetch object"
          << "from object ID." << std::endl;
      return ObjectManagerIF::CHILD_INIT_FAILED;
    }
  }

  result = actionHelper.initialize(commandQueue);
  if (result != RETURN_OK) {
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }

  return RETURN_OK;
}

void SolarArrayDeploymentHandler::handleStateMachine() {
  switch (stateMachine) {
    case WAIT_ON_DELOYMENT_COMMAND:
      readCommandQueue();
      break;
    case SWITCH_8V_ON:
      mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_ON);
      mainSwitchCountdown.setTimeout(mainLineSwitcher->getSwitchDelayMs());
      stateMachine = WAIT_ON_8V_SWITCH;
      break;
    case WAIT_ON_8V_SWITCH:
      performWaitOn8VActions();
      break;
    case SWITCH_DEPL_GPIOS:
      switchDeploymentTransistors();
      break;
    case WAIT_ON_DEPLOYMENT_FINISH:
      handleDeploymentFinish();
      break;
    case WAIT_FOR_MAIN_SWITCH_OFF:
      if (mainLineSwitcher->getSwitchState(mainLineSwitch) == PowerSwitchIF::SWITCH_OFF) {
        stateMachine = WAIT_ON_DELOYMENT_COMMAND;
      } else if (mainSwitchCountdown.hasTimedOut()) {
        triggerEvent(MAIN_SWITCH_OFF_TIMEOUT);
        sif::error << "SolarArrayDeploymentHandler::handleStateMachine: Failed to switch main"
                   << " switch off" << std::endl;
        stateMachine = WAIT_ON_DELOYMENT_COMMAND;
      }
      break;
    default:
      sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Invalid state" << std::endl;
      break;
  }
}

void SolarArrayDeploymentHandler::performWaitOn8VActions() {
  if (mainLineSwitcher->getSwitchState(mainLineSwitch) == PowerSwitchIF::SWITCH_ON) {
    stateMachine = SWITCH_DEPL_GPIOS;
  } else {
    if (mainSwitchCountdown.hasTimedOut()) {
      triggerEvent(MAIN_SWITCH_ON_TIMEOUT);
      actionHelper.finish(false, rememberCommanderId, static_cast<DeviceCommandId_t>(SolarArrayDeploymentCommands::DEPLOY_SOLAR_ARRAYS),
                          MAIN_SWITCH_TIMEOUT_FAILURE);
      stateMachine = WAIT_ON_DELOYMENT_COMMAND;
    }
  }
}

void SolarArrayDeploymentHandler::switchDeploymentTransistors() {
  ReturnValue_t result = RETURN_OK;
  result = gpioInterface->pullHigh(deplSA1);
  if (result != RETURN_OK) {
    sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
                  " array deployment switch 1 high "
               << std::endl;
    /* If gpio switch high failed, state machine is reset to wait for a command reinitiating
     * the deployment sequence. */
    stateMachine = WAIT_ON_DELOYMENT_COMMAND;
    triggerEvent(DEPL_SA1_GPIO_SWTICH_ON_FAILED);
    actionHelper.finish(false, rememberCommanderId, static_cast<DeviceCommandId_t>(SolarArrayDeploymentCommands::DEPLOY_SOLAR_ARRAYS), SWITCHING_DEPL_SA1_FAILED);
    mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
  }
  result = gpioInterface->pullHigh(deplSA2);
  if (result != RETURN_OK) {
    sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
                  " array deployment switch 2 high "
               << std::endl;
    stateMachine = WAIT_ON_DELOYMENT_COMMAND;
    triggerEvent(DEPL_SA2_GPIO_SWTICH_ON_FAILED);
    actionHelper.finish(false, rememberCommanderId, static_cast<DeviceCommandId_t>(SolarArrayDeploymentCommands::DEPLOY_SOLAR_ARRAYS), SWITCHING_DEPL_SA2_FAILED);
    mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
  }
  deploymentCountdown.setTimeout(burnTimeMs);
  stateMachine = WAIT_ON_DEPLOYMENT_FINISH;
}

void SolarArrayDeploymentHandler::handleDeploymentFinish() {
  ReturnValue_t result = RETURN_OK;
  if (deploymentCountdown.hasTimedOut()) {
    actionHelper.finish(true, rememberCommanderId, static_cast<DeviceCommandId_t>(SolarArrayDeploymentCommands::DEPLOY_SOLAR_ARRAYS), RETURN_OK);
    result = gpioInterface->pullLow(deplSA1);
    if (result != RETURN_OK) {
      sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
                    " array deployment switch 1 low "
                 << std::endl;
    }
    result = gpioInterface->pullLow(deplSA2);
    if (result != RETURN_OK) {
      sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
                    " array deployment switch 2 low "
                 << std::endl;
    }
    mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
    mainSwitchCountdown.setTimeout(mainLineSwitcher->getSwitchDelayMs());
    stateMachine = WAIT_FOR_MAIN_SWITCH_OFF;
  }
}

void SolarArrayDeploymentHandler::readCommandQueue() {
  CommandMessage command;
  ReturnValue_t result = commandQueue->receiveMessage(&command);
  if (result != RETURN_OK) {
    return;
  }

  result = actionHelper.handleActionMessage(&command);
  if (result == RETURN_OK) {
    return;
  }
}

ReturnValue_t SolarArrayDeploymentHandler::executeAction(Action* action) {
  if (stateMachine != WAIT_ON_DELOYMENT_COMMAND) {
    sif::error << "SolarArrayDeploymentHandler::executeAction: Received command while not in"
               << "waiting-on-command-state" << std::endl;
    return DEPLOYMENT_ALREADY_EXECUTING;
  }
  //delegates to SolarArrayDeploymentHandler::handleAction()
  return action->handle();
}

ReturnValue_t SolarArrayDeploymentHandler::handleAction(SolarArrayDeploymentAction* action) {
  stateMachine = SWITCH_8V_ON;
  rememberCommanderId = action->commandedBy;
  return RETURN_OK;
}

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