#include "project.h"

#define JUMP_THRESH 0.1
#define JUMP_TICKS  30
#define FEEDBACK    0.001


static int64_t offset;
static int64_t phase;
static int64_t freq = HZ;

static int out_of_lock = JUMP_TICKS + 1;

uint64_t pll_last_update;
int pll_valid = 0;



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

  pd1 = now - phase;

  te = pd1 / freq;
  pd1 %= freq;

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


  pd2 = pd1 + (te * freq);

  phase = now - pd2;
}

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

  {
    int diff, hf;

    pd = edge - phase;
    pd %= freq;

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

    diff = (int) pd;

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


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

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

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

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

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

    modify_freq (edge, (int) f);

  }

  pll_last_update = edge;
}



void pll_set_offset (EPOCH epoch, uint64_t abs)
{
  /* Find nearest second to abs*/
  abs += freq >> 2;
  abs -= phase;
  abs /= freq;

  offset = epoch.s - abs;

  pll_valid = 1;
}




EPOCH pll_decompose (uint64_t abs)
{
  EPOCH ret;

  abs -= phase;

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


  ret.s += offset;

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

  ret.ns = abs;

  return ret;
}