#include "fsfw/timemanager/Clock.h"

#include <rtems/rtems/clockimpl.h>
#include <rtems/score/todimpl.h>

#include "fsfw/ipc/MutexGuard.h"
#include "fsfw/osal/rtems/RtemsBasic.h"

ReturnValue_t Clock::setClock(const timeval* time) {
  TimeOfDay_t time_tod;
  ReturnValue_t result = convertTimevalToTimeOfDay(time, &time_tod);
  if (result != returnvalue::OK) {
    return result;
  }
  rtems_time_of_day timeRtems;
  timeRtems.year = time_tod.year;
  timeRtems.month = time_tod.month;
  timeRtems.day = time_tod.day;
  timeRtems.hour = time_tod.hour;
  timeRtems.minute = time_tod.minute;
  timeRtems.second = time_tod.second;
  timeRtems.ticks =
      static_cast<uint64_t>(time_tod.usecond) * rtems_clock_get_ticks_per_second() / 1e6;
  rtems_status_code status = rtems_clock_set(&timeRtems);
  switch (status) {
    case RTEMS_SUCCESSFUL:
      return returnvalue::OK;
    case RTEMS_INVALID_ADDRESS:
      return returnvalue::FAILED;
    case RTEMS_INVALID_CLOCK:
      return returnvalue::FAILED;
    default:
      return returnvalue::FAILED;
  }
}

ReturnValue_t Clock::getClock_timeval(timeval* time) {
  // Callable from ISR
  rtems_status_code status = rtems_clock_get_tod_timeval(time);
  switch (status) {
    case RTEMS_SUCCESSFUL:
      return returnvalue::OK;
    case RTEMS_NOT_DEFINED:
      return returnvalue::FAILED;
    default:
      return returnvalue::FAILED;
  }
}

timeval Clock::getUptime() {
  // According to docs.rtems.org for rtems 5 this method is more accurate than
  // rtems_clock_get_ticks_since_boot
  timeval time_timeval;
  timespec time;
  rtems_status_code status = rtems_clock_get_uptime(&time);
  time_timeval.tv_sec = time.tv_sec;
  time_timeval.tv_usec = time.tv_nsec / 1000;
  return time_timeval;
}