#include "LibgpiodTest.h"

#include "devices/gpioIds.h"

#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
#include <fsfw/objectmanager/ObjectManager.h>
#include <fsfw/tasks/TaskFactory.h>

LibgpiodTest::LibgpiodTest(object_id_t objectId, object_id_t gpioIfobjectId,
        GpioCookie* gpioCookie):
        TestTask(objectId) {

    gpioInterface = ObjectManager::instance()->get<GpioIF>(gpioIfobjectId);
    if (gpioInterface == nullptr) {
        sif::error << "LibgpiodTest::LibgpiodTest: Invalid Gpio interface." << std::endl;
    }
    gpioInterface->addGpios(gpioCookie);
    testCase = TestCases::BLINK;
}

LibgpiodTest::~LibgpiodTest() {
}

ReturnValue_t LibgpiodTest::performPeriodicAction() {
    int gpioState;
    ReturnValue_t result;

    switch(testCase) {
    case(TestCases::READ): {
        result = gpioInterface->readGpio(gpioIds::TEST_ID_0, &gpioState);
        if (result != RETURN_OK) {
            sif::warning << "LibgpiodTest::performPeriodicAction: Failed to read gpio "
                    << std::endl;
            return RETURN_FAILED;
        }
        else {
            sif::debug << "LibgpiodTest::performPeriodicAction: MIO 0 state = " << gpioState
                    << std::endl;
        }
        break;
    }
    case(TestCases::LOOPBACK): {
        break;
    }
    case(TestCases::BLINK): {
        result = gpioInterface->readGpio(gpioIds::TEST_ID_0, &gpioState);
        if (result != HasReturnvaluesIF::RETURN_OK) {
            sif::warning << "LibgpiodTest::performPeriodicAction: Failed to read gpio "
                    << std::endl;
            return RETURN_FAILED;
        }
        if (gpioState == 1) {
            result = gpioInterface->pullLow(gpioIds::TEST_ID_0);
            if(result != HasReturnvaluesIF::RETURN_OK) {
                sif::warning << "LibgpiodTest::performPeriodicAction: Could not pull GPIO low!"
                        << std::endl;
                return HasReturnvaluesIF::RETURN_FAILED;
            }
        }
        else if (gpioState == 0) {
            result = gpioInterface->pullHigh(gpioIds::TEST_ID_0);
            if(result != HasReturnvaluesIF::RETURN_OK) {
                sif::warning << "LibgpiodTest::performPeriodicAction: Could not pull GPIO high!"
                        << std::endl;
                return HasReturnvaluesIF::RETURN_FAILED;
            }
        }
        else {
            sif::warning << "LibgpiodTest::performPeriodicAction: Invalid GPIO state" << std::endl;
        }

        break;
    }
    default:
        sif::debug << "LibgpiodTest::performPeriodicAction: Invalid test case" << std::endl;
        break;
    }


    return RETURN_OK;
}

ReturnValue_t LibgpiodTest::performOneShotAction() {
    int gpioState;
    ReturnValue_t result;

    switch(testCase) {
    case(TestCases::READ): {
        break;
    }
    case(TestCases::BLINK): {
        break;
    }
    case(TestCases::LOOPBACK): {
        result = gpioInterface->pullHigh(gpioIds::TEST_ID_0);
        if(result == HasReturnvaluesIF::RETURN_OK) {
            sif::info << "LibgpiodTest::performOneShotAction: "
                    "GPIO pulled high successfully for loopback test" << std::endl;
        }
        else {
            sif::warning << "LibgpiodTest::performOneShotAction: Could not pull GPIO high!"
                    << std::endl;
            return HasReturnvaluesIF::RETURN_OK;
        }
        result = gpioInterface->readGpio(gpioIds::TEST_ID_1, &gpioState);
        if(result == HasReturnvaluesIF::RETURN_OK and gpioState == 1) {
            sif::info << "LibgpiodTest::performOneShotAction: "
                    "GPIO state read successfully and is high" << std::endl;
        }
        else {
            sif::warning << "LibgpiodTest::performOneShotAction: GPIO read and is not high!"
                    << std::endl;
            return HasReturnvaluesIF::RETURN_OK;
        }

        result = gpioInterface->pullLow(gpioIds::TEST_ID_0);
        if(result == HasReturnvaluesIF::RETURN_OK) {
            sif::info << "LibgpiodTest::performOneShotAction: "
                    "GPIO pulled low successfully for loopback test" << std::endl;
        }
        result = gpioInterface->readGpio(gpioIds::TEST_ID_1, &gpioState);
        if(result == HasReturnvaluesIF::RETURN_OK and gpioState == 0) {
            sif::info << "LibgpiodTest::performOneShotAction: "
                    "GPIO state read successfully and is low" << std::endl;
        }
        else {
            sif::warning << "LibgpiodTest::performOneShotAction: GPIO read and is not low!"
                    << std::endl;
            return HasReturnvaluesIF::RETURN_OK;
        }
        break;
    }
    }
    return HasReturnvaluesIF::RETURN_OK;
}