#include "ClockDiscipline.h"

#include "Config.h"

namespace field_qa {

ClockDiscipline::ClockDiscipline(TwoWire& wire) : m_wire(wire) {}

uint8_t ClockDiscipline::toBcd(uint8_t value) {
  return (uint8_t)(((value / 10U) << 4U) | (value % 10U));
}

uint8_t ClockDiscipline::fromBcd(uint8_t value) {
  return (uint8_t)(((value >> 4U) * 10U) + (value & 0x0FU));
}

bool ClockDiscipline::isLeapYear(uint16_t year) {
  return ((year % 4U) == 0U && (year % 100U) != 0U) || ((year % 400U) == 0U);
}

uint8_t ClockDiscipline::daysInMonth(uint16_t year, uint8_t month) {
  static const uint8_t kDays[12] = {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};
  if (month == 2U) {
    return (uint8_t)(isLeapYear(year) ? 29U : 28U);
  }
  if (month >= 1U && month <= 12U) {
    return kDays[month - 1U];
  }
  return 0;
}

bool ClockDiscipline::isValidDateTime(const ClockDateTime& dt) {
  if (dt.year < 2000U || dt.year > 2099U) {
    return false;
  }
  if (dt.month < 1U || dt.month > 12U) {
    return false;
  }
  if (dt.day < 1U || dt.day > daysInMonth(dt.year, dt.month)) {
    return false;
  }
  if (dt.hour > 23U || dt.minute > 59U || dt.second > 59U) {
    return false;
  }
  return true;
}

int64_t ClockDiscipline::daysFromCivil(int year, unsigned month, unsigned day) {
  year -= (month <= 2U);
  const int era = (year >= 0 ? year : year - 399) / 400;
  const unsigned yoe = (unsigned)(year - era * 400);
  const unsigned doy = (153U * (month + (month > 2U ? (unsigned)-3 : 9U)) + 2U) / 5U + day - 1U;
  const unsigned doe = yoe * 365U + yoe / 4U - yoe / 100U + doy;
  return era * 146097 + (int)doe - 719468;
}

int64_t ClockDiscipline::toEpochSeconds(const ClockDateTime& dt) {
  const int64_t days = daysFromCivil((int)dt.year, dt.month, dt.day);
  return days * 86400LL + (int64_t)dt.hour * 3600LL + (int64_t)dt.minute * 60LL + (int64_t)dt.second;
}

bool ClockDiscipline::fromEpochSeconds(int64_t seconds, ClockDateTime& out) {
  if (seconds < 0) {
    return false;
  }

  int64_t days = seconds / 86400LL;
  int64_t remainder = seconds % 86400LL;
  if (remainder < 0) {
    remainder += 86400LL;
    days -= 1;
  }

  out.hour = (uint8_t)(remainder / 3600LL);
  remainder %= 3600LL;
  out.minute = (uint8_t)(remainder / 60LL);
  out.second = (uint8_t)(remainder % 60LL);

  days += 719468;
  const int era = (days >= 0 ? days : days - 146096) / 146097;
  const unsigned doe = (unsigned)(days - era * 146097);
  const unsigned yoe = (doe - doe / 1460U + doe / 36524U - doe / 146096U) / 365U;
  int year = (int)yoe + era * 400;
  const unsigned doy = doe - (365U * yoe + yoe / 4U - yoe / 100U);
  const unsigned mp = (5U * doy + 2U) / 153U;
  const unsigned day = doy - (153U * mp + 2U) / 5U + 1U;
  const unsigned month = mp + (mp < 10U ? 3U : (unsigned)-9);
  year += (month <= 2U);

  out.year = (uint16_t)year;
  out.month = (uint8_t)month;
  out.day = (uint8_t)day;
  return isValidDateTime(out);
}

bool ClockDiscipline::readRtc(ClockDateTime& out, bool& lowVoltageFlag) const {
  m_wire.beginTransmission(RTC_I2C_ADDR);
  m_wire.write(0x02);
  if (m_wire.endTransmission(false) != 0) {
    return false;
  }

  const uint8_t need = 7;
  const uint8_t got = m_wire.requestFrom((int)RTC_I2C_ADDR, (int)need);
  if (got != need) {
    return false;
  }

  const uint8_t sec = m_wire.read();
  const uint8_t min = m_wire.read();
  const uint8_t hour = m_wire.read();
  const uint8_t day = m_wire.read();
  (void)m_wire.read();
  const uint8_t month = m_wire.read();
  const uint8_t year = m_wire.read();

  lowVoltageFlag = (sec & 0x80U) != 0;
  out.second = fromBcd(sec & 0x7FU);
  out.minute = fromBcd(min & 0x7FU);
  out.hour = fromBcd(hour & 0x3FU);
  out.day = fromBcd(day & 0x3FU);
  out.month = fromBcd(month & 0x1FU);
  const uint8_t yy = fromBcd(year);
  out.year = (month & 0x80U) ? (1900U + yy) : (2000U + yy);
  return true;
}

bool ClockDiscipline::readValidRtc(ClockDateTime& out, int64_t* epochOut) const {
  bool lowVoltage = false;
  if (!readRtc(out, lowVoltage) || lowVoltage || !isValidDateTime(out)) {
    return false;
  }
  if (epochOut != nullptr) {
    *epochOut = toEpochSeconds(out);
  }
  return true;
}

bool ClockDiscipline::writeRtc(const ClockDateTime& dt) const {
  if (!isValidDateTime(dt)) {
    return false;
  }

  m_wire.beginTransmission(RTC_I2C_ADDR);
  m_wire.write(0x02);
  m_wire.write(toBcd(dt.second & 0x7FU));
  m_wire.write(toBcd(dt.minute));
  m_wire.write(toBcd(dt.hour));
  m_wire.write(toBcd(dt.day));
  m_wire.write(0x00);

  uint8_t monthReg = toBcd(dt.month);
  if (dt.year < 2000U) {
    monthReg |= 0x80U;
  }
  m_wire.write(monthReg);
  m_wire.write(toBcd((uint8_t)(dt.year % 100U)));
  return m_wire.endTransmission() == 0;
}

void ClockDiscipline::formatIsoUtc(const ClockDateTime& dt, char* out, size_t outSize) {
  snprintf(out,
           outSize,
           "%04u-%02u-%02uT%02u:%02u:%02uZ",
           (unsigned)dt.year,
           (unsigned)dt.month,
           (unsigned)dt.day,
           (unsigned)dt.hour,
           (unsigned)dt.minute,
           (unsigned)dt.second);
}

void ClockDiscipline::formatCompactUtc(const ClockDateTime& dt, char* out, size_t outSize) {
  snprintf(out,
           outSize,
           "%04u%02u%02u_%02u%02u%02u",
           (unsigned)dt.year,
           (unsigned)dt.month,
           (unsigned)dt.day,
           (unsigned)dt.hour,
           (unsigned)dt.minute,
           (unsigned)dt.second);
}

void ClockDiscipline::makeRunId(const ClockDateTime& dt, const char* boardId, char* out, size_t outSize) {
  snprintf(out,
           outSize,
           "%04u%02u%02u_%02u%02u%02u_%s",
           (unsigned)dt.year,
           (unsigned)dt.month,
           (unsigned)dt.day,
           (unsigned)dt.hour,
           (unsigned)dt.minute,
           (unsigned)dt.second,
           boardId ? boardId : "NODE");
}

bool ClockDiscipline::fromGnssSample(const GnssSample& sample, ClockDateTime& out) {
  if (!sample.validTime) {
    return false;
  }
  out.year = sample.year;
  out.month = sample.month;
  out.day = sample.day;
  out.hour = sample.hour;
  out.minute = sample.minute;
  out.second = sample.second;
  return isValidDateTime(out);
}

bool ClockDiscipline::disciplineFromGnss(const GnssSample& sample,
                                         WaitForPpsCallback waitForPps,
                                         void* context,
                                         ClockDateTime& disciplinedUtc,
                                         bool& hadPriorRtc,
                                         int64_t& driftSeconds) const {
  ClockDateTime gpsUtc{};
  if (!fromGnssSample(sample, gpsUtc) || waitForPps == nullptr) {
    return false;
  }

  ClockDateTime priorRtc{};
  hadPriorRtc = readValidRtc(priorRtc, nullptr);

  if (!waitForPps(context, kClockPpsWaitTimeoutMs)) {
    return false;
  }

  const int64_t snappedEpoch = toEpochSeconds(gpsUtc);
  if (!fromEpochSeconds(snappedEpoch + 1, disciplinedUtc)) {
    return false;
  }
  if (!writeRtc(disciplinedUtc)) {
    return false;
  }

  driftSeconds = hadPriorRtc ? (toEpochSeconds(priorRtc) - toEpochSeconds(disciplinedUtc)) : 0;
  return true;
}

}  // namespace field_qa
