#include "GPSHyperionHandler.h"
#include "devicedefinitions/GPSDefinitions.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

GPSHyperionHandler::GPSHyperionHandler(object_id_t objectId, object_id_t parentId,
        bool debugHyperionGps):
        ExtendedControllerBase(objectId, objects::NO_OBJECT), gpsSet(this),
        debugHyperionGps(debugHyperionGps) {
}

GPSHyperionHandler::~GPSHyperionHandler() {}

void GPSHyperionHandler::performControlOperation() {
    // 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
}

LocalPoolDataSetBase* GPSHyperionHandler::getDataSetHandle(sid_t sid) {
    return nullptr;
}

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

ReturnValue_t GPSHyperionHandler::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 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(), true, 2.0, false);
    return HasReturnvaluesIF::RETURN_OK;
}

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


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

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