#include "Q7STestTask.h"

#include <bsp_q7s/core/CoreController.h>
#include <bsp_q7s/memory/FileSystemHandler.h>
#include <bsp_q7s/xadc/Xadc.h>
#include <fsfw/objectmanager/ObjectManager.h>
#include <gps.h>
#include <libgpsmm.h>

#include <cstdio>
#include <ctime>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <nlohmann/json.hpp>

#include "bsp_q7s/memory/SdCardManager.h"
#include "bsp_q7s/memory/scratchApi.h"
#include "fsfw/tasks/TaskFactory.h"
#include "fsfw/timemanager/Stopwatch.h"
#include "test/DummyParameter.h"

Q7STestTask::Q7STestTask(object_id_t objectId) : TestTask(objectId) {
  doTestSdCard = false;
  doTestScratchApi = false;
  doTestGpsShm = false;
  doTestGpsSocket = false;
  doTestXadc = false;
}

ReturnValue_t Q7STestTask::performOneShotAction() {
  if (doTestSdCard) {
    testSdCard();
  }
  if (doTestScratchApi) {
    testScratchApi();
  }
  // testJsonLibDirect();
  // testDummyParams();
  if (doTestProtHandler) {
    testProtHandler();
  }
  FsOpCodes opCode = FsOpCodes::APPEND_TO_FILE;
  testFileSystemHandlerDirect(opCode);
  return TestTask::performOneShotAction();
}

ReturnValue_t Q7STestTask::performPeriodicAction() {
  if (doTestGpsShm) {
    testGpsDaemonShm();
  }
  if (doTestGpsSocket) {
    testGpsDaemonSocket();
  }
  if (doTestXadc) {
    xadcTest();
  }
  return TestTask::performPeriodicAction();
}

void Q7STestTask::testSdCard() {
  using namespace std;
  Stopwatch stopwatch;
  int result = std::system("q7hw sd info all > /tmp/sd_status.txt");
  if (result != 0) {
    sif::debug << "system call failed with " << result << endl;
  }
  ifstream sdStatus("/tmp/sd_status.txt");
  string line;
  uint8_t idx = 0;
  while (std::getline(sdStatus, line)) {
    std::istringstream iss(line);
    string word;
    while (iss >> word) {
      if (word == "on") {
        sif::info << "SD card " << static_cast<int>(idx) << " is on" << endl;
      } else if (word == "off") {
        sif::info << "SD card " << static_cast<int>(idx) << " is off" << endl;
      }
    }
    idx++;
  }
  std::remove("/tmp/sd_status.txt");
}

void Q7STestTask::fileTests() {
  using namespace std;
  ofstream testFile("/tmp/test.txt");
  testFile << "Hallo Welt" << endl;
  testFile.close();

  system("echo \"Hallo Welt\" > /tmp/test2.txt");
  system("echo \"Hallo Welt\"");
}

void Q7STestTask::testScratchApi() {
  ReturnValue_t result = scratch::writeNumber("TEST", 1);
  if (result != returnvalue::OK) {
    sif::debug << "Q7STestTask::scratchApiTest: Writing number failed" << std::endl;
  }
  int number = 0;
  result = scratch::readNumber("TEST", number);
  sif::info << "Q7STestTask::testScratchApi: Value for key \"TEST\": " << number << std::endl;
  if (result != returnvalue::OK) {
    sif::debug << "Q7STestTask::scratchApiTest: Reading number failed" << std::endl;
  }

  result = scratch::writeString("TEST2", "halloWelt");
  if (result != returnvalue::OK) {
    sif::debug << "Q7STestTask::scratchApiTest: Writing string failed" << std::endl;
  }
  std::string string;
  result = scratch::readString("TEST2", string);
  if (result != returnvalue::OK) {
    sif::debug << "Q7STestTask::scratchApiTest: Reading number failed" << std::endl;
  }
  sif::info << "Q7STestTask::testScratchApi: Value for key \"TEST2\": " << string << std::endl;

  result = scratch::clearValue("TEST");
  result = scratch::clearValue("TEST2");
}

void Q7STestTask::testJsonLibDirect() {
  Stopwatch stopwatch;
  // for convenience
  using json = nlohmann::json;
  json helloTest;
  // add a number that is stored as double (note the implicit conversion of j to an object)
  helloTest["pi"] = 3.141;
  std::string mntPrefix = SdCardManager::instance()->getCurrentMountPrefix();
  std::string fileName = mntPrefix + "/pretty.json";
  std::ofstream o(fileName);
  o << std::setw(4) << helloTest << std::endl;
}

void Q7STestTask::testDummyParams() {
  std::string mntPrefix = SdCardManager::instance()->getCurrentMountPrefix();
  DummyParameter param(mntPrefix, "dummy_json.txt");
  param.printKeys();
  param.print();
  if (not param.getJsonFileExists()) {
    param.writeJsonFile();
  }

  ReturnValue_t result = param.readJsonFile();
  if (result != returnvalue::OK) {
  }

  param.setValue(DummyParameter::DUMMY_KEY_PARAM_1, 3);
  param.setValue(DummyParameter::DUMMY_KEY_PARAM_2, "blirb");

  param.writeJsonFile();
  param.print();

  int test = 0;
  result = param.getValue<int>(DummyParameter::DUMMY_KEY_PARAM_1, test);
  if (result != returnvalue::OK) {
    sif::warning << "Q7STestTask::testDummyParams: Key " << DummyParameter::DUMMY_KEY_PARAM_1
                 << " does not exist" << std::endl;
  }
  std::string test2;
  result = param.getValue<std::string>(DummyParameter::DUMMY_KEY_PARAM_2, test2);
  if (result != returnvalue::OK) {
    sif::warning << "Q7STestTask::testDummyParams: Key " << DummyParameter::DUMMY_KEY_PARAM_1
                 << " does not exist" << std::endl;
  }
  sif::info << "Test value (3 expected): " << test << std::endl;
  sif::info << "Test value 2 (\"blirb\" expected): " << test2 << std::endl;
}

ReturnValue_t Q7STestTask::initialize() {
  coreController = ObjectManager::instance()->get<CoreController>(objects::CORE_CONTROLLER);
  if (coreController == nullptr) {
    sif::warning << "Q7STestTask::initialize: Could not retrieve CORE_CONTROLLER object"
                 << std::endl;
  }
  return TestTask::initialize();
}

void Q7STestTask::testProtHandler() {
  bool opPerformed = false;
  ReturnValue_t result = returnvalue::OK;
  // If any chips are unlocked, lock them here
  result = coreController->setBootCopyProtection(xsc::Chip::ALL_CHIP, xsc::Copy::ALL_COPY, true,
                                                 opPerformed, true);
  if (result != returnvalue::OK) {
    sif::warning << "Q7STestTask::testProtHandler: Op failed" << std::endl;
  }

  // unlock own copy
  result = coreController->setBootCopyProtection(xsc::Chip::SELF_CHIP, xsc::Copy::SELF_COPY, false,
                                                 opPerformed, true);
  if (result != returnvalue::OK) {
    sif::warning << "Q7STestTask::testProtHandler: Op failed" << std::endl;
  }
  if (not opPerformed) {
    sif::warning << "Q7STestTask::testProtHandler: No op performed" << std::endl;
  }
  int retval = std::system("print-chip-prot-status.sh");
  if (retval != 0) {
    utility::handleSystemError(retval, "Q7STestTask::testProtHandler");
  }

  // lock own copy
  result = coreController->setBootCopyProtection(xsc::Chip::SELF_CHIP, xsc::Copy::SELF_COPY, true,
                                                 opPerformed, true);
  if (result != returnvalue::OK) {
    sif::warning << "Q7STestTask::testProtHandler: Op failed" << std::endl;
  }
  if (not opPerformed) {
    sif::warning << "Q7STestTask::testProtHandler: No op performed" << std::endl;
  }
  retval = std::system("print-chip-prot-status.sh");
  if (retval != 0) {
    utility::handleSystemError(retval, "Q7STestTask::testProtHandler");
  }

  // unlock specific copy
  result = coreController->setBootCopyProtection(xsc::Chip::CHIP_1, xsc::Copy::COPY_1, false,
                                                 opPerformed, true);
  if (result != returnvalue::OK) {
    sif::warning << "Q7STestTask::testProtHandler: Op failed" << std::endl;
  }
  if (not opPerformed) {
    sif::warning << "Q7STestTask::testProtHandler: No op performed" << std::endl;
  }
  retval = std::system("print-chip-prot-status.sh");
  if (retval != 0) {
    utility::handleSystemError(retval, "Q7STestTask::testProtHandler");
  }

  // lock specific copy
  result = coreController->setBootCopyProtection(xsc::Chip::CHIP_1, xsc::Copy::COPY_1, true,
                                                 opPerformed, true);
  if (result != returnvalue::OK) {
    sif::warning << "Q7STestTask::testProtHandler: Op failed" << std::endl;
  }
  if (not opPerformed) {
    sif::warning << "Q7STestTask::testProtHandler: No op performed" << std::endl;
  }
  retval = std::system("print-chip-prot-status.sh");
  if (retval != 0) {
    utility::handleSystemError(retval, "Q7STestTask::testProtHandler");
  }
}

void Q7STestTask::testGpsDaemonShm() {
  gpsmm gpsmm(GPSD_SHARED_MEMORY, "");
  gps_data_t* gps;
  gps = gpsmm.read();
  if (gps == nullptr) {
    sif::warning << "Q7STestTask: Reading GPS data failed" << std::endl;
  }
  sif::info << "-- Q7STestTask: GPS shared memory read test --" << std::endl;
#if LIBGPS_VERSION_MINOR <= 17
  time_t timeRaw = gps->fix.time;
#else
  time_t timeRaw = gps->fix.time.tv_sec;
#endif
  std::tm* time = gmtime(&timeRaw);
  sif::info << "Time: " << std::put_time(time, "%c %Z") << std::endl;
  sif::info << "Visible satellites: " << gps->satellites_visible << std::endl;
  sif::info << "Satellites used: " << gps->satellites_used << std::endl;
  sif::info << "Fix (0:Not Seen|1:No Fix|2:2D|3:3D): " << gps->fix.mode << std::endl;
  sif::info << "Latitude: " << gps->fix.latitude << std::endl;
  sif::info << "Longitude: " << gps->fix.longitude << std::endl;
#if LIBGPS_VERSION_MINOR <= 17
  sif::info << "Altitude(MSL): " << gps->fix.altitude << std::endl;
#else
  sif::info << "Altitude(MSL): " << gps->fix.altMSL << std::endl;
#endif
  sif::info << "Speed(m/s): " << gps->fix.speed << std::endl;
}

void Q7STestTask::testGpsDaemonSocket() {
  if (gpsmmShmPtr == nullptr) {
    gpsmmShmPtr = new gpsmm("localhost", DEFAULT_GPSD_PORT);
  }
  // The data from the device will generally be read all at once. Therefore, we
  // can set all field here
  if (not gpsmmShmPtr->is_open()) {
    if (gpsNotOpenSwitch) {
      // Opening failed
#if FSFW_VERBOSE_LEVEL >= 1
      sif::warning << "Q7STestTask::testGpsDaemonSocket: Opening GPSMM failed | "
                   << "Error " << errno << " | " << gps_errstr(errno) << std::endl;
#endif

      gpsNotOpenSwitch = false;
    }
    return;
  }
  // Stopwatch watch;
  gps_data_t* gps = nullptr;
  gpsmmShmPtr->stream(WATCH_ENABLE | WATCH_JSON);
  if (not gpsmmShmPtr->waiting(50000000)) {
    return;
  }
  gps = gpsmmShmPtr->read();
  if (gps == nullptr) {
    if (gpsReadFailedSwitch) {
      gpsReadFailedSwitch = false;
      sif::warning << "Q7STestTask::testGpsDaemonSocket: Reading GPS data failed" << std::endl;
    }
    return;
  }
  if (MODE_SET != (MODE_SET & gps->set)) {
    if (noModeSetCntr >= 0) {
      noModeSetCntr++;
    }
    if (noModeSetCntr == 10) {
      // TODO: Trigger event here
      sif::warning << "Q7STestTask::testGpsDaemonSocket: No mode could be "
                      "read for 10 consecutive reads"
                   << std::endl;
      noModeSetCntr = -1;
    }
    return;
  } else {
    noModeSetCntr = 0;
  }
  sif::info << "-- Q7STestTask: GPS socket read test --" << std::endl;
#if LIBGPS_VERSION_MINOR <= 17
  time_t timeRaw = gps->fix.time;
#else
  time_t timeRaw = gps->fix.time.tv_sec;
#endif
  std::tm* time = gmtime(&timeRaw);
  sif::info << "Time: " << std::put_time(time, "%c %Z") << std::endl;
  sif::info << "Visible satellites: " << gps->satellites_visible << std::endl;
  sif::info << "Satellites used: " << gps->satellites_used << std::endl;
  sif::info << "Fix (0:Not Seen|1:No Fix|2:2D|3:3D): " << gps->fix.mode << std::endl;
  sif::info << "Latitude: " << gps->fix.latitude << std::endl;
  sif::info << "Longitude: " << gps->fix.longitude << std::endl;
}

void Q7STestTask::testFileSystemHandlerDirect(FsOpCodes opCode) {
  auto fsHandler = ObjectManager::instance()->get<FileSystemHandler>(objects::FILE_SYSTEM_HANDLER);
  if (fsHandler == nullptr) {
    sif::warning << "Q7STestTask::testFileSystemHandlerDirect: No FS handler running.."
                 << std::endl;
  }
  FileSystemHandler::FsCommandCfg cfg = {};
  ReturnValue_t result = returnvalue::OK;

  // Lambda for common code
  auto createNonEmptyTmpDir = [&]() {
    if (not std::filesystem::exists("/tmp/test")) {
      result = fsHandler->createDirectory("/tmp", "test", false, &cfg);
      if (result != returnvalue::OK) {
        return result;
      }
    }
    // Creating sample files
    sif::info << "Creating sample files in directory" << std::endl;
    result = fsHandler->createFile("/tmp/test", "test1.txt", nullptr, 0, &cfg);
    if (result != returnvalue::OK) {
      return result;
    }
    result = fsHandler->createFile("/tmp/test", "test2.txt", nullptr, 0, &cfg);
    if (result != returnvalue::OK) {
      return result;
    }
    return result;
  };

  switch (opCode) {
    case (FsOpCodes::CREATE_EMPTY_FILE_IN_TMP): {
      // No mount prefix, cause file is created in tmp
      cfg.useMountPrefix = false;
      sif::info << "Creating empty file in /tmp folder" << std::endl;
      // Do not delete file, user can check existence in shell
      fsHandler->createFile("/tmp/", "test.txt", nullptr, 0, &cfg);
      break;
    }
    case (FsOpCodes::REMOVE_TMP_FILE): {
      sif::info << "Deleting /tmp/test.txt sample file" << std::endl;
      // No mount prefix, cause file is created in tmp
      cfg.useMountPrefix = false;
      if (not std::filesystem::exists("/tmp/test.txt")) {
        // Creating sample file
        sif::info << "Creating sample file /tmp/test.txt to delete" << std::endl;
        fsHandler->createFile("/tmp/", "test.txt", nullptr, 0, &cfg);
      }
      result = fsHandler->removeFile("/tmp", "test.txt", &cfg);
      if (result == returnvalue::OK) {
        sif::info << "File removed successfully" << std::endl;
      } else {
        sif::warning << "File removal failed!" << std::endl;
      }
      break;
    }
    case (FsOpCodes::CREATE_DIR_IN_TMP): {
      // No mount prefix, cause file is created in tmp
      cfg.useMountPrefix = false;
      sif::info << "Creating empty file in /tmp folder" << std::endl;
      // Do not delete file, user can check existence in shell
      ReturnValue_t result = fsHandler->createDirectory("/tmp/", "test", false, &cfg);
      if (result == returnvalue::OK) {
        sif::info << "Directory created successfully" << std::endl;
      } else {
        sif::warning << "Directory creation failed!" << std::endl;
      }
      break;
    }
    case (FsOpCodes::REMOVE_EMPTY_DIR_IN_TMP): {
      // No mount prefix, cause file is created in tmp
      cfg.useMountPrefix = false;
      if (not std::filesystem::exists("/tmp/test")) {
        result = fsHandler->createDirectory("/tmp", "test", false, &cfg);
      } else {
        // Delete any leftover files to regular dir removal works
        std::remove("/tmp/test/*");
      }
      result = fsHandler->removeDirectory("/tmp/", "test", false, &cfg);
      if (result == returnvalue::OK) {
        sif::info << "Directory removed successfully" << std::endl;
      } else {
        sif::warning << "Directory removal failed!" << std::endl;
      }
      break;
    }
    case (FsOpCodes::REMOVE_FILLED_DIR_IN_TMP): {
      result = createNonEmptyTmpDir();
      if (result != returnvalue::OK) {
        return;
      }
      result = fsHandler->removeDirectory("/tmp/", "test", true, &cfg);
      if (result == returnvalue::OK) {
        sif::info << "Directory removed recursively successfully" << std::endl;
      } else {
        sif::warning << "Recursive directory removal failed!" << std::endl;
      }
      break;
    }
    case (FsOpCodes::ATTEMPT_DIR_REMOVAL_NON_EMPTY): {
      result = createNonEmptyTmpDir();
      if (result != returnvalue::OK) {
        return;
      }
      result = fsHandler->removeDirectory("/tmp/", "test", false, &cfg);
      if (result != returnvalue::OK) {
        sif::info << "Directory removal attempt failed as expected" << std::endl;
      } else {
        sif::warning << "Directory removal worked when it should not have!" << std::endl;
      }
      break;
    }
    case (FsOpCodes::RENAME_FILE): {
      // No mount prefix, cause file is created in tmp
      cfg.useMountPrefix = false;
      if (std::filesystem::exists("/tmp/test.txt")) {
        fsHandler->removeDirectory("/tmp/", "test", false, &cfg);
      }
      sif::info << "Creating empty file /tmp/test.txt and rename to /tmp/test2.txt" << std::endl;
      // Do not delete file, user can check existence in shell
      fsHandler->createFile("/tmp/", "test.txt", nullptr, 0, &cfg);
      fsHandler->renameFile("/tmp/", "test.txt", "test2.txt", &cfg);
      break;
    }
    case (FsOpCodes::APPEND_TO_FILE): {
      // No mount prefix, cause file is created in tmp
      cfg.useMountPrefix = false;
      if (std::filesystem::exists("/tmp/test.txt")) {
        fsHandler->removeDirectory("/tmp/", "test", false, &cfg);
      }
      if (std::filesystem::exists("/tmp/test.txt")) {
        fsHandler->removeDirectory("/tmp/", "test", false, &cfg);
      }
      sif::info << "Creating empty file /tmp/test.txt and adding content" << std::endl;
      std::string content = "Hello World\n";
      // Do not delete file, user can check existence in shell
      fsHandler->createFile("/tmp/", "test.txt", nullptr, 0, &cfg);
      fsHandler->appendToFile("/tmp/", "test.txt", reinterpret_cast<const uint8_t*>(content.data()),
                              content.size(), 0, &cfg);
    }
  }
}

void Q7STestTask::xadcTest() {
  ReturnValue_t result = returnvalue::OK;
  float temperature = 0;
  float vccPint = 0;
  float vccPaux = 0;
  float vccInt = 0;
  float vccAux = 0;
  float vccBram = 0;
  float vccOddr = 0;
  float vrefp = 0;
  float vrefn = 0;
  Xadc xadc;
  result = xadc.getTemperature(temperature);
  if (result == returnvalue::OK) {
    sif::info << "Q7STestTask::xadcTest: Chip Temperature: " << temperature << " °C" << std::endl;
  }
  result = xadc.getVccPint(vccPint);
  if (result == returnvalue::OK) {
    sif::info << "Q7STestTask::xadcTest: VCC PS internal: " << vccPint << " mV" << std::endl;
  }
  result = xadc.getVccPaux(vccPaux);
  if (result == returnvalue::OK) {
    sif::info << "Q7STestTask::xadcTest: VCC PS auxilliary: " << vccPaux << " mV" << std::endl;
  }
  result = xadc.getVccInt(vccInt);
  if (result == returnvalue::OK) {
    sif::info << "Q7STestTask::xadcTest: VCC PL internal: " << vccInt << " mV" << std::endl;
  }
  result = xadc.getVccAux(vccAux);
  if (result == returnvalue::OK) {
    sif::info << "Q7STestTask::xadcTest: VCC PL auxilliary: " << vccAux << " mV" << std::endl;
  }
  result = xadc.getVccBram(vccBram);
  if (result == returnvalue::OK) {
    sif::info << "Q7STestTask::xadcTest: VCC BRAM: " << vccBram << " mV" << std::endl;
  }
  result = xadc.getVccOddr(vccOddr);
  if (result == returnvalue::OK) {
    sif::info << "Q7STestTask::xadcTest: VCC PS I/O DDR : " << vccOddr << " mV" << std::endl;
  }
  result = xadc.getVrefp(vrefp);
  if (result == returnvalue::OK) {
    sif::info << "Q7STestTask::xadcTest: Vrefp : " << vrefp << " mV" << std::endl;
  }
  result = xadc.getVrefn(vrefn);
  if (result == returnvalue::OK) {
    sif::info << "Q7STestTask::xadcTest: Vrefn : " << vrefn << " mV" << std::endl;
  }
}