#include "InitMission.h"
#include "OBSWConfig.h"
#include "objects/systemObjectList.h"
#include "pollingsequence/pollingSequenceFactory.h"

#include "example/utility/TaskCreation.h"
#include "fsfw_tests/integration/assemblies/TestAssembly.h"

#include "fsfw/devicehandlers/DeviceHandlerIF.h"
#include "fsfw/modes/HasModesIF.h"
#include "fsfw/modes/ModeMessage.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 "FreeRTOS.h"

void assemlyDemo();

void InitMission::createTasks() {
  TaskFactory *taskFactory = TaskFactory::instance();
  ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
  static_cast<void>(taskFactory);
  static_cast<void>(result);
#if OBSW_ADD_CORE_COMPONENTS == 1
  /* TMTC Distribution */
  PeriodicTaskIF *distributerTask =
      taskFactory->createPeriodicTask("DIST", 5, 1024 * 2, 0.2, nullptr);
  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);
  }

#if OBSW_ADD_LWIP_COMPONENTS == 1
  /* UDP bridge */
  PeriodicTaskIF *udpBridgeTask = TaskFactory::instance()->createPeriodicTask(
      "UDP_UNIX_BRIDGE", 6, 1024 * 2, 0.2, nullptr);
  result = udpBridgeTask->addComponent(objects::UDP_BRIDGE);
  if (result != HasReturnvaluesIF::RETURN_OK) {
    task::printInitError("UDP bridge", objects::UDP_BRIDGE);
  }
  PeriodicTaskIF *udpPollingTask = TaskFactory::instance()->createPeriodicTask(
      "UDP_POLLING", 8, 1024, 0.1, nullptr);
  result = udpPollingTask->addComponent(objects::UDP_POLLING_TASK);
  if (result != HasReturnvaluesIF::RETURN_OK) {
    task::printInitError("UDP polling task", objects::UDP_POLLING_TASK);
  }
#endif

  PeriodicTaskIF *eventManagerTask =
      TaskFactory::instance()->createPeriodicTask("EVENT_MGMT", 4, 1024 * 2,
                                                  0.1, nullptr);
  result = eventManagerTask->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_PUS_STACK == 1
  /* PUS Services */
  PeriodicTaskIF *pusVerification = TaskFactory::instance()->createPeriodicTask(
      "PUS_VERIF_1", 4, 1024 * 2, 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::instance()->createPeriodicTask(
      "PUS_HIGH_PRIO", 5, 1024 * 2, 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::instance()->createPeriodicTask(
      "PUS_HIGH_PRIO", 4, 1024 * 2, 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_200_MODE_MGMT);
  if (result != HasReturnvaluesIF::RETURN_OK) {
    task::printInitError("PUS 200", objects::PUS_SERVICE_200_MODE_MGMT);
  }

  PeriodicTaskIF *pusLowPrio = TaskFactory::instance()->createPeriodicTask(
      "PUS_LOW_PRIO", 3, 1024 * 2, 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_TASK_EXAMPLE == 1
  FixedTimeslotTaskIF *timeslotDemoTask =
      TaskFactory::instance()->createFixedTimeslotTask("PST_TASK", 6, 1024 * 2,
                                                       0.5, nullptr);
  result = pst::pollingSequenceExamples(timeslotDemoTask);
  if (result != HasReturnvaluesIF::RETURN_OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::error << "InitMission::createTasks: PST initialization failed!"
               << std::endl;
#else
    sif::printError("InitMission::createTasks: PST initialization failed!\n");
#endif
  }

  PeriodicTaskIF *readerTask = TaskFactory::instance()->createPeriodicTask(
      "READER_TASK", 3, 1024, 1.0, nullptr);
  result = readerTask->addComponent(objects::TEST_DUMMY_4);
  if (result != HasReturnvaluesIF::RETURN_OK) {
    task::printInitError("TEST_DUMMY_4", objects::TEST_DUMMY_4);
  }
#endif /* OBSW_ADD_TASK_EXAMPLE == 1 */

#if OBSW_ADD_DEVICE_HANDLER_DEMO == 1
  FixedTimeslotTaskIF *testDevicesTask =
      TaskFactory::instance()->createFixedTimeslotTask("PST_TEST_TASK", 7,
                                                       1024 * 2, 1.0, nullptr);
  result = pst::pollingSequenceDevices(testDevicesTask);
  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 faiedl!\n");
#endif
  }
  PeriodicTaskIF *assemblyTask =
      taskFactory->createPeriodicTask("ASS_TASK", 3, 1024, 2.0, nullptr);
  if (assemblyTask == nullptr) {
    task::printInitError("ASS_TASK", objects::TEST_ASSEMBLY);
  }
  result = assemblyTask->addComponent(objects::TEST_ASSEMBLY);
  if (result != HasReturnvaluesIF::RETURN_OK) {
    task::printInitError("ASS_TASK", objects::TEST_ASSEMBLY);
  }
#endif /* OBSW_ADD_DEVICE_HANDLER_DEMO == 1 */

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

#if OBSW_PERIPHERAL_PST == 1
  FixedTimeslotTaskIF *peripheralPst =
      TaskFactory::instance()->createFixedTimeslotTask("PST_PERIPHERAL_TASK", 9,
                                                       1024 * 2, 2.0, nullptr);
  result = pst::pstPeripheralsTest(peripheralPst);
  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 faiedl!\n");
#endif
  }
#endif

#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();
  eventManagerTask->startTask();
#if OBSW_ADD_LWIP_COMPONENTS == 1
  udpBridgeTask->startTask();
  udpPollingTask->startTask();
#endif
#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
  testDevicesTask->startTask();
  assemblyTask->startTask();
#endif /* OBSW_ADD_DEVICE_HANDLER_DEMO == 1 */

#if OBSW_PERIPHERAL_PST == 1
  peripheralPst->startTask();
#endif /* OBSW_PERIPHERAL_PST == 1 */

  testTask->startTask();

#if FSFW_CPP_OSTREAM_ENABLED == 1
  sif::info << "Remaining FreeRTOS heap size: " << std::dec
            << xPortGetFreeHeapSize() << " bytes." << std::endl;
  sif::info << "Tasks started.." << std::endl;
#else
  sif::printInfo("Remaining FreeRTOS heap size: %lu\n",
                 static_cast<unsigned long>(xPortGetFreeHeapSize()));
  sif::printInfo("Tasks started..\n");
#endif

#if OBSW_ADD_DEVICE_HANDLER_DEMO
  auto *assembly = ObjectManager::instance()->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);

#endif /* OBSW_ADD_DEVICE_HANDLER_DEMO */
}

void assemblyDemo(HasModesIF* assembly) {
    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);
}