#include "obsw.h"

#include <libxiphos.h>
#include <pwd.h>
#include <sys/types.h>
#include <unistd.h>

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

#include "OBSWConfig.h"
#include "bsp_q7s/core/WatchdogHandler.h"
#include "commonConfig.h"
#include "fsfw/tasks/TaskFactory.h"
#include "fsfw/version.h"
#include "memory/scratchApi.h"
#include "mission/acs/defs.h"
#include "mission/com/defs.h"
#include "mission/system/systemTree.h"
#include "q7sConfig.h"
#include "scheduling.h"
#include "watchdog/definitions.h"

static constexpr int OBSW_ALREADY_RUNNING = -2;
#if OBSW_Q7S_EM == 0
static const char* DEV_STRING = "Xiphos Q7S FM";
#else
static const char* DEV_STRING = "Xiphos Q7S EM";
#endif

WatchdogHandler WATCHDOG_HANDLER;

int obsw::obsw(int argc, char* argv[]) {
  using namespace fsfw;
  std::cout << "-- EIVE OBSW --" << std::endl;
  std::cout << "-- Compiled for Linux (" << DEV_STRING << ") --" << std::endl;
  std::cout << "-- OBSW v" << common::OBSW_VERSION << " | FSFW v" << fsfw::FSFW_VERSION << " --"
            << std::endl;
  std::cout << "-- " << __DATE__ << " " << __TIME__ << " --" << std::endl;

#if Q7S_CHECK_FOR_ALREADY_RUNNING_IMG == 1
  std::error_code e;
  // Check special file here. This file is created or deleted by the eive-watchdog application
  // or systemd service!
  if (std::filesystem::exists(watchdog::RUNNING_FILE_NAME, e)) {
    sif::warning << "File " << watchdog::RUNNING_FILE_NAME
                 << " exists so the software might "
                    "already be running. Check if obsw systemd service has been stopped."
                 << std::endl;
    return OBSW_ALREADY_RUNNING;
  }
#endif

  autoSwitchHandling();

  // Delay the boot if applicable.
  bootDelayHandling();

  bool initWatchFunction = false;
  std::string fullExecPath = argv[0];
  if (fullExecPath.find("/usr/bin") != std::string::npos) {
    initWatchFunction = true;
  }
  ReturnValue_t result = WATCHDOG_HANDLER.initialize(initWatchFunction);
  if (result != returnvalue::OK) {
    std::cerr << "Initiating EIVE watchdog handler failed" << std::endl;
  }

  scheduling::initMission();

  // Command the EIVE system to safe mode
#if OBSW_COMMAND_SAFE_MODE_AT_STARTUP == 1
  // This ensures that the PCDU switches were updated.
  TaskFactory::delayTask(1000);
  commandComSubsystemRxOnly();
  commandEiveSystemToSafe();
#else
  announceAllModes();
#endif

  for (;;) {
    WATCHDOG_HANDLER.periodicOperation();
    TaskFactory::delayTask(2000);
  }
  return 0;
}

void obsw::autoSwitchHandling() {
  std::string autoSwitchTarget;
  auto switchToTarget = [](xsc_libnor_chip_t chip, xsc_libnor_copy_t copy) {
    sif::debug << "Detected ASWI=" << autoSwitchTarget
               << "in ProASIC scratch buffer, auto-switching to image " << int(chip) << " "
               << int(copy) << std::endl;
    // A bit of delay to ensure printout works..
    TaskFactory::delayTask(500);
    scratch::clearValue(scratch::AUTO_SWITCH_IMAGE);
    xsc_boot_copy(chip, copy);
  };
  if (scratch::readString(scratch::AUTO_SWITCH_IMAGE, autoSwitchTarget) == returnvalue::OK) {
    if (autoSwitchTarget == "00") {
      switchToTarget(XSC_LIBNOR_CHIP_0, XSC_LIBNOR_COPY_NOMINAL);
    } else if (autoSwitchTarget == "01") {
      switchToTarget(XSC_LIBNOR_CHIP_0, XSC_LIBNOR_COPY_GOLD);
    } else if (autoSwitchTarget == "10") {
      switchToTarget(XSC_LIBNOR_CHIP_1, XSC_LIBNOR_COPY_NOMINAL);
    } else if (autoSwitchTarget == "11") {
      switchToTarget(XSC_LIBNOR_CHIP_1, XSC_LIBNOR_COPY_GOLD);
    } else {
      sif::warning << "Invalid Auto Switch Image (ASWI) value detected: " << autoSwitchTarget
                   << std::endl;
    }
  }
}

void obsw::bootDelayHandling() {
  const char* homedir = nullptr;
  homedir = getenv("HOME");
  if (homedir == nullptr) {
    homedir = getpwuid(getuid())->pw_dir;
  }
  std::filesystem::path bootDelayFile = std::filesystem::path(homedir) / "boot_delay_secs.txt";
  std::error_code e;
  // Init delay handling.
  if (std::filesystem::exists(bootDelayFile, e)) {
    std::ifstream ifile(bootDelayFile);
    std::string lineStr;
    unsigned int bootDelaySecs = 0;
    unsigned int line = 0;
    // Try to reas delay seconds from file.
    while (std::getline(ifile, lineStr)) {
      std::istringstream iss(lineStr);
      if (!(iss >> bootDelaySecs)) {
        break;
      }
      line++;
    }
    if (line == 0) {
      // If the file is empty, assume default of 6 seconds
      bootDelaySecs = 6;
    }
    std::cout << "Delaying OBSW start for " << bootDelaySecs << " seconds" << std::endl;
    TaskFactory::delayTask(bootDelaySecs * 1000);
  }
}

void obsw::commandEiveSystemToSafe() {
  auto sysQueueId = satsystem::EIVE_SYSTEM.getCommandQueue();
  CommandMessage msg;
  ModeMessage::setCmdModeMessage(msg, acs::AcsMode::SAFE, 0);
  ReturnValue_t result =
      MessageQueueSenderIF::sendMessage(sysQueueId, &msg, MessageQueueIF::NO_QUEUE, false);
  if (result != returnvalue::OK) {
    sif::error << "obsw: Sending safe mode command to EIVE system failed" << std::endl;
  }
}

void obsw::commandComSubsystemRxOnly() {
  auto* comSs = ObjectManager::instance()->get<HasModesIF>(objects::COM_SUBSYSTEM);
  if (comSs == nullptr) {
    sif::error << "obsw: Could not retrieve COM subsystem object" << std::endl;
    return;
  }
  CommandMessage msg;
  ModeMessage::setCmdModeMessage(msg, com::RX_ONLY, 0);
  ReturnValue_t result = MessageQueueSenderIF::sendMessage(comSs->getCommandQueue(), &msg,
                                                           MessageQueueIF::NO_QUEUE, false);
  if (result != returnvalue::OK) {
    sif::error << "obsw: Sending RX_ONLY mode command to COM subsystem failed" << std::endl;
  }
}

void obsw::announceAllModes() {
  auto sysQueueId = satsystem::EIVE_SYSTEM.getCommandQueue();
  CommandMessage msg;
  ModeMessage::setModeAnnounceMessage(msg, true);
  ReturnValue_t result =
      MessageQueueSenderIF::sendMessage(sysQueueId, &msg, MessageQueueIF::NO_QUEUE, false);
  if (result != returnvalue::OK) {
    sif::error << "obsw: Sending safe mode command to EIVE system failed" << std::endl;
  }
}