#include "fsfw/pus/Service17Test.h"

#include "fsfw/FSFW.h"
#include "fsfw/objectmanager/SystemObject.h"
#include "fsfw/serviceinterface.h"
#include "fsfw/tmtcpacket/pus/tm/TmPacketStored.h"

Service17Test::Service17Test(object_id_t objectId, uint16_t apid, uint8_t serviceId)
    : PusServiceBase(objectId, apid, serviceId), packetSubCounter(0) {}

Service17Test::~Service17Test() = default;

ReturnValue_t Service17Test::handleRequest(uint8_t subservice) {
  switch (subservice) {
    case Subservice::CONNECTION_TEST: {
#if FSFW_USE_PUS_C_TELEMETRY == 0
      TmPacketStoredPusA connectionPacket(apid, serviceId, Subservice::CONNECTION_TEST_REPORT,
                                          packetSubCounter++);
#else
      TmPacketStoredPusC connectionPacket(apid, serviceId, Subservice::CONNECTION_TEST_REPORT,
                                          packetSubCounter++);
#endif
      connectionPacket.sendPacket(requestQueue->getDefaultDestination(), requestQueue->getId());
      return HasReturnvaluesIF::RETURN_OK;
    }
    case Subservice::EVENT_TRIGGER_TEST: {
#if FSFW_USE_PUS_C_TELEMETRY == 0
      TmPacketStoredPusA connectionPacket(apid, serviceId, Subservice::CONNECTION_TEST_REPORT,
                                          packetSubCounter++);
#else
      TmPacketStoredPusC connectionPacket(apid, serviceId, Subservice::CONNECTION_TEST_REPORT,
                                          packetSubCounter++);
#endif
      connectionPacket.sendPacket(requestQueue->getDefaultDestination(), requestQueue->getId());
      triggerEvent(TEST, 1234, 5678);
      return RETURN_OK;
    }
    default:
      return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
  }
}

ReturnValue_t Service17Test::performService() { return HasReturnvaluesIF::RETURN_OK; }