#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_,
    PowerSwitchIF& mainLineSwitcher_, pcdu::Switches mainLineSwitch_, gpioId_t deplSA1,
    gpioId_t deplSA2)
    : SystemObject(setObjectId_),
      gpioDriverId(gpioDriverId_),
      gpioCookie(gpioCookie_),
      deplSA1(deplSA1),
      deplSA2(deplSA2),
      mainLineSwitcher(mainLineSwitcher_),
      mainLineSwitch(mainLineSwitch_),
      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 returnvalue::OK;
  }
  return returnvalue::OK;
}

ReturnValue_t SolarArrayDeploymentHandler::initialize() {
  ReturnValue_t result = SystemObject::initialize();
  if (result != returnvalue::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 != returnvalue::OK) {
    sif::error << "SolarArrayDeploymentHandler::initialize: Failed to initialize Gpio interface"
               << std::endl;
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }

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

  return returnvalue::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:
//      deploymentTransistorsOn();
//      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, DEPLOY_SOLAR_ARRAYS,
//                          MAIN_SWITCH_TIMEOUT_FAILURE);
//      stateMachine = WAIT_ON_DELOYMENT_COMMAND;
//    }
//  }
}

void SolarArrayDeploymentHandler::deploymentTransistorsOn() {
//  ReturnValue_t result = returnvalue::OK;
//  result = gpioInterface->pullHigh(deplSA1);
//  if (result != returnvalue::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, DEPLOY_SOLAR_ARRAYS, SWITCHING_DEPL_SA2_FAILED);
//    mainLineSwitcher.sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
//  }
//  result = gpioInterface->pullHigh(deplSA2);
//  if (result != returnvalue::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, 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 = returnvalue::OK;
////  if (deploymentCountdown.hasTimedOut()) {
////    actionHelper.finish(true, rememberCommanderId, DEPLOY_SOLAR_ARRAYS, returnvalue::OK);
////    result = gpioInterface->pullLow(deplSA1);
////    if (result != returnvalue::OK) {
////      sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
////                    " array deployment switch 1 low "
////                 << std::endl;
////    }
////    result = gpioInterface->pullLow(deplSA2);
////    if (result != returnvalue::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 != returnvalue::OK) {
    return;
  }

  result = actionHelper.handleActionMessage(&command);
  if (result == returnvalue::OK) {
    return;
  }
}

ReturnValue_t SolarArrayDeploymentHandler::executeAction(ActionId_t actionId,
                                                         MessageQueueId_t commandedBy,
                                                         const uint8_t* data, size_t size) {
  ReturnValue_t result;
//  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;
//  }
//  if (actionId != DEPLOY_SOLAR_ARRAYS) {
//    sif::error << "SolarArrayDeploymentHandler::executeAction: Received invalid command"
//               << std::endl;
//    result = COMMAND_NOT_SUPPORTED;
//  } else {
//    stateMachine = SWITCH_8V_ON;
//    rememberCommanderId = commandedBy;
//    result = returnvalue::OK;
//  }
  return result;
}

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