#include "../../bsp_linux/core/InitMission.h"
#include <OBSWConfig.h>

#include <fsfw/returnvalues/HasReturnvaluesIF.h>
#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
#include <fsfw/tasks/FixedTimeslotTaskIF.h>
#include <fsfw/tasks/PeriodicTaskIF.h>
#include <fsfw/tasks/TaskFactory.h>
#include <iostream>
#include "../../bsp_linux/fsfwconfig/objects/systemObjectList.h"
#include "../../bsp_linux/fsfwconfig/OBSWConfig.h"
#include "../fsfwconfig/pollingsequence/PollingSequenceArduino.h"

void InitMission::createTasks(){

	/* TMTC Distribution */
	PeriodicTaskIF* distributerTask = TaskFactory::instance()->
			createPeriodicTask("DIST", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE,
					0.100, nullptr);
	ReturnValue_t result = distributerTask->addComponent(
			objects::CCSDS_DISTRIBUTOR);
	if(result!=HasReturnvaluesIF::RETURN_OK){
		sif::error << "Object add component failed" << std::endl;
	}
	result = distributerTask->addComponent(objects::PUS_DISTRIBUTOR);
	if(result!=HasReturnvaluesIF::RETURN_OK){
		sif::error << "Object add component failed" << std::endl;
	}
    result = distributerTask->addComponent(objects::TM_FUNNEL);
    if(result != HasReturnvaluesIF::RETURN_OK) {
    	sif::error << "Object add component failed" << std::endl;
    }

    /* UDP bridge */
    PeriodicTaskIF* udpBridgeTask = TaskFactory::instance()->createPeriodicTask(
    		"UDP_UNIX_BRIDGE", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE,
			0.2, nullptr);
    result = udpBridgeTask->addComponent(objects::UDP_BRIDGE);
    if(result != HasReturnvaluesIF::RETURN_OK) {
    	sif::error << "Add component UDP Unix Bridge failed" << std::endl;
    }
    PeriodicTaskIF* udpPollingTask = TaskFactory::instance()->
    		createPeriodicTask("UDP_POLLING", 80,
    		PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, nullptr);
    result = udpPollingTask->addComponent(objects::UDP_POLLING_TASK);
    if(result != HasReturnvaluesIF::RETURN_OK) {
    	sif::error << "Add component UDP Polling failed" << std::endl;
    }



	PeriodicTaskIF* eventTask = TaskFactory::instance()->
			createPeriodicTask("EVENT", 20,
					PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.100, nullptr);
	result = eventTask->addComponent(objects::EVENT_MANAGER);
	if(result!=HasReturnvaluesIF::RETURN_OK){
		sif::error << "Object add component failed" << std::endl;
	}


	/* PUS Services */
	PeriodicTaskIF* pusVerification = TaskFactory::instance()->
			createPeriodicTask("PUS_VERIF_1", 40,
					PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, nullptr);
	result = pusVerification->addComponent(objects::PUS_SERVICE_1_VERIFICATION);
	if(result != HasReturnvaluesIF::RETURN_OK){
		sif::error << "Object add component failed" << std::endl;
	}

	PeriodicTaskIF* pusEvents = TaskFactory::instance()->
			createPeriodicTask("PUS_VERIF_1", 60,
					PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, nullptr);
	result = pusVerification->addComponent(objects::PUS_SERVICE_5_EVENT_REPORTING);
	if(result != HasReturnvaluesIF::RETURN_OK){
		sif::error << "Object add component failed" << std::endl;
	}

	PeriodicTaskIF* pusHighPrio = TaskFactory::instance()->
			createPeriodicTask("PUS_HIGH_PRIO", 50,
					PeriodicTaskIF::MINIMUM_STACK_SIZE,
					0.200, nullptr);
	result = pusHighPrio->addComponent(objects::PUS_SERVICE_2_DEVICE_ACCESS);
	if(result!=HasReturnvaluesIF::RETURN_OK){
		sif::error << "Object add component failed" << std::endl;
	}
	result = pusHighPrio->addComponent(objects::PUS_SERVICE_9_TIME_MGMT);
	if(result!=HasReturnvaluesIF::RETURN_OK){
		sif::error << "Object add component failed" << std::endl;
	}

	PeriodicTaskIF* pusMedPrio = TaskFactory::instance()->
			createPeriodicTask("PUS_HIGH_PRIO", 40,
					PeriodicTaskIF::MINIMUM_STACK_SIZE,
					0.8, nullptr);
	result = pusMedPrio->addComponent(objects::PUS_SERVICE_8_FUNCTION_MGMT);
	if(result!=HasReturnvaluesIF::RETURN_OK){
		sif::error << "Object add component failed" << std::endl;
	}
	result = pusMedPrio->addComponent(objects::PUS_SERVICE_200_MODE_MGMT);
	if(result!=HasReturnvaluesIF::RETURN_OK){
		sif::error << "Object add component failed" << std::endl;
	}

	PeriodicTaskIF* pusLowPrio = TaskFactory::instance()->
			createPeriodicTask("PUSB", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE,
					4, nullptr);
	result = pusLowPrio->addComponent(objects::PUS_SERVICE_17_TEST);
	if(result!=HasReturnvaluesIF::RETURN_OK){
		sif::error << "Object add component failed" << std::endl;
	}
	
	FixedTimeslotTaskIF* arduinoTask = TaskFactory::instance()->
			createFixedTimeslotTask("ARDUINO_TASK",40,
					PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.8/*6.4*/, nullptr);
	result = pollingSequenceArduinoFunction(arduinoTask);
	if(result != HasReturnvaluesIF::RETURN_OK) {
		sif::error << "InitMission::createTasks:ArduinoPST initialization failed!"
				<< std::endl;
	}
	
	PeriodicTaskIF* controllerTask = TaskFactory::instance()->
			createPeriodicTask("CONTROLLER_TASK",40,
					PeriodicTaskIF::MINIMUM_STACK_SIZE, 2, nullptr);
	//result = pollingSequenceControllerFunction(controllerTask);
	result = controllerTask->addComponent(objects::THERMAL_CONTROLLER);
	if(result != HasReturnvaluesIF::RETURN_OK) {
		sif::error << "InitMission::createTasks:ArduinoController initialization failed!"
				<< std::endl;
	}

#if OBSW_ADD_TEST_CODE == 1
	FixedTimeslotTaskIF* testTimeslotTask = TaskFactory::instance()->
			createFixedTimeslotTask("PST_TEST_TASK", 10,
					PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, nullptr);
	result = pst::pollingSequenceTestFunction(testTimeslotTask);
	if(result != HasReturnvaluesIF::RETURN_OK) {
		sif::error << "InitMission::createTasks: Test PST initialization "
				<< "failed!" << std::endl;
	}

	PeriodicTaskIF* testTask = TaskFactory::instance()->
			createPeriodicTask("TEST", 15, PeriodicTaskIF::MINIMUM_STACK_SIZE,
					1.0, nullptr);
	result = testTask->addComponent(objects::TEST_TASK);
	if(result!=HasReturnvaluesIF::RETURN_OK){
		sif::error << "InitMission::createTasks: Adding test task "
				<< "failed" << std::endl;
	}

#endif

	//Main thread sleep
	sif::debug << "Starting Tasks in 2 seconds" << std::endl;
	TaskFactory::delayTask(2400);
	distributerTask->startTask();
	udpBridgeTask->startTask();
	udpPollingTask->startTask();
	//serializeTask->startTask();
	arduinoTask->startTask();
	//controllerTask->startTask();

	//payloadTask->startTask();
	eventTask->startTask();

	pusVerification->startTask();
	pusEvents->startTask();
	pusHighPrio->startTask();
	pusMedPrio->startTask();
	pusLowPrio->startTask();

#if OBSW_ADD_TEST_CODE == 1
	testTimeslotTask->startTask();
	testTask->startTask();
#endif
	sif::debug << "Tasks started.." << std::endl;
}