/*  chu    Synchronize system clock with radio station CHU
    Copyright (C) 1999   William Rossi

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/resource.h>
#include <unistd.h>

#define ADJTIME
#undef USE_TIMEX
#define USE_ADJTIME

#ifdef ADJTIME
#ifdef USE_TIMEX
#include <sys/timex.h>
#endif
#endif
#define SAMPLE_RATE 8000
#define SAMPLES 512
#define OVERLAP 50
#define THRESHOLD 55
#define MINDURATION 10  // Duration of the top of minute pulse 
#define SECDURATION 4   // Duration of the second pulses
#define HANNING_WINDOW
#define HISTORY 500
#define USE_TIMECODE

#define FINE_PREV_SAMPLES 2
#define FINE_POST_SAMPLES 1
#define FINE_TOTAL_SAMPLES (FINE_PREV_SAMPLES + FINE_POST_SAMPLES )
#define FINE_MSPERSAMPLE (SAMPLES*1000/SAMPLE_RATE)
#define FINE_MIN (-FINE_MSPERSAMPLE*FINE_POST_SAMPLES)
#define FINE_MAX ((FINE_MSPERSAMPLE * FINE_PREV_SAMPLES) * 9 / 10) 

/* Finite impulse response filter coefficients  */
/* This filter has a bandpass, one centered on  */
/* 1000 Hertz.                                  */
/* Disable this filter if you are using a       */
/* different sample rate, or different tone     */
/* frequencies.                                 */

#define FIR_COE 64    /* Number of coefficients */
#define FIR_RATE 8000 /* Sample rate FIR filter was designed to work with */
#define FIR_DELAY 9   /* Filter prop delay in milliseconds */


unsigned char firbuf[] = { 0x80, 0x80, 0x80, 0x7f, 
                           0x7c, 0x7b, 0x80, 0x8a, 
                           0x92, 0x90, 0x80, 0x69, 
                           0x59, 0x61, 0x80, 0xa8,
                           0xbf, 0xb0, 0x80, 0x47,
                           0x29, 0x3f, 0x80, 0xc8, 
                           0xec, 0xce, 0x80, 0x2d, 
                           0x06, 0x29, 0x80, 0xd8,
                           0xff, 0xd8, 0x80, 0x29, 
                           0x06, 0x2d, 0x80, 0xce,  
                           0xec, 0xc8, 0x80, 0x3f, 
                           0x29, 0x47, 0x80, 0xb0,
                           0xbf, 0xa8, 0x80, 0x61, 
                           0x59, 0x69, 0x80, 0x90, 
                           0x92, 0x8a, 0x80, 0x7b, 
                           0x7c, 0x7f, 0x80, 0x80 };

#define NUM_TONES 500
#define MIN_NUM_TONES 6

/* Cosine table:   
 * You might be wondering why there is a hard-coded cosine table here.
 * It is to work around a bug found in earlier versions of libc5.
 */

#define TABLE_SIZE 1024
#define COS(x) costabs[x%TABLE_SIZE]
#define SIN(x) costabs[(x+TABLE_SIZE/4)%TABLE_SIZE]

const short costabs[] = {
32767,32766,32765,32761,32757,32752,32745,32737,32728,32717,32705,32692,
32678,32663,32646,32628,32609,32589,32567,32545,32521,32495,32469,32441,
32412,32382,32351,32318,32285,32250,32213,32176,32137,32098,32057,32014,
31971,31926,31880,31833,31785,31736,31685,31633,31580,31526,31470,31414,
31356,31297,31237,31176,31113,31050,30985,30919,30852,30783,30714,30643,
30571,30498,30424,30349,30273,30195,30117,30037,29956,29874,29791,29706,
29621,29534,29447,29358,29268,29177,29085,28992,28898,28803,28706,28609,
28510,28411,28310,28208,28105,28001,27896,27790,27683,27575,27466,27356,
27245,27133,27019,26905,26790,26674,26556,26438,26319,26198,26077,25955,
25832,25708,25582,25456,25329,25201,25072,24942,24811,24680,24547,24413,
24279,24143,24007,23870,23731,23592,23452,23311,23170,23027,22884,22739,
22594,22448,22301,22154,22005,21856,21705,21554,21403,21250,21096,20942,
20787,20631,20475,20317,20159,20000,19841,19680,19519,19357,19195,19032,
18868,18703,18537,18371,18204,18037,17869,17700,17530,17360,17189,17018,
16846,16673,16499,16325,16151,15976,15800,15623,15446,15269,15090,14912,
14732,14553,14372,14191,14010,13828,13645,13462,13279,13094,12910,12725,
12539,12353,12167,11980,11793,11605,11417,11228,11039,10849,10659,10469,
10278,10087,9896,9704,9512,9319,9126,8933,8739,8545,8351,8157,
7962,7767,7571,7375,7179,6983,6786,6590,6393,6195,5998,5800,
5602,5404,5205,5007,4808,4609,4410,4210,4011,3811,3612,3412,
3212,3012,2811,2611,2410,2210,2009,1809,1608,1407,1206,1005,
804,603,402,201,0,-201,-402,-603,-804,-1005,-1206,-1407,
-1608,-1809,-2009,-2210,-2410,-2611,-2811,-3012,-3212,-3412,-3612,-3811,
-4011,-4210,-4410,-4609,-4808,-5007,-5205,-5404,-5602,-5800,-5998,-6195,
-6393,-6590,-6786,-6983,-7179,-7375,-7571,-7767,-7962,-8157,-8351,-8545,
-8739,-8933,-9126,-9319,-9512,-9704,-9896,-10087,-10278,-10469,-10659,-10849,
-11039,-11228,-11417,-11605,-11793,-11980,-12167,-12353,
-12539,-12725,-12910,-13094,-13279,-13462,-13645,-13828,
-14010,-14191,-14372,-14553,-14732,-14912,-15090,-15269,
-15446,-15623,-15800,-15976,-16151,-16325,-16499,-16673,
-16846,-17018,-17189,-17360,-17530,-17700,-17869,-18037,
-18204,-18371,-18537,-18703,-18868,-19032,-19195,-19357,
-19519,-19680,-19841,-20000,-20159,-20317,-20475,-20631,
-20787,-20942,-21096,-21250,-21403,-21554,-21705,-21856,
-22005,-22154,-22301,-22448,-22594,-22739,-22884,-23027,
-23170,-23311,-23452,-23592,-23731,-23870,-24007,-24143,
-24279,-24413,-24547,-24680,-24811,-24942,-25072,-25201,
-25329,-25456,-25582,-25708,-25832,-25955,-26077,-26198,
-26319,-26438,-26556,-26674,-26790,-26905,-27019,-27133,
-27245,-27356,-27466,-27575,-27683,-27790,-27896,-28001,
-28105,-28208,-28310,-28411,-28510,-28609,-28706,-28803,
-28898,-28992,-29085,-29177,-29268,-29358,-29447,-29534,
-29621,-29706,-29791,-29874,-29956,-30037,-30117,-30195,
-30273,-30349,-30424,-30498,-30571,-30643,-30714,-30783,
-30852,-30919,-30985,-31050,-31113,-31176,-31237,-31297,
-31356,-31414,-31470,-31526,-31580,-31633,-31685,-31736,
-31785,-31833,-31880,-31926,-31971,-32014,-32057,-32098,
-32137,-32176,-32213,-32250,-32285,-32318,-32351,-32382,
-32412,-32441,-32469,-32495,-32521,-32545,-32567,-32589,
-32609,-32628,-32646,-32663,-32678,-32692,-32705,-32717,
-32728,-32737,-32745,-32752,-32757,-32761,-32765,-32766,
-32767,-32766,-32765,-32761,-32757,-32752,-32745,-32737,
-32728,-32717,-32705,-32692,-32678,-32663,-32646,-32628,
-32609,-32589,-32567,-32545,-32521,-32495,-32469,-32441,
-32412,-32382,-32351,-32318,-32285,-32250,-32213,-32176,
-32137,-32098,-32057,-32014,-31971,-31926,-31880,-31833,
-31785,-31736,-31685,-31633,-31580,-31526,-31470,-31414,
-31356,-31297,-31237,-31176,-31113,-31050,-30985,-30919,
-30852,-30783,-30714,-30643,-30571,-30498,-30424,-30349,
-30273,-30195,-30117,-30037,-29956,-29874,-29791,-29706,
-29621,-29534,-29447,-29358,-29268,-29177,-29085,-28992,
-28898,-28803,-28706,-28609,-28510,-28411,-28310,-28208,
-28105,-28001,-27896,-27790,-27683,-27575,-27466,-27356,
-27245,-27133,-27019,-26905,-26790,-26674,-26556,-26438,
-26319,-26198,-26077,-25955,-25832,-25708,-25582,-25456,
-25329,-25201,-25072,-24942,-24811,-24680,-24547,-24413,
-24279,-24143,-24007,-23870,-23731,-23592,-23452,-23311,
-23170,-23027,-22884,-22739,-22594,-22448,-22301,-22154,
-22005,-21856,-21705,-21554,-21403,-21250,-21096,-20942,
-20787,-20631,-20475,-20317,-20159,-20000,-19841,-19680,
-19519,-19357,-19195,-19032,-18868,-18703,-18537,-18371,
-18204,-18037,-17869,-17700,-17530,-17360,-17189,-17018,
-16846,-16673,-16499,-16325,-16151,-15976,-15800,-15623,
-15446,-15269,-15090,-14912,-14732,-14553,-14372,-14191,
-14010,-13828,-13645,-13462,-13279,-13094,-12910,-12725,
-12539,-12353,-12167,-11980,-11793,-11605,-11417,-11228,
-11039,-10849,-10659,-10469,-10278,-10087,-9896,-9704,-9512,-9319,-9126,
-8933,-8739,-8545,-8351,-8157,-7962,-7767,-7571,-7375,-7179,-6983,-6786,
-6590,-6393,-6195,-5998,-5800,-5602,-5404,-5205,-5007,-4808,-4609,-4410,
-4210,-4011,-3811,-3612,-3412,-3212,-3012,-2811,-2611,-2410,-2210,-2009,
-1809,-1608,-1407,-1206,-1005,-804,-603,-402,-201,0,201,402,603,804,1005,
1206,1407,1608,1809,2009,2210,2410,2611,2811,3012,3212,3412,3612,3811,
4011,4210,4410,4609,4808,5007,5205,5404,5602,5800,5998,6195,
6393,6590,6786,6983,7179,7375,7571,7767,7962,8157,8351,8545,
8739,8933,9126,9319,9512,9704,9896,10087,10278,10469,10659,10849,
11039,11228,11417,11605,11793,11980,12167,12353,12539,12725,12910,13094,
13279,13462,13645,13828,14010,14191,14372,14553,14732,14912,15090,15269,
15446,15623,15800,15976,16151,16325,16499,16673,16846,17018,17189,17360,
17530,17700,17869,18037,18204,18371,18537,18703,18868,19032,19195,19357,
19519,19680,19841,20000,20159,20317,20475,20631,20787,20942,21096,21250,
21403,21554,21705,21856,22005,22154,22301,22448,22594,22739,22884,23027,
23170,23311,23452,23592,23731,23870,24007,24143,24279,24413,24547,24680,
24811,24942,25072,25201,25329,25456,25582,25708,25832,25955,26077,26198,
26319,26438,26556,26674,26790,26905,27019,27133,27245,27356,27466,27575,
27683,27790,27896,28001,28105,28208,28310,28411,28510,28609,28706,28803,
28898,28992,29085,29177,29268,29358,29447,29534,29621,29706,29791,29874,
29956,30037,30117,30195,30273,30349,30424,30498,30571,30643,30714,30783,
30852,30919,30985,31050,31113,31176,31237,31297,31356,31414,31470,31526,
31580,31633,31685,31736,31785,31833,31880,31926,31971,32014,32057,32098,
32137,32176,32213,32250,32285,32318,32351,32382,32412,32441,32469,32495,
32521,32545,32567,32589,32609,32628,32646,32663,32678,32692,32705,32717,
32728,32737,32745,32752,32757,32761,32765,32766 };

char filterbuf[FIR_COE];
char finefilterbuf[FIR_COE];

char pastsamples[SAMPLES*HISTORY];
char lasttone = 0;      // Counts number of consecutive tone detections
char totaltones = 0;
char didsync = 0;       // Set to one if we did a timex sync.
struct timeval starttime;
int startoffset = 0;    // Time offset given by gettimexoffset
int buffers_read = 0;
//double offsets[NUM_TONES];  // Time offset per tone detection
//double deltaT[NUM_TONES];   // Time of tone occurance
double maxAdjust = 0;       // Maximum adjustment in seconds
int propDelay = 0;          // Propagation delay from CHU in microseconds

#define TONETYPE_MIN 0
#define TONETYPE_SEC 1
struct tonetime
{
  double offset;   // Time offset of tone detection
  double deltaT;   // Time of tone detection
  char type;
};

struct tonetime toneTimes[NUM_TONES];
int lastMinuteToneIndex = -1;

#ifdef USE_TIMECODE
struct typeAFrame
{
  int days;    // Julian day of the year
  int hours;
  int minutes;
  int seconds;
};

struct typeBFrame
{
  int year;    // 4 digit year
  int dut1;    // DUT1 in 1/10 second
  int tai;     // TAI-UTC
};

struct typeAFrame aFrame;   // Type A CHU timecode frame
struct typeBFrame bFrame;   // Type B CHU timecode frame
char timeCodeStatus = 0;    // Status of received time codes
double tcDeltaT;            // Time of time code being received

#define TIME_CODE_A_VALID 1
#define TIME_CODE_B_VALID 2
#define TIME_CODE_BOTH_VALID (TIME_CODE_A_VALID | TIME_CODE_B_VALID)

#define TCOVERLAP 3
#define TCBITSAMPLES 24
#define TCDVSAMPLES 64

#endif

short byteToint(unsigned char c)
{
  return ((short)c-128);
}

unsigned char intToByte(int c)
{
  return (c+128);
}

/* returns the square root of y more or less */
unsigned int sqr(unsigned int y)	
{
  unsigned int x,dx;
  char i,j;

  for (i=0,x=y;x;x>>=1,i++);
  for (j=0,x=y;j<(i>>1);x>>=1,j++);
  if (y==0) return 0;

  do
  {
    dx = y/2/x - x/2;
    x += dx;
  } while (dx & 0xfffffffe);
  return x;
}

#ifdef RECTANGULAR_WINDOW
int window(int sample, int pos)  // Rectangular window function
{
  return sample;
}
#endif

#ifdef TRIANGULAR_WINDOW
int window(int sample, int pos)  // Triangluar window function
{
  if (pos < SAMPLES/2)
    return (sample * pos /(SAMPLES/2));
  else
    return (sample * (SAMPLES-pos) / (SAMPLES/2));
}
#endif

#ifdef HANNING_WINDOW
int window(int sample, int pos) // Hanning window function
{
  return (sample * ((-COS(pos*TABLE_SIZE/SAMPLES))/2 + 16384) / 32768);
}
#endif

char checkTone(char *buf, int freq)
{
  int acccos = 0;
  int accsin = 0;
  int rms = 0;
  int energy;
  int f = freq;
  int sample;
  int phaseangleinc;
  int i;

  phaseangleinc = (TABLE_SIZE * f / SAMPLE_RATE);

  for (i=0; i<SAMPLES; i++)
  {
    sample = window(byteToint(buf[i]), i);

    acccos += sample * COS((int) (i*phaseangleinc));
    accsin += sample * SIN((int) (i*phaseangleinc));

    rms += (sample*sample);
  }

  acccos >>= 16;
  accsin >>= 16;
  energy = sqr(((acccos*acccos + accsin*accsin)<<8)/(rms+1));

#ifdef TEST
  printf("%2.2d ", energy);
#endif

  if (energy > THRESHOLD)
    return 1;
  else 
    return 0;
}


char filter(unsigned char *coe, unsigned char *buf, unsigned char c)
{
  int sum = 0;
  int i;

  memmove(buf+1, buf, (FIR_COE-1)*sizeof(char));
  buf[0] = c;

  for (i=0; i<FIR_COE; i++)
    sum += byteToint(buf[i]) * byteToint(coe[i]);

  sum >>= 12;


  return intToByte(sum);
}

int secondsSinceLastAdj()
{
  int rc = 900;  // default
  char buf[80];
  time_t now;
  FILE *fp;
 
  time(&now);
  
  fp = fopen("/etc/chu.last", "r");
  if (fp != NULL)
  {
    fgets(buf, 80, fp);
    rc = now - atol(buf);
    fclose(fp);
  }
  return rc;
}

void saveAdjTime()
{
  time_t now;
  FILE *fp;

  fp = fopen("/etc/chu.last", "w");
  if (fp != NULL)
  {
    time(&now);
    fprintf(fp, "%d\n", now);
    fclose(fp);
  }
}

#ifdef USE_ADJTIME
void
do_adjtime(int microsec)
{
  struct timeval delta;
  int status;

  printf("do_adjtime adjustment: %d\n", microsec);
  delta.tv_sec = 0;
  delta.tv_usec = microsec;
  status = adjtime(&delta, NULL);
  printf("do_adjtime status: %d\n", status);
}
#endif

#ifdef USE_TIMEX
void timex_adjustment(int microsec)
{
  struct timex t;
  long freq;

  printf("timex adjustment: ");

  memset(&t, 0, sizeof(t));
  adjtimex(&t);

  freq = t.freq;  // save the frequency

  /* Adjust the time */
  t.modes |= ADJ_OFFSET;
  t.offset += microsec;

  /* Set the status to indicate synchronized, PLL locked */
  t.modes |= ADJ_STATUS;
  t.status |= STA_PLL;
  t.status &= (~STA_UNSYNC);

  if (abs(microsec) > 20000)
    t.status |= STA_UNSYNC;

  adjtimex(&t);

  /* Adjust the frequency so maybe we don't have to do
     this so much next time.  Only adjust one eighth as much
     as it would seem to be needed.  This is to avoid
     oscillation.  This should settle to some stable
     value after a while. */

  t.modes = ADJ_FREQUENCY;
//  t.freq += ((microsec<<SHIFT_USEC) / 3600 / 8);
//  t.freq = freq + ((microsec<<(SHIFT_USEC-4)) / 1800);
//  t.freq = freq + ((microsec<<(SHIFT_USEC-4)) / 225);
  t.freq = freq + ((microsec<<(SHIFT_USEC-8)) / secondsSinceLastAdj());
  saveAdjTime();

  printf(" (%3.3f ppm) ", (double)t.freq/(double)65536);
  
  adjtimex(&t);

  /* Set the maximum and estimated error fields.  These are used
     by ntpd.  They don't really affect us. */

  memset(&t, 0, sizeof(t));
  adjtimex(&t);

  t.modes = ADJ_MAXERROR | ADJ_ESTERROR;
  t.esterror = abs(microsec)/2;
  t.maxerror = t.esterror;

  adjtimex(&t);
  if (errno)
    perror("adjtimex()");

  didsync = 1;
}
#endif

double gettimexoffset()
{
  long offset = 0;
#ifdef USE_TIMEX
  struct timex t;

  memset(&t, 0, sizeof(t));
 
  adjtimex(&t);
  offset = t.offset;
#endif
#ifdef USE_ADJTIME
#endif
  return (double) offset; 
}

#ifdef USE_TIMEX
void updateTimexStatus()  // Called if we failed to sync.
{
  struct timex t;
  memset(&t, 0, sizeof(t));
 
  adjtimex(&t);

  t.modes = ADJ_ESTERROR;
  t.esterror += 1000;

  adjtimex(&t);

  if (t.esterror > 10000)  // Declare us unsyncrhonized 
  {
    t.modes = ADJ_STATUS;
    t.status |= STA_UNSYNC;
    adjtimex(&t);
  }
}
#endif


int rmsmillis(char *buf)
{
  int rms, i, sample;

  rms = 0;
  for (i=0; i<(SAMPLE_RATE/1000); i++)
  {
    sample = byteToint(buf[i]);
    rms += (sample*sample);
  }
 
  rms = sqr(rms/i);
  
  return rms;
}


int finetune(int start)
{
  char buf[SAMPLES*(FINE_TOTAL_SAMPLES+1)];
  char buf2[SAMPLES*(FINE_TOTAL_SAMPLES+1)];
  char *orig;
  int fine;
  int i, j, tmp;

  orig = pastsamples+SAMPLES*(HISTORY-start-FINE_PREV_SAMPLES) ;

//  memcpy(buf, pastsamples+SAMPLES*(HISTORY-start-FINE_PREV_SAMPLES), 
//         SAMPLES*(FINE_TOTAL_SAMPLES+1));

  /* 1-Hz comb filter.  This helps us detect the leading edge
   * of the tone. */

  for (i=0; i<(SAMPLES*(FINE_TOTAL_SAMPLES+1)); i++)
  {
    for (j=tmp=0; j<3; j++)
      tmp += byteToint(orig[i - j*SAMPLE_RATE]);

    buf[i] = intToByte(tmp/3);
  }

  memcpy(buf2, buf, SAMPLES*(FINE_TOTAL_SAMPLES+1));
  orig = buf2;

  memset(finefilterbuf, 0x80, FIR_COE);

#if (SAMPLE_RATE == FIR_RATE)   // Only apply filter if rates match
    for (j=0; j<SAMPLES*FINE_TOTAL_SAMPLES; j++)  
      buf[j] = filter(firbuf, finefilterbuf, buf[j]);
#endif

  for (i=0; i<SAMPLES*FINE_TOTAL_SAMPLES; i+=(SAMPLE_RATE/1000))
  {
#ifdef TEST2
    printf("%4.4d %2.2d %2.2d %d %d %d\n", 
           ((SAMPLES*FINE_PREV_SAMPLES)-i)*1000/SAMPLE_RATE, 
           rmsmillis(buf+i), rmsmillis(orig+i),
           checkTone(buf+i, 975), checkTone(buf+i, 1000),
           checkTone(buf+i, 1025));
#endif
  }

  for (i=0; i<SAMPLES*FINE_TOTAL_SAMPLES; i+=(SAMPLE_RATE/1000))
  {
    if (rmsmillis(buf+i) > rmsmillis(orig+i)*10/25 && 
        rmsmillis(buf+i) > 10 &&
        checkTone(buf+i, 1000) && !checkTone(buf+i, 975) && 
        !checkTone(buf+i, 1025))
      break;
  }

  fine = ((SAMPLES*FINE_PREV_SAMPLES)-i)*1000/SAMPLE_RATE;
 
  return fine;
}

void calc_offset(int delay, int fine, char type)
{
  struct timeval tv;
  struct timezone tz;
  double d;
  int ds, dus;
  int elapsedtime;
   
  elapsedtime = 1000000/SAMPLE_RATE*SAMPLES*buffers_read;

  tv.tv_sec = starttime.tv_sec+elapsedtime/1000000;
  tv.tv_usec = starttime.tv_usec+elapsedtime%1000000;
 
  if (tv.tv_usec > 1000000)
  {
    tv.tv_sec += (tv.tv_usec/1000000);
    tv.tv_usec = tv.tv_usec%1000000;
  }

  d = (tv.tv_sec)*1000000 + tv.tv_usec;
  ds = tv.tv_sec;
  dus = tv.tv_usec;

  if (type == TONETYPE_MIN)
    tv.tv_sec = ((tv.tv_sec+30)/60) * 60;

  // Account for processing time
  tv.tv_usec = ((SAMPLES*delay*1000/SAMPLE_RATE) + fine - FIR_DELAY) * 1000;
  tv.tv_usec = tv.tv_usec - propDelay;
  if (tv.tv_usec > 1000000)
  {
    tv.tv_sec += tv.tv_usec / 1000000;
    tv.tv_usec = tv.tv_usec % 1000000;
  }

  d = d - ((tv.tv_sec)*1000000 + tv.tv_usec);

  d = d + startoffset;

  if (type == TONETYPE_MIN)
    lastMinuteToneIndex = lasttone;

  toneTimes[lasttone].deltaT = (double) elapsedtime;
  toneTimes[lasttone].type = type;
  toneTimes[lasttone++].offset = d;

  if (type == TONETYPE_MIN)
  {
    printf("%f\n", (d/1000000.0));
    printf("%d %d %d %d\n", ds, dus, delay, fine);
    fflush(stdout);
  }
}

void adj_time(double d)
{
  struct timeval tv;
  struct timezone tz;
  int rc;

  if (fabs(d) > maxAdjust && maxAdjust != 0)
  {
    printf("WARNING: Adjustment exceeds maximum allowed: ");
    fflush(stdout);
    return;
  }

  /* Raise our priority temporiarily */
  errno = 0;
  setpriority(PRIO_PROCESS, 0, -20);
  if (errno)
    perror("setpriority()");

  gettimeofday(&tv, &tz);

  tv.tv_sec += (int) (-d / 1000000.0);
  tv.tv_usec += (-d - ((double)((int)(-d/1000000.0)*1000000.0)));

  if (tv.tv_usec > 1000000)
  {
    tv.tv_sec += tv.tv_usec / 1000000;
    tv.tv_usec = tv.tv_usec % 1000000;
  }
  
  if (tv.tv_usec < 0)
  {
    tv.tv_sec --;
    tv.tv_usec += 1000000;
  }

#ifdef ADJTIME
#ifdef USE_TIMEX
  if (fabs(d) < ((double)MAXPHASE))
  {
    timex_adjustment((int)-d);
  }
#else
  if (fabs(d) < ((double)1000000))
    do_adjtime((int)-d);
  else
#endif
#endif
  {
    printf("standard adjustment ");

    rc = settimeofday(&tv, &tz);

    if (errno)
      perror("settimeofday()");
  }

  fflush(stdout);

  errno = 0;
  setpriority(PRIO_PROCESS, 0, 0);
  if (errno)
    perror("setpriority()");
}

double timeCodeAdjust(double d)
{
#ifdef USE_TIMECODE
  if (timeCodeStatus == TIME_CODE_BOTH_VALID)
  {
    int tcsec, clksec, diff;

    tcsec = (bFrame.year-1970)*365 + (bFrame.year-1969)/4 + aFrame.days-1;
    tcsec = tcsec*24 + aFrame.hours;
    tcsec = (tcsec*60 + aFrame.minutes) * 60;

    clksec = starttime.tv_sec+(int)(tcDeltaT)/1000000-aFrame.seconds;
    clksec = ((clksec+30)/60) * 60;

    diff = clksec - tcsec;

    d = d + (double)(diff) * 1000000;
  }
#endif
  return d;
}

/*
void calc_adjust()
{
  double ave_offset = 0;
  double max_variance = 0;
  double tmp;
  int i;

  for (i=0; i<3; i++)
    ave_offset += offsets[i];

  ave_offset = ave_offset/3;

  for (i=0; i<3; i++)
  {
    tmp = fabs(offsets[i] - ave_offset);
    if (tmp > max_variance)
      max_variance = tmp;
  }


  printf("ave_offset = %f\n", ave_offset/1000000.0);
  printf("max_variance = %f\n", max_variance/1000000.0);
  fflush(stdout);

  if (max_variance < 10000.0)  // Less than 10 mS.
  {
    adj_time(ave_offset);
    printf("%f\n", ave_offset/1000000.0);
    fflush(stdout);
  }
}
*/

void calc_adjust()
{
  double sx=0, sxx=0, sy=0, sxy=0;
  double m, b;
  double max_variance, tmp;
  double mean_variance;
  int i, worst=0;

  for (i=0; i<lasttone; i++)
  {
    sx += toneTimes[i].deltaT;
    sy += toneTimes[i].offset;
    sxy += (toneTimes[i].offset*toneTimes[i].deltaT);
    sxx += (toneTimes[i].deltaT*toneTimes[i].deltaT);
  }

  m = (lasttone*sxy - sx*sy) / (lasttone*sxx - sx*sx);
  b = (sy - m*sx)/lasttone;

  mean_variance = 0;
  for (i=0; i<lasttone; i++)
  {
    tmp = fabs(toneTimes[i].offset - (m*toneTimes[i].deltaT+b));
    if (tmp > max_variance)
    {
      max_variance = tmp;
      worst = i;
    }
    mean_variance += tmp;
  }
  mean_variance = mean_variance / lasttone;


  if (max_variance < 1000.0)  // Less than 1 mS.
  {
    printf("Computing with %d tone samples\n", lasttone);
    printf("offset = %f\n", b/1000000.0);
    printf("sound card error = %3.3f ppm\n", m*1000000.0);
    printf("max_variance = %f\n", max_variance/1000000.0);
    printf("mean_variance = %f\n", mean_variance/1000000.0);
    fflush(stdout);

    b = timeCodeAdjust(b);
    adj_time(b);
    printf("%f\n", b/1000000.0);
    fflush(stdout);
  }
  else  // Throw out worst sample and try again
  {
    if (lasttone > MIN_NUM_TONES && lasttone > (totaltones/3))
    {
      toneTimes[worst] = toneTimes[lasttone-1];
//      offsets[worst] = offsets[lasttone-1];
//      deltaT[worst] = deltaT[lasttone-1];
      lasttone--;
      calc_adjust();
      return;
    }
  }
}


void process(char *buf)
{
#ifdef TEST
  checkTone(buf,975);
  checkTone(buf,1000);
  checkTone(buf,1025);
  printf("\n");
#else

  int i, sum, fine;
  static char hist[MINDURATION];

  memmove(hist+sizeof(char), hist, (MINDURATION-1)*sizeof(char));

  hist[0] = 0;

  if (checkTone(buf, 1000) && 
      !checkTone(buf, 975) && !checkTone(buf, 1025))
    hist[0] = 1;

  for (sum=i=0; i<MINDURATION; i++)
   sum+=hist[i];

//  printf("%d %d\n", hist[0],sum);
  
  if (sum > (MINDURATION*3/4) && hist[0] == 0)
  {
    for (i=(MINDURATION-1); i>=0; i--)  /* Find start of tone */
      if (hist[i])
        break;

    i++;

    fine = finetune(i);

    memset(hist, 0, sizeof(hist)); // Reset the history

    if (fine != FINE_MIN && fine < FINE_MAX)
    {
      calc_offset(i, fine, TONETYPE_MIN);
      system("date");
    }
    else
    {
      printf("Close call\n");
      system("date");
    }
  }

  if (sum > SECDURATION && lastMinuteToneIndex != -1 &&
      fabs(toneTimes[lastMinuteToneIndex].offset) < 500000.0 &&
      hist[0] == 0 && hist[1] == 0 && hist[2] == 0 && hist[3] == 0)
  {
    for (i=(SECDURATION+3); i>=0; i--)  /* Find start of tone */
      if (hist[i])
        break;

    memset(hist, 0, sizeof(hist)); // Reset the history

    i++;

    fine = finetune(i);

    if (fine != FINE_MIN && fine < FINE_MAX)
    {
      calc_offset(i, fine, TONETYPE_SEC);
//      system("date");
    }
//    else
//    {
//      printf("Close call\n");
//      system("date");
//    }
  }

#endif
}

#ifdef USE_TIMECODE
#define BELL103_FIR_COE 64

char bell103filterbuf[BELL103_FIR_COE];

/* Finite impulse response filter coefficients       */
/* for the Bell 103 data stream.                     */
/* This filter has a bandpass, centered on 2125 Hz,  */
/* -3 dB bandwidth is 520 Hz, passband ripple is     */
/* less than 0.1 dB, stopband attenuation is greater */
/* than -40 dB.                                      */

unsigned char bell103firbuf[] = { 0x80, 0x80, 0x80, 0x80,
                                  0x80, 0x80, 0x80, 0x80,
                                  0x7f, 0x82, 0x82, 0x7a,
                                  0x7d, 0x8a, 0x82, 0x73,
                                  0x80, 0x8a, 0x7f, 0x7f,
                                  0x7e, 0x73, 0x8f, 0x9c,
                                  0x5d, 0x58, 0xc0, 0xac,
                                  0x23, 0x60, 0xf3, 0x8b,
                                  0x03, 0x8a, 0xf3, 0x60,
                                  0x23, 0xaa, 0xc2, 0x58,
                                  0x5d, 0x9d, 0x8f, 0x72,
                                  0x7e, 0x7f, 0x7f, 0x8a,
                                  0x80, 0x74, 0x82, 0x8a,
                                  0x7c, 0x7a, 0x82, 0x82,
                                  0x7f, 0x80, 0x80, 0x80,
                                  0x80, 0x80, 0x80, 0x80 }; 

char bell103filter(unsigned char *coe, unsigned char *buf, unsigned char c)
{
  int sum = 0;
  int i;

  memmove(buf+1, buf, BELL103_FIR_COE*sizeof(char));
  buf[0] = c;

  for (i=0; i<BELL103_FIR_COE; i++)
    sum += byteToint(buf[i]) * byteToint(coe[i]);

  sum >>= 10;

  return intToByte(sum);
}

/* Returns the current second based on received time signal, or
   -1 if current second cannot be determined */

int currentSecond() 
{
  int timeSinceToneRx;
  int seconds;

  if (lastMinuteToneIndex == -1)
    return -1;

  timeSinceToneRx = (1000000/SAMPLE_RATE*SAMPLES*buffers_read) - 
                    (int) toneTimes[lastMinuteToneIndex].deltaT;

  seconds = (timeSinceToneRx/1000000) % 60;
  return seconds;
}

double currentDeltaT()
{
  return (double)(1000000/SAMPLE_RATE*SAMPLES*buffers_read);
}

int rectWindow(int sample, int pos, int numsamples)  // Rectangular window function
{
  return sample;
}

char tcCheckTone(char *buf, int numsamples, int freq)
{
  int acccos = 0;
  int accsin = 0;
  int rms = 0;
  int energy;
  int f = freq;
  int sample;
  int phaseangleinc;
  int i;

  phaseangleinc = (TABLE_SIZE * f / SAMPLE_RATE);

  for (i=0; i<numsamples; i++)
  {
    sample = rectWindow(byteToint(buf[i]), i, numsamples);

    acccos += sample * COS((int) (i*phaseangleinc));
    accsin += sample * SIN((int) (i*phaseangleinc));

    rms += (sample*sample);
  }

  acccos >>= 16;
  accsin >>= 16;
  energy = sqr(((acccos*acccos + accsin*accsin)<<8)/(rms+1));

  return energy;
}


void uart(int dv, int bit, char *buf, int *numchars)
{
  static state = 0;
  static unsigned char c = 0;
  static int i;
  static unsigned char sum;

  if (!dv)  /* No carrier */
  {
    state = 0;
    c = 0; 
    i = 0;
    sum = 0;
    return;
  }

  if (dv && state == 0 && bit == 0) /* Start bit */ 
  {
    state = 1;
    i = 0;
    c = 0;
    sum = 0;
  }

  if (state > 0)
    i++;

  if (i == 82)
  {
//    printf("%d %2.2X\n", bit, c);
//    fflush(stdout);
    buf[(*numchars)++] = c;
    state = 0;
    i = 0;
    c = 0;
    sum = 0;
  }

  if (state > 0 && i<=5 && bit != 0)
  {
    /* Invalid start bit */
    state = 0;
    i = 0;
    c = 0;
    sum = 0;
  }

  if (i > 6)
  {
    if ( (i%9) >= 1 && (i%9) <= 5)
      sum += bit;

    if ( (i%9) == 6)  /* Grab a bit */
    {
      sum += bit;

      c >>= 1;

      if (sum >= 3) 
        c |= 0x80;

      sum = 0;
    }
  }  
}

void processFrame(unsigned char *buf)
{
  int type;  /* 0 for invalid, 1 for type A, 2 for type B */
  int i;
  
  type = 1;
  for (i=0; i<5; i++)
    if (buf[i] != buf[i+5])
      type = 0;

  if (type == 0)
  {
    type = 2;
    for (i=0; i<5; i++)
      if (buf[i] != (buf[i+5] ^ 0xff))
	type = 0;
  }

  if (type == 0)  /* Invalid data */
    return;

  if (type == 1)  /* Type A */
  {
    aFrame.days = (buf[0] >> 4)*100 + (buf[1]&0xf)*10 + (buf[1]>>4);
    aFrame.hours = (buf[2]&0xf)*10 + (buf[2]>>4);
    aFrame.minutes = (buf[3]&0xf)*10 + (buf[3]>>4);
    aFrame.seconds = (buf[4]&0xf)*10 + (buf[4]>>4);
    timeCodeStatus |= TIME_CODE_A_VALID;

    printf("Type A: %3.3d %2.2d:%2.2d:%2.2d\n", aFrame.days,

           aFrame.hours,
           aFrame.minutes,
           aFrame.seconds);

    fflush(stdout);
  }
 
  if (type == 2) /* Type B */
  {
    int x, y, z, t, a;
    char strbuf[80];

    x = buf[0] & 0xff;
    z = buf[0] >> 4;

    y = (buf[1] & 0xf) * 1000 +
        (buf[1] >> 4) * 100 +
        (buf[2] & 0xf) * 10 +
        (buf[2] >> 4);

    t = (buf[3] & 0xf) * 10 +
        (buf[3] >> 4);

    a = (buf[4] & 0xf) * 10 +
        (buf[4] >> 4);

    *strbuf = 0;

    bFrame.year = y;
    bFrame.dut1 = ((x&0x1)?1:-1)*z;
    bFrame.tai = t;
    timeCodeStatus |= TIME_CODE_B_VALID;

    if (x & 0x2)
      sprintf(strbuf, "WARNING: Leap second to be inserted.");

    if (x & 0x4)
      sprintf(strbuf, "WARNING: Leap second to be deleted.");

    printf("Type B: Year=%d DUT1=%s0.%d TAI-UTC=%d DL=%d %s\n",
            y, (x&0x1)?"+":"-", z, t, a, strbuf);

//    printf("%2.2x %2.2x %2.2x %2.2x %2.2x\n", buf[0], buf[1], buf[2], buf[3], buf[4]);
//    printf("%d %d %d %d %d\n", x, y, z, t, a);
            
    fflush(stdout);
  }
}

void processTimeCode(char *buf)
{
  int mark, space;
  static int dvtime = 0;
  static int lastdvtime;
  static char rcvbuf[30];
  static int rxchars = 0;
  int dv;
  
  dv = tcCheckTone(buf, TCDVSAMPLES, 2225) +
       tcCheckTone(buf, TCDVSAMPLES, 2025);

  if (dv > 30 && rmsmillis(buf) > 9)
    dvtime++;
  else 
    dvtime--;

  if (dvtime < 0)
    dvtime = 0;

  if (dvtime > 50)
    dvtime = 50;

//  if (dvtime > 20 && lastdvtime <= 20)
//    printf("Carrier active\n");

  if (dvtime <= 20 && lastdvtime > 20)
  {
    dvtime = 0;
//    printf("Carrier de-active\n");

    if (rxchars == 10)
      processFrame(rcvbuf);

    rxchars = 0;
  }

  lastdvtime = dvtime;

  if (dvtime > 20)
  {
    mark = tcCheckTone(&buf[TCDVSAMPLES-TCBITSAMPLES], TCBITSAMPLES, 2225);
    space = tcCheckTone(&buf[TCDVSAMPLES-TCBITSAMPLES], TCBITSAMPLES, 2025);
  }

  if (rxchars > 12) 
    rxchars = 12;

  uart((dvtime>20)?1:0, (mark>space)?1:0, rcvbuf, &rxchars);
}

void lookForTimeCode(char *buf)
{
  static char tcbuf[TCDVSAMPLES];
  int second, lastsecond = -2;
  int bufptr;
  static int bufmod = 0;
  int bytesCopied;
  int i;

  second = currentSecond();

  if (second != lastsecond && second == 40 && 
      timeCodeStatus != TIME_CODE_BOTH_VALID)
  {
    timeCodeStatus = 0;
  }

  if (second != -1 && second > 29 && second < 40 && 
      (timeCodeStatus & TIME_CODE_A_VALID) == 0)
  {
    if (second > 32 && (timeCodeStatus & TIME_CODE_B_VALID) == 0)
    {
      lastsecond = second;
      return;
    }

    for (bufptr=0; bufptr<SAMPLES;)
    {
      bytesCopied = TCOVERLAP-bufmod;
      if (bytesCopied + bufptr > SAMPLES)
        bytesCopied = SAMPLES-bufptr;

      memmove(tcbuf, tcbuf+bytesCopied, TCDVSAMPLES-bytesCopied);
      memcpy(&tcbuf[TCDVSAMPLES-bytesCopied], &buf[bufptr], bytesCopied);
      bufptr+=bytesCopied;

      /* Pass the new samples through the Bell 103 FIR filter */
      for (i=TCDVSAMPLES-bytesCopied; i<TCDVSAMPLES; i++)
        tcbuf[i] = bell103filter(bell103firbuf, bell103filterbuf, tcbuf[i]);

      bufmod = (bufmod+bytesCopied) % TCOVERLAP;
      processTimeCode(tcbuf);
      if (timeCodeStatus & TIME_CODE_A_VALID)
        break;
    }

    if (timeCodeStatus == TIME_CODE_BOTH_VALID)
      tcDeltaT = currentDeltaT();
  }
  lastsecond = second;
}
#endif

char *parseArgs(int argc, char **argv)
{
  char *dspdev = "/dev/dsp";
  char *fname;
  int i;

  /* Process arguments */
  fname = dspdev;
  maxAdjust = 0;

  for (i=1; i<argc; i++)
  {
    if (strncmp(argv[i], "-l:", 3) == 0)
    {
      maxAdjust = ((double)atoi(argv[i]+3))*1000000.0;
      continue;
    }

    if (strncmp(argv[i], "-d:", 3) == 0)
    {
      propDelay = atoi(argv[i]+3);
      continue;
    }
   
    if (*argv[i] != '-')
    {
      fname = argv[i];
      break;
    }
    else
    {
      printf("Invalid switch: %s\n", argv[i]);
      return NULL;
    }
  }
  return fname;
}

void main(int argc, char **argv)
{
  FILE *fp;
  signed char buf[8192];
  signed char filteredbuf[8192];
  struct timezone tz;
  char *fname;
  int i, j;

  fname = parseArgs(argc, argv);
  if (fname == NULL)
    return;

  /* Set priority to maximum -- just long enough for us
     to open the audio device and timestamp it.  This should
     reduce the risk of getting interrupted at this critical point. */

  errno = 0;
  setpriority(PRIO_PROCESS, 0, -20);
  if (errno)
    perror("setpriority()");

  fp = fopen(fname, "r");
  if (fp == NULL)
  {
    printf("fopen(): Unable to open file: %s  %s\n", fname, strerror(errno));
    return;
  }

  gettimeofday(&starttime, &tz);
  startoffset = gettimexoffset();

  /* Reduce the priority back to normal */
  errno = 0;
  setpriority(PRIO_PROCESS, 0, 0);
  if (errno)
    perror("setpriority()");

  /* Listen for 10 minutes */
  for (i=0; i<(SAMPLE_RATE/SAMPLES)*600; i++)
  {
    if (fread(buf, SAMPLES, 1, fp) != 1)
      break;

    buffers_read ++;

    memmove(pastsamples, pastsamples+SAMPLES, SAMPLES*(HISTORY-1));
    memcpy(pastsamples+SAMPLES*(HISTORY-1), buf, SAMPLES);

#if (SAMPLE_RATE == FIR_RATE)   // Only apply filter if rates match
    for (j=0; j<SAMPLES; j++)  
      filteredbuf[j] = filter(firbuf, filterbuf, buf[j]);
#else
    memcpy(filteredbuf, buf, SAMPLES);
#endif

    process(filteredbuf);

#ifdef USE_TIMECODE
    lookForTimeCode(buf);
#endif    

    if (lasttone > 50)
      break;
  }

  fclose(fp);

  if (lasttone >= MIN_NUM_TONES)
  {
    /* at this time we have measured enough time offsets */
    /* Compare them, make sure they agree and all        */
    totaltones = lasttone;
    calc_adjust();
  }

#ifdef USE_TIMEX
  if (didsync == 0)  // Did not timex sync
  {
    updateTimexStatus();
  }
#endif 
}