#include "InitMission.h"
#include <OBSWConfig.h>

#include <bsp_linux/fsfwconfig/objects/systemObjectList.h>
#include <bsp_linux/fsfwconfig/pollingsequence/pollingSequenceFactory.h>

#include <mission/utility/TaskCreation.h>

#include <fsfw/returnvalues/HasReturnvaluesIF.h>
#include <fsfw/serviceinterface/ServiceInterface.h>
#include <fsfw/tasks/FixedTimeslotTaskIF.h>
#include <fsfw/tasks/PeriodicTaskIF.h>
#include <fsfw/tasks/TaskFactory.h>

#include <fsfw/ipc/MessageQueueSenderIF.h>
#include <fsfw/ipc/CommandMessage.h>
#include <fsfw/modes/ModeMessage.h>
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
#include <mission/assemblies/TestAssembly.h>

void InitMission::createTasks() {
    TaskFactory* taskFactory = TaskFactory::instance();
    if(taskFactory == nullptr) {
        return;
    }

#if OBSW_ADD_CORE_COMPONENTS == 1
    /* TMTC Distribution */
    PeriodicTaskIF* distributerTask = taskFactory->
            createPeriodicTask("DIST", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, nullptr);
    ReturnValue_t result = distributerTask->addComponent(objects::CCSDS_DISTRIBUTOR);
    if (result != HasReturnvaluesIF::RETURN_OK) {
        task::printInitError("CCSDS distributor", objects::CCSDS_DISTRIBUTOR);
    }
    result = distributerTask->addComponent(objects::PUS_DISTRIBUTOR);
    if (result!=HasReturnvaluesIF::RETURN_OK) {
        task::printInitError("PUS distributor", objects::PUS_DISTRIBUTOR);
    }
    result = distributerTask->addComponent(objects::TM_FUNNEL);
    if (result != HasReturnvaluesIF::RETURN_OK) {
        task::printInitError("TM funnel", objects::TM_FUNNEL);
    }

    /* UDP bridge */
    PeriodicTaskIF* udpBridgeTask = taskFactory->createPeriodicTask(
            "UDP_UNIX_BRIDGE", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, nullptr);
    result = udpBridgeTask->addComponent(objects::UDP_BRIDGE);
    if(result != HasReturnvaluesIF::RETURN_OK) {
        task::printInitError("UDP bridge", objects::UDP_BRIDGE);
    }
    /* UDP polling task */
    PeriodicTaskIF* udpPollingTask = taskFactory->createPeriodicTask(
            "UDP_POLLING", 80, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.1, nullptr);
    result = udpPollingTask->addComponent(objects::UDP_POLLING_TASK);
    if(result != HasReturnvaluesIF::RETURN_OK) {
        task::printInitError("UDP polling", objects::UDP_POLLING_TASK);
    }

    PeriodicTaskIF* eventTask = taskFactory->createPeriodicTask(
            "EVENT_MGMT", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.1, nullptr);
    result = eventTask->addComponent(objects::EVENT_MANAGER);
    if(result != HasReturnvaluesIF::RETURN_OK){
        task::printInitError("Event Manager", objects::EVENT_MANAGER);
    }
#endif /* OBSW_ADD_CORE_COMPONENTS == 1 */

#if OBSW_ADD_TASK_EXAMPLE == 1
    FixedTimeslotTaskIF* timeslotDemoTask = taskFactory->createFixedTimeslotTask
            ("PST_TASK", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.5, nullptr);
    result = pst::pollingSequenceExamples(timeslotDemoTask);
    if(result != HasReturnvaluesIF::RETURN_OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
        sif::error << "InitMission::createTasks: Timeslot demo task"
                <<" initialization failed!" << std::endl;
#else
        sif::printError("InitMission::createTasks: Timeslot demo task"
                " initialization failed!\n");
#endif
    }

    PeriodicTaskIF* readerTask = taskFactory->
            createPeriodicTask("READER_TASK", 40,
                    PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, nullptr);
    result = readerTask->addComponent(objects::TEST_DUMMY_4);
    if(result != HasReturnvaluesIF::RETURN_OK) {
        task::printInitError("Dummy 4", objects::TEST_DUMMY_4);
    }
#endif /* OBSW_ADD_TASK_EXAMPLE == 1 */

#if OBSW_ADD_PUS_STACK == 1
    /* PUS Services */
    PeriodicTaskIF* pusVerification = taskFactory->createPeriodicTask(
            "PUS_VERIF_1", 45, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, nullptr);
    result = pusVerification->addComponent(objects::PUS_SERVICE_1_VERIFICATION);
    if(result != HasReturnvaluesIF::RETURN_OK) {
        task::printInitError("PUS 1", objects::PUS_SERVICE_1_VERIFICATION);
    }

    PeriodicTaskIF* pusHighPrio = taskFactory->createPeriodicTask(
            "PUS_HIGH_PRIO", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, nullptr);
    result = pusHighPrio->addComponent(objects::PUS_SERVICE_2_DEVICE_ACCESS);
    if (result != HasReturnvaluesIF::RETURN_OK) {
        task::printInitError("PUS 2", objects::PUS_SERVICE_2_DEVICE_ACCESS);
    }
    result = pusHighPrio->addComponent(objects::PUS_SERVICE_5_EVENT_REPORTING);
    if(result != HasReturnvaluesIF::RETURN_OK){
        task::printInitError("PUS 5",objects::PUS_SERVICE_5_EVENT_REPORTING);
    }
    result = pusHighPrio->addComponent(objects::PUS_SERVICE_9_TIME_MGMT);
    if (result != HasReturnvaluesIF::RETURN_OK) {
        task::printInitError("PUS 9", objects::PUS_SERVICE_9_TIME_MGMT);
    }

    PeriodicTaskIF* pusMedPrio = taskFactory->createPeriodicTask(
            "PUS_MED_PRIO", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.8, nullptr);
    result = pusMedPrio->addComponent(objects::PUS_SERVICE_8_FUNCTION_MGMT);
    if (result != HasReturnvaluesIF::RETURN_OK) {
        task::printInitError("PUS 8", objects::PUS_SERVICE_8_FUNCTION_MGMT);
    }
    result = pusMedPrio->addComponent(objects::PUS_SERVICE_20_PARAMETERS);
    if (result != HasReturnvaluesIF::RETURN_OK) {
        task::printInitError("PUS 20", objects::PUS_SERVICE_20_PARAMETERS);
    }
    result = pusMedPrio->addComponent(objects::PUS_SERVICE_200_MODE_MGMT);
    if (result != HasReturnvaluesIF::RETURN_OK) {
        task::printInitError("PUS 200", objects::PUS_SERVICE_200_MODE_MGMT);
    }

    PeriodicTaskIF* pusLowPrio = taskFactory->createPeriodicTask(
            "PUS_LOW_PRIO", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.6, nullptr);
    result = pusLowPrio->addComponent(objects::PUS_SERVICE_17_TEST);
    if(result != HasReturnvaluesIF::RETURN_OK) {
        task::printInitError("PUS 17", objects::PUS_SERVICE_17_TEST);
    }
#endif /* OBSW_ADD_PUS_STACK == 1 */

#if OBSW_ADD_DEVICE_HANDLER_DEMO == 1
    FixedTimeslotTaskIF* testDeviceTask = taskFactory->createFixedTimeslotTask(
            "PST_TEST_TASK", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, nullptr);
    result = pst::pollingSequenceDevices(testDeviceTask);
    if(result != HasReturnvaluesIF::RETURN_OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
        sif::error << "InitMission::createTasks: Test PST initialization failed!" << std::endl;
#else
        sif::printError("InitMission::createTasks: Test PST initialization failed!\n\r");
#endif
    }
    PeriodicTaskIF* assemblyTask = taskFactory->createPeriodicTask("ASS_TASK", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, nullptr);
    if(assemblyTask == nullptr){
		#if FSFW_CPP_OSTREAM_ENABLED == 1
				sif::error << "InitMission::createTasks: Test Assembly Task creation failed!" << std::endl;
		#else
				sif::printError("InitMission::createTasks: Test Assembly Task creation failed!\n\r");
		#endif
    }
    result = assemblyTask->addComponent(objects::TEST_ASSEMBLY);
    if(result != HasReturnvaluesIF::RETURN_OK) {
        if(assemblyTask == nullptr){
    		#if FSFW_CPP_OSTREAM_ENABLED == 1
    				sif::error << "InitMission::createTasks: Test Assembly Task adding failed!" << std::endl;
    		#else
    				sif::printError("InitMission::createTasks: Test Assembly Task adding failed!\n\r");
    		#endif
        }
    }
#endif /* OBSW_ADD_DEVICE_HANDLER_DEMO == 1 */

#if OBSW_ADD_CONTROLLER_DEMO == 1
    PeriodicTaskIF* controllerTask = taskFactory->createPeriodicTask(
            "TEST_CTRL", 45, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.8, nullptr);
    result = controllerTask->addComponent(objects::TEST_CONTROLLER);
    if(result != HasReturnvaluesIF::RETURN_OK) {
        task::printInitError("Controller Task", objects::TEST_CONTROLLER);
    }
#endif /* OBSW_ADD_CONTROLLER_DEMO == 1 */

    PeriodicTaskIF* testTask = taskFactory->createPeriodicTask(
            "TEST", 15, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, nullptr);
    result = testTask->addComponent(objects::TEST_TASK);
    if(result != HasReturnvaluesIF::RETURN_OK) {
        task::printInitError("Test Task", objects::TEST_TASK);
    }

#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::info << "Starting tasks.." << std::endl;
#else
    sif::printInfo("Starting tasks..\n");
#endif

#if OBSW_ADD_CORE_COMPONENTS == 1
    distributerTask->startTask();
    udpBridgeTask->startTask();
    udpPollingTask->startTask();
    eventTask->startTask();
#endif /* OBSW_ADD_CORE_COMPONENTS == 1 */

#if OBSW_ADD_PUS_STACK == 1
    pusVerification->startTask();
    pusHighPrio->startTask();
    pusMedPrio->startTask();
    pusLowPrio->startTask();
#endif /* OBSW_ADD_PUS_STACK == 1 */

#if OBSW_ADD_TASK_EXAMPLE == 1
    timeslotDemoTask->startTask();
    readerTask->startTask();
#endif /* OBSW_ADD_TASK_EXAMPLE == 1 */

#if OBSW_ADD_DEVICE_HANDLER_DEMO == 1
    testDeviceTask->startTask();
    assemblyTask->startTask();
#endif /* OBSW_ADD_DEVICE_HANDLER_DEMO == 1 */

#if OBSW_ADD_CONTROLLER_DEMO == 1
    controllerTask->startTask();
#endif /* OBSW_ADD_CONTROLLER_DEMO == 1 */

    testTask->startTask();

#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::info << "Tasks started.." << std::endl;
#else
    sif::printInfo("Tasks started..\n");
#endif


#if OBSW_ADD_DEVICE_HANDLER_DEMO
    HasModesIF* assembly = objectManager->get<HasModesIF>(objects::TEST_ASSEMBLY);
    if (assembly == nullptr){
    	return;
    }
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::info << "Waiting 5 Seconds and then command Test Assembly to Normal, Dual" << std::endl;
#else
    sif::printInfo("Waiting 5 Seconds and then command Test Assembly to Normal, Dual \n");
#endif

    TaskFactory::delayTask(5000);
    CommandMessage modeMessage;
    ModeMessage::setModeMessage(&modeMessage, ModeMessage::CMD_MODE_COMMAND, DeviceHandlerIF::MODE_NORMAL, TestAssembly::submodes::DUAL);
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::info << "Commanding Test Assembly to Normal, Dual" << std::endl;
#else
    sif::printInfo("Commanding Test Assembly to Normal, Dual \n");
#endif
    MessageQueueSenderIF::sendMessage(assembly->getCommandQueue(), &modeMessage, MessageQueueIF::NO_QUEUE);
#endif
}