#include "project.h"

static volatile unsigned us_ref_idx;
static volatile uint64_t us_ref_freq[2];
static volatile uint64_t us_ref_offset[2];
static volatile uint64_t us_ref_phase[2];

static volatile unsigned us_ro_idx;
static volatile uint32_t us_ro_high_tick[2];
static volatile int us_ro_mm[2];


static int64_t ref_offset;
static int64_t ref_phase;
static int64_t ref_freq = HW_CLOCK_HZ;

uint64_t ref_last_update;
int ref_valid = 0;
int ref_offset_known = 0;


static void us_put (void)
{
  unsigned i = us_ref_idx;

  i ^= 1;
  us_ref_freq[i] = ref_freq;
  us_ref_offset[i] = ref_offset;
  us_ref_phase[i] = ref_phase;

  compiler_mb();
  us_ref_idx = i;
}

static void us_get (uint64_t *f, uint64_t *o, uint64_t *p)
{
  unsigned i;

  do {
    i = us_ref_idx;
    *f = us_ref_freq[i];
    *o = us_ref_offset[i];
    *p = us_ref_phase[i];
    compiler_mb();
  } while (i != us_ref_idx);
}

#if HW_CLOCK_LEN == 32

#define QUARTER (1UL << 29)
#define HALF (1UL << 30)
#define THREE_QUARTERS (HALF+QUARTER)
#define ONE (~(uint32_t)0)

uint64_t ref_extend_irq (uint32_t now)
{
  uint64_t ret;
  uint32_t ht;
  int m;

  ht = us_ro_high_tick[us_ro_idx];
  m = us_ro_mm[us_ro_idx];

  if (!m) {
    ret = ht;
    ret <<= 32;
    ret |= now;
  } else {
    if (now < HALF) {
      ret = ht;
      ret <<= 32;
      ret |= now;
    } else {
      ret = ht - 1;
      ret <<= 32;
      ret |= now;
    }
  }

  return ret;
}

uint64_t ref_extend (uint32_t now)
{
  uint64_t ret;

  uint32_t ht, oht;
  int m, om;
  unsigned i = us_ro_idx;

  oht = ht = us_ro_high_tick[i];
  om = m = us_ro_mm[i];


  if (!m) {
    ret = ht;
    ret <<= 32;
    ret |= now;


    if ((now > THREE_QUARTERS) && (now <= ONE)) {
      ht++;
      m = 1;
    }

  } else {
    if (now < HALF) {
      ret = ht;
      ret <<= 32;
      ret |= now;
    } else {
      ret = ht - 1;
      ret <<= 32;
      ret |= now;
    }

    if ((now > QUARTER) && (now < HALF))
      m = 0;
  }


  if ((ht != oht) || (m != om))  {
    i ^= 1;
    us_ro_high_tick[i] = ht;
    us_ro_mm[i] = m;
    compiler_mb();
    us_ro_idx = i;
  }

  return ret;
}
#elif HW_CLOCK_LEN == 31

#define QUARTER (1UL << 28)
#define HALF (1UL << 29)
#define THREE_QUARTERS (HALF+QUARTER)
#define ONE (0x7fffffff)


uint64_t ref_extend_irq (uint32_t now)
{
  uint64_t ret;


  uint32_t ht;
  int m;

  ht = us_ro_high_tick[us_ro_idx];
  m = us_ro_mm[us_ro_idx];


  if (!m) {
    ret = ht;
    ret <<= 31;
    ret |= now;
  } else {
    if (now < HALF) {
      ret = ht;
      ret <<= 31;
      ret |= now;
    } else {
      ret = ht - 1;
      ret <<= 31;
      ret |= now;
    }
  }

  return ret;
}


uint64_t ref_extend (uint32_t now)
{
  uint64_t ret;


  uint32_t ht, oht;
  int m, om;
  unsigned i = us_ro_idx;

  oht = ht = us_ro_high_tick[i];
  om = m = us_ro_mm[i];




  if (!m) {
    ret = ht;
    ret <<= 31;
    ret |= now;

    if ((now > THREE_QUARTERS) && (now <= ONE)) {
      ht++;
      m = 1;
    }

  } else {
    if (now < HALF) {
      ret = ht;
      ret <<= 31;
      ret |= now;
    } else {
      ret = ht - 1;
      ret <<= 31;
      ret |= now;
    }

    if ((now > QUARTER) && (now < HALF))
      m = 0;
  }



  if ((ht != oht) || (m != om))  {
    i ^= 1;
    us_ro_high_tick[i] = ht;
    us_ro_mm[i] = m;
    compiler_mb();
    us_ro_idx = i;
  }

  return ret;
}





#else
#error unknown hardware clock length
#endif


uint64_t ref_get_irq (void)
{
  uint32_t now = HW_CLOCK_REG;
  return ref_extend_irq (now);
}


uint64_t ref_get (void)
{
  uint32_t now = HW_CLOCK_REG;
  return ref_extend (now);
}


void ref_slow_tick()
{
  ref_get();
}



static void modify_ref_freq (uint64_t now, uint64_t new)
{
  int64_t pd1, pd2, te;

  pd1 = now - ref_phase;
  te = pd1 / ref_freq;
  pd1 %= ref_freq;

  if (pd1 > (ref_freq >> 1)) {
    te++;
    pd1 = pd1 - ref_freq;
  }

  ref_freq = new;

  pd2 = pd1 + (te * ref_freq);

  ref_phase = now - pd2;

}

uint64_t make_happy (uint64_t abs, int64_t shift)
{
  shift *= HW_CLOCK_HZ;

  if (shift < 0) {
    shift = -shift;

    if (abs < (uint64_t) shift) return 0;
    else
      return abs - shift;
  }

  return abs + shift;


}

#define FF_B 16
#define FF_A ((FF_B)-1)
#define FF_C 16

static uint64_t fff;

static void fll_init (uint64_t start_freq)
{
  fff = start_freq * FF_C;
}

static uint64_t fll (uint64_t obs_freq)
{
  uint64_t new_freq;



  fff *= FF_A;
  fff += obs_freq * FF_C;
  fff += FF_B / 2;
  fff /= FF_B;

  new_freq = fff;
  new_freq += FF_C / 2;
  new_freq /= FF_C;


  return new_freq;
}


static int64_t edge_to_phase (uint64_t edge)
{
  int64_t obs_phase;
  obs_phase = edge - ref_phase;
  obs_phase %= ref_freq;

  if (obs_phase > ref_freq / 2)
    obs_phase -= ref_freq;

  return obs_phase;
}

#define PF_A 16
static uint64_t pll (int64_t obs_phase)
{
  int64_t pd ;


  pd = obs_phase / PF_A;

  if (!pd) {
    if (obs_phase < 0) pd--;

    if (obs_phase > 0) pd++;
  }


  return pd;
}

char ref_info[128];


void ref_dispatch (uint64_t edge, const char *src)
{
  static uint64_t last_edge;
  uint64_t obs_freq, obs_phase, new_freq;
  static int jump_start = 1;

  if (!last_edge) {
    last_edge = edge;
    return;
  }


  obs_freq = edge - last_edge;
  last_edge = edge;
  obs_phase = edge_to_phase (edge);



  //  delta_f = obs_freq - ref_freq;


  snprintf (ref_info, sizeof (ref_info) - 1, "REF PLL: obs_f=%9d delta_phi=%5d f=%9d %s\n",
            (int) obs_freq,
            (int) obs_phase,
            (int) ref_freq, src);

#ifdef CHATTY_PLLS
  printf ("REF PLL: obs_f=%9d delta_phi=%5d f=%9d %s\r\n",
          (int) obs_freq,
          (int) obs_phase,
          (int) ref_freq, src);
#endif


  /*Ignore bogus observations*/
  if (obs_freq > (HW_CLOCK_HZ + (HW_CLOCK_HZ / 2)))
    return;

  if (obs_freq < (HW_CLOCK_HZ - (HW_CLOCK_HZ / 2)))
    return;

  if (jump_start) {
    new_freq = obs_freq;
    fll_init (new_freq);
    modify_ref_freq (edge, new_freq);
    ref_phase += obs_phase;

    jump_start = 0;
  } else {
    new_freq = fll (obs_freq);
    modify_ref_freq (edge, new_freq);
    ref_phase += pll (obs_phase);
  }

  if (ref_offset_known)
    ref_valid = 1;

  us_put();
  ref_last_update = edge;
}



void ref_set_offset (EPOCH epoch, uint64_t abs)
{
  int64_t new_offset;
  int diff;


  /* Find nearest second to abs*/
  abs += ref_freq >> 2;
  abs -= ref_phase;
  abs /= ref_freq;

  new_offset = epoch.s - abs;

  if (new_offset != ref_offset) {
    diff = (int) (new_offset - ref_offset);


    printf ("REF wallclock offset moved by %d\r\n", diff);
    ref_offset = new_offset;
  }

  us_put();
  ref_offset_known = 1;
  time_known = 1;
}




static EPOCH _ref_decompose (uint64_t abs, uint64_t f, uint64_t o)
{
  EPOCH ret;


  ret.s = abs / f;
  abs -= f * ret.s;

  ret.s += o;

  abs *= (uint64_t) 1000000000;
  abs = abs / f;

  ret.ns = abs;

  return ret;
}

EPOCH ref_decompose_diff (int64_t diff)
{
  EPOCH ret;
  uint64_t f, o, p;

  us_get (&f, &o, &p);

  if (diff >= 0)
    return _ref_decompose (diff, f, o);

  ret = _ref_decompose (-diff, f, o);
  ret.s = -ret.s;
  ret.ns = -ret.ns;

  return ret;
}


EPOCH ref_decompose (uint64_t abs)
{
  uint64_t f, o, p;
  us_get (&f, &o, &p);

  abs -= p;

  return _ref_decompose (abs, f, o);
}