#include "STM32TestTask.h"
#include "stm32h7xx_nucleo.h"
#include "OBSWConfig.h"

#include "fsfw_hal/stm32h7/devicetest/GyroL3GD20H.h"
#include "fsfw/serviceinterface/ServiceInterface.h"

STM32TestTask::STM32TestTask(object_id_t objectId, bool enablePrintout,
		bool blinkyLed): TestTask(objectId, enablePrintout),
		blinkyLed(blinkyLed) {
}

ReturnValue_t STM32TestTask::performPeriodicAction() {
    if(blinkyLed) {
#if OBSW_ETHERNET_USE_LEDS == 0
        BSP_LED_Toggle(LED1);
        BSP_LED_Toggle(LED2);
#endif
        BSP_LED_Toggle(LED3);
    }
    if(gyro != nullptr) {
        gyro->performOperation();
    }

    return TestTask::performPeriodicAction();
}

ReturnValue_t STM32TestTask::performOneShotAction() {
    // Might be confusing but if this is defined to 1, the real device handler is tested and we
    // should not mess with the SPI bus
#if OBSW_PERFORM_L3GD20H_TEST == 0
    performSpiL3gd20hTest();
#endif
    return HasReturnvaluesIF::RETURN_OK;
}

void STM32TestTask::performSpiL3gd20hTest() {
    gyro = new GyroL3GD20H(&spiHandle, spi::TransferModes::DMA);
    gyro->initialize();
    gyro->performOperation();
}