#include "InitMission.h"

#include <fsfw/devicehandlers/DeviceHandlerIF.h>
#include <fsfw/tasks/PeriodicTaskIF.h>
#include <mission/utility/InitMission.h>

#include "OBSWConfig.h"
#include "ObjectFactory.h"
#include "eive/objects.h"

void scheduling::schedulingScex(TaskFactory& factory, PeriodicTaskIF*& scexDevHandler,
                                PeriodicTaskIF*& scexReaderTask) {
  using namespace initmission;
  ReturnValue_t result = returnvalue::OK;
#if OBSW_PRINT_MISSED_DEADLINES == 1
  void (*missedDeadlineFunc)(void) = TaskFactory::printMissedDeadline;
#else
  void (*missedDeadlineFunc)(void) = nullptr;
#endif
  scexDevHandler = factory.createPeriodicTask(
      "SCEX_DEV", 35, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.5, missedDeadlineFunc);

  result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::PERFORM_OPERATION);
  if (result != returnvalue::OK) {
    printAddObjectError("SCEX_DEV", objects::SCEX);
  }
  result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::SEND_WRITE);
  if (result != returnvalue::OK) {
    printAddObjectError("SCEX_DEV", objects::SCEX);
  }
  result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::GET_WRITE);
  if (result != returnvalue::OK) {
    printAddObjectError("SCEX_DEV", objects::SCEX);
  }
  result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::SEND_READ);
  if (result != returnvalue::OK) {
    printAddObjectError("SCEX_DEV", objects::SCEX);
  }
  result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::GET_READ);

  result = returnvalue::OK;
  scexReaderTask = factory.createPeriodicTask(
      "SCEX_UART_READER", 20, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc);
  result = scexReaderTask->addComponent(objects::SCEX_UART_READER);
  if (result != returnvalue::OK) {
    printAddObjectError("SCEX_UART_READER", objects::SCEX_UART_READER);
  }
}

void scheduling::addMpsocSupvHandlers(PeriodicTaskIF* plTask) {
#if OBSW_ADD_PLOC_SUPERVISOR == 1
  plTask->addComponent(objects::PLOC_SUPERVISOR_HANDLER, DeviceHandlerIF::PERFORM_OPERATION);
  plTask->addComponent(objects::PLOC_SUPERVISOR_HANDLER, DeviceHandlerIF::SEND_READ);
  plTask->addComponent(objects::PLOC_SUPERVISOR_HANDLER, DeviceHandlerIF::GET_READ);
  plTask->addComponent(objects::PLOC_SUPERVISOR_HANDLER, DeviceHandlerIF::SEND_WRITE);
  plTask->addComponent(objects::PLOC_SUPERVISOR_HANDLER, DeviceHandlerIF::GET_WRITE);
#endif

#if OBSW_ADD_PLOC_MPSOC == 1
  plTask->addComponent(objects::PLOC_MPSOC_HANDLER, DeviceHandlerIF::PERFORM_OPERATION);
  plTask->addComponent(objects::PLOC_MPSOC_HANDLER, DeviceHandlerIF::SEND_READ);
  plTask->addComponent(objects::PLOC_MPSOC_HANDLER, DeviceHandlerIF::GET_READ);
  plTask->addComponent(objects::PLOC_MPSOC_HANDLER, DeviceHandlerIF::SEND_WRITE);
  plTask->addComponent(objects::PLOC_MPSOC_HANDLER, DeviceHandlerIF::GET_WRITE);
#endif
}