#include "fsfw/timemanager/Stopwatch.h"

#include "fsfw/serviceinterface/ServiceInterface.h"

#if FSFW_CPP_OSTREAM_ENABLED == 1
#include <iomanip>
#endif

Stopwatch::Stopwatch(bool displayOnDestruction, StopwatchDisplayMode displayMode)
    : displayOnDestruction(displayOnDestruction), displayMode(displayMode) {
  // Measures start time on initialization.
  Clock::getClockMonotonic(&startTime);
}

void Stopwatch::start() { Clock::getClockMonotonic(&startTime); }

dur_millis_t Stopwatch::stop(bool display) {
  stopInternal();
  if (display) {
    this->display();
  }
  return elapsedTime.tv_sec * 1000 + elapsedTime.tv_usec / 1000;
}

double Stopwatch::stopSeconds() {
  stopInternal();
  return timevalOperations::toDouble(elapsedTime);
}

void Stopwatch::display() {
  if (displayMode == StopwatchDisplayMode::MILLIS) {
    dur_millis_t timeMillis =
        static_cast<dur_millis_t>(elapsedTime.tv_sec * 1000 + elapsedTime.tv_usec / 1000);
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::info << "Stopwatch: Operation took " << timeMillis << " milliseconds" << std::endl;
#else
    sif::printInfo("Stopwatch: Operation took %lu milliseconds\n\r",
                   static_cast<unsigned int>(timeMillis));
#endif
  } else if (displayMode == StopwatchDisplayMode::SECONDS) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::info << "Stopwatch: Operation took " << std::setprecision(3) << std::fixed
              << timevalOperations::toDouble(elapsedTime) << " seconds" << std::endl;
#else
    sif::printInfo("Stopwatch: Operation took %.3f seconds\n\r",
                   static_cast<float>(timevalOperations::toDouble(elapsedTime)));
#endif
  }
}

Stopwatch::~Stopwatch() {
  if (displayOnDestruction) {
    stopInternal();
    display();
  }
}

void Stopwatch::setDisplayMode(StopwatchDisplayMode displayMode) {
  this->displayMode = displayMode;
}

StopwatchDisplayMode Stopwatch::getDisplayMode() const { return displayMode; }

void Stopwatch::stopInternal() {
  timeval endTime;
  Clock::getClockMonotonic(&endTime);
  elapsedTime = endTime - startTime;
}