#include "project.h"

static volatile uint32_t hz = HZ;
static volatile uint32_t cycle_ref;
static volatile uint32_t rtc_ref;

#define RTC_IN_LH 0x21f7
#define RTC_IN_UH 0x6a6d

static EPOCH rtc_offset;
static uint16_t rtc_half;

volatile unsigned rtc_ready;




static EPOCH bkp_read_off (void)
{
  EPOCH e;
  uint64_t v;

  v = BKP_DR1 & 0xffff;
  v <<= 16;
  v |= (BKP_DR2 & 0xffff);
  v <<= 16;
  v |= (BKP_DR3 & 0xffff);
  v <<= 16;
  v |= (BKP_DR4 & 0xffff);

  e.s = (int64_t) v;

  v = BKP_DR5 & 0xffff;
  v <<= 16;
  v |= (BKP_DR6 & 0xffff);

  e.ns = (int64_t) v;

  return e;
}

static void bkp_write_off (EPOCH e)
{
  uint64_t v;

  v = (uint64_t) e.s;

  BKP_DR4 = (uint16_t) (v & 0xffff);
  v >>= 16;
  BKP_DR3 = (uint16_t) (v & 0xffff);
  v >>= 16;
  BKP_DR2 = (uint16_t) (v & 0xffff);
  v >>= 16;
  BKP_DR1 = (uint16_t) (v & 0xffff);

  v = (uint64_t) e.ns;

  BKP_DR6 = (uint16_t) (v & 0xffff);
  v >>= 16;
  BKP_DR5 = (uint16_t) (v & 0xffff);

}



void rtc_isr (void)
{
  uint32_t now;
  static uint32_t then;
  uint32_t v;
  static int warm_up = 3;


  now = DWT_CYCCNT;
  compiler_mb();
  rtc_clear_flag (RTC_SEC);

  v = rtc_get_counter_val();

  //  TOGGLE (LED1);

  cycle_ref = now;

  if (warm_up) warm_up--;
  else hz = now - then;


  switch (rtc_half) {
  case RTC_IN_LH:
    if (! (v & 0x8000000)) break;

    BKP_DR7 = rtc_half = RTC_IN_UH;
    break;

  case RTC_IN_UH:
    if (v & 0x8000000) break;

    BKP_DR7 = rtc_half = RTC_IN_LH;
    rtc_offset.s += 0x100000000ULL;
    bkp_write_off (rtc_offset);
    break;

  default:
    if (v & 0x8000000)
      BKP_DR7 = rtc_half = RTC_IN_UH;
    else
      BKP_DR7 = rtc_half = RTC_IN_LH;
  }


  compiler_mb();
  rtc_ref = v;
  rtc_ready = 1;


  // printf ("Ctr %d %u %u\r\n", (int) v, (int) (now - then),(unsigned) rtc_offset.s);

  then = now;
}


EPOCH rtc_get (void)
{
  EPOCH e, o;
  uint32_t h, c, r1, r2;
  uint32_t now = DWT_CYCCNT;

  do {
    r1 = rtc_ref;
    compiler_mb();
    c = cycle_ref;
    h = hz;
    o = rtc_offset;
    compiler_mb();
    r2 = rtc_ref;
  } while (r1 != r2);

  now -= c;

  e.ns = now;
  e.ns *= 1000000000ULL;

  if (h) e.ns /= h;
  else e.ns /= HZ;

  e.s = r1;

  return time_epoch_add (e, o);
}


void rtc_init (void)
{
  rtc_offset = bkp_read_off();
  rtc_half = BKP_DR7;

#if 1
  rtc_offset.s = 1637490753 + 46850+ 48565+85980;
  rtc_offset.ns = 498866999;
  bkp_write_off (rtc_offset);
#endif


  rtc_auto_awake (RCC_LSE, 0x7fff); //32768Hz
  rtc_interrupt_enable (RTC_SEC);
  nvic_enable_irq (NVIC_RTC_IRQ);

}