eive-obsw/mission/SolarArrayDeploymentHandler.cpp

#include <fsfw/filesystem/HasFileSystemIF.h>
#include <fsfw/tasks/TaskFactory.h>
#include <mission/SolarArrayDeploymentHandler.h>
#include <mission/utility/trace.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"

static constexpr bool DEBUG_MODE = true;

SolarArrayDeploymentHandler::SolarArrayDeploymentHandler(object_id_t setObjectId_,
                                                         GpioIF& gpioInterface,
                                                         PowerSwitchIF& mainLineSwitcher_,
                                                         power::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;
#if OBSW_THREAD_TRACING == 1
  trace::threadTrace(opCounter, "SA DEPL");
#endif
  if (opDivider.checkAndIncrement()) {
    auto activeSdc = sdcMan.getActiveSdCard();
    std::error_code e;
    if (activeSdc and activeSdc.value() == sd::SdCard::SLOT_0 and
        sdcMan.isSdCardUsable(activeSdc.value())) {
      if (exists(SD_0_DEPL_FILE, e)) {
        // perform autonomous deployment handling
        performAutonomousDepl(sd::SdCard::SLOT_0, dryRunStringInFile(SD_0_DEPL_FILE));
      }
    } else if (activeSdc and activeSdc.value() == sd::SdCard::SLOT_1 and
               sdcMan.isSdCardUsable(activeSdc.value())) {
      if (exists(SD_1_DEPL_FILE, e)) {
        // perform autonomous deployment handling
        performAutonomousDepl(sd::SdCard::SLOT_1, dryRunStringInFile(SD_1_DEPL_FILE));
      }
    } else {
      // TODO: This is FDIR domain. If both SD cards are not available for whatever reason,
      //       there is not much we can do except somehow use the scratch buffer which is
      //       not non-volatile. Implementation effort is considerable as well.
    }
  }
  readCommandQueue();
  handleStateMachine();
  return returnvalue::OK;
}

void SolarArrayDeploymentHandler::handleStateMachine() {
  if (stateMachine == MAIN_POWER_ON) {
    mainLineSwitcher.sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_ON);
    mainSwitchCountdown.setTimeout(mainLineSwitcher.getSwitchDelayMs());
    stateMachine = WAIT_MAIN_POWER_ON;
    sif::info << "S/A Deployment: Deployment power line on" << std::endl;
  }
  if (stateMachine == MAIN_POWER_OFF) {
    // These should never fail
    allOff();
    stateMachine = WAIT_MAIN_POWER_OFF;
    sif::info << "S/A Deployment: Deployment power line off" << std::endl;
  }
  if (stateMachine == WAIT_MAIN_POWER_ON) {
    if (checkMainPowerOn()) {
      if (DEBUG_MODE) {
        sif::debug << "SA DEPL FSM: WAIT_MAIN_POWER_ON done -> SWITCH_DEPL_GPIOS" << std::endl;
      }
      stateMachine = SWITCH_DEPL_GPIOS;
    }
  }
  if (stateMachine == WAIT_MAIN_POWER_OFF) {
    if (checkMainPowerOff()) {
      if (DEBUG_MODE) {
        sif::debug << "SA DEPL FSM: WAIT_MAIN_POWER_OFF done -> FSM DONE" << std::endl;
      }
      sif::info << "S/A Deployment: FSM done" << std::endl;
      finishFsm(returnvalue::OK);
    }
  }
  if (stateMachine == SWITCH_DEPL_GPIOS) {
    burnCountdown.setTimeout(fsmInfo.burnCountdownMs);
    // This should never fail
    channelAlternationCd.resetTimer();
    if (not fsmInfo.dryRun) {
      if (fsmInfo.initChannel == 0) {
        sa2Off();
        sa1On();
        fsmInfo.alternationDummy = true;
      } else {
        sa1Off();
        sa2On();
        fsmInfo.alternationDummy = false;
      }
    }
    sif::info << "S/A Deployment: Burning" << std::endl;
    triggerEvent(BURN_PHASE_START, fsmInfo.burnCountdownMs, fsmInfo.dryRun);
    channelAlternationCd.resetTimer();
    stateMachine = BURNING;
  }
  if (stateMachine == BURNING) {
    saGpioAlternation();
    if (burnCountdown.hasTimedOut()) {
      if (DEBUG_MODE) {
        sif::debug << "SA DEPL FSM: BURNING done -> WAIT_MAIN_POWER_OFF" << std::endl;
      }
      allOff();
      triggerEvent(BURN_PHASE_DONE, fsmInfo.burnCountdownMs, fsmInfo.dryRun);
      stateMachine = WAIT_MAIN_POWER_OFF;
    }
  }
}

ReturnValue_t SolarArrayDeploymentHandler::performAutonomousDepl(sd::SdCard sdCard, bool dryRun) {
  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";
  };
  std::error_code e;
  if (sdCard == sd::SdCard::SLOT_0) {
    if (not exists(SD_0_DEPLY_INFO, e)) {
      initFile(SD_0_DEPLY_INFO);
    }
    if (not autonomousDeplForFile(sd::SdCard::SLOT_0, SD_0_DEPLY_INFO, dryRun)) {
      initFile(SD_0_DEPLY_INFO);
    }
  } else if (sdCard == sd::SdCard::SLOT_1) {
    if (not exists(SD_1_DEPLY_INFO, e)) {
      initFile(SD_1_DEPLY_INFO);
    }
    if (not autonomousDeplForFile(sd::SdCard::SLOT_1, SD_1_DEPLY_INFO, dryRun)) {
      initFile(SD_1_DEPLY_INFO);
    }
  }
  return returnvalue::OK;
}

bool SolarArrayDeploymentHandler::autonomousDeplForFile(sd::SdCard sdCard, const char* infoFile,
                                                        bool dryRun) {
  using namespace std;
  std::error_code e;
  ifstream file(infoFile);
  if (file.bad()) {
    return false;
  }
  string line;
  string word;
  unsigned int lineNum = 0;
  AutonomousDeplState deplState = AutonomousDeplState::INIT;
  bool stateSwitch = false;
  uint32_t secsSinceBoot = 0;
  while (std::getline(file, line)) {
    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 (iss.bad()) {
        return false;
      }
      if (word.find("secs_since_start:") == string::npos) {
        return false;
      }

      iss >> secsSinceBoot;
      if (iss.bad()) {
        return false;
      }
      if (not initUptime) {
        initUptime = secsSinceBoot;
      }

      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 or firstAutonomousCycle) {
    if (deplState == AutonomousDeplState::FIRST_BURN or
        deplState == AutonomousDeplState::SECOND_BURN) {
      startFsmOn(config::SA_DEPL_BURN_TIME_SECS, (config::SA_DEPL_BURN_TIME_SECS / 2) * 1000, 0,
                 dryRun);
    } else if (deplState == AutonomousDeplState::WAIT or deplState == AutonomousDeplState::DONE or
               deplState == AutonomousDeplState::INIT) {
      startFsmOff();
    }
  }
  if (deplState == AutonomousDeplState::DONE) {
    std::filesystem::remove(infoFile, e);
    if (sdCard == sd::SdCard::SLOT_0) {
      std::filesystem::remove(SD_0_DEPL_FILE, e);
    } else {
      std::filesystem::remove(SD_1_DEPL_FILE, e);
    }
    triggerEvent(AUTONOMOUS_DEPLOYMENT_COMPLETED);
  } else {
    std::ofstream of(infoFile);
    if (of.bad()) {
      return false;
    }
    of << "phase: ";
    if (deplState == AutonomousDeplState::INIT) {
      of << PHASE_INIT_STR << "\n";
    } else if (deplState == AutonomousDeplState::FIRST_BURN) {
      of << PHASE_FIRST_BURN_STR << "\n";
    } else if (deplState == AutonomousDeplState::WAIT) {
      of << PHASE_WAIT_STR << "\n";
    } else if (deplState == AutonomousDeplState::SECOND_BURN) {
      of << PHASE_SECOND_BURN_STR << "\n";
    }
    of << "secs_since_start: " << std::to_string(secsSinceBoot) << "\n";
  }
  if (firstAutonomousCycle) {
    firstAutonomousCycle = false;
  }
  return true;
}

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::startFsmOn(uint32_t burnCountdownSecs,
                                             uint32_t channelAlternationIntervalMs,
                                             uint8_t initChannel, bool dryRun) {
  if (stateMachine != StateMachine::IDLE) {
    return false;
  }
  channelAlternationCd.setTimeout(channelAlternationIntervalMs);
  if (burnCountdownSecs > config::SA_DEPL_MAX_BURN_TIME) {
    burnCountdownSecs = config::SA_DEPL_MAX_BURN_TIME;
  }
  fsmInfo.dryRun = dryRun;
  fsmInfo.burnCountdownMs = burnCountdownSecs * 1000;
  fsmInfo.initChannel = initChannel;
  stateMachine = StateMachine::MAIN_POWER_ON;
  retryCounter = 0;
  return true;
}

void SolarArrayDeploymentHandler::startFsmOff() {
  if (stateMachine != StateMachine::IDLE) {
    // off commands override the state machine. Cancel any active action commands.
    finishFsm(returnvalue::FAILED);
  }
  retryCounter = 0;
  stateMachine = StateMachine::MAIN_POWER_OFF;
}

void SolarArrayDeploymentHandler::finishFsm(ReturnValue_t resultForActionHelper) {
  retryCounter = 0;
  stateMachine = StateMachine::IDLE;
  fsmInfo.dryRun = false;
  fsmInfo.alternationDummy = false;
  if (actionActive) {
    bool success = false;
    if (resultForActionHelper == returnvalue::OK or
        resultForActionHelper == HasActionsIF::EXECUTION_FINISHED) {
      success = true;
    }
    actionHelper.finish(success, rememberCommanderId, activeCmd, resultForActionHelper);
  }
}

void SolarArrayDeploymentHandler::allOff() {
  deploymentTransistorsOff();
  mainLineSwitcher.sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
  mainSwitchCountdown.setTimeout(mainLineSwitcher.getSwitchDelayMs());
}

bool SolarArrayDeploymentHandler::dryRunStringInFile(const char* filename) {
  std::ifstream ifile(filename);
  if (ifile.bad()) {
    return false;
  }
  std::string line;
  while (getline(ifile, line)) {
    if (line.find("dryrun") != std::string::npos) {
      return true;
    }
  }
  return false;
}

ReturnValue_t SolarArrayDeploymentHandler::executeAction(ActionId_t actionId,
                                                         MessageQueueId_t commandedBy,
                                                         const uint8_t* data, size_t size) {
  ReturnValue_t result = returnvalue::OK;
  if (actionId == DEPLOY_SOLAR_ARRAYS_MANUALLY) {
    ManualDeploymentCommand cmd;
    if (size < cmd.getSerializedSize()) {
      return HasActionsIF::INVALID_PARAMETERS;
    }
    result = cmd.deSerialize(&data, &size, SerializeIF::Endianness::NETWORK);
    if (result != returnvalue::OK) {
      return result;
    }
    uint32_t burnCountdown = cmd.getBurnTimeSecs();
    if (not startFsmOn(burnCountdown, cmd.getSwitchIntervalMs(), cmd.getInitChannel(),
                       cmd.isDryRun())) {
      return HasActionsIF::IS_BUSY;
    }
    actionActive = true;
    rememberCommanderId = commandedBy;
    return result;
  } else if (actionId == SWITCH_OFF_DEPLOYMENT) {
    startFsmOff();
    actionActive = true;
    rememberCommanderId = commandedBy;
    return result;
  } else {
    return HasActionsIF::INVALID_ACTION_ID;
  }
  return result;
}

ReturnValue_t SolarArrayDeploymentHandler::saGpioAlternation() {
  ReturnValue_t status = returnvalue::OK;
  ReturnValue_t result;
  if (channelAlternationCd.hasTimedOut() and not fsmInfo.dryRun) {
    if (fsmInfo.alternationDummy) {
      result = sa1Off();
      if (result != returnvalue::OK) {
        status = result;
      }
      TaskFactory::delayTask(1);
      result = sa2On();
      if (result != returnvalue::OK) {
        status = result;
      }
    } else {
      result = sa2Off();
      if (result != returnvalue::OK) {
        status = result;
      }
      TaskFactory::delayTask(1);
      result = sa1On();
      if (result != returnvalue::OK) {
        status = result;
      }
    }
    fsmInfo.alternationDummy = not fsmInfo.alternationDummy;
    channelAlternationCd.resetTimer();
  }
  return status;
}

ReturnValue_t SolarArrayDeploymentHandler::deploymentTransistorsOff() {
  ReturnValue_t status = returnvalue::OK;
  ReturnValue_t result = sa1Off();
  if (result != returnvalue::OK) {
    status = result;
  }
  result = sa2Off();
  if (result != returnvalue::OK) {
    status = result;
  }
  return status;
}

ReturnValue_t SolarArrayDeploymentHandler::sa1On() {
  ReturnValue_t result = gpioInterface.pullHigh(deplSA1);
  if (result != returnvalue::OK) {
    sif::warning << "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);
  }
  return result;
}

ReturnValue_t SolarArrayDeploymentHandler::sa1Off() {
  ReturnValue_t result = gpioInterface.pullLow(deplSA1);
  if (result != returnvalue::OK) {
    sif::warning << "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_OFF_FAILED);
  }
  return result;
}

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

ReturnValue_t SolarArrayDeploymentHandler::sa2Off() {
  ReturnValue_t result = gpioInterface.pullLow(deplSA2);
  if (result != returnvalue::OK) {
    sif::warning << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
                    " array deployment switch 2 low"
                 << std::endl;
    // If gpio switch high failed, state machine is reset to wait for a command re-initiating
    // the deployment sequence.
    triggerEvent(DEPL_SA2_GPIO_SWTICH_OFF_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;
  }
}

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

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