ref: 83a3a0a316e27909c6f31655bc7c8c8a2585e2b1
dir: /test/model.c/
/* model.c -- program to help test DSP filter and rate-conversion code. Copyright (C) 1999 Stanley J. Brooks <stabro@megsinet.net> 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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include <string.h> // for open,read,write: #include <sys/types.h> #include <sys/stat.h> #include <fcntl.h> #include <errno.h> #include <unistd.h> #include <math.h> #include <stdio.h> #include <stdlib.h> #define FLOAT double #ifndef LSAMPL # define SAMPL short # define MAXSAMPL 0x7fff # define MINSAMPL -0x8000 #else # define SAMPL long # define MAXSAMPL 0x7fffff # define MINSAMPL -0x800000 #endif static struct _Env { int r; /* rise */ int c; /* center */ int f; /* fall */ double v; /* volume */ double d; /* decay */ } Env = {0,0,0,0.5,1.0}; static void Usage(void)__attribute__((noreturn)); static void Usage(void) { fprintf(stderr,"Usage: ./model rate0,rate1 [-l len] [-f freq] <in-file>\n"); exit(-1); } static int ReadN(int fd, SAMPL *v, int n) { int r; do { r = read(fd, (char*)(v), n*sizeof(SAMPL)); }while(r==-1 && errno==EINTR); if (r==-1 || r%sizeof(SAMPL)) { perror("Error reading input"); exit(-1); } return r/sizeof(SAMPL); } #define BSIZ 400000/*0x10000*/ static double bestfit(double sx, double sy, double h11, double h12, double h22, double *cx, double *cy) { double a,su,uu,cu,w2; a=0; *cy=0; if (h22>1e-20*h11) { a = h12/h22; /* u[] = x[] - a*y[] is orthogonal to y[] */ *cy = sy/h22; /* *cy*y[] is projection of s[] onto y[] */ } /* <s,x-ay> = sx -a*sy */ su = sx - a*sy; /* su is iprod of s[] and u[] */ /* <u,u> = <x-ay,x-ay> = h11 - 2a*h11 + a*a*h22 */ uu = h11 - 2*a*h12 + a*a*h22; cu = 0; if (uu>1e-20*h11) cu = su/uu; /* s[] = *cy * y[] + cu * u[] is orthogonal decomposition */ w2 = *cy * *cy * h22; w2 += cu * cu * uu; *cx = cu; *cy -= a*cu; return w2; } static double eCenter(const SAMPL *ibuff, int len) { double v0,v1,v2; int n; v0 = v1 = v2 = 0; for (n=0; n<len; n++) { double w = ibuff[n]; w = w*w; v0 += w; v1 += n*w; v2 += (n*n)*w; } v1 /= v0; v2 /= v0; v2 -= v1*v1; fprintf(stderr,"eCenter %.2f, STD %.2f\n",v1,sqrt(v2)); return v1; } static void bigcalc(double Factor, double Freq1, const SAMPL *ibuff, int len) { double c,del; double x,y; double thx,thy; double Len1; int k1,n; long double h11,h12,h22; long double sx,sy,ss; double cn,cx,cy; double s2=0, v2=0; const SAMPL *ip; static inline void a_init(double frq) { x = 1; y = 0; thx = cos(frq*M_PI); thy = sin(frq*M_PI); } static inline const double a(double k, double L) { double a, l, u; l = L/2.0; u = k/l - 1; /* in (-1,+1) */ a = 0.5*(1 + cos(M_PI * u)); return a; } static inline void a_post(int k) { double x1; x1 = x*thx - y*thy; y = x*thy + y*thx; x = x1; /* update (x,y) for next tic */ if ((k&0x0f)==0x0f) { /* norm correction each 16 samples */ x1 = 1/sqrt(x*x+y*y); x *= x1; y *= x1; } } c = eCenter(ibuff,len); Len1 = Env.c*Factor*0.6; /* 60% of original const-amplitude area */ c += Env.c*Factor*0.15; /* beginning after 30% */ { double a; int b; a = c-Len1/2; b = ceil(a-0.001); ip = ibuff + b; del = b-a; } fprintf(stderr,"del %.3f\n", del); k1 = Len1-del; a_init(Freq1); h11 = h12 = h22 = 0; sx = sy = ss = 0; for(n=0; n<=k1; n++) { double f,s; double u,v; s = ip[n]; /* sigval at n */ f = 1; //a(n+del,Len1); u = f*x; v = f*y; sx += s*u; sy += s*v; ss += s*s; h11 += u*u; h12 += u*v; h22 += v*v; a_post(n); } //fprintf(stderr,"h12 %.10f\n", (double)h12/(sqrt(h11)*sqrt(h22))); v2 = ss/(k1+1); s2 = bestfit(sx,sy,h11,h12,h22,&cx,&cy)/(k1+1); cn = sqrt(cx*cx+cy*cy); fprintf(stderr,"amp %.1f, cx %.10f, cy %.10f\n", cn, cx/cn, cy/cn); fprintf(stderr,"[%.1f,%.1f] s2max %.2f, v2max %.2f, rmserr %.2f\n", c/Factor, c, sqrt(s2), sqrt(v2), (s2<=v2)? sqrt(v2-s2):-sqrt(s2-v2)); } int main(int argct, char **argv) { int optc; int fd1; int len; SAMPL *ibuff; int rate0, rate1; double Freq0=0, Freq1; /* of nyquist */ double Factor; char *fnam1; /* Parse the options */ while ((optc = getopt(argct, argv, "d:e:f:h")) != -1) { char *p; switch(optc) { case 'd': Env.d = strtod(optarg,&p); if (p==optarg || *p) { fprintf(stderr,"option -%c expects float value (%s)\n", optc, optarg); Usage(); } break; case 'f': Freq0 = strtod(optarg,&p); if (p==optarg || *p) { fprintf(stderr,"option -%c expects float value (%s)\n", optc, optarg); Usage(); } break; case 'e': p = optarg; Env.c = strtol(p,&p,10); if (*p++ == ':') { Env.r = Env.f = Env.c; Env.c = strtol(p,&p,10); } if (*p++ == ':') { Env.f = strtol(p,&p,10); } if (*p || Env.c<=0 || Env.r<0 || Env.f<0) { fprintf(stderr,"option -%c not valid (%s)\n", optc, optarg); Usage(); } break; case 'h': default: Usage(); } } //fprintf(stderr,"optind=%d argct=%d\n",optind,argct); if (optind != argct-2) Usage(); { char *p0, *p; p0 = argv[optind]; rate0 = rate1 = strtol(p0,&p,10); if (*p) { if (*p != ':') { fprintf(stderr,"Invalid rate parameter (%s)\n", p0); Usage(); } p0 = p+1; rate1 = strtol(p0,&p,10); if (*p) { fprintf(stderr,"Invalid 2nd rate parameter (%s)\n", p0); Usage(); } } if (rate0<=0 || rate1<=0) { fprintf(stderr,"Invalid rate parameter (%s)\n", argv[optind]); Usage(); } optind++; } Factor = (double)rate1 / (double)rate0; Freq1 = Freq0/Factor; //if (optind != argct-1) Usage(); fnam1=NULL; fd1=-1; fnam1=argv[optind++]; fd1=open(fnam1,O_RDONLY); if (fd1<0) { fprintf(stderr,"Open: %s %s\n",fnam1,strerror(errno)); return(1); } //fprintf(stderr, "Files: %s %s\n",fnam1,fnam2); ibuff=(SAMPL*)malloc(BSIZ*sizeof(SAMPL)); { int n,r; for (n=0; n<BSIZ; n+=r) { r = ReadN(fd1,ibuff+n,BSIZ-n); if (r==0) break; } len = n; } fprintf(stderr,"Read %d input samples\n",len); bigcalc(Factor, Freq1, ibuff, len); return 0; }