#include "CoreController.h"

#include <fsfw/events/EventManager.h>

#include "OBSWConfig.h"
#include "OBSWVersion.h"
#include "fsfw/FSFWVersion.h"
#include "fsfw/serviceinterface/ServiceInterface.h"
#include "fsfw/timemanager/Stopwatch.h"
#include "watchdogConf.h"
#if OBSW_USE_TMTC_TCP_BRIDGE == 0
#include "fsfw/osal/common/UdpTmTcBridge.h"
#else
#include "fsfw/osal/common/TcpTmTcServer.h"
#endif

#include <fcntl.h>
#include <unistd.h>

#include <filesystem>

#include "bsp_q7s/memory/SdCardManager.h"
#include "bsp_q7s/memory/scratchApi.h"

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

CoreController::CoreController(object_id_t objectId)
    : ExtendedControllerBase(objectId, objects::NO_OBJECT, 5), opDivider(5) {
  ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
  try {
    result = initWatchdogFifo();
    if (result != HasReturnvaluesIF::RETURN_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);
    }
    sdStateMachine();

    result = initBootCopy();
    if (result != HasReturnvaluesIF::RETURN_OK) {
      sif::warning << "CoreController::CoreController: Boot copy init" << std::endl;
    }
  } catch (const std::filesystem::filesystem_error &e) {
    sif::error << "CoreController::CoreController: Failed with exception " << e.what() << std::endl;
  }
}

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

void CoreController::performControlOperation() {
  performWatchdogControlOperation();
  sdStateMachine();
}

ReturnValue_t CoreController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                      LocalDataPoolManager &poolManager) {
  return HasReturnvaluesIF::RETURN_OK;
}

LocalPoolDataSetBase *CoreController::getDataSetHandle(sid_t sid) { return nullptr; }

ReturnValue_t CoreController::initialize() {
  ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;

  result = scratch::writeNumber(scratch::ALLOC_FAILURE_COUNT, 0);
  if (result != HasReturnvaluesIF::RETURN_OK) {
    sif::warning << "CoreController::initialize: Setting up alloc failure "
                    "count failed"
                 << std::endl;
  }

  sdStateMachine();
  return ExtendedControllerBase::initialize();
}

ReturnValue_t CoreController::initializeAfterTaskCreation() {
  ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
  if (BLOCKING_SD_INIT) {
    ReturnValue_t result = initSdCardBlocking();
    if (result != HasReturnvaluesIF::RETURN_OK and result != SdCardManager::ALREADY_MOUNTED) {
      sif::warning << "CoreController::CoreController: SD card init failed" << std::endl;
    }
  }
  sdStateMachine();
  result = initVersionFile();
  if (result != HasReturnvaluesIF::RETURN_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";
  setenv("PATH", updatedEnvPath.c_str(), true);
  updateProtInfo();
  initPrint();
  auto eventManager = ObjectManager::instance()->get<EventManager>(objects::EVENT_MANAGER);
  if (eventManager != nullptr) {
    eventManager->printListeners();
  }
  return result;
}

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

ReturnValue_t CoreController::initSdCardBlocking() {
  // Create update status file
  ReturnValue_t result = sdcMan->updateSdCardStateFile();
  if (result != HasReturnvaluesIF::RETURN_OK) {
    sif::warning << "CoreController::initialize: Updating SD card state file failed" << std::endl;
  }
#if Q7S_SD_CARD_CONFIG == Q7S_SD_NONE
  sif::info << "No SD card initialization will be performed" << std::endl;
  return HasReturnvaluesIF::RETURN_OK;
#else

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

#if Q7S_SD_CARD_CONFIG == Q7S_SD_COLD_REDUNDANT
  determinePreferredSdCard();
  updateSdInfoOther();
  sif::info << "Cold redundant SD card configuration, preferred SD card: "
            << static_cast<int>(sdInfo.pref) << std::endl;
  result = sdColdRedundantBlockingInit();
  // Update status file
  sdcMan->updateSdCardStateFile();
  return result;
#elif Q7S_SD_CARD_CONFIG == Q7S_SD_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 HasReturnvaluesIF::RETURN_OK;
#endif

#endif /* Q7S_SD_CARD_CONFIG != Q7S_SD_NONE */
}

ReturnValue_t CoreController::sdStateMachine() {
  ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
  SdCardManager::Operations operation;

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

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

  // 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) {
      sdInfo.state = newStateOnSuccess;
      sdInfo.commandExecuted = false;
      sdInfo.cycleCount = 0;
      return true;
    } else if (sdInfo.cycleCount > 4) {
      sif::warning << "CoreController::sdInitStateMachine: " << opPrintout << " takes too long"
                   << std::endl;
      return false;
    }
    return false;
  };

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

  if (sdInfo.state == SdStates::SET_STATE_SELF) {
    if (not sdInfo.commandExecuted) {
      result = sdcMan->getSdCardActiveStatus(sdInfo.currentState);
      determinePreferredSdCard();
      updateSdInfoOther();
      if (sdInfo.pref != sd::SdCard::SLOT_0 and sdInfo.pref != sd::SdCard::SLOT_1) {
        sif::warning << "Preferred SD card invalid. Setting to card 0.." << std::endl;
        sdInfo.pref = sd::SdCard::SLOT_0;
      }
      if (result != HasReturnvaluesIF::RETURN_OK) {
        sif::warning << "Getting SD card activity status failed" << std::endl;
      }
#if Q7S_SD_CARD_CONFIG == Q7S_SD_COLD_REDUNDANT
      sif::info << "Cold redundant SD card configuration, preferred SD card: "
                << static_cast<int>(sdInfo.pref) << std::endl;
#endif
      if (sdInfo.prefState == sd::SdState::MOUNTED) {
#if OBSW_VERBOSE_LEVEL >= 1
        std::string mountString;
        if (sdInfo.pref == sd::SdCard::SLOT_0) {
          mountString = SdCardManager::SD_0_MOUNT_POINT;
        } else {
          mountString = SdCardManager::SD_1_MOUNT_POINT;
        }
        sif::info << "SD card " << sdInfo.prefChar << " already on and mounted at " << mountString
                  << std::endl;
#endif
        sdInfo.state = SdStates::DETERMINE_OTHER;
      } else if (sdInfo.prefState == sd::SdState::OFF) {
        sdCardSetup(sdInfo.pref, sd::SdState::ON, sdInfo.prefChar, false);
        sdInfo.commandExecuted = true;
      } else if (sdInfo.prefState == sd::SdState::ON) {
        sdInfo.state = SdStates::MOUNT_SELF;
      }
    } else {
      if (nonBlockingOpChecking(SdStates::MOUNT_SELF, 10, "Setting SDC state")) {
        sdInfo.prefState = sd::SdState::ON;
        currentStateSetter(sdInfo.pref, sd::SdState::ON);
      }
    }
  }

  if (sdInfo.state == SdStates::MOUNT_SELF) {
    if (not sdInfo.commandExecuted) {
      result = sdCardSetup(sdInfo.pref, sd::SdState::MOUNTED, sdInfo.prefChar);
      sdInfo.commandExecuted = true;
    } else {
      if (nonBlockingOpChecking(SdStates::DETERMINE_OTHER, 5, "Mounting SD card")) {
        sdInfo.prefState = sd::SdState::MOUNTED;
        currentStateSetter(sdInfo.pref, sd::SdState::MOUNTED);
      }
    }
  }

  if (sdInfo.state == 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 Q7S_SD_CARD_CONFIG == Q7S_SD_COLD_REDUNDANT
    if (sdInfo.otherState == sd::SdState::ON) {
      sdInfo.state = SdStates::SET_STATE_OTHER;
    } else if (sdInfo.otherState == sd::SdState::MOUNTED) {
      sdInfo.state = SdStates::MOUNT_UNMOUNT_OTHER;
    } else {
      // Is already off, update info, but with a small delay
      sdInfo.state = SdStates::SKIP_CYCLE_BEFORE_INFO_UPDATE;
    }
#elif Q7S_SD_CARD_CONFIG == Q7S_SD_HOT_REDUNDANT
    if (sdInfo.otherState == sd::SdState::OFF) {
      sdInfo.state = SdStates::SET_STATE_OTHER;
    } else if (sdInfo.otherState == sd::SdState::ON) {
      sdInfo.state = SdStates::MOUNT_UNMOUNT_OTHER;
    } else {
      // Is already on and mounted, update info
      sdInfo.state = SdStates::SKIP_CYCLE_BEFORE_INFO_UPDATE;
    }
#endif
  }

  if (sdInfo.state == SdStates::SET_STATE_OTHER) {
    // Set state of other SD card to ON or OFF, depending on redundancy mode
#if Q7S_SD_CARD_CONFIG == Q7S_SD_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);
      }
    }
#elif Q7S_SD_CARD_CONFIG == Q7S_SD_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);
      }
    }
#endif
  }

  if (sdInfo.state == SdStates::MOUNT_UNMOUNT_OTHER) {
    // Mount or unmount other SD card, depending on redundancy mode
#if Q7S_SD_CARD_CONFIG == Q7S_SD_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);
      }
    }
#elif Q7S_SD_CARD_CONFIG == Q7S_SD_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);
      }
    }
#endif
  }

  if (sdInfo.state == SdStates::SKIP_CYCLE_BEFORE_INFO_UPDATE) {
    sdInfo.state = SdStates::UPDATE_INFO;
  } else if (sdInfo.state == 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 != HasReturnvaluesIF::RETURN_OK) {
      sif::warning << "CoreController::initialize: Updating SD card state file failed" << std::endl;
    }
    sdInfo.commandExecuted = false;
    sdInfo.state = SdStates::IDLE;
    sdInfo.cycleCount = 0;
    sdcMan->setBlocking(false);
    sdcMan->getSdCardActiveStatus(sdInfo.currentState);
    if (not sdInfo.initFinished) {
      updateSdInfoOther();
      sdInfo.initFinished = true;
      sif::info << "SD card initialization finished" << std::endl;
    }
  }

  if (sdInfo.state == SdStates::SET_STATE_FROM_COMMAND) {
    if (not sdInfo.commandExecuted) {
      executeNextExternalSdCommand();
    } else {
      checkExternalSdCommandStatus();
    }
  }

  sdInfo.cycleCount++;
  return HasReturnvaluesIF::RETURN_OK;
}

void CoreController::executeNextExternalSdCommand() {
  std::string sdChar;
  sd::SdState currentStateOfCard = sd::SdState::OFF;
  if (sdInfo.commandedCard == sd::SdCard::SLOT_0) {
    sdChar = "0";
    currentStateOfCard = sdInfo.currentState.first;
  } else {
    sdChar = "1";
    currentStateOfCard = sdInfo.currentState.second;
  }
  if (currentStateOfCard == sd::SdState::OFF) {
    if (sdInfo.commandedState == sd::SdState::ON) {
      sdInfo.currentlyCommandedState = sdInfo.commandedState;
    } else if (sdInfo.commandedState == sd::SdState::MOUNTED) {
      sdInfo.currentlyCommandedState = sd::SdState::ON;
    } else {
      // SD card is already on target state
      sdInfo.commandFinished = true;
      sdInfo.state = SdStates::IDLE;
    }
  } else if (currentStateOfCard == sd::SdState::ON) {
    if (sdInfo.commandedState == sd::SdState::OFF or
        sdInfo.commandedState == sd::SdState::MOUNTED) {
      sdInfo.currentlyCommandedState = sdInfo.commandedState;
    } else {
      // Already on target state
      sdInfo.commandFinished = true;
      sdInfo.state = SdStates::IDLE;
    }
  } else if (currentStateOfCard == sd::SdState::MOUNTED) {
    if (sdInfo.commandedState == sd::SdState::ON) {
      sdInfo.currentlyCommandedState = sdInfo.commandedState;
    } else if (sdInfo.commandedState == sd::SdState::OFF) {
      // This causes an unmount in sdCardSetup
      sdInfo.currentlyCommandedState = sd::SdState::ON;
    } else {
      sdInfo.commandFinished = true;
    }
  }
  sdCardSetup(sdInfo.commandedCard, sdInfo.commandedState, sdChar);
  sdInfo.commandExecuted = true;
}

void CoreController::checkExternalSdCommandStatus() {
  SdCardManager::Operations operation;
  SdCardManager::OpStatus status = sdcMan->checkCurrentOp(operation);
  if (status == SdCardManager::OpStatus::SUCCESS) {
    if (sdInfo.currentlyCommandedState == sdInfo.commandedState) {
      sdInfo.state = SdStates::SKIP_CYCLE_BEFORE_INFO_UPDATE;
      sdInfo.commandFinished = true;
    } else {
      // stay on same state machine state because the target state was not reached yet.
      sdInfo.cycleCount = 0;
    }
    currentStateSetter(sdInfo.commandedCard, sdInfo.currentlyCommandedState);
    sdInfo.commandExecuted = false;
  } else if (sdInfo.cycleCount > 4) {
    sif::warning << "CoreController::sdStateMachine: Commanding SD state "
                    "takes too long"
                 << std::endl;
  }
}

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 = SdCardManager::SD_0_MOUNT_POINT;
  } else {
    mountString = SdCardManager::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 HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
                                            const uint8_t *data, size_t size) {
  switch (actionId) {
    case (LIST_DIRECTORY_INTO_FILE): {
      return actionListDirectoryIntoFile(actionId, commandedBy, data, size);
    }
    case (REBOOT_OBC): {
      return actionPerformReboot(data, size);
    }
    default: {
      return HasActionsIF::INVALID_ACTION_ID;
    }
  }
}

ReturnValue_t CoreController::sdColdRedundantBlockingInit() {
  ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;

  result = sdCardSetup(sdInfo.pref, sd::SdState::MOUNTED, sdInfo.prefChar);
  if (result != SdCardManager::ALREADY_MOUNTED and result != HasReturnvaluesIF::RETURN_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.pref, sd::SdState::MOUNTED, sdInfo.prefChar);
    result = HasReturnvaluesIF::RETURN_FAILED;
  }

  if (result != HasReturnvaluesIF::RETURN_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 != HasReturnvaluesIF::RETURN_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 != HasReturnvaluesIF::RETURN_OK) {
    return result;
  }
  count++;
  return scratch::writeNumber(scratch::ALLOC_FAILURE_COUNT, count);
}

ReturnValue_t CoreController::initVersionFile() {
  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(SW_VERSION) + "." +
                                      std::to_string(SW_SUBVERSION) + "." +
                                      std::to_string(SW_REVISION);
  std::string fullFsfwVersionString = "FSFW: v" + std::to_string(FSFW_VERSION) + "." +
                                      std::to_string(FSFW_SUBVERSION) + "." +
                                      std::to_string(FSFW_REVISION);
  std::string systemString = "System: " + unameLine;
  std::string mountPrefix = SdCardManager::instance()->getCurrentMountPrefix();
  std::string versionFilePath = mountPrefix + 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 HasReturnvaluesIF::RETURN_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 HasReturnvaluesIF::RETURN_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 HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t CoreController::initBootCopy() {
  if (not std::filesystem::exists(CURR_COPY_FILE)) {
    // Thils 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");
    }
  }
  std::ifstream file(CURR_COPY_FILE);
  std::string line;
  std::getline(file, line);
  std::istringstream iss(line);
  int value = 0;
  iss >> value;
  CURRENT_CHIP = static_cast<Chip>(value);
  iss >> value;
  CURRENT_COPY = static_cast<Copy>(value);
  return HasReturnvaluesIF::RETURN_OK;
}

void CoreController::getCurrentBootCopy(Chip &chip, Copy &copy) {
  // Not really thread-safe but it does not need to be
  chip = CURRENT_CHIP;
  copy = CURRENT_COPY;
}

ReturnValue_t CoreController::initWatchdogFifo() {
  if (not std::filesystem::exists(watchdog::FIFO_NAME)) {
    // Still return RETURN_OK for now
    sif::info << "Watchdog FIFO " << watchdog::FIFO_NAME << " does not exist, can't initiate"
              << " watchdog" << std::endl;
    return HasReturnvaluesIF::RETURN_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 HasReturnvaluesIF::RETURN_FAILED;
    }
  }
  return HasReturnvaluesIF::RETURN_OK;
}

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

ReturnValue_t CoreController::actionPerformReboot(const uint8_t *data, size_t size) {
  if (size < 1) {
    return HasActionsIF::INVALID_PARAMETERS;
  }
  bool rebootSameBootCopy = data[0];
  bool protOpPerformed;
  if (rebootSameBootCopy) {
#if OBSW_VERBOSE_LEVEL >= 1
    sif::info << "CoreController::actionPerformReboot: Rebooting on current image" << std::endl;
#endif
    // Attempt graceful shutdown by unmounting and switching off SD cards
    SdCardManager::instance()->switchOffSdCard(sd::SdCard::SLOT_0);
    SdCardManager::instance()->switchOffSdCard(sd::SdCard::SLOT_1);
    // If any boot copies are unprotected
    ReturnValue_t retval =
        setBootCopyProtection(Chip::SELF_CHIP, Copy::SELF_COPY, true, protOpPerformed, false);
    if (retval == HasReturnvaluesIF::RETURN_OK and protOpPerformed) {
      sif::info << "Running slot was writeprotected before reboot" << std::endl;
    }
    int result = std::system("xsc_boot_copy -r");
    if (result != 0) {
      utility::handleSystemError(result, "CoreController::executeAction");
      return HasReturnvaluesIF::RETURN_FAILED;
    }
    return HasActionsIF::EXECUTION_FINISHED;
  }
  if (size < 3) {
    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
  ReturnValue_t retval = setBootCopyProtection(
      static_cast<Chip>(data[1]), static_cast<Copy>(data[2]), true, protOpPerformed, false);
  if (retval == HasReturnvaluesIF::RETURN_OK and protOpPerformed) {
    sif::info << "Target slot was writeprotected before reboot" << std::endl;
  }

  // The second byte in data is the target chip, the third byte is the target copy
  std::string cmdString =
      "xsc_boot_copy " + std::to_string(data[1]) + " " + std::to_string(data[2]);
  int result = std::system(cmdString.c_str());
  if (result != 0) {
    utility::handleSystemError(result, "CoreController::executeAction");
    return HasReturnvaluesIF::RETURN_FAILED;
  }
  return HasActionsIF::EXECUTION_FINISHED;
}

CoreController::~CoreController() {}

void CoreController::determinePreferredSdCard() {
  if (sdInfo.pref == sd::SdCard::NONE) {
    ReturnValue_t result = sdcMan->getPreferredSdCard(sdInfo.pref);
    if (result != HasReturnvaluesIF::RETURN_OK) {
      if (result == scratch::KEY_NOT_FOUND) {
        sif::warning << "CoreController::sdCardInit: "
                        "Preferred SD card not set. Setting to 0"
                     << std::endl;
        sdcMan->setPreferredSdCard(sd::SdCard::SLOT_0);
        sdInfo.pref = sd::SdCard::SLOT_0;
      } else {
        sif::warning << "CoreController::sdCardInit: Could not get preferred SD card"
                        "information from the scratch buffer"
                     << std::endl;
      }
    }
  }
}

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

  } else if (sdInfo.pref == sd::SdCard::SLOT_1) {
    sdInfo.prefChar = "1";
    sdInfo.otherChar = "0";
    sdInfo.otherState = sdInfo.currentState.first;
    sdInfo.prefState = 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 HasReturnvaluesIF::RETURN_FAILED;
  }
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t CoreController::setBootCopyProtection(Chip targetChip, 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 (Chip::ALL_CHIP): {
      allChips = true;
      break;
    }
    case (Chip::NO_CHIP): {
      return HasReturnvaluesIF::RETURN_OK;
    }
    case (Chip::SELF_CHIP): {
      selfChip = true;
      targetChip = CURRENT_CHIP;
      break;
    }
    default: {
      break;
    }
  }
  switch (targetCopy) {
    case (Copy::ALL_COPY): {
      allCopies = true;
      break;
    }
    case (Copy::NO_COPY): {
      return HasReturnvaluesIF::RETURN_OK;
    }
    case (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 HasReturnvaluesIF::RETURN_OK;
}

int CoreController::handleBootCopyProtAtIndex(Chip targetChip, 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;
    }
  }
  Chip currentChip;
  Copy currentCopy;
  oss << "writeprotect ";
  if (arrIdx == 0 or arrIdx == 1) {
    oss << "0 ";
    currentChip = Chip::CHIP_0;
  } else {
    oss << "1 ";
    currentChip = Chip::CHIP_1;
  }
  if (arrIdx == 0 or arrIdx == 2) {
    oss << "0 ";
    currentCopy = Copy::COPY_0;
  } else {
    oss << "1 ";
    currentCopy = 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 = HasReturnvaluesIF::RETURN_OK;
  if (regenerateChipStateFile) {
    result = generateChipStateFile();
    if (result != HasReturnvaluesIF::RETURN_OK) {
      sif::warning << "CoreController::updateProtInfo: Generating chip state file failed"
                   << std::endl;
      return result;
    }
  }
  if (not filesystem::exists(CHIP_STATE_FILE)) {
    return HasReturnvaluesIF::RETURN_FAILED;
  }
  ifstream chipStateFile(CHIP_STATE_FILE);
  if (not chipStateFile.good()) {
    return HasReturnvaluesIF::RETURN_FAILED;
  }
  string nextLine;
  uint8_t lineCounter = 0;
  string word;
  while (getline(chipStateFile, nextLine)) {
    ReturnValue_t result = handleProtInfoUpdateLine(nextLine);
    if (result != HasReturnvaluesIF::RETURN_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 HasReturnvaluesIF::RETURN_OK;
}

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

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

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

void CoreController::performWatchdogControlOperation() {
  // Only perform each fifth iteration
  if (watchdogFifoFd != 0 and opDivider.checkAndIncrement()) {
    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;
      }
    }
  }
}