#include "CoreController.h"

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

#include "HasActionsIF.h"
#include "SdCardManager.h"
#include "conf.h"
#include "event.h"
#include "fsfw/serviceinterface/ServiceInterface.h"
#include "libxiphos.h"

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

CoreController::CoreController() {
  sdcMan = new SdCardManager();
  setCurrentBootCopy(xsc::CHIP_0, xsc::COPY_0);
}

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

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

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

  if (rebootWatchdogFile.mechanismNextChip != xsc::NO_CHIP and
      rebootWatchdogFile.mechanismNextCopy != xsc::NO_COPY) {
    if (CURRENT_CHIP != rebootWatchdogFile.mechanismNextChip or
        CURRENT_COPY != rebootWatchdogFile.mechanismNextCopy) {
      std::string infoString = std::to_string(rebootWatchdogFile.mechanismNextChip) + " " +
                               std::to_string(rebootWatchdogFile.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 (rebootWatchdogFile.mechanismNextChip == xsc::CHIP_0) {
        if (rebootWatchdogFile.mechanismNextCopy == xsc::COPY_0) {
          rebootWatchdogFile.img00Lock = true;
        } else {
          rebootWatchdogFile.img01Lock = true;
        }
      } else {
        if (rebootWatchdogFile.mechanismNextCopy == xsc::COPY_0) {
          rebootWatchdogFile.img10Lock = true;
        } else {
          rebootWatchdogFile.img11Lock = true;
        }
      }
    }
  }

  rebootWatchdogFile.lastChip = CURRENT_CHIP;
  rebootWatchdogFile.lastCopy = CURRENT_COPY;
  // Only reboot if the reboot functionality is enabled.
  // The handler will still increment the boot counts
  if (rebootWatchdogFile.enabled and
      (*rebootWatchdogFile.relevantBootCnt >= rebootWatchdogFile.maxCount)) {
    // Reboot to other image
    bool doReboot = false;
    xsc::Chip tgtChip = xsc::NO_CHIP;
    xsc::Copy tgtCopy = xsc::NO_COPY;
    determineAndExecuteReboot(rebootWatchdogFile, doReboot, tgtChip, tgtCopy);
    if (doReboot) {
      rebootWatchdogFile.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
      rebootWatchdogFile.mechanismNextChip = tgtChip;
      rebootWatchdogFile.mechanismNextCopy = tgtCopy;
      rewriteRebootWatchdogFile(rebootWatchdogFile);
      xsc_boot_copy(static_cast<xsc_libnor_chip_t>(tgtChip),
                    static_cast<xsc_libnor_copy_t>(tgtCopy));
    }
  } else {
    rebootWatchdogFile.mechanismNextChip = xsc::NO_CHIP;
    rebootWatchdogFile.mechanismNextCopy = xsc::NO_COPY;
  }
  rewriteRebootWatchdogFile(rebootWatchdogFile);
}

void CoreController::determineAndExecuteReboot(RebootWatchdogFile &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::parseRebootWatchdogFile(std::string path, RebootWatchdogFile &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 = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_WATCHDOG_FILE;
  // Disable the reboot file mechanism
  parseRebootWatchdogFile(path, rebootWatchdogFile);
  if (tgtChip == xsc::ALL_CHIP and tgtCopy == xsc::ALL_COPY) {
    rebootWatchdogFile.img00Cnt = 0;
    rebootWatchdogFile.img01Cnt = 0;
    rebootWatchdogFile.img10Cnt = 0;
    rebootWatchdogFile.img11Cnt = 0;
  } else {
    if (tgtChip == xsc::CHIP_0) {
      if (tgtCopy == xsc::COPY_0) {
        rebootWatchdogFile.img00Cnt = 0;
      } else {
        rebootWatchdogFile.img01Cnt = 0;
      }
    } else {
      if (tgtCopy == xsc::COPY_0) {
        rebootWatchdogFile.img10Cnt = 0;
      } else {
        rebootWatchdogFile.img11Cnt = 0;
      }
    }
  }
  rewriteRebootWatchdogFile(rebootWatchdogFile);
}

void CoreController::rewriteRebootWatchdogFile(RebootWatchdogFile file) {
  std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_WATCHDOG_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";
  }
}

ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
                                            const uint8_t *data, size_t size) {
  switch (actionId) {
    case (SWITCH_REBOOT_FILE_HANDLING): {
      if (size < 1) {
        return HasActionsIF::INVALID_PARAMETERS;
      }
      std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_WATCHDOG_FILE;
      // Disable the reboot file mechanism
      parseRebootWatchdogFile(path, rebootWatchdogFile);
      if (data[0] == 0) {
        rebootWatchdogFile.enabled = false;
        rewriteRebootWatchdogFile(rebootWatchdogFile);
      } else if (data[0] == 1) {
        rebootWatchdogFile.enabled = true;
        rewriteRebootWatchdogFile(rebootWatchdogFile);
      } 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 (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(sdInfo.active) + REBOOT_WATCHDOG_FILE;
      // Disable the reboot file mechanism
      parseRebootWatchdogFile(path, rebootWatchdogFile);
      rebootWatchdogFile.maxCount = data[0];
      rewriteRebootWatchdogFile(rebootWatchdogFile);
      return HasActionsIF::EXECUTION_FINISHED;
    }
    default: {
      return HasActionsIF::INVALID_ACTION_ID;
    }
  }
}

void CoreController::setRebootMechanismLock(bool lock, xsc::Chip tgtChip, xsc::Copy tgtCopy) {
  std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_WATCHDOG_FILE;
  // Disable the reboot file mechanism
  parseRebootWatchdogFile(path, rebootWatchdogFile);
  if (tgtChip == xsc::CHIP_0) {
    if (tgtCopy == xsc::COPY_0) {
      rebootWatchdogFile.img00Lock = lock;
    } else {
      rebootWatchdogFile.img01Lock = lock;
    }
  } else {
    if (tgtCopy == xsc::COPY_0) {
      rebootWatchdogFile.img10Lock = lock;
    } else {
      rebootWatchdogFile.img11Lock = lock;
    }
  }
  rewriteRebootWatchdogFile(rebootWatchdogFile);
}

void CoreController::setCurrentBootCopy(xsc::Chip chip, xsc::Copy copy) {
  CURRENT_CHIP = chip;
  CURRENT_COPY = copy;
}