ghost/monty/sinusoid/test_sinusoids.c

   1 #define _GNU_SOURCE
   2 #include <math.h>
   3 #include <stdio.h>
   4 #include <stdlib.h>
   5 #include <string.h>
   6 #include "sinusoids.h"
   7 #include "smallft.h"
   8 #include "scales.h"
   9
  10 #define BLOCK_SIZE 1024
  11
  12 static float float_in[BLOCK_SIZE*2];
  13 static float float_out[262144];
  14 static short short_in[BLOCK_SIZE];
  15 static int outcount=0;
  16
  17 static void dump_vec(float *data, int n, char *base, int i){
  18   char *filename;
  19   FILE *f;
  20
  21   asprintf(&filename,"%s_%d.m",base,i);
  22   f=fopen(filename,"w");
  23
  24   for(i=0;i<n;i++)
  25     fprintf(f,"%f %f\n",
  26             (float)i/n,data[i]);
  27
  28   fclose(f);
  29
  30 }
  31
  32 static void dump_vec2(float *x, float *y, int n, char *base, int i){
  33   char *filename;
  34   FILE *f;
  35
  36   asprintf(&filename,"%s_%d.m",base,i);
  37   f=fopen(filename,"w");
  38
  39   for(i=0;i<n;i++){
  40     fprintf(f,"%f -120\n",
  41             x[i]);
  42     fprintf(f,"%f %f\n\n",
  43             x[i],y[i]);
  44   }
  45
  46   fclose(f);
  47
  48 }
  49
  50 #define A0 .35875f
  51 #define A1 .48829f
  52 #define A2 .14128f
  53 #define A3 .01168f
  54
  55 void blackmann_harris(float *out, float *in, int n){
  56   int i;
  57   float scale = 2*M_PI/n;
  58
  59   for(i=0;i<n;i++){
  60     float i5 = i+.5;
  61     float w = A0 - A1*cos(scale*i5) + A2*cos(scale*i5*2) - A3*cos(scale*i5*3);
  62     out[i] = in[i]*w;
  63   }
  64 }
  65
  66 static void hanning(float *out, float *in, int n){
  67   int i;
  68   float scale = 2*M_PI/n;
  69
  70   for(i=0;i<n;i++){
  71     float i5 = i+.5;
  72     out[i] = in[i]*(.5-.5*cos(scale*i5));
  73   }
  74 }
  75
  76 static void hanningW(float *out, int n){
  77   int i;
  78   float scale = 2*M_PI/n;
  79
  80   for(i=0;i<n;i++){
  81     float i5 = i+.5;
  82     out[i] = (.5-.5*cos(scale*i5));
  83   }
  84 }
  85
  86
  87 void mag_dB(float *log,float *d, int n){
  88   int i;
  89   log[0] = todB(d[0]*d[0])*.5;
  90   for(i=2;i<n;i+=2)
  91     log[i>>1] = todB(d[i-1]*d[i-1] + d[i]*d[i])*.5;
  92   log[n/2] = todB(d[n-1]*d[n-1])*.5;
  93 }
  94
  95 int main(int argc, char **argv){
  96   FILE *fin, *fout;
  97   int i,frame=0;
  98   drft_lookup fft;
  99
 100   drft_init(&fft, BLOCK_SIZE*2);
 101
 102   if (argc != 3){
 103     fprintf (stderr, "usage: testghost input_file output_file\nWhere the input and output are raw mono files sampled at 44.1 kHz or 48 kHz\n");
 104     exit(1);
 105   }
 106
 107   fin = fopen(argv[1], "r");
 108   fout = fopen(argv[2], "w");
 109
 110   /*
 111     for(i=0;i<BLOCK_SIZE*2;i++)
 112     float_in[i]=1.;
 113     blackmann_harris(float_in, float_in, BLOCK_SIZE*2);
 114     dump_vec(float_in,BLOCK_SIZE*2,"window",0);
 115     memset(float_in,0,sizeof(float_in));
 116   */
 117
 118   while (1){
 119     int i,j;
 120     memmove(float_in,float_in+BLOCK_SIZE,sizeof(*float_in)*BLOCK_SIZE);
 121     fread(short_in, sizeof(short), BLOCK_SIZE, fin);
 122
 123     if (feof(fin))
 124       break;
 125     for (i=0;i<BLOCK_SIZE;i++)
 126       float_in[i+BLOCK_SIZE] = short_in[i] * .000030517578125;
 127
 128     if(frame==220){
 129
 130       /* generate a log spectrum */
 131       float fft_buf[BLOCK_SIZE*2];
 132       float log_fft[BLOCK_SIZE+1];
 133       float weight[BLOCK_SIZE+1];
 134
 135       // polish the strongest peaks from weighting
 136       float w[BLOCK_SIZE];
 137       float Wout[BLOCK_SIZE];
 138       float Aout[BLOCK_SIZE]={0};
 139       float Pout[BLOCK_SIZE]={0};
 140       float dAout[BLOCK_SIZE]={0};
 141       float ddAout[BLOCK_SIZE]={0};
 142       float dWout[BLOCK_SIZE]={0};
 143       float y[BLOCK_SIZE*2];
 144       float window[BLOCK_SIZE*2];
 145
 146       hanning(fft_buf, float_in, BLOCK_SIZE*2);
 147       dump_vec(float_in,BLOCK_SIZE*2,"data",frame);
 148       drft_forward(&fft, fft_buf);
 149       for(i=0;i<BLOCK_SIZE*2;i++)fft_buf[i] *= 1./BLOCK_SIZE;
 150
 151       mag_dB(log_fft,fft_buf,BLOCK_SIZE*2);
 152       dump_vec(log_fft,BLOCK_SIZE+1,"logmag",frame);
 153
 154       window_weight(log_fft,weight,BLOCK_SIZE+1, 0.f, 512,256, 30, 44100);
 155       dump_vec(weight,BLOCK_SIZE+1,"weight",frame);
 156
 157       j=2;
 158       w[0]=.0044*BLOCK_SIZE;
 159       w[1]=.136*BLOCK_SIZE;
 160
 161       /*int j,k;
 162         for(j=0;j<20;j++){
 163         int best=-120;
 164         int besti=-1;
 165         for(i=0;i<BLOCK_SIZE+1;i++){
 166           float v = log_fft[i] - weight[i];
 167           if(v>best){
 168             for(k=0;k<j;k++)
 169               if(i == w[j])break;
 170             if(k==j){
 171               besti = i;
 172               best = v;
 173             }
 174           }
 175         }
 176         w[j] = besti;
 177         }*/
 178
 179
 180       extract_sinusoids(float_in, Aout, w, Pout, dAout, dWout, y, j, BLOCK_SIZE*2, 200);
 181       /*
 182         extract_modulated_sinusoidsB(float_in, w, Aout, Wout, Pout, dAout, dWout, ddAout, y, j, BLOCK_SIZE*2);
 183         for(i=0;i<j;i++)
 184           w[i]=Wout[i];
 185         extract_modulated_sinusoidsB(float_in, w, Aout, Wout, Pout, dAout, dWout, ddAout, y, j, BLOCK_SIZE*2);
 186         for(i=0;i<j;i++)
 187           w[i]=Wout[i];
 188         extract_modulated_sinusoidsB(float_in, w, Aout, Wout, Pout, dAout, dWout, ddAout, y, j, BLOCK_SIZE*2);
 189         for(i=0;i<j;i++)
 190           w[i]=Wout[i];
 191         extract_modulated_sinusoidsB(float_in, w, Aout, Wout, Pout, dAout, dWout, ddAout, y, j, BLOCK_SIZE*2);
 192         for(i=0;i<j;i++)
 193           w[i]=Wout[i];
 194       */
 195
 196
 197       for(i=0;i<j;i++)
 198         fprintf(stdout, "%d %f\n\n",frame,w[i]/BLOCK_SIZE*22050);
 199
 200
 201       for(i=0;i<BLOCK_SIZE*2;i++)
 202         fft_buf[i] = float_in[i]-y[i];
 203
 204       dump_vec(fft_buf,BLOCK_SIZE*2,"res",0);
 205
 206       hanning(fft_buf, fft_buf, BLOCK_SIZE*2);
 207       drft_forward(&fft, fft_buf);
 208       for(i=0;i<BLOCK_SIZE*2;i++)fft_buf[i] *= 1./BLOCK_SIZE;
 209       mag_dB(log_fft,fft_buf,BLOCK_SIZE*2);
 210
 211       dump_vec(log_fft,BLOCK_SIZE+1,"exlogmag",0);
 212
 213       for(i=0;i<j;i++){
 214         Aout[i]=todB(Aout[i]);
 215         dAout[i]=todB(dAout[i]);
 216         w[i] /= BLOCK_SIZE;
 217       }
 218
 219       dump_vec2(w,Aout,j,"ex",0);
 220       dump_vec2(w,dAout,j,"dA",0);
 221       dump_vec2(w,dWout,j,"dW",0);
 222       dump_vec(y,BLOCK_SIZE*2,"extract",0);
 223
 224       //for (i=0;i<BLOCK_SIZE*2;i++)
 225       //  float_out[i+outcount] += fft_buf[i];
 226       //outcount+=BLOCK_SIZE;
 227
 228       //if(outcount+BLOCK_SIZE*2>262144){
 229
 230       //          hanning(float_out, float_out, 262144);
 231       //drft_init(&fft, 262144);
 232       //drft_forward(&fft, float_out);
 233       //for(i=0;i<262144;i++)float_out[i] *= 1./262144;
 234
 235       //mag_dB(float_out,float_out,262144);
 236       //dump_vec(float_out,262144/2,"res",0);
 237       //exit(0);
 238
 239       //}
 240         //}
 241
 242     }
 243
 244
 245
 246     //fwrite(short_in, sizeof(short), BLOCK_SIZE, fout);
 247     frame++;
 248   }
 249
 250   return 0;
 251 }
 252