#include "LibgpioTest.h"
#include "devices/gpioIds.h"
#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
#include <fsfw/objectmanager/ObjectManagerIF.h>

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

    gpioInterface = objectManager->get<GpioIF>(gpioIfobjectId);
    if (gpioInterface == nullptr) {
        sif::error << "LibgpioTest::LibgpioTest: Invalid Gpio interface." << std::endl;
    }
    gpioInterface->initialize(gpioCookie);
}

LibgpioTest::~LibgpioTest() {
}

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

    result = gpioInterface->readGpio(gpioIds::Test_ID, &gpioState);
    if (result != RETURN_OK) {
        sif::debug << "LibgpioTest::performPeriodicAction: Failed to read gpio "
                << std::endl;
        return RETURN_FAILED;
    }
    else {
        sif::debug << "LibgpioTest::performPeriodicAction: MIO 0 state = " << gpioState
                << std::endl;
    }
    return RETURN_OK;
}