#include "CoreController.h"

#include <fsfw/events/EventManager.h>
#include <fsfw/filesystem/HasFileSystemIF.h>
#include <fsfw/ipc/QueueFactory.h>
#include <fsfw/tasks/TaskFactory.h>

#include "commonConfig.h"
#include "fsfw/serviceinterface/ServiceInterface.h"
#include "fsfw/timemanager/Stopwatch.h"
#include "fsfw/version.h"
#include "watchdog/definitions.h"
#if OBSW_ADD_TMTC_UDP_SERVER == 1
#include "fsfw/osal/common/UdpTmTcBridge.h"
#endif
#if OBSW_ADD_TMTC_TCP_SERVER == 1
#include "fsfw/osal/common/TcpTmTcServer.h"
#endif
#include <fcntl.h>
#include <unistd.h>

#include <algorithm>
#include <filesystem>

#include "bsp_q7s/fs/SdCardManager.h"
#include "bsp_q7s/memory/scratchApi.h"
#include "bsp_q7s/xadc/Xadc.h"
#include "eive/definitions.h"
#include "linux/utility/utility.h"

xsc::Chip CoreController::CURRENT_CHIP = xsc::Chip::NO_CHIP;
xsc::Copy CoreController::CURRENT_COPY = xsc::Copy::NO_COPY;

CoreController::CoreController(object_id_t objectId)
    : ExtendedControllerBase(objectId, 5), opDivider5(5), opDivider10(10), hkSet(this) {
  ReturnValue_t result = returnvalue::OK;
  try {
    result = initWatchdogFifo();
    if (result != returnvalue::OK) {
      sif::warning << "CoreController::CoreController: Watchdog FIFO init failed" << std::endl;
    }
    sdcMan = SdCardManager::instance();
    if (sdcMan == nullptr) {
      sif::error << "CoreController::CoreController: SD card manager invalid!" << std::endl;
    }

    if (not BLOCKING_SD_INIT) {
      sdcMan->setBlocking(false);
    }

    getCurrentBootCopy(CURRENT_CHIP, CURRENT_COPY);

    initClockFromTimeFile();
  } catch (const std::filesystem::filesystem_error &e) {
    sif::error << "CoreController::CoreController: Failed with exception " << e.what() << std::endl;
  }
  sdCardCheckCd.timeOut();
  eventQueue = QueueFactory::instance()->createMessageQueue(5, EventMessage::MAX_MESSAGE_SIZE);
}

CoreController::~CoreController() {}

ReturnValue_t CoreController::handleCommandMessage(CommandMessage *message) {
  return ExtendedControllerBase::handleCommandMessage(message);
}

void CoreController::performControlOperation() {
#if OBSW_THREAD_TRACING == 1
  trace::threadTrace(opCounter, "CORE CTRL");
#endif
  EventMessage event;
  for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK;
       result = eventQueue->receiveMessage(&event)) {
    switch (event.getEvent()) {
      case (GpsHyperion::GPS_FIX_CHANGE): {
        gpsFix = static_cast<GpsHyperion::FixMode>(event.getParameter2());
        break;
      }
    }
  }
  performWatchdogControlOperation();
  sdStateMachine();
  performMountedSdCardOperations();
  if (sdCardCheckCd.hasTimedOut()) {
    if (shortSdCardCdCounter < 2) {
      shortSdCardCdCounter++;
    }
    if (shortSdCardCdCounter == 2) {
      sdCardCheckCd.setTimeout(DEFAULT_SD_CARD_CHECK_TIMEOUT);
    }
    performSdCardCheck();
    sdCardCheckCd.resetTimer();
  }
  readHkData();
  opDivider5.checkAndIncrement();
  opDivider10.checkAndIncrement();
}

ReturnValue_t CoreController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                      LocalDataPoolManager &poolManager) {
  localDataPoolMap.emplace(core::TEMPERATURE, &tempPoolEntry);
  localDataPoolMap.emplace(core::PS_VOLTAGE, &psVoltageEntry);
  localDataPoolMap.emplace(core::PL_VOLTAGE, &plVoltageEntry);
  poolManager.subscribeForRegularPeriodicPacket({hkSet.getSid(), false, 10.0});
  return returnvalue::OK;
}

LocalPoolDataSetBase *CoreController::getDataSetHandle(sid_t sid) {
  if (sid.ownerSetId == core::HK_SET_ID) {
    return &hkSet;
  }
  return nullptr;
}

ReturnValue_t CoreController::initialize() {
  ReturnValue_t result = ExtendedControllerBase::initialize();
  if (result != returnvalue::OK) {
    sif::warning << "CoreController::initialize: Base init failed" << std::endl;
  }
  result = scratch::writeNumber(scratch::ALLOC_FAILURE_COUNT, 0);
  if (result != returnvalue::OK) {
    sif::warning << "CoreController::initialize: Setting up alloc failure "
                    "count failed"
                 << std::endl;
  }

  sdStateMachine();

  triggerEvent(REBOOT_SW, CURRENT_CHIP, CURRENT_COPY);
  EventManagerIF *eventManager =
      ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
  if (eventManager == nullptr or eventQueue == nullptr) {
    sif::warning << "CoreController::initialize: No valid event manager found or "
                    "queue invalid"
                 << std::endl;
  }
  result = eventManager->registerListener(eventQueue->getId());
  if (result != returnvalue::OK) {
    sif::warning << "CoreController::initialize: Registering as event listener failed" << std::endl;
  }
  result = eventManager->subscribeToEvent(eventQueue->getId(),
                                          event::getEventId(GpsHyperion::GPS_FIX_CHANGE));
  if (result != returnvalue::OK) {
    sif::warning << "Subscribing for GPS GPS_FIX_CHANGE event failed" << std::endl;
  }
  return returnvalue::OK;
}

ReturnValue_t CoreController::initializeAfterTaskCreation() {
  ReturnValue_t result = returnvalue::OK;
  auto sdCard = sdcMan->getPreferredSdCard();
  if (not sdCard) {
    return returnvalue::FAILED;
  }
  sdInfo.active = sdCard.value();
  if (sdInfo.active == sd::SdCard::NONE) {
    sif::error << "CoreController::initializeAfterTaskCreation: "
                  "Issues getting preferred SD card, setting to 0"
               << std::endl;
    sdInfo.active = sd::SdCard::SLOT_0;
  }
  sdcMan->setActiveSdCard(sdInfo.active);
  currMntPrefix = sdcMan->getCurrentMountPrefix();
  if (currMntPrefix == "") {
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }
  if (BLOCKING_SD_INIT) {
    result = initSdCardBlocking();
    if (result != returnvalue::OK and result != SdCardManager::ALREADY_MOUNTED) {
      sif::warning << "CoreController::CoreController: SD card init failed" << std::endl;
    }
  }
  sdStateMachine();
  performMountedSdCardOperations();
  if (result != returnvalue::OK) {
    sif::warning << "CoreController::initialize: Version initialization failed" << std::endl;
  }
  // Add script folder to path
  char *currentEnvPath = getenv("PATH");
  std::string updatedEnvPath = std::string(currentEnvPath) + ":/home/root/scripts:/usr/local/bin";
  setenv("PATH", updatedEnvPath.c_str(), true);
  updateProtInfo();
  initPrint();
  return ExtendedControllerBase::initializeAfterTaskCreation();
}

ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
                                            const uint8_t *data, size_t size) {
  switch (actionId) {
    case (ANNOUNCE_VERSION): {
      uint32_t p1 = (common::OBSW_VERSION_MAJOR << 24) | (common::OBSW_VERSION_MINOR << 16) |
                    (common::OBSW_VERSION_REVISION << 8);
      uint32_t p2 = 0;
      if (strcmp("", common::OBSW_VERSION_CST_GIT_SHA1) != 0) {
        p1 |= 1;
        auto shaAsStr = std::string(common::OBSW_VERSION_CST_GIT_SHA1);
        size_t posDash = shaAsStr.find("-");
        auto gitHash = shaAsStr.substr(posDash + 2, 4);
        // Only copy first 4 letters of git hash
        memcpy(&p2, gitHash.c_str(), 4);
      }

      triggerEvent(VERSION_INFO, p1, p2);
      return HasActionsIF::EXECUTION_FINISHED;
    }
    case (ANNOUNCE_CURRENT_IMAGE): {
      triggerEvent(CURRENT_IMAGE_INFO, CURRENT_CHIP, CURRENT_COPY);
      return HasActionsIF::EXECUTION_FINISHED;
    }
    case (LIST_DIRECTORY_INTO_FILE): {
      return actionListDirectoryIntoFile(actionId, commandedBy, data, size);
    }
    case (SWITCH_REBOOT_FILE_HANDLING): {
      if (size < 1) {
        return HasActionsIF::INVALID_PARAMETERS;
      }
      std::string path = sdcMan->getCurrentMountPrefix() + REBOOT_FILE;
      // Disable the reboot file mechanism
      parseRebootFile(path, rebootFile);
      if (data[0] == 0) {
        rebootFile.enabled = false;
        rewriteRebootFile(rebootFile);
      } else if (data[0] == 1) {
        rebootFile.enabled = true;
        rewriteRebootFile(rebootFile);
      } else {
        return HasActionsIF::INVALID_PARAMETERS;
      }
      return HasActionsIF::EXECUTION_FINISHED;
    }
    case (RESET_REBOOT_COUNTERS): {
      if (size == 0) {
        resetRebootCount(xsc::ALL_CHIP, xsc::ALL_COPY);
      } else if (size == 2) {
        if (data[0] > 1 or data[1] > 1) {
          return HasActionsIF::INVALID_PARAMETERS;
        }
        resetRebootCount(static_cast<xsc::Chip>(data[0]), static_cast<xsc::Copy>(data[1]));
      }
      return HasActionsIF::EXECUTION_FINISHED;
    }
    case (OBSW_UPDATE_FROM_SD_0): {
      return executeSwUpdate(SwUpdateSources::SD_0, data, size);
    }
    case (OBSW_UPDATE_FROM_SD_1): {
      return executeSwUpdate(SwUpdateSources::SD_1, data, size);
    }
    case (OBSW_UPDATE_FROM_TMP): {
      return executeSwUpdate(SwUpdateSources::TMP_DIR, data, size);
    }
    case (SWITCH_TO_SD_0): {
      if (not startSdStateMachine(sd::SdCard::SLOT_0, SdCfgMode::COLD_REDUNDANT, commandedBy,
                                  actionId)) {
        return HasActionsIF::IS_BUSY;
      }
      // Completion will be reported by SD card state machine
      return returnvalue::OK;
    }
    case (SWITCH_TO_SD_1): {
      if (not startSdStateMachine(sd::SdCard::SLOT_1, SdCfgMode::COLD_REDUNDANT, commandedBy,
                                  actionId)) {
        return HasActionsIF::IS_BUSY;
      }
      // Completion will be reported by SD card state machine
      return returnvalue::OK;
    }
    case (SWITCH_TO_BOTH_SD_CARDS): {
      // An active SD still needs to be specified because the system needs to know which SD
      // card to use for regular operations like telemetry storage.
      if (size != 1) {
        return HasActionsIF::INVALID_PARAMETERS;
      }
      if (data[0] != 0 and data[0] != 1) {
        return HasActionsIF::INVALID_PARAMETERS;
      }
      auto active = static_cast<sd::SdCard>(data[0]);
      if (not startSdStateMachine(active, SdCfgMode::HOT_REDUNDANT, commandedBy, actionId)) {
        return HasActionsIF::IS_BUSY;
      }
      // Completion will be reported by SD card state machine
      return returnvalue::OK;
    }
    case (SWITCH_IMG_LOCK): {
      if (size != 3) {
        return HasActionsIF::INVALID_PARAMETERS;
      }
      if (data[1] > 1 or data[2] > 1) {
        return HasActionsIF::INVALID_PARAMETERS;
      }
      setRebootMechanismLock(data[0], static_cast<xsc::Chip>(data[1]),
                             static_cast<xsc::Copy>(data[2]));
      return HasActionsIF::EXECUTION_FINISHED;
    }
    case (SET_MAX_REBOOT_CNT): {
      if (size < 1) {
        return HasActionsIF::INVALID_PARAMETERS;
      }
      std::string path = sdcMan->getCurrentMountPrefix() + REBOOT_FILE;
      // Disable the reboot file mechanism
      parseRebootFile(path, rebootFile);
      rebootFile.maxCount = data[0];
      rewriteRebootFile(rebootFile);
      return HasActionsIF::EXECUTION_FINISHED;
    }
    case (XSC_REBOOT_OBC): {
      // Warning: This function will never return, because it reboots the system
      return actionXscReboot(data, size);
    }
    case (REBOOT_OBC): {
      // Warning: This function will never return, because it reboots the system
      return actionReboot(data, size);
    }
    default: {
      return HasActionsIF::INVALID_ACTION_ID;
    }
  }
}

ReturnValue_t CoreController::checkModeCommand(Mode_t mode, Submode_t submode,
                                               uint32_t *msToReachTheMode) {
  return returnvalue::OK;
}

ReturnValue_t CoreController::initSdCardBlocking() {
  // Create update status file
  ReturnValue_t result = sdcMan->updateSdCardStateFile();
  if (result != returnvalue::OK) {
    sif::warning << "CoreController::initialize: Updating SD card state file failed" << std::endl;
  }
  if (sdInfo.cfgMode == SdCfgMode::PASSIVE) {
    sif::info << "No SD card initialization will be performed" << std::endl;
    return returnvalue::OK;
  }

  result = sdcMan->getSdCardsStatus(sdInfo.currentState);
  if (result != returnvalue::OK) {
    sif::warning << "Getting SD card activity status failed" << std::endl;
  }

  if (sdInfo.cfgMode == SdCfgMode::COLD_REDUNDANT) {
    updateSdInfoOther();
    sif::info << "Cold redundant SD card configuration, preferred SD card: "
              << static_cast<int>(sdInfo.active) << std::endl;
    result = sdColdRedundantBlockingInit();
    // Update status file
    sdcMan->updateSdCardStateFile();
    return result;
  }
  if (sdInfo.cfgMode == SdCfgMode::HOT_REDUNDANT) {
    sif::info << "Hot redundant SD card configuration" << std::endl;
    sdCardSetup(sd::SdCard::SLOT_0, sd::SdState::MOUNTED, "0", false);
    sdCardSetup(sd::SdCard::SLOT_1, sd::SdState::MOUNTED, "1", false);
    // Update status file
    sdcMan->updateSdCardStateFile();
  }
  return returnvalue::OK;
}

ReturnValue_t CoreController::sdStateMachine() {
  ReturnValue_t result = returnvalue::OK;
  SdCardManager::Operations operation;

  if (sdFsmState == SdStates::IDLE) {
    // Nothing to do
    return result;
  }

  if (sdFsmState == SdStates::START) {
    // Init will be performed by separate function
    if (BLOCKING_SD_INIT) {
      sdFsmState = SdStates::IDLE;
      sdInfo.initFinished = true;
      return result;
    } else {
      // Still update SD state file
      if (sdInfo.cfgMode == SdCfgMode::PASSIVE) {
        sdFsmState = SdStates::UPDATE_INFO;
      } else {
        sdInfo.cycleCount = 0;
        sdInfo.commandExecuted = false;
        sdFsmState = SdStates::GET_INFO;
      }
    }
  }

  // This lambda checks the non-blocking operation and assigns the new state on success.
  // It returns true for an operation success and false otherwise
  auto nonBlockingOpChecking = [&](SdStates newStateOnSuccess, uint16_t maxCycleCount,
                                   std::string opPrintout) {
    SdCardManager::OpStatus status = sdcMan->checkCurrentOp(operation);
    if (status == SdCardManager::OpStatus::SUCCESS) {
      sdFsmState = newStateOnSuccess;
      sdInfo.commandExecuted = false;
      sdInfo.cycleCount = 0;
      return true;
    } else if (sdInfo.cycleCount > 4) {
      sif::warning << "CoreController::sdStateMachine: " << opPrintout << " takes too long"
                   << std::endl;
      return false;
    }
    return false;
  };

  if (sdFsmState == SdStates::GET_INFO) {
    if (not sdInfo.commandExecuted) {
      // Create updated status file
      result = sdcMan->updateSdCardStateFile();
      if (result != returnvalue::OK) {
        sif::warning << "CoreController::sdStateMachine: Updating SD card state file failed"
                     << std::endl;
      }
      sdInfo.commandExecuted = true;
    } else {
      nonBlockingOpChecking(SdStates::SET_STATE_SELF, 4, "Updating SDC file");
    }
  }

  if (sdFsmState == SdStates::SET_STATE_SELF) {
    if (not sdInfo.commandExecuted) {
      result = sdcMan->getSdCardsStatus(sdInfo.currentState);
      updateSdInfoOther();
      if (sdInfo.active != sd::SdCard::SLOT_0 and sdInfo.active != sd::SdCard::SLOT_1) {
        sif::warning << "Preferred SD card invalid. Setting to card 0.." << std::endl;
        sdInfo.active = sd::SdCard::SLOT_0;
      }
      if (result != returnvalue::OK) {
        sif::warning << "Getting SD card activity status failed" << std::endl;
      }
      if (sdInfo.cfgMode == SdCfgMode::COLD_REDUNDANT) {
        sif::info << "Cold redundant SD card configuration, target SD card: "
                  << static_cast<int>(sdInfo.active) << std::endl;
      }
      if (sdInfo.activeState == sd::SdState::MOUNTED) {
#if OBSW_VERBOSE_LEVEL >= 1
        std::string mountString;
        if (sdInfo.active == sd::SdCard::SLOT_0) {
          mountString = config::SD_0_MOUNT_POINT;
        } else {
          mountString = config::SD_1_MOUNT_POINT;
        }
        sif::info << "SD card " << sdInfo.activeChar << " already on and mounted at " << mountString
                  << std::endl;
#endif
        sdcMan->setActiveSdCard(sdInfo.active);
        currMntPrefix = sdcMan->getCurrentMountPrefix();
        sdFsmState = SdStates::DETERMINE_OTHER;
      } else if (sdInfo.activeState == sd::SdState::OFF) {
        sdCardSetup(sdInfo.active, sd::SdState::ON, sdInfo.activeChar, false);
        sdInfo.commandExecuted = true;
      } else if (sdInfo.activeState == sd::SdState::ON) {
        sdFsmState = SdStates::MOUNT_SELF;
      }
    } else {
      if (nonBlockingOpChecking(SdStates::MOUNT_SELF, 10, "Setting SDC state")) {
        sdInfo.activeState = sd::SdState::ON;
        currentStateSetter(sdInfo.active, sd::SdState::ON);
      }
    }
  }

  if (sdFsmState == SdStates::MOUNT_SELF) {
    if (not sdInfo.commandExecuted) {
      result = sdCardSetup(sdInfo.active, sd::SdState::MOUNTED, sdInfo.activeChar);
      sdInfo.commandExecuted = true;
    } else {
      if (nonBlockingOpChecking(SdStates::DETERMINE_OTHER, 5, "Mounting SD card")) {
        sdcMan->setActiveSdCard(sdInfo.active);
        currMntPrefix = sdcMan->getCurrentMountPrefix();
        sdInfo.activeState = sd::SdState::MOUNTED;
        currentStateSetter(sdInfo.active, sd::SdState::MOUNTED);
      }
    }
  }

  if (sdFsmState == SdStates::DETERMINE_OTHER) {
    // Determine whether any additional operations have to be done for the other SD card
    // 1. Cold redundant case: Other SD card needs to be unmounted and switched off
    // 2. Hot redundant case: Other SD card needs to be mounted and switched on
    if (sdInfo.cfgMode == SdCfgMode::COLD_REDUNDANT) {
      if (sdInfo.otherState == sd::SdState::ON) {
        sdFsmState = SdStates::SET_STATE_OTHER;
      } else if (sdInfo.otherState == sd::SdState::MOUNTED) {
        sdFsmState = SdStates::MOUNT_UNMOUNT_OTHER;
      } else {
        // Is already off, update info, but with a small delay
        sdFsmState = SdStates::SKIP_CYCLE_BEFORE_INFO_UPDATE;
      }
    } else if (sdInfo.cfgMode == SdCfgMode::HOT_REDUNDANT) {
      if (sdInfo.otherState == sd::SdState::OFF) {
        sdFsmState = SdStates::SET_STATE_OTHER;
      } else if (sdInfo.otherState == sd::SdState::ON) {
        sdFsmState = SdStates::MOUNT_UNMOUNT_OTHER;
      } else {
        // Is already on and mounted, update info
        sdFsmState = SdStates::SKIP_CYCLE_BEFORE_INFO_UPDATE;
      }
    }
  }

  if (sdFsmState == SdStates::SET_STATE_OTHER) {
    // Set state of other SD card to ON or OFF, depending on redundancy mode
    if (sdInfo.cfgMode == SdCfgMode::COLD_REDUNDANT) {
      if (not sdInfo.commandExecuted) {
        result = sdCardSetup(sdInfo.other, sd::SdState::OFF, sdInfo.otherChar, false);
        sdInfo.commandExecuted = true;
      } else {
        if (nonBlockingOpChecking(SdStates::SKIP_CYCLE_BEFORE_INFO_UPDATE, 10,
                                  "Switching off other SD card")) {
          sdInfo.otherState = sd::SdState::OFF;
          currentStateSetter(sdInfo.other, sd::SdState::OFF);
        }
      }
    } else if (sdInfo.cfgMode == SdCfgMode::HOT_REDUNDANT) {
      if (not sdInfo.commandExecuted) {
        result = sdCardSetup(sdInfo.other, sd::SdState::ON, sdInfo.otherChar, false);
        sdInfo.commandExecuted = true;
      } else {
        if (nonBlockingOpChecking(SdStates::MOUNT_UNMOUNT_OTHER, 10,
                                  "Switching on other SD card")) {
          sdInfo.otherState = sd::SdState::ON;
          currentStateSetter(sdInfo.other, sd::SdState::ON);
        }
      }
    }
  }

  if (sdFsmState == SdStates::MOUNT_UNMOUNT_OTHER) {
    // Mount or unmount other SD card, depending on redundancy mode
    if (sdInfo.cfgMode == SdCfgMode::COLD_REDUNDANT) {
      if (not sdInfo.commandExecuted) {
        result = sdCardSetup(sdInfo.other, sd::SdState::ON, sdInfo.otherChar);
        sdInfo.commandExecuted = true;
      } else {
        if (nonBlockingOpChecking(SdStates::SET_STATE_OTHER, 10, "Unmounting other SD card")) {
          sdInfo.otherState = sd::SdState::ON;
          currentStateSetter(sdInfo.other, sd::SdState::ON);
        }
      }
    } else if (sdInfo.cfgMode == SdCfgMode::HOT_REDUNDANT) {
      if (not sdInfo.commandExecuted) {
        result = sdCardSetup(sdInfo.other, sd::SdState::MOUNTED, sdInfo.otherChar);
        sdInfo.commandExecuted = true;
      } else {
        if (nonBlockingOpChecking(SdStates::UPDATE_INFO, 4, "Mounting other SD card")) {
          sdInfo.otherState = sd::SdState::MOUNTED;
          currentStateSetter(sdInfo.other, sd::SdState::MOUNTED);
        }
      }
    }
  }

  if (sdFsmState == SdStates::SKIP_CYCLE_BEFORE_INFO_UPDATE) {
    sdFsmState = SdStates::UPDATE_INFO;
  } else if (sdFsmState == SdStates::UPDATE_INFO) {
    // It is assumed that all tasks are running by the point this section is reached.
    // Therefore, perform this operation in blocking mode because it does not take long
    // and the ready state of the SD card is available sooner
    sdcMan->setBlocking(true);
    // Update status file
    result = sdcMan->updateSdCardStateFile();
    if (result != returnvalue::OK) {
      sif::warning << "CoreController::initialize: Updating SD card state file failed" << std::endl;
    }
    sdInfo.commandExecuted = false;
    sdFsmState = SdStates::IDLE;
    sdInfo.cycleCount = 0;
    sdcMan->setBlocking(false);
    sdcMan->getSdCardsStatus(sdInfo.currentState);
    if (sdCommandingInfo.cmdPending) {
      sdCommandingInfo.cmdPending = false;
      actionHelper.finish(true, sdCommandingInfo.commander, sdCommandingInfo.actionId,
                          returnvalue::OK);
    }
    if (not sdInfo.initFinished) {
      updateSdInfoOther();
      sdInfo.initFinished = true;
      sif::info << "SD card initialization finished" << std::endl;
    }
  }

  sdInfo.cycleCount++;
  return returnvalue::OK;
}

void CoreController::currentStateSetter(sd::SdCard sdCard, sd::SdState newState) {
  if (sdCard == sd::SdCard::SLOT_0) {
    sdInfo.currentState.first = newState;
  } else {
    sdInfo.currentState.second = newState;
  }
}

ReturnValue_t CoreController::sdCardSetup(sd::SdCard sdCard, sd::SdState targetState,
                                          std::string sdChar, bool printOutput) {
  std::string mountString;
  sdcMan->setPrintCommandOutput(printOutput);
  if (sdCard == sd::SdCard::SLOT_0) {
    mountString = config::SD_0_MOUNT_POINT;
  } else {
    mountString = config::SD_1_MOUNT_POINT;
  }

  sd::SdState state = sd::SdState::OFF;
  if (sdCard == sd::SdCard::SLOT_0) {
    state = sdInfo.currentState.first;
  } else {
    state = sdInfo.currentState.second;
  }
  if (state == sd::SdState::MOUNTED) {
    if (targetState == sd::SdState::OFF) {
      sif::info << "Switching off SD card " << sdChar << std::endl;
      return sdcMan->switchOffSdCard(sdCard, true, &sdInfo.currentState);
    } else if (targetState == sd::SdState::ON) {
      sif::info << "Unmounting SD card " << sdChar << std::endl;
      return sdcMan->unmountSdCard(sdCard);
    } else {
      if (std::filesystem::exists(mountString)) {
        sif::info << "SD card " << sdChar << " already on and mounted at " << mountString
                  << std::endl;
        return SdCardManager::ALREADY_MOUNTED;
      }
      sif::error << "SD card mounted but expected mount point " << mountString << " not found!"
                 << std::endl;
      return SdCardManager::MOUNT_ERROR;
    }
  }

  if (state == sd::SdState::OFF) {
    if (targetState == sd::SdState::MOUNTED) {
      sif::info << "Switching on and mounting SD card " << sdChar << " at " << mountString
                << std::endl;
      return sdcMan->switchOnSdCard(sdCard, true, &sdInfo.currentState);
    } else if (targetState == sd::SdState::ON) {
      sif::info << "Switching on SD card " << sdChar << std::endl;
      return sdcMan->switchOnSdCard(sdCard, false, &sdInfo.currentState);
    }
  }

  else if (state == sd::SdState::ON) {
    if (targetState == sd::SdState::MOUNTED) {
      sif::info << "Mounting SD card " << sdChar << " at " << mountString << std::endl;
      return sdcMan->mountSdCard(sdCard);
    } else if (targetState == sd::SdState::OFF) {
      sif::info << "Switching off SD card " << sdChar << std::endl;
      return sdcMan->switchOffSdCard(sdCard, false, &sdInfo.currentState);
    }
  } else {
    sif::warning << "CoreController::sdCardSetup: Invalid state for this call" << std::endl;
  }
  return returnvalue::OK;
}

ReturnValue_t CoreController::sdColdRedundantBlockingInit() {
  ReturnValue_t result = returnvalue::OK;

  result = sdCardSetup(sdInfo.active, sd::SdState::MOUNTED, sdInfo.activeChar);
  if (result != SdCardManager::ALREADY_MOUNTED and result != returnvalue::OK) {
    sif::warning << "Setting up preferred card " << sdInfo.otherChar
                 << " in cold redundant mode failed" << std::endl;
    // Try other SD card and mark set up operation as failed
    sdCardSetup(sdInfo.active, sd::SdState::MOUNTED, sdInfo.activeChar);
    result = returnvalue::FAILED;
  }

  if (result != returnvalue::FAILED and sdInfo.otherState != sd::SdState::OFF) {
    sif::info << "Switching off secondary SD card " << sdInfo.otherChar << std::endl;
    // Switch off other SD card in cold redundant mode if setting up preferred one worked
    // without issues
    ReturnValue_t result2 =
        sdcMan->switchOffSdCard(sdInfo.other, sdInfo.otherState, &sdInfo.currentState);
    if (result2 != returnvalue::OK and result2 != SdCardManager::ALREADY_OFF) {
      sif::warning << "Switching off secondary SD card " << sdInfo.otherChar
                   << " in cold redundant mode failed" << std::endl;
    }
  }
  return result;
}

ReturnValue_t CoreController::incrementAllocationFailureCount() {
  uint32_t count = 0;
  ReturnValue_t result = scratch::readNumber(scratch::ALLOC_FAILURE_COUNT, count);
  if (result != returnvalue::OK) {
    return result;
  }
  count++;
  return scratch::writeNumber(scratch::ALLOC_FAILURE_COUNT, count);
}

ReturnValue_t CoreController::initVersionFile() {
  using namespace fsfw;
  std::string unameFileName = "/tmp/uname_version.txt";
  // TODO: No -v flag for now. If the kernel version is used, need to cut off first few letters
  std::string unameCmd = "uname -mnrso > " + unameFileName;
  int result = std::system(unameCmd.c_str());
  if (result != 0) {
    utility::handleSystemError(result, "CoreController::versionFileInit");
  }
  std::ifstream unameFile(unameFileName);
  std::string unameLine;
  if (not std::getline(unameFile, unameLine)) {
    sif::warning << "CoreController::versionFileInit: Retrieving uname line failed" << std::endl;
  }

  std::string fullObswVersionString = "OBSW: v" + std::to_string(common::OBSW_VERSION_MAJOR) + "." +
                                      std::to_string(common::OBSW_VERSION_MINOR) + "." +
                                      std::to_string(common::OBSW_VERSION_REVISION);
  char versionString[16] = {};
  fsfw::FSFW_VERSION.getVersion(versionString, sizeof(versionString));
  std::string fullFsfwVersionString = "FSFW: v" + std::string(versionString);
  std::string systemString = "System: " + unameLine;
  std::string versionFilePath = currMntPrefix + VERSION_FILE;
  std::fstream versionFile;

  if (not std::filesystem::exists(versionFilePath)) {
    sif::info << "Writing version file " << versionFilePath << ".." << std::endl;
    versionFile.open(versionFilePath, std::ios_base::out);
    versionFile << fullObswVersionString << std::endl;
    versionFile << fullFsfwVersionString << std::endl;
    versionFile << systemString << std::endl;
    return returnvalue::OK;
  }

  // Check whether any version has changed
  bool createNewFile = false;
  versionFile.open(versionFilePath);
  std::string currentVersionString;
  uint8_t idx = 0;
  while (std::getline(versionFile, currentVersionString)) {
    if (idx == 0) {
      if (currentVersionString != fullObswVersionString) {
        sif::info << "OBSW version changed" << std::endl;
        sif::info << "From " << currentVersionString << " to " << fullObswVersionString
                  << std::endl;
        createNewFile = true;
      }
    } else if (idx == 1) {
      if (currentVersionString != fullFsfwVersionString) {
        sif::info << "FSFW version changed" << std::endl;
        sif::info << "From " << currentVersionString << " to " << fullFsfwVersionString
                  << std::endl;
        createNewFile = true;
      }
    } else if (idx == 2) {
      if (currentVersionString != systemString) {
        sif::info << "System version changed" << std::endl;
        sif::info << "Old: " << currentVersionString << std::endl;
        sif::info << "New: " << systemString << std::endl;
        createNewFile = true;
      }
    } else {
      sif::warning << "Invalid version file! Rewriting it.." << std::endl;
      createNewFile = true;
    }
    idx++;
  }

  // Overwrite file if necessary
  if (createNewFile) {
    sif::info << "Rewriting version.txt file with updated versions.." << std::endl;
    versionFile.close();
    versionFile.open(versionFilePath, std::ios_base::out | std::ios_base::trunc);
    versionFile << fullObswVersionString << std::endl;
    versionFile << fullFsfwVersionString << std::endl;
    versionFile << systemString << std::endl;
  }

  return returnvalue::OK;
}

ReturnValue_t CoreController::actionListDirectoryIntoFile(ActionId_t actionId,
                                                          MessageQueueId_t commandedBy,
                                                          const uint8_t *data, size_t size) {
  // TODO: Packet definition for clean deserialization
  // 2 bytes for a and R flag, at least 5 bytes for minimum valid path /tmp with
  // null termination, at least 7 bytes for minimum target file name /tmp/a with
  // null termination.
  if (size < 14) {
    return HasActionsIF::INVALID_PARAMETERS;
  }
  // We could also make -l optional, but I can't think of a reason why to not use -l..

  // This flag specifies to run ls with -a
  bool aFlag = data[0];
  data += 1;
  // This flag specifies to run ls with -R
  bool RFlag = data[1];
  data += 1;

  size_t remainingSize = size - 2;
  // One larger for null termination, which prevents undefined behaviour if the sent
  // strings are not 0 terminated properly
  std::vector<uint8_t> repoAndTargetFileBuffer(remainingSize + 1, 0);
  std::memcpy(repoAndTargetFileBuffer.data(), data, remainingSize);
  const char *currentCharPtr = reinterpret_cast<const char *>(repoAndTargetFileBuffer.data());
  // Full target file name
  std::string repoName(currentCharPtr);
  size_t repoLength = repoName.length();
  // The other string needs to be at least one letter plus NULL termination to be valid at all
  // The first string also needs to be NULL terminated, but the termination is not included
  // in the string length, so this is subtracted from the remaining size as well
  if (repoLength > remainingSize - 3) {
    return HasActionsIF::INVALID_PARAMETERS;
  }
  // The file length will not include the NULL termination, so we skip it
  currentCharPtr += repoLength + 1;
  std::string targetFileName(currentCharPtr);
  std::ostringstream oss;
  oss << "ls -l";
  if (aFlag) {
    oss << "a";
  }
  if (RFlag) {
    oss << "R";
  }

  oss << " " << repoName << " > " << targetFileName;
  int result = std::system(oss.str().c_str());
  if (result != 0) {
    utility::handleSystemError(result, "CoreController::actionListDirectoryIntoFile");
    actionHelper.finish(false, commandedBy, actionId);
  }
  return returnvalue::OK;
}

ReturnValue_t CoreController::initBootCopyFile() {
  if (not std::filesystem::exists(CURR_COPY_FILE)) {
    // This file is created by the systemd service eive-early-config so this should
    // not happen normally
    std::string cmd = "xsc_boot_copy > " + std::string(CURR_COPY_FILE);
    int result = std::system(cmd.c_str());
    if (result != 0) {
      utility::handleSystemError(result, "CoreController::initBootCopy");
    }
  }
  return returnvalue::OK;
}

void CoreController::getCurrentBootCopy(xsc::Chip &chip, xsc::Copy &copy) {
  xsc_libnor_chip_t xscChip;
  xsc_libnor_copy_t xscCopy;
  xsc_boot_get_chip_copy(&xscChip, &xscCopy);
  // Not really thread-safe but it does not need to be
  chip = static_cast<xsc::Chip>(xscChip);
  copy = static_cast<xsc::Copy>(xscCopy);
}

ReturnValue_t CoreController::initWatchdogFifo() {
  if (not std::filesystem::exists(watchdog::FIFO_NAME)) {
    // Still return returnvalue::OK for now
    sif::info << "Watchdog FIFO " << watchdog::FIFO_NAME << " does not exist, can't initiate"
              << " watchdog" << std::endl;
    return returnvalue::OK;
  }
  // Open FIFO write only and non-blocking to prevent SW from killing itself.
  watchdogFifoFd = open(watchdog::FIFO_NAME.c_str(), O_WRONLY | O_NONBLOCK);
  if (watchdogFifoFd < 0) {
    if (errno == ENXIO) {
      watchdogFifoFd = RETRY_FIFO_OPEN;
      sif::info << "eive-watchdog not running. FIFO can not be opened" << std::endl;
    } else {
      sif::error << "Opening pipe " << watchdog::FIFO_NAME << " write-only failed with " << errno
                 << ": " << strerror(errno) << std::endl;
      return returnvalue::FAILED;
    }
  }
  return returnvalue::OK;
}

void CoreController::initPrint() {
#if OBSW_VERBOSE_LEVEL >= 1
  if (watchdogFifoFd > 0) {
    sif::info << "Opened watchdog FIFO successfully.." << std::endl;
  }
#endif
}

ReturnValue_t CoreController::actionXscReboot(const uint8_t *data, size_t size) {
  if (size < 1) {
    return HasActionsIF::INVALID_PARAMETERS;
  }
  bool rebootSameBootCopy = data[0];
  bool protOpPerformed = false;
  SdCardManager::instance()->setBlocking(true);
  if (rebootSameBootCopy) {
#if OBSW_VERBOSE_LEVEL >= 1
    sif::info << "CoreController::actionPerformReboot: Rebooting on current image" << std::endl;
#endif
    gracefulShutdownTasks(xsc::Chip::SELF_CHIP, xsc::Copy::SELF_COPY, protOpPerformed);
    int result = std::system("xsc_boot_copy -r");
    if (result != 0) {
      utility::handleSystemError(result, "CoreController::executeAction");
      return returnvalue::FAILED;
    }
    return HasActionsIF::EXECUTION_FINISHED;
  }
  if (size < 3 or (data[1] > 1 or data[2] > 1)) {
    return HasActionsIF::INVALID_PARAMETERS;
  }
#if OBSW_VERBOSE_LEVEL >= 1
  sif::info << "CoreController::actionPerformReboot: Rebooting on " << static_cast<int>(data[1])
            << " " << static_cast<int>(data[2]) << std::endl;
#endif

  // Check that the target chip and copy is writeprotected first
  generateChipStateFile();
  // If any boot copies are unprotected, protect them here
  auto tgtChip = static_cast<xsc::Chip>(data[1]);
  auto tgtCopy = static_cast<xsc::Copy>(data[2]);

  // This function can not really fail
  gracefulShutdownTasks(tgtChip, tgtCopy, protOpPerformed);

  switch (tgtChip) {
    case (xsc::Chip::CHIP_0): {
      switch (tgtCopy) {
        case (xsc::Copy::COPY_0): {
          xsc_boot_copy(XSC_LIBNOR_CHIP_0, XSC_LIBNOR_COPY_NOMINAL);
          break;
        }
        case (xsc::Copy::COPY_1): {
          xsc_boot_copy(XSC_LIBNOR_CHIP_0, XSC_LIBNOR_COPY_GOLD);
          break;
        }
        default: {
          break;
        }
      }
      break;
    }
    case (xsc::Chip::CHIP_1): {
      switch (tgtCopy) {
        case (xsc::Copy::COPY_0): {
          xsc_boot_copy(XSC_LIBNOR_CHIP_1, XSC_LIBNOR_COPY_NOMINAL);
          break;
        }
        case (xsc::Copy::COPY_1): {
          xsc_boot_copy(XSC_LIBNOR_CHIP_1, XSC_LIBNOR_COPY_GOLD);
          break;
        }
        default: {
          break;
        }
      }
      break;
    }
    default:
      break;
  }
  return returnvalue::FAILED;
}

ReturnValue_t CoreController::actionReboot(const uint8_t *data, size_t size) {
  bool protOpPerformed = false;
  gracefulShutdownTasks(xsc::Chip::CHIP_0, xsc::Copy::COPY_0, protOpPerformed);
  std::system("reboot");
  return returnvalue::OK;
}

ReturnValue_t CoreController::gracefulShutdownTasks(xsc::Chip chip, xsc::Copy copy,
                                                    bool &protOpPerformed) {
  sdcMan->setBlocking(true);
  sdcMan->markUnusable();
  // Wait two seconds to ensure no one uses the SD cards
  TaskFactory::delayTask(2000);
  // Attempt graceful shutdown by unmounting and switching off SD cards
  sdcMan->switchOffSdCard(sd::SdCard::SLOT_0);
  sdcMan->switchOffSdCard(sd::SdCard::SLOT_1);
  // If any boot copies are unprotected
  ReturnValue_t result = setBootCopyProtection(xsc::Chip::SELF_CHIP, xsc::Copy::SELF_COPY, true,
                                               protOpPerformed, false);
  if (result == returnvalue::OK and protOpPerformed) {
    // TODO: Would be nice to notify operator. But we can't use the filesystem anymore
    // and a reboot is imminent. Use scratch buffer?
    sif::info << "Running slot was writeprotected before reboot" << std::endl;
  }
  return result;
}

void CoreController::updateSdInfoOther() {
  if (sdInfo.active == sd::SdCard::SLOT_0) {
    sdInfo.activeChar = "0";
    sdInfo.otherChar = "1";
    sdInfo.otherState = sdInfo.currentState.second;
    sdInfo.activeState = sdInfo.currentState.first;
    sdInfo.other = sd::SdCard::SLOT_1;

  } else if (sdInfo.active == sd::SdCard::SLOT_1) {
    sdInfo.activeChar = "1";
    sdInfo.otherChar = "0";
    sdInfo.otherState = sdInfo.currentState.first;
    sdInfo.activeState = sdInfo.currentState.second;
    sdInfo.other = sd::SdCard::SLOT_0;
  } else {
    sif::warning << "CoreController::updateSdInfoOther: Invalid SD card passed" << std::endl;
  }
}

bool CoreController::sdInitFinished() const { return sdInfo.initFinished; }

ReturnValue_t CoreController::generateChipStateFile() {
  int result = std::system(CHIP_PROT_SCRIPT);
  if (result != 0) {
    utility::handleSystemError(result, "CoreController::generateChipStateFile");
    return returnvalue::FAILED;
  }
  return returnvalue::OK;
}

ReturnValue_t CoreController::setBootCopyProtection(xsc::Chip targetChip, xsc::Copy targetCopy,
                                                    bool protect, bool &protOperationPerformed,
                                                    bool updateProtFile) {
  bool allChips = false;
  bool allCopies = false;
  bool selfChip = false;
  bool selfCopy = false;
  protOperationPerformed = false;

  switch (targetChip) {
    case (xsc::Chip::ALL_CHIP): {
      allChips = true;
      break;
    }
    case (xsc::Chip::NO_CHIP): {
      return returnvalue::OK;
    }
    case (xsc::Chip::SELF_CHIP): {
      selfChip = true;
      targetChip = CURRENT_CHIP;
      break;
    }
    default: {
      break;
    }
  }
  switch (targetCopy) {
    case (xsc::Copy::ALL_COPY): {
      allCopies = true;
      break;
    }
    case (xsc::Copy::NO_COPY): {
      return returnvalue::OK;
    }
    case (xsc::Copy::SELF_COPY): {
      selfCopy = true;
      targetCopy = CURRENT_COPY;
      break;
    }
    default: {
      break;
    }
  }

  for (uint8_t arrIdx = 0; arrIdx < protArray.size(); arrIdx++) {
    int result = handleBootCopyProtAtIndex(targetChip, targetCopy, protect, protOperationPerformed,
                                           selfChip, selfCopy, allChips, allCopies, arrIdx);
    if (result != 0) {
      break;
    }
  }
  if (protOperationPerformed and updateProtFile) {
    updateProtInfo();
  }
  return returnvalue::OK;
}

int CoreController::handleBootCopyProtAtIndex(xsc::Chip targetChip, xsc::Copy targetCopy,
                                              bool protect, bool &protOperationPerformed,
                                              bool selfChip, bool selfCopy, bool allChips,
                                              bool allCopies, uint8_t arrIdx) {
  bool currentProt = protArray[arrIdx];
  std::ostringstream oss;
  bool performOp = false;
  if (protect == currentProt) {
    return 0;
  }
  if (protOperationPerformed) {
    if ((selfChip and selfCopy) or (not allCopies and not allChips)) {
      // No need to continue, only one operation was requested
      return 1;
    }
  }
  xsc::Chip currentChip;
  xsc::Copy currentCopy;
  oss << "writeprotect ";
  if (arrIdx == 0 or arrIdx == 1) {
    oss << "0 ";
    currentChip = xsc::Chip::CHIP_0;
  } else {
    oss << "1 ";
    currentChip = xsc::Chip::CHIP_1;
  }
  if (arrIdx == 0 or arrIdx == 2) {
    oss << "0 ";
    currentCopy = xsc::Copy::COPY_0;
  } else {
    oss << "1 ";
    currentCopy = xsc::Copy::COPY_1;
  }
  if (protect) {
    oss << "1";
  } else {
    oss << "0";
  }

  int result = 0;
  if (allChips and allCopies) {
    performOp = true;
  } else if (allChips) {
    if ((selfCopy and CURRENT_COPY == targetCopy) or (currentCopy == targetCopy)) {
      performOp = true;
    }
  } else if (allCopies) {
    if ((selfChip and CURRENT_COPY == targetCopy) or (currentChip == targetChip)) {
      performOp = true;
    }
  } else if (selfChip and (currentChip == targetChip)) {
    if (selfCopy) {
      if (currentCopy == targetCopy) {
        performOp = true;
      }
    } else {
      performOp = true;
    }

  } else if (selfCopy and (currentCopy == targetCopy)) {
    if (selfChip) {
      if (currentChip == targetChip) {
        performOp = true;
      }
    } else {
      performOp = true;
    }
  } else if ((targetChip == currentChip) and (targetCopy == currentCopy)) {
    performOp = true;
  }
  if (result != 0) {
    utility::handleSystemError(result, "CoreController::checkAndSetBootCopyProtection");
  }
  if (performOp) {
    // TODO: Lock operation take a long time. Use command executor? That would require a
    // new state machine..
    protOperationPerformed = true;
    sif::info << "Executing command: " << oss.str() << std::endl;
    result = std::system(oss.str().c_str());
  }
  return 0;
}

ReturnValue_t CoreController::updateProtInfo(bool regenerateChipStateFile) {
  using namespace std;
  ReturnValue_t result = returnvalue::OK;
  if (regenerateChipStateFile) {
    result = generateChipStateFile();
    if (result != returnvalue::OK) {
      sif::warning << "CoreController::updateProtInfo: Generating chip state file failed"
                   << std::endl;
      return result;
    }
  }
  if (not filesystem::exists(CHIP_STATE_FILE)) {
    return returnvalue::FAILED;
  }
  ifstream chipStateFile(CHIP_STATE_FILE);
  if (not chipStateFile.good()) {
    return returnvalue::FAILED;
  }
  string nextLine;
  uint8_t lineCounter = 0;
  string word;
  while (getline(chipStateFile, nextLine)) {
    result = handleProtInfoUpdateLine(nextLine);
    if (result != returnvalue::OK) {
      sif::warning << "CoreController::updateProtInfo: Protection info update failed!" << std::endl;
      return result;
    }
    ++lineCounter;
    if (lineCounter > 4) {
      sif::warning << "CoreController::checkAndProtectBootCopy: "
                      "Line counter larger than 4"
                   << std::endl;
    }
  }
  return returnvalue::OK;
}

ReturnValue_t CoreController::handleProtInfoUpdateLine(std::string nextLine) {
  using namespace std;
  string word;
  uint8_t wordIdx = 0;
  uint8_t arrayIdx = 0;
  istringstream iss(nextLine);
  xsc::Chip currentChip = xsc::Chip::CHIP_0;
  xsc::Copy currentCopy = xsc::Copy::COPY_0;
  while (iss >> word) {
    if (wordIdx == 1) {
      currentChip = static_cast<xsc::Chip>(stoi(word));
    }
    if (wordIdx == 3) {
      currentCopy = static_cast<xsc::Copy>(stoi(word));
    }

    if (wordIdx == 3) {
      if (currentChip == xsc::Chip::CHIP_0) {
        if (currentCopy == xsc::Copy::COPY_0) {
          arrayIdx = 0;
        } else if (currentCopy == xsc::Copy::COPY_1) {
          arrayIdx = 1;
        }
      }

      else if (currentChip == xsc::Chip::CHIP_1) {
        if (currentCopy == xsc::Copy::COPY_0) {
          arrayIdx = 2;
        } else if (currentCopy == xsc::Copy::COPY_1) {
          arrayIdx = 3;
        }
      }
    }
    if (wordIdx == 5) {
      if (word == "unlocked.") {
        protArray[arrayIdx] = false;
      } else {
        protArray[arrayIdx] = true;
      }
    }
    wordIdx++;
  }
  return returnvalue::OK;
}

void CoreController::performWatchdogControlOperation() {
  // Only perform each fifth iteration
  if (watchdogFifoFd != 0 and opDivider5.check()) {
    if (watchdogFifoFd == RETRY_FIFO_OPEN) {
      // Open FIFO write only and non-blocking
      watchdogFifoFd = open(watchdog::FIFO_NAME.c_str(), O_WRONLY | O_NONBLOCK);
      if (watchdogFifoFd < 0) {
        if (errno == ENXIO) {
          watchdogFifoFd = RETRY_FIFO_OPEN;
          // No printout for now, would be spam
          return;
        } else {
          sif::error << "Opening pipe " << watchdog::FIFO_NAME << " write-only failed with "
                     << errno << ": " << strerror(errno) << std::endl;
          return;
        }
      }
      sif::info << "Opened " << watchdog::FIFO_NAME << " successfully" << std::endl;
    } else if (watchdogFifoFd > 0) {
      // Write to OBSW watchdog FIFO here
      const char writeChar = 'a';
      ssize_t writtenBytes = write(watchdogFifoFd, &writeChar, 1);
      if (writtenBytes < 0) {
        sif::error << "Errors writing to watchdog FIFO, code " << errno << ": " << strerror(errno)
                   << std::endl;
      }
    }
  }
}

void CoreController::performMountedSdCardOperations() {
  auto mountedSdCardOp = [&](sd::SdCard sdCard, std::string mntPoint) {
    if (not performOneShotSdCardOpsSwitch) {
      std::ostringstream path;
      path << mntPoint << "/" << CONF_FOLDER;
      if (not std::filesystem::exists(path.str())) {
        std::filesystem::create_directory(path.str());
      }
      initVersionFile();
      ReturnValue_t result = initBootCopyFile();
      if (result != returnvalue::OK) {
        sif::warning << "CoreController::CoreController: Boot copy init" << std::endl;
      }
      if (not timeFileInitDone) {
        initClockFromTimeFile();
      }
      performRebootFileHandling(false);
    }
    backupTimeFileHandler();
  };
  bool someSdCardActive = false;
  if (sdInfo.active == sd::SdCard::SLOT_0 and sdcMan->isSdCardUsable(sd::SdCard::SLOT_0)) {
    mountedSdCardOp(sd::SdCard::SLOT_0, config::SD_0_MOUNT_POINT);
    someSdCardActive = true;
  }
  if (sdInfo.active == sd::SdCard::SLOT_1 and sdcMan->isSdCardUsable(sd::SdCard::SLOT_1)) {
    mountedSdCardOp(sd::SdCard::SLOT_1, config::SD_1_MOUNT_POINT);
    someSdCardActive = true;
  }
  if (someSdCardActive) {
    performOneShotSdCardOpsSwitch = true;
  }
}

ReturnValue_t CoreController::performSdCardCheck() {
  bool mountedReadOnly = false;
  SdCardManager::SdStatePair active;
  sdcMan->getSdCardsStatus(active);
  if (sdFsmState != SdStates::IDLE) {
    return returnvalue::OK;
  }
  auto sdCardCheck = [&](sd::SdCard sdCard) {
    ReturnValue_t result = sdcMan->isSdCardMountedReadOnly(sdCard, mountedReadOnly);
    if (result != returnvalue::OK) {
      sif::error << "CoreController::performSdCardCheck: Could not check "
                    "read-only mount state"
                 << std::endl;
    }
    if (mountedReadOnly) {
      int linuxErrno = 0;
      result = sdcMan->performFsck(sdCard, true, linuxErrno);
      if (result != returnvalue::OK) {
        sif::error << "CoreController::performSdCardCheck: fsck command on SD Card "
                   << static_cast<uint8_t>(sdCard) << " failed with code " << linuxErrno << " | "
                   << strerror(linuxErrno);
      }
      result = sdcMan->remountReadWrite(sdCard);
      if (result == returnvalue::OK) {
        sif::warning << "CoreController::performSdCardCheck: Remounted SD Card "
                     << static_cast<uint8_t>(sdCard) << " read-write";
      } else {
        sif::error << "CoreController::performSdCardCheck: Remounting SD Card "
                   << static_cast<uint8_t>(sdCard) << " read-write failed";
      }
    }
  };
  if (active.first == sd::SdState::MOUNTED) {
    sdCardCheck(sd::SdCard::SLOT_0);
  }
  if (active.second == sd::SdState::MOUNTED) {
    sdCardCheck(sd::SdCard::SLOT_1);
  }
#if OBSW_SD_CARD_MUST_BE_ON == 1
  // This is FDIR. The core controller will attempt once to get some SD card working
  bool someSdCardActive = false;
  if ((sdInfo.active == sd::SdCard::SLOT_0 and sdcMan->isSdCardUsable(sd::SdCard::SLOT_0)) or
      (sdInfo.active == sd::SdCard::SLOT_1 and sdcMan->isSdCardUsable(sd::SdCard::SLOT_1))) {
    someSdCardActive = true;
  }
  if (not someSdCardActive and remountAttemptFlag) {
    triggerEvent(NO_SD_CARD_ACTIVE);
    initSdCardBlocking();
    remountAttemptFlag = false;
  }
#endif
  return returnvalue::OK;
}

void CoreController::performRebootFileHandling(bool recreateFile) {
  using namespace std;
  std::string path = currMntPrefix + REBOOT_FILE;
  if (not std::filesystem::exists(path) or recreateFile) {
#if OBSW_VERBOSE_LEVEL >= 1
    sif::info << "CoreController::performRebootFileHandling: Recreating reboot file" << std::endl;
#endif
    rebootFile.enabled = false;
    rebootFile.img00Cnt = 0;
    rebootFile.img01Cnt = 0;
    rebootFile.img10Cnt = 0;
    rebootFile.img11Cnt = 0;
    rebootFile.lastChip = xsc::Chip::CHIP_0;
    rebootFile.lastCopy = xsc::Copy::COPY_0;
    rebootFile.img00Lock = false;
    rebootFile.img01Lock = false;
    rebootFile.img10Lock = false;
    rebootFile.img11Lock = false;
    rebootFile.mechanismNextChip = xsc::Chip::NO_CHIP;
    rebootFile.mechanismNextCopy = xsc::Copy::NO_COPY;
    rebootFile.bootFlag = false;
    rewriteRebootFile(rebootFile);
  } else {
    if (not parseRebootFile(path, rebootFile)) {
      performRebootFileHandling(true);
    }
  }

  if (CURRENT_CHIP == xsc::CHIP_0) {
    if (CURRENT_COPY == xsc::COPY_0) {
      rebootFile.img00Cnt++;
    } else {
      rebootFile.img01Cnt++;
    }
  } else {
    if (CURRENT_COPY == xsc::COPY_0) {
      rebootFile.img10Cnt++;
    } else {
      rebootFile.img11Cnt++;
    }
  }

  if (rebootFile.bootFlag) {
    // Trigger event to inform ground that a reboot was triggered
    uint32_t p1 = rebootFile.lastChip << 16 | rebootFile.lastCopy;
    uint32_t p2 = rebootFile.img00Cnt << 24 | rebootFile.img01Cnt << 16 | rebootFile.img10Cnt << 8 |
                  rebootFile.img11Cnt;
    triggerEvent(REBOOT_MECHANISM_TRIGGERED, p1, p2);
    // Clear the boot flag
    rebootFile.bootFlag = false;
  }

  if (rebootFile.mechanismNextChip != xsc::NO_CHIP and
      rebootFile.mechanismNextCopy != xsc::NO_COPY) {
    if (CURRENT_CHIP != rebootFile.mechanismNextChip or
        CURRENT_COPY != rebootFile.mechanismNextCopy) {
      std::string infoString = std::to_string(rebootFile.mechanismNextChip) + " " +
                               std::to_string(rebootFile.mechanismNextCopy);
      sif::warning << "CoreController::performRebootFileHandling: Expected to be on image "
                   << infoString << " but currently on other image. Locking " << infoString
                   << std::endl;
      // Firmware or other component might be corrupt and we are on another image then the target
      // image specified by the mechanism. We can't really trust the target image anymore.
      // Lock it for now
      if (rebootFile.mechanismNextChip == xsc::CHIP_0) {
        if (rebootFile.mechanismNextCopy == xsc::COPY_0) {
          rebootFile.img00Lock = true;
        } else {
          rebootFile.img01Lock = true;
        }
      } else {
        if (rebootFile.mechanismNextCopy == xsc::COPY_0) {
          rebootFile.img10Lock = true;
        } else {
          rebootFile.img11Lock = true;
        }
      }
    }
  }

  rebootFile.lastChip = CURRENT_CHIP;
  rebootFile.lastCopy = CURRENT_COPY;
  // Only reboot if the reboot functionality is enabled.
  // The handler will still increment the boot counts
  if (rebootFile.enabled and (*rebootFile.relevantBootCnt >= rebootFile.maxCount)) {
    // Reboot to other image
    bool doReboot = false;
    xsc::Chip tgtChip = xsc::NO_CHIP;
    xsc::Copy tgtCopy = xsc::NO_COPY;
    determineAndExecuteReboot(rebootFile, doReboot, tgtChip, tgtCopy);
    if (doReboot) {
      rebootFile.bootFlag = true;
#if OBSW_VERBOSE_LEVEL >= 1
      sif::info << "Boot counter for image " << CURRENT_CHIP << " " << CURRENT_COPY
                << " too high. Rebooting to " << tgtChip << " " << tgtCopy << std::endl;
#endif
      rebootFile.mechanismNextChip = tgtChip;
      rebootFile.mechanismNextCopy = tgtCopy;
      rewriteRebootFile(rebootFile);
      xsc_boot_copy(static_cast<xsc_libnor_chip_t>(tgtChip),
                    static_cast<xsc_libnor_copy_t>(tgtCopy));
    }
  } else {
    rebootFile.mechanismNextChip = xsc::NO_CHIP;
    rebootFile.mechanismNextCopy = xsc::NO_COPY;
  }
  rewriteRebootFile(rebootFile);
}

void CoreController::determineAndExecuteReboot(RebootFile &rf, bool &needsReboot,
                                               xsc::Chip &tgtChip, xsc::Copy &tgtCopy) {
  tgtChip = xsc::CHIP_0;
  tgtCopy = xsc::COPY_0;
  needsReboot = false;
  if ((CURRENT_CHIP == xsc::CHIP_0) and (CURRENT_COPY == xsc::COPY_0) and
      (rf.img00Cnt >= rf.maxCount)) {
    needsReboot = true;
    if (rf.img01Cnt < rf.maxCount and not rf.img01Lock) {
      tgtCopy = xsc::COPY_1;
      return;
    }
    if (rf.img10Cnt < rf.maxCount and not rf.img10Lock) {
      tgtChip = xsc::CHIP_1;
      return;
    }
    if (rf.img11Cnt < rf.maxCount and not rf.img11Lock) {
      tgtChip = xsc::CHIP_1;
      tgtCopy = xsc::COPY_1;
      return;
    }
    // Can't really do much here. Stay on image
    sif::warning
        << "All reboot counts too high or all fallback images locked, already on fallback image"
        << std::endl;
    needsReboot = false;
    return;
  }
  if ((CURRENT_CHIP == xsc::CHIP_0) and (CURRENT_COPY == xsc::COPY_1) and
      (rf.img01Cnt >= rf.maxCount)) {
    needsReboot = true;
    if (rf.img00Cnt < rf.maxCount and not rf.img00Lock) {
      // Reboot on fallback image
      return;
    }
    if (rf.img10Cnt < rf.maxCount and not rf.img10Lock) {
      tgtChip = xsc::CHIP_1;
      return;
    }
    if (rf.img11Cnt < rf.maxCount and not rf.img11Lock) {
      tgtChip = xsc::CHIP_1;
      tgtCopy = xsc::COPY_1;
    }
    if (rf.img00Lock) {
      needsReboot = false;
    }
    // Reboot to fallback image
  }
  if ((CURRENT_CHIP == xsc::CHIP_1) and (CURRENT_COPY == xsc::COPY_0) and
      (rf.img10Cnt >= rf.maxCount)) {
    needsReboot = true;
    if (rf.img11Cnt < rf.maxCount and not rf.img11Lock) {
      tgtChip = xsc::CHIP_1;
      tgtCopy = xsc::COPY_1;
      return;
    }
    if (rf.img00Cnt < rf.maxCount and not rf.img00Lock) {
      return;
    }
    if (rf.img01Cnt < rf.maxCount and not rf.img01Lock) {
      tgtCopy = xsc::COPY_1;
      return;
    }
    if (rf.img00Lock) {
      needsReboot = false;
    }
    // Reboot to fallback image
  }
  if ((CURRENT_CHIP == xsc::CHIP_1) and (CURRENT_COPY == xsc::COPY_1) and
      (rf.img11Cnt >= rf.maxCount)) {
    needsReboot = true;
    if (rf.img10Cnt < rf.maxCount and not rf.img10Lock) {
      tgtChip = xsc::CHIP_1;
      return;
    }
    if (rf.img00Cnt < rf.maxCount and not rf.img00Lock) {
      return;
    }
    if (rf.img01Cnt < rf.maxCount and not rf.img01Lock) {
      tgtCopy = xsc::COPY_1;
      return;
    }
    if (rf.img00Lock) {
      needsReboot = false;
    }
    // Reboot to fallback image
  }
}

bool CoreController::parseRebootFile(std::string path, RebootFile &rf) {
  using namespace std;
  std::string selfMatch;
  if (CURRENT_CHIP == xsc::CHIP_0) {
    if (CURRENT_COPY == xsc::COPY_0) {
      selfMatch = "00";
    } else {
      selfMatch = "01";
    }
  } else {
    if (CURRENT_COPY == xsc::COPY_0) {
      selfMatch = "10";
    } else {
      selfMatch = "11";
    }
  }
  ifstream file(path);
  string word;
  string line;
  uint8_t lineIdx = 0;
  while (std::getline(file, line)) {
    istringstream iss(line);
    switch (lineIdx) {
      case 0: {
        iss >> word;
        if (word.find("on:") == string::npos) {
          // invalid file
          return false;
        }
        iss >> rf.enabled;
        break;
      }
      case 1: {
        iss >> word;
        if (word.find("maxcnt:") == string::npos) {
          return false;
        }
        iss >> rf.maxCount;
        break;
      }
      case 2: {
        iss >> word;
        if (word.find("img00:") == string::npos) {
          return false;
        }
        iss >> rf.img00Cnt;
        if (word.find(selfMatch) != string::npos) {
          rf.relevantBootCnt = &rf.img00Cnt;
        }
        break;
      }
      case 3: {
        iss >> word;
        if (word.find("img01:") == string::npos) {
          return false;
        }
        iss >> rf.img01Cnt;
        if (word.find(selfMatch) != string::npos) {
          rf.relevantBootCnt = &rf.img01Cnt;
        }
        break;
      }
      case 4: {
        iss >> word;
        if (word.find("img10:") == string::npos) {
          return false;
        }
        iss >> rf.img10Cnt;
        if (word.find(selfMatch) != string::npos) {
          rf.relevantBootCnt = &rf.img10Cnt;
        }
        break;
      }
      case 5: {
        iss >> word;
        if (word.find("img11:") == string::npos) {
          return false;
        }
        iss >> rf.img11Cnt;
        if (word.find(selfMatch) != string::npos) {
          rf.relevantBootCnt = &rf.img11Cnt;
        }
        break;
      }
      case 6: {
        iss >> word;
        if (word.find("img00lock:") == string::npos) {
          return false;
        }
        iss >> rf.img00Lock;
        break;
      }
      case 7: {
        iss >> word;
        if (word.find("img01lock:") == string::npos) {
          return false;
        }
        iss >> rf.img01Lock;
        break;
      }
      case 8: {
        iss >> word;
        if (word.find("img10lock:") == string::npos) {
          return false;
        }
        iss >> rf.img10Lock;
        break;
      }
      case 9: {
        iss >> word;
        if (word.find("img11lock:") == string::npos) {
          return false;
        }
        iss >> rf.img11Lock;
        break;
      }
      case 10: {
        iss >> word;
        if (word.find("bootflag:") == string::npos) {
          return false;
        }
        iss >> rf.bootFlag;
        break;
      }
      case 11: {
        iss >> word;
        int copyRaw = 0;
        int chipRaw = 0;
        if (word.find("last:") == string::npos) {
          return false;
        }
        iss >> chipRaw;
        if (iss.fail()) {
          return false;
        }
        iss >> copyRaw;
        if (iss.fail()) {
          return false;
        }

        if (chipRaw > 1 or copyRaw > 1) {
          return false;
        }
        rf.lastChip = static_cast<xsc::Chip>(chipRaw);
        rf.lastCopy = static_cast<xsc::Copy>(copyRaw);
        break;
      }
      case 12: {
        iss >> word;
        int copyRaw = 0;
        int chipRaw = 0;
        if (word.find("next:") == string::npos) {
          return false;
        }
        iss >> chipRaw;
        if (iss.fail()) {
          return false;
        }
        iss >> copyRaw;
        if (iss.fail()) {
          return false;
        }

        if (chipRaw > 2 or copyRaw > 2) {
          return false;
        }
        rf.mechanismNextChip = static_cast<xsc::Chip>(chipRaw);
        rf.mechanismNextCopy = static_cast<xsc::Copy>(copyRaw);
        break;
      }
    }
    if (iss.fail()) {
      return false;
    }
    lineIdx++;
  }
  if (lineIdx < 12) {
    return false;
  }
  return true;
}

void CoreController::resetRebootCount(xsc::Chip tgtChip, xsc::Copy tgtCopy) {
  std::string path = currMntPrefix + REBOOT_FILE;
  // Disable the reboot file mechanism
  parseRebootFile(path, rebootFile);
  if (tgtChip == xsc::ALL_CHIP and tgtCopy == xsc::ALL_COPY) {
    rebootFile.img00Cnt = 0;
    rebootFile.img01Cnt = 0;
    rebootFile.img10Cnt = 0;
    rebootFile.img11Cnt = 0;
  } else {
    if (tgtChip == xsc::CHIP_0) {
      if (tgtCopy == xsc::COPY_0) {
        rebootFile.img00Cnt = 0;
      } else {
        rebootFile.img01Cnt = 0;
      }
    } else {
      if (tgtCopy == xsc::COPY_0) {
        rebootFile.img10Cnt = 0;
      } else {
        rebootFile.img11Cnt = 0;
      }
    }
  }
  rewriteRebootFile(rebootFile);
}

void CoreController::rewriteRebootFile(RebootFile file) {
  std::string path = currMntPrefix + REBOOT_FILE;
  std::ofstream rebootFile(path);
  if (rebootFile.is_open()) {
    // Initiate reboot file first. Reboot handling will be on on initialization
    rebootFile << "on: " << file.enabled << "\nmaxcnt: " << file.maxCount
               << "\nimg00: " << file.img00Cnt << "\nimg01: " << file.img01Cnt
               << "\nimg10: " << file.img10Cnt << "\nimg11: " << file.img11Cnt
               << "\nimg00lock: " << file.img00Lock << "\nimg01lock: " << file.img01Lock
               << "\nimg10lock: " << file.img10Lock << "\nimg11lock: " << file.img11Lock
               << "\nbootflag: " << file.bootFlag << "\nlast: " << static_cast<int>(file.lastChip)
               << " " << static_cast<int>(file.lastCopy)
               << "\nnext: " << static_cast<int>(file.mechanismNextChip) << " "
               << static_cast<int>(file.mechanismNextCopy) << "\n";
  }
}

void CoreController::setRebootMechanismLock(bool lock, xsc::Chip tgtChip, xsc::Copy tgtCopy) {
  std::string path = currMntPrefix + REBOOT_FILE;
  // Disable the reboot file mechanism
  parseRebootFile(path, rebootFile);
  if (tgtChip == xsc::CHIP_0) {
    if (tgtCopy == xsc::COPY_0) {
      rebootFile.img00Lock = lock;
    } else {
      rebootFile.img01Lock = lock;
    }
  } else {
    if (tgtCopy == xsc::COPY_0) {
      rebootFile.img10Lock = lock;
    } else {
      rebootFile.img11Lock = lock;
    }
  }
  rewriteRebootFile(rebootFile);
}

ReturnValue_t CoreController::backupTimeFileHandler() {
  // Always set time. We could only set it if it is updated by GPS, but then the backup time would
  // become obsolete on GPS problems.
  if (opDivider10.check()) {
    // It is assumed that the system time is set from the GPS time
    timeval currentTime = {};
    ReturnValue_t result = Clock::getClock_timeval(&currentTime);
    if (result != returnvalue::OK) {
      return result;
    }
    std::string fileName = currMntPrefix + BACKUP_TIME_FILE;
    std::ofstream timeFile(fileName);
    if (not timeFile.good()) {
      sif::error << "CoreController::timeFileHandler: Error opening time file: " << strerror(errno)
                 << std::endl;
      return returnvalue::FAILED;
    }
    timeFile << "UNIX SECONDS: " << currentTime.tv_sec + BOOT_OFFSET_SECONDS << std::endl;
  }
  return returnvalue::OK;
}

ReturnValue_t CoreController::initClockFromTimeFile() {
  using namespace GpsHyperion;
  using namespace std;
  std::string fileName = currMntPrefix + BACKUP_TIME_FILE;
  if (sdcMan->isSdCardUsable(std::nullopt) and std::filesystem::exists(fileName) and
      ((gpsFix == FixMode::UNKNOWN or gpsFix == FixMode::NOT_SEEN) or
       not utility::timeSanityCheck())) {
    ifstream timeFile(fileName);
    string nextWord;
    getline(timeFile, nextWord);
    istringstream iss(nextWord);
    iss >> nextWord;
    if (iss.bad() or nextWord != "UNIX") {
      return returnvalue::FAILED;
    }
    iss >> nextWord;
    if (iss.bad() or nextWord != "SECONDS:") {
      return returnvalue::FAILED;
    }
    iss >> nextWord;
    timeval currentTime = {};
    char *checkPtr;
    currentTime.tv_sec = strtol(nextWord.c_str(), &checkPtr, 10);
    if (iss.bad() or *checkPtr) {
      return returnvalue::FAILED;
    }
#if OBSW_VERBOSE_LEVEL >= 1
    time_t timeRaw = currentTime.tv_sec;
    std::tm *time = std::gmtime(&timeRaw);
    sif::info << "Setting system time from time files: " << std::put_time(time, "%c %Z")
              << std::endl;
#endif
    timeFileInitDone = true;
    return Clock::setClock(&currentTime);
  }
  return returnvalue::OK;
}

void CoreController::readHkData() {
  ReturnValue_t result = returnvalue::OK;
  result = hkSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
  if (result != returnvalue::OK) {
    return;
  }
  Xadc xadc;
  result = xadc.getTemperature(hkSet.temperature.value);
  if (result != returnvalue::OK) {
    hkSet.temperature.setValid(false);
  } else {
    hkSet.temperature.setValid(true);
  }
  result = xadc.getVccPint(hkSet.psVoltage.value);
  if (result != returnvalue::OK) {
    hkSet.psVoltage.setValid(false);
  } else {
    hkSet.psVoltage.setValid(true);
  }
  result = xadc.getVccInt(hkSet.plVoltage.value);
  if (result != returnvalue::OK) {
    hkSet.plVoltage.setValid(false);
  } else {
    hkSet.plVoltage.setValid(true);
  }
#if OBSW_PRINT_CORE_HK == 1
  hkSet.printSet();
#endif /* OBSW_PRINT_CORE_HK == 1 */
  result = hkSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
  if (result != returnvalue::OK) {
    return;
  }
}

const char *CoreController::getXscMountDir(xsc::Chip chip, xsc::Copy copy) {
  if (chip == xsc::Chip::CHIP_0) {
    if (copy == xsc::Copy::COPY_0) {
      return CHIP_0_COPY_0_MOUNT_DIR;
    } else if (copy == xsc::Copy::COPY_1) {
      return CHIP_0_COPY_1_MOUNT_DIR;
    }
  } else if (chip == xsc::Chip::CHIP_1) {
    if (copy == xsc::Copy::COPY_0) {
      return CHIP_1_COPY_0_MOUNT_DIR;
    } else if (copy == xsc::Copy::COPY_1) {
      return CHIP_1_COPY_1_MOUNT_DIR;
    }
  }
  sif::error << "Invalid chip or copy passed to CoreController::getXscMountDir" << std::endl;
  return CHIP_0_COPY_0_MOUNT_DIR;
}

ReturnValue_t CoreController::executeSwUpdate(SwUpdateSources sourceDir, const uint8_t *data,
                                              size_t size) {
  using namespace std;
  using namespace std::filesystem;
  // At the very least, chip and copy ID need to be included in the command
  if (size < 2) {
    return HasActionsIF::INVALID_PARAMETERS;
  }
  if (data[0] > 1 or data[1] > 1) {
    return HasActionsIF::INVALID_PARAMETERS;
  }
  auto chip = static_cast<xsc::Chip>(data[0]);
  auto copy = static_cast<xsc::Copy>(data[1]);
  const char *sourceStr = "unknown";
  if (sourceDir == SwUpdateSources::SD_0) {
    sourceStr = "SD 0";
  } else if (sourceDir == SwUpdateSources::SD_1) {
    sourceStr = "SD 1";
  } else {
    sourceStr = "tmp directory";
  }
  bool sameChipAndCopy = false;
  if (chip == CURRENT_CHIP and copy == CURRENT_COPY) {
    // This is problematic if the OBSW is running as a systemd service.
    // Do not allow for now.
    return HasActionsIF::INVALID_PARAMETERS;
    // sameChipAndCopy = true;
  }
  sif::info << "Executing SW update for Chip " << static_cast<int>(data[0]) << " Copy "
            << static_cast<int>(data[1]) << " from " << sourceStr << std::endl;
  path prefixPath;
  if (sourceDir == SwUpdateSources::SD_0) {
    prefixPath = path(config::SD_0_MOUNT_POINT);
  } else if (sourceDir == SwUpdateSources::SD_1) {
    prefixPath = path(config::SD_1_MOUNT_POINT);
  } else if (sourceDir == SwUpdateSources::TMP_DIR) {
    prefixPath = path("/tmp");
  }
  path archivePath = prefixPath / path(config::OBSW_UPDATE_ARCHIVE_FILE_NAME);
  if (not exists(archivePath)) {
    return HasFileSystemIF::FILE_DOES_NOT_EXIST;
  }
  ostringstream cmd("tar -xJf", ios::app);
  cmd << " " << archivePath << " -C " << prefixPath;
  int result = system(cmd.str().c_str());
  if (result != 0) {
    utility::handleSystemError(result, "CoreController::executeAction: SW Update Decompression");
  }
  path strippedImagePath = prefixPath / path(config::STRIPPED_OBSW_BINARY_FILE_NAME);
  if (!exists(strippedImagePath)) {
    // TODO: Custom returnvalue?
    return returnvalue::FAILED;
  }
  path obswVersionFilePath = prefixPath / path(config::OBSW_VERSION_FILE_NAME);
  if (!exists(obswVersionFilePath)) {
    // TODO: Custom returnvalue?
    return returnvalue::FAILED;
  }
  cmd.str("");
  cmd.clear();
  path obswDestPath;
  path obswVersionDestPath;
  if (not sameChipAndCopy) {
    cmd << "xsc_mount_copy " << std::to_string(data[0]) << " " << std::to_string(data[1]);
    result = system(cmd.str().c_str());
    if (result != 0) {
      std::string contextString = "CoreController::executeAction: SW Update Mounting " +
                                  std::to_string(data[0]) + " " + std::to_string(data[1]);
      utility::handleSystemError(result, contextString);
    }
    cmd.str("");
    cmd.clear();
    path xscMountDest(getXscMountDir(chip, copy));
    obswDestPath = xscMountDest / path(relative(config::OBSW_PATH, "/"));
    obswVersionDestPath = xscMountDest / path(relative(config::OBSW_VERSION_FILE_PATH, "/"));
  } else {
    obswDestPath = path(config::OBSW_PATH);
    obswVersionDestPath = path(config::OBSW_VERSION_FILE_PATH);
    cmd << "writeprotect " << std::to_string(CURRENT_CHIP) << " " << std::to_string(CURRENT_COPY)
        << " 0";
    result = system(cmd.str().c_str());
    if (result != 0) {
      std::string contextString = "CoreController::executeAction: Unlocking current chip";
      utility::handleSystemError(result, contextString);
    }
    cmd.str("");
    cmd.clear();
  }

  cmd << "cp " << strippedImagePath << " " << obswDestPath;
  result = system(cmd.str().c_str());
  if (result != 0) {
    utility::handleSystemError(result, "CoreController::executeAction: Copying SW update");
  }
  cmd.str("");
  cmd.clear();

  cmd << "cp " << obswVersionFilePath << " " << obswVersionDestPath;
  result = system(cmd.str().c_str());
  if (result != 0) {
    utility::handleSystemError(result, "CoreController::executeAction: Copying SW version file");
  }
  cmd.str("");
  cmd.clear();

  // Set correct permission for both files
  cmd << "chmod 0755 " << obswDestPath;
  result = system(cmd.str().c_str());
  if (result != 0) {
    utility::handleSystemError(result,
                               "CoreController::executeAction: Setting SW permissions 0755");
  }
  cmd.str("");
  cmd.clear();

  cmd << "chmod 0644 " << obswVersionDestPath;
  result = system(cmd.str().c_str());
  if (result != 0) {
    utility::handleSystemError(
        result, "CoreController::executeAction: Setting version file permission 0644");
  }
  cmd.str("");
  cmd.clear();

  // TODO: This takes a long time and will block the core controller.. Maybe use command executor?
  //       For now dont care..
  cmd << "writeprotect " << std::to_string(data[0]) << " " << std::to_string(data[1]) << " 1";
  result = system(cmd.str().c_str());
  if (result != 0) {
    std::string contextString = "CoreController::executeAction: Writeprotecting " +
                                std::to_string(data[0]) + " " + std::to_string(data[1]);
    utility::handleSystemError(result, contextString);
  }
  sif::info << "SW update complete" << std::endl;
  return HasActionsIF::EXECUTION_FINISHED;
}

bool CoreController::startSdStateMachine(sd::SdCard targetActiveSd, SdCfgMode mode,
                                         MessageQueueId_t commander, DeviceCommandId_t actionId) {
  if (sdFsmState != SdStates::IDLE or sdCommandingInfo.cmdPending) {
    return false;
  }
  sdFsmState = SdStates::START;
  sdInfo.active = targetActiveSd;
  sdInfo.cfgMode = mode;
  sdCommandingInfo.actionId = actionId;
  sdCommandingInfo.commander = commander;
  sdCommandingInfo.cmdPending = true;
  return true;
}

bool CoreController::isNumber(const std::string &s) {
  return !s.empty() && std::find_if(s.begin(), s.end(),
                                    [](unsigned char c) { return !std::isdigit(c); }) == s.end();
}