/**
 * Add polling sequence initialization which are not common to every BSP here.
 */
#include "pollingSequenceFactory.h"
#include "OBSWConfig.h"

#include <fsfw/devicehandlers/DeviceHandlerIF.h>
#include <fsfw/tasks/FixedTimeslotTaskIF.h>

ReturnValue_t pst::pstPeripheralsTest(FixedTimeslotTaskIF *thisSequence) {
  uint32_t length = thisSequence->getPeriodMs();
  static_cast<void>(length);

#if OBSW_PERFORM_L3GD20H_TEST == 1
  thisSequence->addSlot(objects::SPI_DEVICE_TEST, 0,
                        DeviceHandlerIF::PERFORM_OPERATION);
  thisSequence->addSlot(objects::SPI_DEVICE_TEST, 0.3,
                        DeviceHandlerIF::SEND_WRITE);
  thisSequence->addSlot(objects::SPI_DEVICE_TEST, 0.45 * length,
                        DeviceHandlerIF::GET_WRITE);
  thisSequence->addSlot(objects::SPI_DEVICE_TEST, 0.6 * length,
                        DeviceHandlerIF::SEND_READ);
  thisSequence->addSlot(objects::SPI_DEVICE_TEST, 0.8 * length,
                        DeviceHandlerIF::GET_READ);
#endif

  if (thisSequence->checkSequence() == HasReturnvaluesIF::RETURN_OK) {
    return HasReturnvaluesIF::RETURN_OK;
  }
#if FSFW_CPP_OSTREAM_ENABLED == 1
  sif::error << "pst::pollingSequenceInitFunction: Initialization errors!"
             << std::endl;
#else
  sif::printError("pst::pollingSequenceInitFunction: Initialization errors!\n");
#endif
  return HasReturnvaluesIF::RETURN_FAILED;
}