#include <ctime>
#include <cstdio>
#include <sched.h>      // sched_setscheduler
#include <asm/msr.h>
#include "DTAPI.h"

#define BUFFER_SIZE (188*7)
#define CPU_MHZ 1266.794
#define US_DELAY 40

double us_period = 0;

static void set_SCHED_RR()
{
  struct sched_param param = { 0 };
  param.sched_priority = 80;
  if (sched_setscheduler(0, SCHED_RR, &param) != 0) perror("SCHED_RR");
}

static void init_hardware(DtDevice& Dev, TsInpChannel& TsIn)
{
  int rate = 0;
  DTAPI_RESULT res;

  res = Dev.AttachToType(140);
  if (res != DTAPI_OK) printf("AttachToType: %lu\n", res);

  res = TsIn.Attach(&Dev);
  if (res != DTAPI_OK) printf("Attach: %lu\n", res);

  res = TsIn.GetTsRateBps(rate);
  if (res != DTAPI_OK) printf("GetTsRateBps: %lu\n", res);
  printf("INCOMING RATE = %d bit/s\n", rate);
  us_period = 8e6*BUFFER_SIZE / rate;
  printf("EXPECTED PERIOD = %f µs\n", us_period);

  res = TsIn.SetRxMode(DTAPI_RXMODE_ST188);
  if (res != DTAPI_OK) printf("SetRxMode: %lu\n", res);

  res = TsIn.SetRxControl(DTAPI_RXCTRL_RCV);
  if (res != DTAPI_OK) printf("SetRxControl: %lu\n", res);
}

int main(void)
{
  char buf[BUFFER_SIZE];
  DtDevice Dev;
  TsInpChannel TsIn;

  set_SCHED_RR();
  init_hardware(Dev, TsIn);

  unsigned prev_tsc, curr_tsc;
  struct timespec prev_spec, curr_spec, last_spec;
  for (int i=0; i< 20; ++i) clock_gettime(CLOCK_MONOTONIC, &prev_spec);

  TsIn.Read(buf, sizeof buf);
  rdtscl(prev_tsc);
  clock_gettime(CLOCK_MONOTONIC, &prev_spec);
  last_spec = prev_spec;

  while ( 1 )
  {
    TsIn.Read(buf, sizeof buf);
    rdtscl(curr_tsc);
    clock_gettime(CLOCK_MONOTONIC, &curr_spec);
    double tsc_usdiff = (curr_tsc - prev_tsc) / CPU_MHZ;
    double spec_usdiff = (curr_spec.tv_sec-prev_spec.tv_sec)*1e6 + (curr_spec.tv_nsec-prev_spec.tv_nsec)/1e3;
    if (spec_usdiff > us_period+US_DELAY)
    {
      printf("NOW=%ld.%09ld\n", curr_spec.tv_sec, curr_spec.tv_nsec);
      double temp = (curr_spec.tv_sec-last_spec.tv_sec) + (curr_spec.tv_nsec-last_spec.tv_nsec)/1e9;
      last_spec = curr_spec;
      printf("LAST: %f s\n", temp);
      printf("CLOCK: %f µs\n", spec_usdiff);
    }
    if (tsc_usdiff > us_period+US_DELAY)
    {
      printf("TSC: %f µs\n", tsc_usdiff);
      puts("+++++");
    }
    prev_tsc = curr_tsc;
    prev_spec = curr_spec;
  }

  return 0;
}