#include "GPSHyperionHandler.h"
#include "devicedefinitions/GPSDefinitions.h"

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

#include "lwgps/lwgps.h"

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) {
    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::info << "GPSHandler::scanForReply: Received " << len << " bytes" << std::endl;
        if (internalState == InternalStates::WAIT_FIRST_MESSAGE) {
            // TODO: Check whether data is valid by chcking 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;
            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;
            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);
            }
        }
        *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(), true, 2.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);
}

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

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