#include "InitMission.h"

#include <OBSWConfig.h>
#include <config/objects/systemObjectList.h>
#include <mission/core/ObjectFactory.h>

#include <fsfw/objectmanager/ObjectManagerIF.h>
#include <fsfw/returnvalues/HasReturnvaluesIF.h>
#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
#include <fsfw/datapool/DataPool.h>
#include <fsfw/objectmanager/ObjectManager.h>
#include <fsfw/tasks/FixedTimeslotTaskIF.h>
#include <fsfw/tasks/PeriodicTaskIF.h>
#include <fsfw/tasks/TaskFactory.h>

#include <iostream>

// This is configured for linux without \cr
ServiceInterfaceStream sif::debug("DEBUG", false);
ServiceInterfaceStream sif::info("INFO", false);
ServiceInterfaceStream sif::warning("WARNING", false);
ServiceInterfaceStream sif::error("ERROR", false, false, true);

ObjectManagerIF *objectManager = nullptr;

//Initialize Data Pool
DataPool dataPool(nullptr);

void InitMission::initMission() {
	sif::info << "Building global objects.." << std::endl;
	/* Instantiate global object manager and also create all objects */
    objectManager = new ObjectManager(ObjectFactory::produce);
    sif::info << "Initializing all objects.." << std::endl;
    objectManager->initialize();

    /* This function creates and starts all tasks */
    initTasks();
}

void InitMission::initTasks(){
	/* TMTC Distribution */
	PeriodicTaskIF* TmTcDistributor = TaskFactory::instance()->
			createPeriodicTask("DIST", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE,
					0.100, nullptr);
	ReturnValue_t result = TmTcDistributor->addComponent(
			objects::CCSDS_PACKET_DISTRIBUTOR);
	if(result!=HasReturnvaluesIF::RETURN_OK){
		sif::error << "Object add component failed" << std::endl;
	}
	result = TmTcDistributor->addComponent(objects::PUS_PACKET_DISTRIBUTOR);
	if(result!=HasReturnvaluesIF::RETURN_OK){
		sif::error << "Object add component failed" << std::endl;
	}
    result = TmTcDistributor->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;
    }

	/* 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,
					1.6, nullptr);
	result = PusLowPrio->addComponent(objects::PUS_SERVICE_17_TEST);
	if(result!=HasReturnvaluesIF::RETURN_OK){
		sif::error << "Object add component failed" << std::endl;
	}


#if 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;
	}
#endif

	//Main thread sleep
	sif::info << "Starting tasks.." << std::endl;
	TmTcDistributor->startTask();
	UdpBridgeTask->startTask();
	UdpPollingTask->startTask();

	PusVerification->startTask();
	PusEvents->startTask();
	PusHighPrio->startTask();
	PusMedPrio->startTask();
	PusLowPrio->startTask();
#if ADD_TEST_CODE == 1
	TestTimeslotTask->startTask();
#endif
	sif::info << "Tasks started.." << std::endl;
}