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
Usage(void)55 static void Usage(void)
56 {
57 fprintf(stderr,"Usage: ./model rate0,rate1 [-l len] [-f freq] <in-file>\n"); exit(-1);
58 }
59
ReadN(int fd,SAMPL * v,int n)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
bestfit(double sx,double sy,double h11,double h12,double h22,double * cx,double * cy)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
eCenter(const SAMPL * ibuff,int len)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
bigcalc(double Factor,double Freq1,const SAMPL * ibuff,int len)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
main(int argct,char ** argv)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 }
319