#include "project.h"

#define JUMP_THRESH 0.1
#define JUMP_TICKS  30
#define FEEDBACK    0.0001
#define WARM_UP     30


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;

int pll_ready = 0;


# if 0
#define   PLL_BW    0.01
#define   PLL_DAMP  0.707
#define   PLL_GAIN  0.001

#define   F_T1    ((PLL_GAIN)/((PLL_BW)*(PLL_BW)))
#define   F_T2    ((2*(PLL_DAMP))/(PLL_BW))


#define   F_B0    (((4*(PLL_GAIN))/(F_T1))*(1.0+((F_T2)/(2.0))))
#define   F_B1    ((8*(PLL_GAIN))/(F_T1))
#define   F_B2    (((4*(PLL_GAIN))/(F_T1))*(1.0-((F_T2)/(2.0))))

#define   F_A0    (1.0)
#define   F_A1    (-2.0)
#define   F_A2    (1.0)

void pll_dump_filter (void)
{
  printf ("%g %g %g\n", F_A0, F_A1, F_A2);
  printf ("%g %g %g\n", F_B0, F_B1, F_B2);
}



static double filter (double in)
{
  static double v[3];
  double ret;

  v[2] = v[1];
  v[1] = v[0];
  v[0] = in - (v[1] * F_A1) - (v[2] * F_A2);

  return (v[0] * F_B0) + (v[1] * F_B1) + (v[2] * F_B2);

}
#endif


#define PLL_ALPHA (0.005)
#define PLL_BETA  (0.5*PLL_ALPHA*PLL_ALPHA)







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 (pd1 > (pll_freq >> 1)) {
    te++;
    pd1 = pd1 - pll_freq;
  }


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


  pd2 = pd1 + (te * pll_freq);

  phase = now - pd2;
}

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

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

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

  return abs + shift;


}


void pll_dispatch (uint64_t happy, uint64_t edge, const char *src)
{
  double f, g;
  int64_t pd;



#if 0
  {
    int h1, h2, h3, h4;
    EPOCH e;
    UTC u;
    char s1[80];
    char s2[80];
    char s3[80];
    char s4[80];


    e = pll_decompose (happy);
    u = time_epoch_to_utc (e);
    utc_to_str (s1, u);
    h1 = e.s;

    e = pll_decompose (gps_last_happy);
    u = time_epoch_to_utc (e);
    utc_to_str (s2, u);
    h2 = e.s;

    e = pll_decompose (dcf77_last_happy);
    u = time_epoch_to_utc (e);
    utc_to_str (s3, u);
    h3 = e.s;

    e = pll_decompose (msf_last_happy);
    u = time_epoch_to_utc (e);
    utc_to_str (s4, u);
    h4 = e.s;

    printf ("H %d %d %d %d\r\n", h1 - h2, h2 - h2, h3 - h2, h4 - h2);
    // printf ("H %s %s %s %s\r\n",s1,s2,s3,s4);
  }
#endif


  if ((!gps_last_happy) && (!dcf77_last_happy) && (!msf_last_happy)) return;

  if (happy < gps_last_happy) return;

  if (happy < dcf77_last_happy) return;

  if (happy < msf_last_happy) return;

  if (!pll_ready && (edge < ((uint64_t) WARM_UP  * (uint64_t) HZ)))
    return;

  led_blink (100);


#if 1
  printf ("EDGE %08x%08x\r\n",
          (unsigned) (edge >> 32),
          (unsigned) (edge & 0xffffffff));
#endif

  {
    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;
    g = f / (double) pll_freq;
  }

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

  printf ("PLL pd %.3f %.1f pll_freq %d phase %d %s\r\n", (float) g, (float)f, (int) pll_freq, (int) phase, src);

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

    phase += (int) (f * PLL_BETA);
    modify_pll_freq (edge, (int) (f * PLL_ALPHA));

  }

  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);
}