/*
 * PollingSequenceCentroidFunction.cpp
 *
 *  Created on: May 1, 2021
 *      Author: mala
 */

#include <fsfw/objectmanager/ObjectManagerIF.h>
#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
#include <fsfw/tasks/FixedTimeslotTaskIF.h>
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
#include "../../../bsp_linux/fsfwconfig/objects/systemObjectList.h"
#include "../../../bsp_linux/fsfwconfig/OBSWConfig.h"
#include "PollingSequenceArduino.h"
ReturnValue_t pollingSequenceArduinoFunction(
		FixedTimeslotTaskIF* thisSequence){
	uint32_t length = thisSequence->getPeriodMs();

	//thisSequence->addSlot(objects::CENTROID_INJECTOR, length * 0, 2);
	//thisSequence->addSlot(objects::CENTROID_WRITER, length * 0.1, 2);
	thisSequence->addSlot(objects::ARDUINO_HANDLER, length * 0, 0);
	thisSequence->addSlot(objects::ARDUINO_HANDLER, length * 0.2, 1);
	thisSequence->addSlot(objects::ARDUINO_HANDLER, length * 0.4, 2);
	thisSequence->addSlot(objects::ARDUINO_HANDLER, length * 0.6, 3);
	//thisSequence->addSlot(objects::CENTROID_WRITER, length * 0.1, 2);

	if (thisSequence->checkSequence() == HasReturnvaluesIF::RETURN_OK) {
		return HasReturnvaluesIF::RETURN_OK;
	}
	else {
		sif::error << "pollingSequenceCentroidFunction::initialize has errors!"
				<< std::endl;
		return HasReturnvaluesIF::RETURN_FAILED;
	}
}