/*  Loads a 0.05sec section of a sound data file which
    contains a (possibly distorted) sinewave. Finds the
    sinewave frequency and the THD level.
    
    
    J.C.G.Lesurf June 2006
    
     */

#include <stdio.h>
#include <math.h>
#include "kernel.h"
#include <swis.h>

_kernel_swi_regs rin,rout;

FILE* infile;
FILE* outfile;

int ilength,offset,block_number,samples_per_block;


double tstart,block_duration,total_time,t,dt,f,f0,pi2;
double f0_left,f0_right,phi_left,phi_right,rms_left,rms_right;
double al,ar,atotal,rer,imr,rel,iml,sl,sr,s0,c0;
double rer0,imr0,rel0,iml0;

double hleft[10],hright[10],htotal[10];
double thdleft,thdright,thdtotal;

char tc;

char datablock[8820];
double left[2205], right[2205];

char infilename[128];
char pathname[128];
char leafname[64];
char outfilename[128];

void read_path_file(void);
double check_inputfile(void);
int get_start_time(void);
void load_data_block(void);
void check_data(void);
void transform(void);
double costrans(double,double*);
double sintrans(double,double*);

double find_freq(void);
void   find_null(void);
void   spectrum(int,char*);
double db(double);
double percent(double);

int main(void)
{
  samples_per_block=2205;         /* Number of samples per channel per block */
  block_duration=0.05;            /* Duration of block in seconds */
  dt=1.0/44100.0;                 /* sampling interval */
  pi2=2.0*3.1415927;
  read_path_file();
  printf("\nInput file name => ");
  scanf("%s",leafname);
  sprintf(infilename,"%s.%s\0",pathname,leafname);
  printf("\nFile is =   %s\n",infilename);  
  total_time=check_inputfile();
  offset=get_start_time();
  printf("Offset %d\n",offset);
  load_data_block();
  check_data();
  f0=find_freq();
  printf("Freq found = %6.4lf Hz\n",f0);
  if(f0>30.0)
  {
    if(f0<6000.0)
    {
      spectrum(1,"raw_spectrum\0");
    }
  }
  find_null();
  if(f0>30.0)
  {
    if(f0<6000.0)
    {
      spectrum(0,"nulled_spectrum\0");
    }
  }
  return 0;
}

double db(double pin)
{
  /* This proceedure takes the power and converts it
     into a value in decibels. It traps under-range
     values and sets them to -90.0
  */
  double answer;
  if(pin<0.000000001) pin=0.000000001;
  answer=10.0*log10(pin);
  return answer;
}

double percent(double pin)
{
  /* This proceedure takes an input in dB and
     returns the result as an amplitude percentage
  */
  double answer;
  answer=pow(10.0,pin/20.0);
  answer=answer*100.0;
  return answer;
}

void spectrum(int ice,char* cp)
{
  /* This proceedure carries out the spectral analysis and works out
     the total harmonic distortion levels. If the variable ice
     is set to 1 it stores the values for the fundamental in
     the hleft[0] hright[0] and htotal[0] locations. These
     are then used for scaling the output correctly in terms
     of the fundamental. This allows a waveform which has had
     its fundamental component nulled to still have its spectrum
     analysed in terms of levels relative to the (now absent)
     fundamental.
  */
  char sfn[64];
  int n,istop,nstop;
  double fs;
  sprintf(sfn,"ram:%s",cp);
  printf("\n\nGenerating %s - please wait!\n",cp);
  outfile=fopen(sfn,"wt");
  thdleft=0.0; thdright=0.0; thdtotal=0.0;
  n=1; istop=0;
  do
  {
    fs=(double)n;
    fs=f0*fs/8.0;
    if(fs<20000.0)
    {
      rel=costrans(fs,left); 
      iml=sintrans(fs,left); 
      al=2.0*(rel*rel+iml*iml);
      rer=costrans(fs,right);
      imr=sintrans(fs,right);
      ar=2.0*(rer*rer+imr*imr);
      atotal=(ar+al);
    }
    else
    {
      istop=1;
    }
    n++;
    if(fs>(8.0*f0))  istop=1;
    fprintf(outfile,"%8.2lf,  %8.4lf,  %8.4lf,  %8.4lf\n",fs,db(al),db(ar),db(atotal));
  } while (istop==0);
  fclose(outfile);
  printf("Spectrum Saved\n\n Levels ref Full Scale\n");
  n=1; istop=0;
  do
  {
    fs=f0*(double)n;
    if(fs<20000.0)
    {
      rel=costrans(fs,left); 
      iml=sintrans(fs,left); 
      al=2.0*(rel*rel+iml*iml);
      rer=costrans(fs,right);
      imr=sintrans(fs,right);
      ar=2.0*(rer*rer+imr*imr);
      atotal=(ar+al);
      if(n>1)
      {
        thdleft+=al;
        thdright+=ar;
        thdtotal+=atotal;
      }
      hleft[n]=db(al);
      hright[n]=db(ar);
      htotal[n]=db(atotal);
      printf("%d  %8.2lf Hz Left %6.3lf dB  Right %6.3lf dB Total %6.3lf dB\n",n,fs,hleft[n],hright[n],htotal[n]);
    }
    else
    {
      istop=1;
    }
    n++;
    if(n>6) istop=1;
  } while (istop==0);
  nstop=n;
  if (ice==1)
  {
    hleft[0]=hleft[1]; hright[0]=hright[1]; htotal[0]=htotal[1];
  }
  printf("\n\nLevels ref raw fundamental\n");
  n=1;
  do
  {
    hleft[n]=hleft[n]-hleft[0];
    hright[n]=hright[n]-hright[0];
    htotal[n]=htotal[n]-htotal[0];
    fs=f0*(double)n;
    printf("%d  %8.2lf Hz Left %6.3lf dB  Right %6.3lf dB Total %6.3lf\n",n,fs,hleft[n],hright[n],htotal[n]);
    n++;
  } while (n<nstop);
  thdleft=db(thdleft)-hleft[0];
  thdright=db(thdright)-hright[0];
  thdtotal=db(thdtotal)-htotal[0];
  printf("THD Left %6.2lf dB (%8.3lf percent)\n",thdleft,percent(thdleft));
  printf("   Right %6.2lf dB (%8.3lf percent)\n",thdright,percent(thdright));
  printf("   Total %6.2lf dB (%8.3lf percent)\n",thdtotal,percent(thdtotal));
}

void find_null(void)
{
  /* This proceedure determines the amplitude and phase of the
     fundamental component in the signal. It then subtracts this
     to produce a result which should only consist of the
     noise or distortion.
  */
  int ii;
  outfile=fopen("ram:nulled_waves\n","wt");
  t=0.0; ii=0;
  do
  {
    s0=sin(pi2*f0*t);  c0=cos(pi2*f0*t);
    sl=rel0*c0+iml0*s0;  sl=2.0*sl;
    sr=rer0*c0+imr0*s0;  sr=2.0*sr;
    sl=left[ii]-sl;    sr=right[ii]-sr;
    left[ii]=sl;  right[ii]=sr;
    fprintf(outfile,"%8.4lf, %9.6lf, %9.6lf\n",t*1000.0, sl, sr);
    ii++;
    t+=dt;
  } while (ii<2205);
  fclose(outfile);
}

double find_freq(void)
{
  /* This proceedure employs a fringe counting method to
     determine the frequency of the periodic signal.
     It then sets f0 to be the found value for whichever
     channel has the highest power level.
  */
  int last_left,last_right, first_left,first_right;
  int ii,detect_left,detect_right,count_left,count_right;
  double ffound;
  last_left=-10;  last_right=-10;
  count_left=0;   count_right=0;
  ii=1;
  do
  {
    detect_left=0;
    if (left[ii-1]<=0)
    {
      if (left[ii]>0)
      {
        detect_left=1;
        
        if(last_left<0)
        {
          first_left=ii;
          last_left=detect_left;
        }
        else
        {
          if ((ii-last_left>4))
          {
            last_left=ii;
            count_left++;
          }
        }
      }
    }
      
    detect_right=0;
    if (right[ii-1]<=0)
    {
      if (right[ii]>0)
      {
        detect_right=1;
        
        if(last_right<0)
        {
          first_right=ii;
          last_right=detect_right;
        }
        else
        {
          if ((ii-last_right>4))
          {
            last_right=ii;
            count_right++;
          }
        }
      }
    }
      ii++;
    } while(ii<2205);
    printf("Left  %d to %d has %d cycles\n",first_left,last_left,count_left);
    printf("Right %d to %d has %d cycles\n",first_right,last_right,count_right);
    ffound=(double)(last_left-first_left);
    ffound=ffound/(double)(count_left);
    f0_left=44100.0/ffound;
    ffound=(double)(last_right-first_right);
    ffound=ffound/(double)(count_right);
    f0_right=44100.0/ffound;
    printf("Frequencies %6.2lf Hz Left  %6.2lf Right\n",f0_left,f0_right);    
    if(rms_left>rms_right)
    {
      ffound=f0_left;
      printf("Left chosen\n");
    }
    else
    {
      ffound=f0_right;
      printf("Right chosen\n");
    }
  rel=costrans(ffound,left);  rel0=rel;
  iml=sintrans(ffound,left);  iml0=iml;
  al=2.0*sqrt(rel*rel+iml*iml);
  hleft[0]=al;
  phi_left=atan2(iml,rel);
  printf("Left a = %8.4lf phase = %8.4lf\n",al,phi_left);
  rer=costrans(ffound,right);  rer0=rer;
  imr=sintrans(ffound,right);  imr0=imr;
  ar=2.0*sqrt(rer*rer+imr*imr);
  hright[0]=ar;
  phi_right=atan2(imr,rer);
  printf("Right a = %8.4lf phase = %8.4lf\n",ar,phi_right);
  return ffound;
}

    
double costrans(double fin,double* array)
{
  /* This procedure does the cosine transform of
     the data at frequency 'fin' for the selected channel. */
  
  int ii;
  double phi,w,answer;
  t=0.0;  answer=0.0;
  ii=0;
  w=pi2*fin;
  do
  {
    phi=w*t;
    answer+=array[ii]*cos(phi);
    t+=dt;
    ii++;
  } while (ii<2205);
  answer=answer/2205.0;
  return answer;
}  
  
double sintrans(double fin,double* array)
{
  /* This procedure dies the sine transform of
     the data at the frequency 'fin' for the selected channel. */
  
  int ii;
  double phi,w,answer;
  t=0.0;  answer=0.0;
  ii=0;
  w=pi2*fin;
  do
  {
    phi=w*t;
    answer+=array[ii]*sin(phi);
    t+=dt;
    ii++;
  } while (ii<2205);
  answer=answer/2205.0;
  return answer;
}  

void check_data(void)
{
  /* This procedure prints out a file of the values obtained when
     the sound data was converted into real values. This is so
     you can examine the values to check them. */
     
  int ii;
  t=0.0;
  ii=0;
  sprintf(outfilename,"ram:waves_%s\0",leafname);
  outfile=fopen(outfilename,"wt");
  
  do
  {
    fprintf(outfile,"%8.4lf, %9.6lf, %9.6lf\n",t*1000.0, left[ii], right[ii]);
    ii++;
    t+=dt;
  } while (ii<2205);
  
  fclose(outfile);
}


void load_data_block(void)
{
  
  /* This procedure finds and loads the selected block of sample
     data and then converts the raw data into a set of real sample
     values for analysis. */
  
  int block_size_read;
  int icut,icut2;
  int io,ii,lt,rt;
  double samscale;
  double dc_left,dc_right;
  
  icut=32768; icut2=icut*2;
  samscale=1.0/(double)icut;
  dc_left=0.0; dc_right=0.0;
  rms_left=0.0; rms_right=0.0;
  
  infile=fopen(infilename,"rd");
  fseek(infile,offset,SEEK_SET);
  
  block_size_read=fread(datablock,sizeof(char),8820,infile);
  
  fclose(infile);
  
  ii=0; io=0;
  
  do
  {
    lt=  (int)datablock[ii+1] + 256*(int)datablock[ii];
    ii+=2;
    rt= (int)datablock[ii+1] + 256*(int)datablock[ii];
    ii+=2;
    if (lt>=icut) lt-=icut2;
    if (rt>=icut) rt-=icut2;
    left[io]=samscale*(double)lt;
    right[io]=samscale*(double)rt;
    dc_left+=left[io];
    dc_right+=right[io];
    
    rms_right+=right[io]*right[io];
    rms_left+=left[io]*left[io];
    
    io++;
  } while (io<2205);
  
  dc_right=dc_right/2205.0;
  dc_left=dc_left/2205.0;
  rms_left=rms_left/2205.0;
  rms_right=rms_right/2205.0;
  rms_left=sqrt(rms_left);
  rms_right=sqrt(rms_right);
  
  printf("\nFor section from %8.2lf to %8.2lf ms",tstart*1000.0,tstart*1000.0+50.0);
  
  printf("\nMean (dc) levels\n   left %8.5lf  right  %8.5lf\n",dc_left,dc_right);
  printf("\nRMS levels\n   left %8.5lf  right  %8.5lf\n",rms_left,rms_right);
  
}



int get_start_time(void)
{
  /* This procedure asks the user what time (in seconds from the
     start of the file) is the beginning of the chunk to be loaded
     and analysed. */
  
  int ir;
  ir=0;
  printf("Enter start time of section to analyse [seconds]\n   => ");
  
  scanf("%lf",&tstart);
  
  printf("\nRequested start time %7.2lf\n",tstart);
  
  if (tstart>(total_time-0.05))
  {
    ir=-1;
    printf("\nTime requested goes beyond end of file!\n");
  }
  
  if (tstart<0.0)
  {
    ir=-2;
    printf("\nTime requested before file starts!\n");
  }
  
  if (ir==0)
  {
    ir=(int)(4.0*44100.0*tstart);
  }
  return ir;
}



void read_path_file(void)
{
  
  /* This proceedure reads the 'path' file and sets the path
     so that the input files and the edit list can be found. */
  
  printf("\nTrackTHD V1.01  12 Feb 2007\n");
  FILE *pathfile; 
  pathfile=fopen("<Obey$Dir>.Path\0","rt");  
  fscanf(pathfile,"%s",pathname); 
  printf("\nTrackTHD file dir = %s\n",pathname);
  fclose(pathfile);
}



double check_inputfile(void)
{
  
  /* This procedure checks that the named input file exists
     and if it does, reports its duration */
  
  int ilb;
  double flow;
  
  rin.r[0]=5;
  rin.r[1]=(int)infilename;
  _kernel_swi(OS_File,&rin,&rout);
  if(rout.r[0]==1)
  {
    ilb=rout.r[4];
    
    flow=(double)ilb;
    flow=flow/(44100.0*4.0);
    
    
    
    ilb=ilb/(44100*4);
    
    printf("Input file %5.1lf seconds long\n",flow);
  }
  else
  {
    ilb=0;
    flow=0.0;
    printf("Input file does not exist!\n");
  }
  return flow;
}


