#include "GPSHyperionLinuxController.h"

#include "fsfw/datapool/PoolReadGuard.h"
#include "fsfw/timemanager/Clock.h"

#if FSFW_DEV_HYPERION_GPS_CREATE_NMEA_CSV == 1
#include <filesystem>
#include <fstream>
#endif
#include <cmath>

GPSHyperionLinuxController::GPSHyperionLinuxController(object_id_t objectId, object_id_t parentId,
                                                       bool debugHyperionGps)
    : ExtendedControllerBase(objectId, objects::NO_OBJECT),
      gpsSet(this),
      myGpsmm(GPSD_SHARED_MEMORY, nullptr),
      debugHyperionGps(debugHyperionGps) {}

GPSHyperionLinuxController::~GPSHyperionLinuxController() {}

void GPSHyperionLinuxController::performControlOperation() {
#ifdef FSFW_OSAL_LINUX
  readGpsDataFromGpsd();
#endif
}

LocalPoolDataSetBase *GPSHyperionLinuxController::getDataSetHandle(sid_t sid) { return &gpsSet; }

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

ReturnValue_t GPSHyperionLinuxController::executeAction(ActionId_t actionId,
                                                        MessageQueueId_t commandedBy,
                                                        const uint8_t *data, size_t size) {
  switch (actionId) {
    case (GpsHyperion::TRIGGER_RESET_PIN): {
      if (resetCallback != nullptr) {
        PoolReadGuard pg(&gpsSet);
        // Set HK entries invalid
        gpsSet.setValidity(false, true);
        resetCallback(resetCallbackArgs);
        return HasActionsIF::EXECUTION_FINISHED;
      }
      return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
    }
  }
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t GPSHyperionLinuxController::initializeLocalDataPool(
    localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) {
  localDataPoolMap.emplace(GpsHyperion::ALTITUDE, new PoolEntry<double>({0.0}));
  localDataPoolMap.emplace(GpsHyperion::LONGITUDE, new PoolEntry<double>({0.0}));
  localDataPoolMap.emplace(GpsHyperion::LATITUDE, new PoolEntry<double>({0.0}));
  localDataPoolMap.emplace(GpsHyperion::SPEED, new PoolEntry<double>({0.0}));
  localDataPoolMap.emplace(GpsHyperion::YEAR, new PoolEntry<uint16_t>());
  localDataPoolMap.emplace(GpsHyperion::MONTH, new PoolEntry<uint8_t>());
  localDataPoolMap.emplace(GpsHyperion::DAY, new PoolEntry<uint8_t>());
  localDataPoolMap.emplace(GpsHyperion::HOURS, new PoolEntry<uint8_t>());
  localDataPoolMap.emplace(GpsHyperion::MINUTES, new PoolEntry<uint8_t>());
  localDataPoolMap.emplace(GpsHyperion::SECONDS, new PoolEntry<uint8_t>());
  localDataPoolMap.emplace(GpsHyperion::UNIX_SECONDS, new PoolEntry<uint32_t>());
  localDataPoolMap.emplace(GpsHyperion::SATS_IN_USE, new PoolEntry<uint8_t>());
  localDataPoolMap.emplace(GpsHyperion::SATS_IN_VIEW, new PoolEntry<uint8_t>());
  localDataPoolMap.emplace(GpsHyperion::FIX_MODE, new PoolEntry<uint8_t>());
#if OBSW_ENABLE_PERIODIC_HK == 1
  poolManager.subscribeForPeriodicPacket(gpsSet.getSid(), true, 2.0, false);
#endif
  return HasReturnvaluesIF::RETURN_OK;
}

void GPSHyperionLinuxController::setResetPinTriggerFunction(gpioResetFunction_t resetCallback,
                                                            void *args) {
  this->resetCallback = resetCallback;
  resetCallbackArgs = args;
}

ReturnValue_t GPSHyperionLinuxController::initialize() {
  ReturnValue_t result = ExtendedControllerBase::initialize();
  if (result != HasReturnvaluesIF::RETURN_OK) {
    return result;
  }
  return result;
}

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

#ifdef FSFW_OSAL_LINUX
void GPSHyperionLinuxController::readGpsDataFromGpsd() {
  // The data from the device will generally be read all at once. Therefore, we
  // can set all field here
  if (not myGpsmm.is_open()) {
    // Opening failed
#if FSFW_VERBOSE_LEVEL >= 1
    sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Opening GPSMM failed" << std::endl;
#endif
  }
  gps_data_t *gps = nullptr;
  gps = myGpsmm.read();
  if (gps == nullptr) {
    sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading GPS data failed" << std::endl;
  }
  PoolReadGuard pg(&gpsSet);
  if (pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) {
#if FSFW_VERBOSE_LEVEL >= 1
    sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading dataset failed" << std::endl;
#endif
  }

  // 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix
  gpsSet.fixMode.value = gps->fix.mode;
  if (gps->fix.mode == 0 or gps->fix.mode == 1) {
    gpsSet.setValidity(false, true);
  } else if (gps->satellites_used > 0) {
    gpsSet.setValidity(true, true);
  }

  gpsSet.satInUse.value = gps->satellites_used;
  gpsSet.satInView.value = gps->satellites_visible;

  if (std::isfinite(gps->fix.latitude)) {
    // Negative latitude -> South direction
    gpsSet.latitude.value = gps->fix.latitude;
  } else {
    gpsSet.latitude.setValid(false);
  }

  if (std::isfinite(gps->fix.longitude)) {
    // Negative longitude -> West direction
    gpsSet.longitude.value = gps->fix.longitude;
  } else {
    gpsSet.longitude.setValid(false);
  }

  if (std::isfinite(gps->fix.altitude)) {
    gpsSet.altitude.value = gps->fix.altitude;
  } else {
    gpsSet.altitude.setValid(false);
  }

  if (std::isfinite(gps->fix.speed)) {
    gpsSet.speed.value = gps->fix.speed;
  } else {
    gpsSet.speed.setValid(false);
  }

  gpsSet.unixSeconds.value = gps->fix.time.tv_sec;
  timeval time = {};
  time.tv_sec = gpsSet.unixSeconds.value;
  time.tv_usec = gps->fix.time.tv_nsec / 1000;
  Clock::TimeOfDay_t timeOfDay = {};
  Clock::convertTimevalToTimeOfDay(&time, &timeOfDay);
  gpsSet.year = timeOfDay.year;
  gpsSet.month = timeOfDay.month;
  gpsSet.day = timeOfDay.day;
  gpsSet.hours = timeOfDay.hour;
  gpsSet.minutes = timeOfDay.minute;
  gpsSet.seconds = timeOfDay.second;
  if (debugHyperionGps) {
    sif::info << "-- Hyperion GPS Data --" << std::endl;
    time_t timeRaw = gps->fix.time.tv_sec;
    std::tm *time = gmtime(&timeRaw);
    std::cout << "Time: " << std::put_time(time, "%c %Z") << std::endl;
    std::cout << "Visible satellites: " << gps->satellites_visible << std::endl;
    std::cout << "Satellites used: " << gps->satellites_used << std::endl;
    std::cout << "Fix (0:Not Seen|1:No Fix|2:2D|3:3D): " << gps->fix.mode << std::endl;
    std::cout << "Latitude: " << gps->fix.latitude << std::endl;
    std::cout << "Longitude: " << gps->fix.longitude << std::endl;
    std::cout << "Altitude(MSL): " << gps->fix.altMSL << std::endl;
    std::cout << "Speed(m/s): " << gps->fix.speed << std::endl;
  }
}
#endif