#include "GPSHyperionHandler.h"

#include "devicedefinitions/GPSDefinitions.h"
#include "fsfw/datapool/PoolReadGuard.h"
#include "fsfw/timemanager/Clock.h"
#include "lwgps/lwgps.h"

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

GPSHyperionHandler::GPSHyperionHandler(object_id_t objectId, object_id_t deviceCommunication,
                                       CookieIF *comCookie, bool debugHyperionGps)
    : DeviceHandlerBase(objectId, deviceCommunication, comCookie),
      gpsSet(this),
      debugHyperionGps(debugHyperionGps) {
  lwgps_init(&gpsData);
}

GPSHyperionHandler::~GPSHyperionHandler() {}

void GPSHyperionHandler::doStartUp() {
  if (internalState == InternalStates::NONE) {
    commandExecuted = false;
    internalState = InternalStates::WAIT_FIRST_MESSAGE;
  }

  if (internalState == InternalStates::WAIT_FIRST_MESSAGE) {
    if (commandExecuted) {
      internalState = InternalStates::IDLE;
      setMode(MODE_ON);
      commandExecuted = false;
    }
  }
}

void GPSHyperionHandler::doShutDown() {
  internalState = InternalStates::NONE;
  commandExecuted = false;
  setMode(_MODE_POWER_DOWN);
}

ReturnValue_t GPSHyperionHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
  return NOTHING_TO_SEND;
}

ReturnValue_t GPSHyperionHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
  return NOTHING_TO_SEND;
}

ReturnValue_t GPSHyperionHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
                                                          const uint8_t *commandData,
                                                          size_t commandDataLen) {
  // By default, send nothing
  rawPacketLen = 0;
  switch (deviceCommand) {
    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 GPSHyperionHandler::scanForReply(const uint8_t *start, size_t len,
                                               DeviceCommandId_t *foundId, size_t *foundLen) {
  // Pass data to GPS library
  if (len > 0) {
    // sif::debug << "GPSHandler::scanForReply: Received " << len << " bytes" << std::endl;
    if (internalState == InternalStates::WAIT_FIRST_MESSAGE) {
      // TODO: Check whether data is valid by checking whether NMEA start string is valid?
      commandExecuted = true;
    }
    int result = lwgps_process(&gpsData, start, len);
    if (result != 1) {
      sif::warning << "GPSHandler::scanForReply: Issue processing GPS data with lwgps" << std::endl;
    } else {
      // The data from the device will generally be read all at once. Therefore, we
      // can set all field here
      PoolReadGuard pg(&gpsSet);
      if (pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) {
#if FSFW_VERBOSE_LEVEL >= 1
        sif::warning << "GPSHyperionHandler::scanForReply: Reading dataset failed" << std::endl;
#endif
      }
      // Print messages
      if (gpsData.is_valid) {
        // Set all entries valid now, set invalid on case basis if values are sanitized
        gpsSet.setValidity(true, true);
      }
      // Negative latitude -> South direction
      gpsSet.latitude.value = gpsData.latitude;
      // Negative longitude -> West direction
      gpsSet.longitude.value = gpsData.longitude;
      if (gpsData.altitude > 600000.0 or gpsData.altitude < 400000.0) {
        gpsSet.altitude.setValid(false);
      } else {
        gpsSet.altitude.setValid(true);
        gpsSet.altitude.value = gpsData.altitude;
      }
      gpsSet.fixMode.value = gpsData.fix_mode;
      gpsSet.satInUse.value = gpsData.sats_in_use;
      Clock::TimeOfDay_t timeStruct = {};
      timeStruct.day = gpsData.date;
      timeStruct.hour = gpsData.hours;
      timeStruct.minute = gpsData.minutes;
      timeStruct.month = gpsData.month;
      timeStruct.second = gpsData.seconds;
      // Convert two-digit year to full year (AD)
      timeStruct.year = gpsData.year + 2000;
      timeval timeval = {};
      Clock::convertTimeOfDayToTimeval(&timeStruct, &timeval);
      gpsSet.year = timeStruct.year;
      gpsSet.month = gpsData.month;
      gpsSet.day = gpsData.date;
      gpsSet.hours = gpsData.hours;
      gpsSet.minutes = gpsData.minutes;
      gpsSet.seconds = gpsData.seconds;
      gpsSet.unixSeconds = timeval.tv_sec;
      if (debugHyperionGps) {
        sif::info << "GPS Data" << std::endl;
        printf("Valid status: %d\n", gpsData.is_valid);
        printf("Latitude: %f degrees\n", gpsData.latitude);
        printf("Longitude: %f degrees\n", gpsData.longitude);
        printf("Altitude: %f meters\n", gpsData.altitude);
      }
#if FSFW_DEV_HYPERION_GPS_CREATE_NMEA_CSV == 1
      std::string filename = "/mnt/sd0/gps_log.txt";
      std::ofstream gpsFile;
      if (not std::filesystem::exists(filename)) {
        gpsFile.open(filename, std::ofstream::out);
      }
      gpsFile.open(filename, std::ofstream::out | std::ofstream::app);
      gpsFile.write("\n", 1);
      gpsFile.write(reinterpret_cast<const char *>(start), len);
#endif
    }
    *foundLen = len;
    *foundId = GpsHyperion::GPS_REPLY;
  }
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t GPSHyperionHandler::interpretDeviceReply(DeviceCommandId_t id,
                                                       const uint8_t *packet) {
  return HasReturnvaluesIF::RETURN_OK;
}

uint32_t GPSHyperionHandler::getTransitionDelayMs(Mode_t from, Mode_t to) { return 5000; }

ReturnValue_t GPSHyperionHandler::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::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::FIX_MODE, new PoolEntry<uint8_t>());
  poolManager.subscribeForPeriodicPacket(gpsSet.getSid(), false, 30.0, false);
  return HasReturnvaluesIF::RETURN_OK;
}

void GPSHyperionHandler::fillCommandAndReplyMap() {
  // Reply length does not matter, packets should always arrive periodically
  insertInReplyMap(GpsHyperion::GPS_REPLY, 4, &gpsSet, 0, true);
  insertInCommandMap(GpsHyperion::TRIGGER_RESET_PIN);
}

void GPSHyperionHandler::modeChanged() { internalState = InternalStates::NONE; }

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

void GPSHyperionHandler::debugInterface(uint8_t positionTracker, object_id_t objectId,
                                        uint32_t parameter) {}

ReturnValue_t GPSHyperionHandler::initialize() {
  ReturnValue_t result = DeviceHandlerBase::initialize();
  if (result != HasReturnvaluesIF::RETURN_OK) {
    return result;
  }
  // Enable reply immediately for now
  return updatePeriodicReply(true, GpsHyperion::GPS_REPLY);
}

ReturnValue_t GPSHyperionHandler::acceptExternalDeviceCommands() {
  return DeviceHandlerBase::acceptExternalDeviceCommands();
}