test/model.c

   1 /*
   2         model.c -- program to help test DSP filter and rate-conversion code.
   3
   4         Copyright (C) 1999 Stanley J. Brooks <stabro@megsinet.net>
   5
   6   This program is free software; you can redistribute it and/or modify
   7   it under the terms of the GNU General Public License as published by
   8   the Free Software Foundation; either version 2 of the License, or
   9   (at your option) any later version.
  10
  11   This program is distributed in the hope that it will be useful,
  12   but WITHOUT ANY WARRANTY; without even the implied warranty of
  13   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14   GNU General Public License for more details.
  15
  16   You should have received a copy of the GNU General Public License
  17   along with this program; if not, write to the Free Software
  18   Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  19
  20 */
  21
  22 #include <string.h>
  23 // for open,read,write:
  24 #include <sys/types.h>
  25 #include <sys/stat.h>
  26 #include <fcntl.h>
  27 #include <errno.h>
  28 #include <unistd.h>
  29 #include <math.h>
  30 #include <stdio.h>
  31 #include <stdlib.h>
  32
  33 #define FLOAT double
  34
  35 #ifndef LSAMPL
  36 #       define SAMPL short
  37 #       define MAXSAMPL 0x7fff
  38 #       define MINSAMPL -0x8000
  39 #else
  40 #       define SAMPL long
  41 #       define MAXSAMPL 0x7fffff
  42 #       define MINSAMPL -0x800000
  43 #endif
  44
  45 static struct _Env {
  46         int r;    /* rise   */
  47         int c;    /* center */
  48         int f;    /* fall   */
  49         double v; /* volume */
  50         double d; /* decay */
  51 } Env = {0,0,0,0.5,1.0};
  52
  53 static void Usage(void)__attribute__((noreturn));
  54
  55 static void Usage(void)
  56 {
  57         fprintf(stderr,"Usage: ./model rate0,rate1 [-l len] [-f freq] <in-file>\n"); exit(-1);
  58 }
  59
  60 static int ReadN(int fd, SAMPL *v, int n)
  61 {
  62         int r;
  63
  64         do {
  65                 r = read(fd, (char*)(v), n*sizeof(SAMPL));
  66         }while(r==-1 && errno==EINTR);
  67         if (r==-1 || r%sizeof(SAMPL)) {
  68                 perror("Error reading input"); exit(-1);
  69         }
  70         return r/sizeof(SAMPL);
  71 }
  72
  73 #define BSIZ 400000/*0x10000*/
  74
  75 static double bestfit(double sx, double sy,
  76                                                                       double h11, double h12, double h22,
  77                                                                                         double *cx, double *cy)
  78 {
  79         double a,su,uu,cu,w2;
  80
  81         a=0; *cy=0;
  82         if (h22>1e-20*h11) {
  83                 a = h12/h22;  /* u[] = x[] - a*y[] is orthogonal to y[] */
  84         *cy = sy/h22; /* *cy*y[] is projection of s[] onto y[]  */
  85         }
  86         /* <s,x-ay> = sx -a*sy */
  87         su = sx - a*sy; /* su is iprod of s[] and u[] */
  88         /* <u,u> = <x-ay,x-ay> = h11 - 2a*h11 + a*a*h22 */
  89         uu = h11 - 2*a*h12 + a*a*h22;
  90         cu = 0;
  91         if (uu>1e-20*h11)
  92                 cu = su/uu;  /* s[] = *cy * y[] + cu * u[] is orthogonal decomposition */
  93         w2  =  *cy * *cy * h22;
  94         w2 +=   cu *  cu *  uu;
  95         *cx = cu;
  96         *cy -= a*cu;
  97         return w2;
  98 }
  99
 100 static double eCenter(const SAMPL *ibuff, int len)
 101 {
 102         double v0,v1,v2;
 103         int n;
 104
 105         v0 = v1 = v2 = 0;
 106         for (n=0; n<len; n++) {
 107                 double w = ibuff[n];
 108                 w = w*w;
 109                 v0 += w;
 110                 v1 += n*w;
 111                 v2 += (n*n)*w;
 112         }
 113         v1 /= v0;
 114         v2 /= v0;
 115         v2 -= v1*v1;
 116         fprintf(stderr,"eCenter %.2f,  STD %.2f\n",v1,sqrt(v2));
 117         return v1;
 118 }
 119
 120 static void
 121 bigcalc(double Factor, double Freq1, const SAMPL *ibuff, int len)
 122 {
 123         double c,del;
 124         double x,y;
 125         double thx,thy;
 126         double Len1;
 127         int k1,n;
 128         long double h11,h12,h22;
 129         long double sx,sy,ss;
 130         double cn,cx,cy;
 131         double s2=0, v2=0;
 132         const SAMPL *ip;
 133
 134         inline void a_init(double frq)
 135         {
 136                 x = 1;
 137                 y = 0;
 138                 thx = cos(frq*M_PI);
 139                 thy = sin(frq*M_PI);
 140         }
 141
 142         inline double a(double k, double L)
 143         {
 144                 double a, l, u;
 145                 l = L/2.0;
 146                 u = k/l - 1; /* in (-1,+1) */
 147                 a = 0.5*(1 + cos(M_PI * u));
 148                 return a;
 149         }
 150
 151         inline void a_post(int k)
 152         {
 153                 double x1;
 154                 x1 = x*thx - y*thy;
 155                 y  = x*thy + y*thx;
 156                 x = x1;
 157                 /* update (x,y) for next tic */
 158                 if ((k&0x0f)==0x0f) {  /* norm correction each 16 samples */
 159                         x1 = 1/sqrt(x*x+y*y);
 160                         x *= x1;
 161                         y *= x1;
 162                 }
 163         }
 164
 165         c = eCenter(ibuff,len);
 166         Len1 = Env.c*Factor*0.6; /* 60% of original const-amplitude area */
 167         c += Env.c*Factor*0.15;  /* beginning after 30% */
 168         {
 169                 double a;
 170                 int b;
 171                 a = c-Len1/2;
 172                 b = ceil(a-0.001);
 173                 ip = ibuff + b;
 174                 del = b-a;
 175         }
 176         fprintf(stderr,"del %.3f\n", del);
 177         k1 = Len1-del;
 178
 179         a_init(Freq1);
 180         h11 = h12 = h22 = 0;
 181         sx = sy = ss = 0;
 182         for(n=0; n<=k1; n++) {
 183                 double f,s;
 184                 double u,v;
 185
 186                 s = ip[n];    /* sigval at n      */
 187
 188                 f = 1; //a(n+del,Len1);
 189                 u = f*x; v = f*y;
 190                 sx += s*u;
 191                 sy += s*v;
 192                 ss += s*s;
 193                 h11 += u*u;
 194                 h12 += u*v;
 195                 h22 += v*v;
 196
 197                 a_post(n);
 198         }
 199         //fprintf(stderr,"h12 %.10f\n", (double)h12/(sqrt(h11)*sqrt(h22)));
 200
 201         v2 = ss/(k1+1);
 202         s2 = bestfit(sx,sy,h11,h12,h22,&cx,&cy)/(k1+1);
 203         cn = sqrt(cx*cx+cy*cy);
 204         fprintf(stderr,"amp %.1f, cx %.10f, cy %.10f\n", cn, cx/cn, cy/cn);
 205
 206         fprintf(stderr,"[%.1f,%.1f]  s2max %.2f, v2max %.2f, rmserr %.2f\n",
 207                                   c/Factor, c, sqrt(s2), sqrt(v2), (s2<=v2)? sqrt(v2-s2):-sqrt(s2-v2));
 208
 209 }
 210
 211 int main(int argct, char **argv)
 212 {
 213         int optc;
 214         int fd1;
 215         int len;
 216         SAMPL *ibuff;
 217         int rate0, rate1;
 218         double Freq0=0, Freq1;   /* of nyquist */
 219         double Factor;
 220         char *fnam1;
 221
 222          /* Parse the options */
 223         while ((optc = getopt(argct, argv, "d:e:f:h")) != -1) {
 224                 char *p;
 225                 switch(optc) {
 226                         case 'd':
 227                                 Env.d = strtod(optarg,&p);
 228                                 if (p==optarg || *p) {
 229                                         fprintf(stderr,"option -%c expects float value (%s)\n", optc, optarg);
 230                                         Usage();
 231                                 }
 232                                 break;
 233                         case 'f':
 234                                 Freq0 = strtod(optarg,&p);
 235                                 if (p==optarg || *p) {
 236                                         fprintf(stderr,"option -%c expects float value (%s)\n", optc, optarg);
 237                                         Usage();
 238                                 }
 239                                 break;
 240                         case 'e':
 241                                 p = optarg;
 242                                 Env.c  = strtol(p,&p,10);
 243                                 if (*p++ == ':') {
 244                                         Env.r = Env.f = Env.c;
 245                                         Env.c = strtol(p,&p,10);
 246                                 }
 247                                 if (*p++ == ':') {
 248                                         Env.f = strtol(p,&p,10);
 249                                 }
 250                                 if (*p || Env.c<=0 || Env.r<0 || Env.f<0) {
 251                                         fprintf(stderr,"option -%c not valid (%s)\n", optc, optarg);
 252                                         Usage();
 253                                 }
 254                                 break;
 255                         case 'h':
 256                         default:
 257                                 Usage();
 258                 }
 259         }
 260
 261         //fprintf(stderr,"optind=%d argct=%d\n",optind,argct);
 262
 263         if (optind != argct-2) Usage();
 264
 265         {
 266                 char *p0, *p;
 267                 p0 = argv[optind];
 268                 rate0 = rate1 = strtol(p0,&p,10);
 269                 if (*p) {
 270                         if (*p != ':') {
 271                                 fprintf(stderr,"Invalid rate parameter (%s)\n", p0);
 272                                 Usage();
 273                         }
 274                         p0 = p+1;
 275                         rate1 = strtol(p0,&p,10);
 276                         if (*p) {
 277                                 fprintf(stderr,"Invalid 2nd rate parameter (%s)\n", p0);
 278                                 Usage();
 279                         }
 280                 }
 281                 if (rate0<=0 || rate1<=0) {
 282                         fprintf(stderr,"Invalid rate parameter (%s)\n", argv[optind]);
 283                         Usage();
 284                 }
 285                 optind++;
 286         }
 287
 288         Factor = (double)rate1 / (double)rate0;
 289         Freq1 = Freq0/Factor;
 290
 291         //if (optind != argct-1) Usage();
 292
 293         fnam1=NULL; fd1=-1;
 294         fnam1=argv[optind++];
 295         fd1=open(fnam1,O_RDONLY);
 296         if (fd1<0) {
 297                 fprintf(stderr,"Open: %s %s\n",fnam1,strerror(errno)); return(1);
 298         }
 299
 300         //fprintf(stderr, "Files: %s %s\n",fnam1,fnam2);
 301
 302         ibuff=(SAMPL*)malloc(BSIZ*sizeof(SAMPL));
 303         {
 304                 int n,r;
 305
 306                 for (n=0; n<BSIZ; n+=r) {
 307                         r = ReadN(fd1,ibuff+n,BSIZ-n);
 308                         if (r==0) break;
 309                 }
 310                 len = n;
 311         }
 312         fprintf(stderr,"Read %d input samples\n",len);
 313
 314         bigcalc(Factor, Freq1, ibuff, len);
 315
 316         return 0;
 317
 318 }