#include "InitMission.h"

#include "example/utility/TaskCreation.h"
#include <OBSWConfig.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 <objects/systemObjectList.h>
#include <pollingsequence/pollingSequenceFactory.h>

#include <rtems/rtems/support.h>
#include <rtems/score/heapinfo.h>

void InitMission::createTasks() {
  ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
#if OBSW_ADD_CORE_COMPONENTS == 1
  /* TMTC Distribution */
  PeriodicTaskIF *distributerTask = TaskFactory::instance()->createPeriodicTask(
      "DIST", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 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::CCSDS_DISTRIBUTOR);
  }
  result = distributerTask->addComponent(objects::TM_FUNNEL);
  if (result != HasReturnvaluesIF::RETURN_OK) {
    task::printInitError("TM funnel", objects::TM_FUNNEL);
  }

  PeriodicTaskIF *eventManagerTask =
      TaskFactory::instance()->createPeriodicTask(
          "EVENT_MGMT", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.1, nullptr);
  result = eventManagerTask->addComponent(objects::EVENT_MANAGER);
  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", 60, 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);
  }

  PeriodicTaskIF *udpPollingTask = TaskFactory::instance()->createPeriodicTask(
      "UDP_POLLING", 75, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 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 /* OBSW_ADD_LWIP_COMPONENTS == 1 */
#endif /* OBSW_ADD_CORE_COMPONENTS == 1 */

#if OBSW_ADD_PUS_STACK == 1
  /* PUS Services */
  PeriodicTaskIF *pusVerification = TaskFactory::instance()->createPeriodicTask(
      "PUS_VERIF_1", 45, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, 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", 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::instance()->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_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", 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_TASK_EXAMPLE == 1
  FixedTimeslotTaskIF *timeslotDemoTask =
      TaskFactory::instance()->createFixedTimeslotTask(
          "PST_TASK", 60, 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: Examples PST initialization failed!"
        << std::endl;
#else
    sif::printError(
        "InitMission::createTasks: Examples PST initialization failed!\n");
#endif
  }

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

#if OBSW_ADD_DEVICE_HANDLER_DEMO == 1
  FixedTimeslotTaskIF *testTimeslotTask =
      TaskFactory::instance()->createFixedTimeslotTask(
          "PST_TEST_TASK", 10, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0,
          nullptr);
  result = pst::pollingSequenceDevices(testTimeslotTask);
  if (result != HasReturnvaluesIF::RETURN_OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::error << "InitMission::createTasks: Devices PST initialization failed!"
               << std::endl;
#else
    sif::printError(
        "InitMission::createTasks: Devices PST initialization failed!\n");
#endif
  }
#endif /* OBSW_ADD_DEVICE_HANDLER_DEMO == 1 */

  PeriodicTaskIF *testTask = TaskFactory::instance()->createPeriodicTask(
      "TEST", 20, 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();
  eventManagerTask->startTask();
#if OBSW_ADD_LWIP_COMPONENTS == 1
  udpBridgeTask->startTask();
  udpPollingTask->startTask();
#endif /* OBSW_ADD_LWIP_COMPONENTS == 1 */
#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
  testTimeslotTask->startTask();
#endif /* OBSW_ADD_DEVICE_HANDLER_DEMO == 1 */

  testTask->startTask();

  Heap_Information_block block;
  rtems_workspace_get_information(&block);
  uint32_t failedAllocs = block.Stats.failed_allocs;

#if FSFW_CPP_OSTREAM_ENABLED == 1
  sif::info << "Tasks started.." << std::endl;
  sif::info << "Remaining RTEMS heap: " << block.Stats.free_size << " bytes"
            << std::endl;
#else
  sif::printInfo("Tasks started..\n");
  sif::printInfo("Allocated RTEMS heap: %lu bytes\n",
                 static_cast<unsigned long>(block.Stats.size));
  sif::printInfo("Remaining RTEMS heap: %lu bytes\n",
                 static_cast<unsigned long>(block.Stats.free_size));
  if (failedAllocs > 0) {
    sif::printWarning("%d allocations from RTEMS workspace failed!\n",
                      failedAllocs);
  }
#endif
}