#include "fsfw/pus/Service9TimeManagement.h"

#include <cmath>

#include "fsfw/pus/servicepackets/Service9Packets.h"
#include "fsfw/returnvalues/returnvalue.h"
#include "fsfw/serialize/SerializeAdapter.h"
#include "fsfw/timemanager/CCSDSTime.h"

Service9TimeManagement::Service9TimeManagement(PsbParams params) : PusServiceBase(params) {
  params.name = "PUS 9 Time MGMT";
}

Service9TimeManagement::~Service9TimeManagement() = default;

ReturnValue_t Service9TimeManagement::performService() { return returnvalue::OK; }

ReturnValue_t Service9TimeManagement::handleRequest(uint8_t subservice) {
  switch (subservice) {
    case Subservice::SET_TIME: {
      reportCurrentTime(CLOCK_DUMP_BEFORE_SETTING_TIME);
      ReturnValue_t result = setTime();
      reportCurrentTime(CLOCK_DUMP_AFTER_SETTING_TIME);
      return result;
    }
    case Subservice::DUMP_TIME: {
      reportCurrentTime();
      return returnvalue::OK;
    }
    case Subservice::RELATIVE_TIMESHIFT: {
      timeval currentTime;
      ReturnValue_t result = Clock::getClock(&currentTime);
      if (result != returnvalue::OK) {
        return result;
      }
      reportTime(CLOCK_DUMP_BEFORE_SETTING_TIME, currentTime);

      if (currentPacket.getUserDataLen() != 8) {
        return AcceptsTelecommandsIF::ILLEGAL_APPLICATION_DATA;
      }
      size_t deserLen = 8;
      int64_t timeshiftNanos = 0;
      result = SerializeAdapter::deSerialize(&timeshiftNanos, currentPacket.getUserData(),
                                             &deserLen, SerializeIF::Endianness::NETWORK);
      if (result != returnvalue::OK) {
        return result;
      }
      bool positiveShift = true;
      if (timeshiftNanos < 0) {
        positiveShift = false;
      }
      timeval offset{};
      offset.tv_sec = std::abs(timeshiftNanos) / NANOS_PER_SECOND;
      offset.tv_usec = (std::abs(timeshiftNanos) % NANOS_PER_SECOND) / 1000;

      timeval newTime;
      if (positiveShift) {
        newTime = currentTime + offset;
      } else {
        newTime = currentTime - offset;
      }
      result = Clock::setClock(&newTime);
      if (result == returnvalue::OK) {
        reportTime(CLOCK_DUMP_AFTER_SETTING_TIME, newTime);
      }
      return result;
    }
    default:
      return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
  }
}

ReturnValue_t Service9TimeManagement::setTime() {
  Clock::TimeOfDay_t timeToSet;
  TimePacket timePacket(currentPacket.getUserData(), currentPacket.getUserDataLen());
  ReturnValue_t result =
      CCSDSTime::convertFromCcsds(&timeToSet, timePacket.getTime(), timePacket.getTimeSize());
  if (result != returnvalue::OK) {
    triggerEvent(CLOCK_SET_FAILURE, result, 0);
    return result;
  }

  result = Clock::setClock(&timeToSet);
  if (result != returnvalue::OK) {
    triggerEvent(CLOCK_SET_FAILURE, result, 0);
    return returnvalue::FAILED;
  }
  return result;
}

void Service9TimeManagement::reportCurrentTime(Event event) {
  timeval currentTime{};
  Clock::getClock(&currentTime);
  triggerEvent(event, currentTime.tv_sec, currentTime.tv_usec);
}

void Service9TimeManagement::reportTime(Event event, timeval time) {
  triggerEvent(event, time.tv_sec, time.tv_usec);
}