#include "project.h"

#define JUMP_THRESH 0.1
#define JUMP_TICKS  30
#define FEEDBACK    0.01
#define WARM_UP     3


static int64_t offset;
static int64_t phase;
int64_t pll_freq = HZ;

static int out_of_lock = JUMP_TICKS + 1;

uint64_t pll_last_update;
int pll_valid = 0;


static int warming_up = WARM_UP;


void pll_meh (void)
{
  printf ("                  %"PRId64" %"PRId64" %" PRId64 "\r\n",
          pll_freq, offset, phase);

}


static void modify_pll_freq (uint64_t now, int d)
{
  int64_t pd1, pd2, te;

  pd1 = now - phase;

  te = pd1 / pll_freq;
  pd1 %= pll_freq;

  if (d > 0)
    pll_freq += d;
  else
    pll_freq -= -d;


  pd2 = pd1 + (te * pll_freq);

  phase = now - pd2;
}

void pll_dispatch (uint64_t edge)
{
  double f;
  int64_t pd;

  if (warming_up) {
    warming_up--;
    return;
  }


  {
    int diff, hf;

    pd = edge - phase;
    pd %= pll_freq;

    hf = (int) (pll_freq >> 1);

    diff = (int) pd;

    if (diff > hf)
      diff = diff - (int) pll_freq;


    f = (double) diff;
    f /= (double) pll_freq;
  }

  if ((f > (JUMP_THRESH)) || (f < - (JUMP_THRESH)))
    out_of_lock++;
  else if (out_of_lock <= JUMP_TICKS)
    out_of_lock = 0;

#if 1
  printf ("PLL pd %.3f pll_freq %d phase %d\r\n", (float) f, (int) pll_freq, (int) phase);
#endif

  if (out_of_lock > JUMP_TICKS) {
    phase += pd;
    out_of_lock = 0;
    printf ("PLL - jumping\r\n");
    pll_freq = HZ;
  } else {

    f *= FEEDBACK;
    f *= (double) pll_freq;

    modify_pll_freq (edge, (int) f);
  }

  pll_last_update = edge;
}



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

  /* Find nearest second to abs*/
  abs += pll_freq >> 2;
  abs -= phase;
  abs /= pll_freq;

  new_offset = epoch.s - abs;

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


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

  pll_valid = 1;
  time_known = 1;
}



EPOCH _pll_decompose (uint64_t abs)
{
  EPOCH ret;

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

  ret.s += offset;

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

  ret.ns = abs;

  return ret;
}

EPOCH pll_decompose_diff (int64_t diff)
{
  EPOCH ret;

  if (diff >= 0)
    return _pll_decompose (diff);

  ret = _pll_decompose (-diff);
  ret.s = -ret.s;
  ret.ns = -ret.ns;

  return ret;
}


EPOCH pll_decompose (uint64_t abs)
{

  abs -= phase;

  return _pll_decompose (abs);
}