#include <stdio.h>
#include <stdint.h>

static int samp;
static int nco_out;

static uint16_t phacc=0; //phase accumulator
static uint16_t control_word=1;

static float f1=0,f2=0; //frequencies


void nco(void){  //advance NCO
  phacc+=control_word;
  nco_out=((phacc&0x8000)!=0);
}

int div10(int a){  //frequency divider
  static int prev=0,n,output=0;
  if(a!=prev) { //edge detect a
    n++;
    if(n==10){
      n=0;
      output=1-output;
    }
  }
  prev=a;
  return output;
}

static int Qa=0,Qb=0; //phase comparator flip flops

void comp(int a,int b){ //phase comparator
  static int prev_a,prev_b;

  int rst=!(Qa&&Qb);
  Qa=((a&&!prev_a)||Qa)&&rst;
  Qb=((b&&!prev_b)||Qb)&&rst;
  prev_a=a;
  prev_b=b;

  //  printf("Qa: %d\t Qb: %d\n",Qa,Qb);

}

void input_counter(void){  //input frequency counter
  static int prev_samp=0,t=0;

  if(samp&&!prev_samp){ //edge detect input samples
    f1=8000.0f/t;
    t=0;
  }
  else t++;
  prev_samp=samp;
}

void output_counter(void){ //NCO frequency counter
  static int prev_nco=0,t=0;

  if(nco_out&&!prev_nco){ //edge detect NCO
    f2=8000.f/t;
    t=0;
  }
  else t++;
  prev_nco=nco_out;
}


void run(void){

  comp(samp,div10(nco_out));
  //comp(samp,nco_out);
  nco();
  input_counter();
  output_counter();

  //Qa=1: fNCO<fWAV
  //Qb=1: fNCO>fWAV

  float filt(float);
  static float freq=0;

  freq=freq-filt(Qa-Qb)/128;
  control_word=freq*8.192;

 done:  printf("f1:%2.3f\t f2:%2.3f\t\n",f1,f2);

}

float filt(float a){
  static float last;
  static float deriv=200;

  float res= a+(a-last)*deriv;

  last=a;
  return res;
}

int main(void){

  FILE *f=fopen("1.wav","rb"); //must be 8 kHz, 16 bit signed


  for(int i=0;i<44;i++) //skip header
    fgetc(f);

  int16_t t;
  while(fread(&t,1,2,f)){
    samp=(t>0);
    run();
  }
  fclose(f);


}