#include "SolarArrayDeploymentHandler.h"

#include <filesystem>
#include <fstream>
#include <iostream>

#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_,
                                                         GpioIF& gpioInterface,
                                                         PowerSwitchIF& mainLineSwitcher_,
                                                         pcdu::Switches mainLineSwitch_,
                                                         gpioId_t deplSA1, gpioId_t deplSA2,
                                                         SdCardMountedIF& sdcMountedIF)
    : SystemObject(setObjectId_),
      gpioInterface(gpioInterface),
      deplSA1(deplSA1),
      deplSA2(deplSA2),
      mainLineSwitcher(mainLineSwitcher_),
      mainLineSwitch(mainLineSwitch_),
      sdcMan(sdcMountedIF),
      actionHelper(this, nullptr) {
  auto mqArgs = MqArgs(setObjectId_, static_cast<void*>(this));
  commandQueue = QueueFactory::instance()->createMessageQueue(
      cmdQueueSize, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
}

SolarArrayDeploymentHandler::~SolarArrayDeploymentHandler() = default;

ReturnValue_t SolarArrayDeploymentHandler::performOperation(uint8_t operationCode) {
  using namespace std::filesystem;
  auto activeSdc = sdcMan.getActiveSdCard();
  if (activeSdc and activeSdc.value() == sd::SdCard::SLOT_0 and
      sdcMan.isSdCardUsable(activeSdc.value())) {
    if (exists(SD_0_DEPL_FILE)) {
      // perform autonomous deployment handling
      performAutonomousDepl(sd::SdCard::SLOT_0);
    }
  } else if (activeSdc and activeSdc.value() == sd::SdCard::SLOT_1 and
             sdcMan.isSdCardUsable(activeSdc.value())) {
    if (exists(SD_1_DEPL_FILE)) {
      // perform autonomous deployment handling
      performAutonomousDepl(sd::SdCard::SLOT_1);
    }
  }
  readCommandQueue();
  handleStateMachine();
  return returnvalue::OK;
}

ReturnValue_t SolarArrayDeploymentHandler::performAutonomousDepl(sd::SdCard sdCard) {
  using namespace std::filesystem;
  using namespace std;
  auto initFile = [](const char* filename) {
    ofstream of(filename);
    of << "phase: init\n";
    of << "secs_since_start: 0\n";
  };
  if (sdCard == sd::SdCard::SLOT_0) {
    if (not exists(SD_0_DEPLY_INFO)) {
      initFile(SD_0_DEPLY_INFO);
    }
    if (not autonomousDeplForFile(SD_0_DEPLY_INFO)) {
      initFile(SD_0_DEPLY_INFO);
    }
  } else if (sdCard == sd::SdCard::SLOT_1) {
    if (not exists(SD_1_DEPLY_INFO)) {
      initFile(SD_1_DEPLY_INFO);
    }
    if (not autonomousDeplForFile(SD_1_DEPLY_INFO)) {
      initFile(SD_1_DEPLY_INFO);
    }
  }
  return returnvalue::OK;
}

bool SolarArrayDeploymentHandler::autonomousDeplForFile(const char* filename) {
  using namespace std;
  ifstream file(filename);
  string line;
  string word;
  unsigned int lineNum = 0;
  AutonomousDeplState deplState;
  bool stateSwitch = false;
  uint32_t secsSinceBoot = 0;
  while (std::getline(file, line)) {
    if (lineNum == 0) {
      std::istringstream iss(line);
      if (lineNum == 0) {
        iss >> word;
        if (word.find("phase:") == string::npos) {
          return false;
        }
        iss >> word;
        if (word.find(PHASE_INIT_STR) != string::npos) {
          deplState = AutonomousDeplState::INIT;
        } else if (word.find(PHASE_FIRST_BURN_STR) != string::npos) {
          deplState = AutonomousDeplState::FIRST_BURN;
        } else if (word.find(PHASE_WAIT_STR) != string::npos) {
          deplState = AutonomousDeplState::WAIT;
        } else if (word.find(PHASE_SECOND_BURN_STR) != string::npos) {
          deplState = AutonomousDeplState::SECOND_BURN;
        } else if (word.find(PHASE_DONE) != string::npos) {
          deplState = AutonomousDeplState::DONE;
        } else {
          return false;
        }
      } else if (lineNum == 1) {
        iss >> word;
        if (word.find("phase:") == string::npos) {
          return false;
        }

        iss >> secsSinceBoot;
        if (not initUptime) {
          initUptime = secsSinceBoot;
        }
        if (iss.bad()) {
          return false;
        }
        auto switchCheck = [&](AutonomousDeplState expected) {
          if (deplState != expected) {
            deplState = expected;
            stateSwitch = true;
          }
        };
        if ((secsSinceBoot > FIRST_BURN_START_TIME) and (secsSinceBoot < FIRST_BURN_END_TIME)) {
          switchCheck(AutonomousDeplState::FIRST_BURN);
        } else if ((secsSinceBoot > WAIT_START_TIME) and (secsSinceBoot < WAIT_END_TIME)) {
          switchCheck(AutonomousDeplState::WAIT);
        } else if ((secsSinceBoot > SECOND_BURN_START_TIME) and
                   (secsSinceBoot < SECOND_BURN_END_TIME)) {
          switchCheck(AutonomousDeplState::SECOND_BURN);
        } else if (secsSinceBoot > SECOND_BURN_END_TIME) {
          switchCheck(AutonomousDeplState::DONE);
        }
      }
    }
    lineNum++;
  }
  if (initUptime) {
    secsSinceBoot = initUptime.value();
  }
  // Uptime has increased by X seconds so we need to update the uptime count inside the file
  secsSinceBoot += Clock::getUptime().tv_sec;
  if (stateSwitch) {
    if (deplState == AutonomousDeplState::FIRST_BURN or
        deplState == AutonomousDeplState::SECOND_BURN) {
      startFsm(true, true);
    } else if (deplState == AutonomousDeplState::WAIT or deplState == AutonomousDeplState::DONE) {
      startFsm(false, false);
    }
  }
  return true;
}

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

void SolarArrayDeploymentHandler::handleStateMachine() {
  if (stateMachine == MAIN_POWER_ON) {
    mainLineSwitcher.sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_ON);
    mainSwitchCountdown.setTimeout(mainLineSwitcher.getSwitchDelayMs());
    stateMachine = WAIT_MAIN_POWER_ON;
  }
  if (stateMachine == MAIN_POWER_OFF) {
    // This should never fail
    deploymentTransistorsOff();
    mainLineSwitcher.sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_ON);
    mainSwitchCountdown.setTimeout(mainLineSwitcher.getSwitchDelayMs());
    stateMachine = WAIT_MAIN_POWER_OFF;
  }
  if (stateMachine == WAIT_MAIN_POWER_ON) {
    if (checkMainPowerOn()) {
      stateMachine = SWITCH_DEPL_GPIOS;
    }
  }
  if (stateMachine == WAIT_MAIN_POWER_OFF) {
    if (checkMainPowerOff()) {
      finishFsm(returnvalue::OK);
    }
  }
  if (stateMachine == SWITCH_DEPL_GPIOS) {
    // This should never fail
    deploymentTransistorsOn();
    finishFsm(returnvalue::OK);
  }
}

bool SolarArrayDeploymentHandler::checkMainPowerOn() { return checkMainPower(true); }

bool SolarArrayDeploymentHandler::checkMainPowerOff() { return checkMainPower(false); }

bool SolarArrayDeploymentHandler::checkMainPower(bool onOff) {
  if ((onOff and mainLineSwitcher.getSwitchState(mainLineSwitch) == PowerSwitchIF::SWITCH_ON) or
      (not onOff and
       mainLineSwitcher.getSwitchState(mainLineSwitch) == PowerSwitchIF::SWITCH_OFF)) {
    return true;
  }
  if (mainSwitchCountdown.hasTimedOut()) {
    if (onOff) {
      triggerEvent(MAIN_SWITCH_ON_TIMEOUT);
    } else {
      triggerEvent(MAIN_SWITCH_OFF_TIMEOUT);
    }
    if (retryCounter < 3) {
      if (onOff) {
        stateMachine = MAIN_POWER_ON;
      } else {
        stateMachine = MAIN_POWER_OFF;
      }
      retryCounter++;
    } else {
      finishFsm(MAIN_SWITCH_TIMEOUT_FAILURE);
    }
  }
  return false;
}

bool SolarArrayDeploymentHandler::startFsm(std::optional<bool> sa1OnOff,
                                           std::optional<bool> sa2OnOff) {
  if ((stateMachine != StateMachine::IDLE) or (not sa1OnOff and not sa2OnOff)) {
    return false;
  }
  retryCounter = 0;
  return true;
}

void SolarArrayDeploymentHandler::finishFsm(ReturnValue_t resultForActionHelper) {
  retryCounter = 0;
  stateMachine = StateMachine::IDLE;
  if (actionActive) {
    actionHelper.finish(true, rememberCommanderId, activeCmd, resultForActionHelper);
  }
}

ReturnValue_t SolarArrayDeploymentHandler::deploymentTransistorsOn() {
  ReturnValue_t 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 re-initiating
    // the deployment sequence.
    triggerEvent(DEPL_SA1_GPIO_SWTICH_ON_FAILED);
  }
  result = gpioInterface.pullHigh(deplSA2);
  if (result != returnvalue::OK) {
    sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
                  " array deployment switch 2 high "
               << std::endl;
    triggerEvent(DEPL_SA2_GPIO_SWTICH_ON_FAILED);
  }
  return result;
}

ReturnValue_t SolarArrayDeploymentHandler::deploymentTransistorsOff() {
  ReturnValue_t result = gpioInterface.pullLow(deplSA1);
  if (result != returnvalue::OK) {
    sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
                  " array deployment switch 1 low"
               << std::endl;
    // If gpio switch high failed, state machine is reset to wait for a command re-initiating
    // the deployment sequence.
    triggerEvent(DEPL_SA1_GPIO_SWTICH_ON_FAILED);
  }
  result = gpioInterface.pullLow(deplSA2);
  if (result != returnvalue::OK) {
    sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
                  " array deployment switch 2 low"
               << std::endl;
    triggerEvent(DEPL_SA2_GPIO_SWTICH_ON_FAILED);
  }
  return result;
}

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 = returnvalue::OK;
  //  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();
}