#include <fsfw/objectmanager/ObjectManagerIF.h>
#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
#include <fsfw/tasks/FixedTimeslotTaskIF.h>
#include <fsfwconfig/objects/systemObjectList.h>
#include <fsfwconfig/pollingsequence/PollingSequenceFactory.h>

ReturnValue_t pst::pollingSequenceInitDefault(FixedTimeslotTaskIF *thisSequence)
{
	/* Length of a communication cycle */
	uint32_t length = thisSequence->getPeriodMs();

    thisSequence->addSlot(objects::TMP1075_HANDLER_1, length * 0,
            DeviceHandlerIF::PERFORM_OPERATION);
    thisSequence->addSlot(objects::TMP1075_HANDLER_2, length * 0,
            DeviceHandlerIF::PERFORM_OPERATION);
    thisSequence->addSlot(objects::HEATER_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);

    thisSequence->addSlot(objects::TMP1075_HANDLER_1, length * 0.2, DeviceHandlerIF::SEND_WRITE);
    thisSequence->addSlot(objects::TMP1075_HANDLER_2, length * 0.2, DeviceHandlerIF::SEND_WRITE);

    thisSequence->addSlot(objects::TMP1075_HANDLER_1, length * 0.4, DeviceHandlerIF::GET_WRITE);
    thisSequence->addSlot(objects::TMP1075_HANDLER_2, length * 0.4, DeviceHandlerIF::GET_WRITE);

    thisSequence->addSlot(objects::TMP1075_HANDLER_1, length * 0.6, DeviceHandlerIF::SEND_READ);
    thisSequence->addSlot(objects::TMP1075_HANDLER_2, length * 0.6, DeviceHandlerIF::SEND_READ);

    thisSequence->addSlot(objects::TMP1075_HANDLER_1, length * 0.8, DeviceHandlerIF::GET_READ);
    thisSequence->addSlot(objects::TMP1075_HANDLER_2, length * 0.8, DeviceHandlerIF::GET_READ);

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

ReturnValue_t pst::gomspacePstInit(FixedTimeslotTaskIF *thisSequence){

	uint32_t length = thisSequence->getPeriodMs();

	thisSequence->addSlot(objects::PCDU_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
	thisSequence->addSlot(objects::PCDU_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);

	thisSequence->addSlot(objects::P60DOCK_HANDLER,
	            length * 0,  DeviceHandlerIF::PERFORM_OPERATION);
	thisSequence->addSlot(objects::PDU1_HANDLER,
	            length * 0,  DeviceHandlerIF::PERFORM_OPERATION);
	thisSequence->addSlot(objects::PDU2_HANDLER,
	            length * 0,  DeviceHandlerIF::PERFORM_OPERATION);
	thisSequence->addSlot(objects::ACU_HANDLER,
	            length * 0,  DeviceHandlerIF::PERFORM_OPERATION);

	thisSequence->addSlot(objects::P60DOCK_HANDLER,
	            length * 0.2,  DeviceHandlerIF::SEND_WRITE);
	thisSequence->addSlot(objects::PDU1_HANDLER,
	            length * 0.2,  DeviceHandlerIF::SEND_WRITE);
	thisSequence->addSlot(objects::PDU2_HANDLER,
	            length * 0.2,  DeviceHandlerIF::SEND_WRITE);
	thisSequence->addSlot(objects::ACU_HANDLER,
	            length * 0.2,  DeviceHandlerIF::SEND_WRITE);

	thisSequence->addSlot(objects::P60DOCK_HANDLER,
			length * 0.4, DeviceHandlerIF::GET_WRITE);
	thisSequence->addSlot(objects::PDU1_HANDLER,
			length * 0.4, DeviceHandlerIF::GET_WRITE);
	thisSequence->addSlot(objects::PDU2_HANDLER,
			length * 0.4, DeviceHandlerIF::GET_WRITE);
	thisSequence->addSlot(objects::ACU_HANDLER,
			length * 0.4, DeviceHandlerIF::GET_WRITE);

	thisSequence->addSlot(objects::P60DOCK_HANDLER,
			length * 0.6,  DeviceHandlerIF::SEND_READ);
	thisSequence->addSlot(objects::PDU1_HANDLER,
			length * 0.6,  DeviceHandlerIF::SEND_READ);
	thisSequence->addSlot(objects::PDU2_HANDLER,
			length * 0.6,  DeviceHandlerIF::SEND_READ);
	thisSequence->addSlot(objects::ACU_HANDLER,
			length * 0.6,  DeviceHandlerIF::SEND_READ);

	thisSequence->addSlot(objects::P60DOCK_HANDLER,
			length * 0.8, DeviceHandlerIF::GET_READ);
	thisSequence->addSlot(objects::PDU1_HANDLER,
			length * 0.8, DeviceHandlerIF::GET_READ);
	thisSequence->addSlot(objects::PDU2_HANDLER,
			length * 0.8, DeviceHandlerIF::GET_READ);
	thisSequence->addSlot(objects::ACU_HANDLER,
			length * 0.8, DeviceHandlerIF::GET_READ);

	if (thisSequence->checkSequence() == HasReturnvaluesIF::RETURN_OK) {
		return HasReturnvaluesIF::RETURN_OK;
	}
	else {
		sif::error << "Initialization of GomSpace PST failed" << std::endl;
		return HasReturnvaluesIF::RETURN_FAILED;
	}
}