#include <bsp_linux/InitMission.h>
#include <bsp_linux/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 <fsfwconfig/objects/systemObjectList.h>
#include <fsfwconfig/OBSWConfig.h>
#include <fsfwconfig/pollingsequence/PollingSequenceFactory.h>

#include <iostream>

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

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;
	}

//	PeriodicTaskIF* P60DockTask = TaskFactory::instance()->
//			createPeriodicTask("P60Dock Task", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE*4,
//					1.6, nullptr);
//	result = P60DockTask->addComponent(objects::P60DOCK_HANDLER);
//	if(result!=HasReturnvaluesIF::RETURN_OK){
//		sif::error << "Object add component failed" << std::endl;
//	}

	FixedTimeslotTaskIF* GomSpacePstTask = TaskFactory::instance()->
			createFixedTimeslotTask("GS_PST_TASK", 50,
					PeriodicTaskIF::MINIMUM_STACK_SIZE*4, 1.0, nullptr);
	result = pst::gomspacePstInit(GomSpacePstTask);
	if(result != HasReturnvaluesIF::RETURN_OK) {
		sif::error << "InitMission::initTasks: GomSpace PST initialization "
				<< "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();

	GomSpacePstTask->startTask();

	PusVerification->startTask();
	PusEvents->startTask();
	PusHighPrio->startTask();
	PusMedPrio->startTask();
	PusLowPrio->startTask();

//	P60DockTask->startTask();

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