#include "TestTask.h"
#include <OBSWConfig.h>

#include <fsfw/objectmanager/frameworkObjects.h>
#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
#include <fsfw/objectmanager/ObjectManagerIF.h>
#include <fsfw/timemanager/Stopwatch.h>
#include <fsfw/globalfunctions/arrayprinter.h>

#include <array>
#include <cstring>


TestTask::TestTask(object_id_t objectId_):
SystemObject(objectId_), testMode(testModes::A) {
    IPCStore = objectManager->get<StorageManagerIF>(objects::IPC_STORE);
}

TestTask::~TestTask() {
}

ReturnValue_t TestTask::performOperation(uint8_t operationCode) {
    ReturnValue_t result = RETURN_OK;

    if(oneShotAction) {
        /* Add code here which should only be run once */
        performOneShotAction();
        oneShotAction = false;
    }

    /* Add code here which should only be run once per performOperation */
    performPeriodicAction();

    /* Add code here which should only be run on alternating cycles. */
    if(testMode == testModes::A) {
        performActionA();
        testMode = testModes::B;
    }
    else if(testMode == testModes::B) {
        performActionB();
        testMode = testModes::A;
    }
    return result;
}

#include <etl/vector.h>
#include <lwgps/lwgps.h>

/**
 * @brief           Dummy data from GPS receiver. Will be replaced witgh hyperion data later.
 */
const char
gps_rx_data[] = ""
        "$GPRMC,183729,A,3907.356,N,12102.482,W,000.0,360.0,080301,015.5,E*6F\r\n"
        "$GPRMB,A,,,,,,,,,,,,V*71\r\n"
        "$GPGGA,183730,3907.356,N,12102.482,W,1,05,1.6,646.4,M,-24.1,M,,*75\r\n"
        "$GPGSA,A,3,02,,,07,,09,24,26,,,,,1.6,1.6,1.0*3D\r\n"
        "$GPGSV,2,1,08,02,43,088,38,04,42,145,00,05,11,291,00,07,60,043,35*71\r\n"
        "$GPGSV,2,2,08,08,02,145,00,09,46,303,47,24,16,178,32,26,18,231,43*77\r\n"
        "$PGRME,22.0,M,52.9,M,51.0,M*14\r\n"
        "$GPGLL,3907.360,N,12102.481,W,183730,A*33\r\n"
        "$PGRMZ,2062,f,3*2D\r\n"
        "$PGRMM,WGS84*06\r\n"
        "$GPBOD,,T,,M,,*47\r\n"
        "$GPRTE,1,1,c,0*07\r\n"
        "$GPRMC,183731,A,3907.482,N,12102.436,W,000.0,360.0,080301,015.5,E*67\r\n"
        "$GPRMB,A,,,,,,,,,,,,V*71\r\n";

ReturnValue_t TestTask::performOneShotAction() {
#if OBSW_ADD_TEST_CODE == 1
    //performLwgpsTest();
#endif
    return HasReturnvaluesIF::RETURN_OK;
}


ReturnValue_t TestTask::performPeriodicAction() {
    ReturnValue_t result = RETURN_OK;
    return result;
}

ReturnValue_t TestTask::performActionA() {
    ReturnValue_t result = RETURN_OK;
    /* Add periodically executed code here */
    return result;
}

ReturnValue_t TestTask::performActionB() {
    ReturnValue_t result = RETURN_OK;
    /* Add periodically executed code here */
    return result;
}

void TestTask::performLwgpsTest() {
    /* Everything here will only be performed once. */
    etl::vector<uint8_t, 30> testVec;

    lwgps_t gpsStruct;
    sif::info << "Size of GPS struct: " << sizeof(gpsStruct) << std::endl;
    lwgps_init(&gpsStruct);

    /* Process all input data */
    lwgps_process(&gpsStruct, gps_rx_data, strlen(gps_rx_data));

    /* Print messages */
    printf("Valid status: %d\r\n", gpsStruct.is_valid);
    printf("Latitude: %f degrees\r\n", gpsStruct.latitude);
    printf("Longitude: %f degrees\r\n", gpsStruct.longitude);
    printf("Altitude: %f meters\r\n", gpsStruct.altitude);
}