1 /*
2 Copyright (c) 2018, Rafat Hussain
3 */
4 #include "wtmath.h"
5
dwt_per_stride(double * inp,int N,double * lpd,double * hpd,int lpd_len,double * cA,int len_cA,double * cD,int istride,int ostride)6 void dwt_per_stride(double *inp, int N, double *lpd,double*hpd,int lpd_len,double *cA, int len_cA, double *cD, int istride, int ostride) {
7 int l, l2, isodd, i, t, len_avg,is,os;
8
9 len_avg = lpd_len;
10 l2 = len_avg / 2;
11 isodd = N % 2;
12
13 for (i = 0; i < len_cA; ++i) {
14 t = 2 * i + l2;
15 os = i *ostride;
16 cA[os] = 0.0;
17 cD[os] = 0.0;
18 for (l = 0; l < len_avg; ++l) {
19 if ((t - l) >= l2 && (t - l) < N) {
20 is = (t - l) * istride;
21 cA[os] += lpd[l] * inp[is];
22 cD[os] += hpd[l] * inp[is];
23 }
24 else if ((t - l) < l2 && (t - l) >= 0) {
25 is = (t - l) * istride;
26 cA[os] += lpd[l] * inp[is];
27 cD[os] += hpd[l] * inp[is];
28 }
29 else if ((t - l) < 0 && isodd == 0) {
30 is = (t - l + N) * istride;
31 cA[os] += lpd[l] * inp[is];
32 cD[os] += hpd[l] * inp[is];
33 }
34 else if ((t - l) < 0 && isodd == 1) {
35 if ((t - l) != -1) {
36 is = (t - l + N + 1) * istride;
37 cA[os] += lpd[l] * inp[is];
38 cD[os] += hpd[l] * inp[is];
39 }
40 else {
41 is = (N - 1) * istride;
42 cA[os] += lpd[l] * inp[is];
43 cD[os] += hpd[l] * inp[is];
44 }
45 }
46 else if ((t - l) >= N && isodd == 0) {
47 is = (t - l - N) * istride;
48 cA[os] += lpd[l] * inp[is];
49 cD[os] += hpd[l] * inp[is];
50 }
51 else if ((t - l) >= N && isodd == 1) {
52 is = (t - l - (N + 1)) * istride;
53 if (t - l != N) {
54 cA[os] += lpd[l] * inp[is];
55 cD[os] += hpd[l] * inp[is];
56 }
57 else {
58 is = (N - 1) * istride;
59 cA[os] += lpd[l] * inp[is];
60 cD[os] += hpd[l] * inp[is];
61 }
62 }
63
64 }
65
66 }
67
68 }
69
dwt_sym_stride(double * inp,int N,double * lpd,double * hpd,int lpd_len,double * cA,int len_cA,double * cD,int istride,int ostride)70 void dwt_sym_stride(double *inp, int N, double *lpd, double*hpd, int lpd_len, double *cA, int len_cA, double *cD, int istride, int ostride) {
71 int i, l, t, len_avg;
72 int is, os;
73 len_avg = lpd_len;
74
75 for (i = 0; i < len_cA; ++i) {
76 t = 2 * i + 1;
77 os = i *ostride;
78 cA[os] = 0.0;
79 cD[os] = 0.0;
80 for (l = 0; l < len_avg; ++l) {
81 if ((t - l) >= 0 && (t - l) < N) {
82 is = (t - l) * istride;
83 cA[os] += lpd[l] * inp[is];
84 cD[os] += hpd[l] * inp[is];
85 }
86 else if ((t - l) < 0) {
87 is = (-t + l - 1) * istride;
88 cA[os] += lpd[l] * inp[is];
89 cD[os] += hpd[l] * inp[is];
90 }
91 else if ((t - l) >= N) {
92 is = (2 * N - t + l - 1) * istride;
93 cA[os] += lpd[l] * inp[is];
94 cD[os] += hpd[l] * inp[is];
95 }
96
97 }
98 }
99
100
101 }
102
modwt_per_stride(int M,double * inp,int N,double * filt,int lpd_len,double * cA,int len_cA,double * cD,int istride,int ostride)103 void modwt_per_stride(int M, double *inp, int N, double *filt, int lpd_len, double *cA, int len_cA, double *cD, int istride, int ostride) {
104 int l, i, t, len_avg;
105 int is, os;
106 len_avg = lpd_len;
107
108
109 for (i = 0; i < len_cA; ++i) {
110 t = i;
111 os = i *ostride;
112 is = t *istride;
113 cA[os] = filt[0] * inp[is];
114 cD[os] = filt[len_avg] * inp[is];
115 for (l = 1; l < len_avg; l++) {
116 t -= M;
117 while (t >= len_cA) {
118 t -= len_cA;
119 }
120 while (t < 0) {
121 t += len_cA;
122 }
123 os = i * ostride;
124 is = t * istride;
125 cA[os] += filt[l] * inp[is];
126 cD[os] += filt[len_avg + l] * inp[is];
127
128 }
129 }
130
131 }
132
swt_per_stride(int M,double * inp,int N,double * lpd,double * hpd,int lpd_len,double * cA,int len_cA,double * cD,int istride,int ostride)133 void swt_per_stride(int M, double *inp, int N, double *lpd, double*hpd, int lpd_len, double *cA, int len_cA, double *cD, int istride, int ostride) {
134 int l, l2, isodd, i, t, len_avg, j;
135 int is, os;
136 len_avg = M * lpd_len;
137 l2 = len_avg / 2;
138 isodd = N % 2;
139
140 for (i = 0; i < len_cA; ++i) {
141 t = i + l2;
142 os = i *ostride;
143 cA[os] = 0.0;
144 cD[os] = 0.0;
145 l = -1;
146 for (j = 0; j < len_avg; j += M) {
147 l++;
148 while (j >= len_cA) {
149 j -= len_cA;
150 }
151 if ((t - j) >= l2 && (t - j) < N) {
152 is = (t - j)*istride;
153 cA[os] += lpd[l] * inp[is];
154 cD[os] += hpd[l] * inp[is];
155 }
156 else if ((t - j) < l2 && (t - j) >= 0) {
157 is = (t - j)*istride;
158 cA[os] += lpd[l] * inp[is];
159 cD[os] += hpd[l] * inp[is];
160 }
161 else if ((t - j) < 0) {
162 is = (t - j + N)*istride;
163 cA[os] += lpd[l] * inp[is];
164 cD[os] += hpd[l] * inp[is];
165 }
166 else if ((t - j) >= N && isodd == 0) {
167 is = (t - j - N)*istride;
168 cA[os] += lpd[l] * inp[is];
169 cD[os] += hpd[l] * inp[is];
170 }
171 else if ((t - j) >= N && isodd == 1) {
172 if (t - l != N) {
173 is = (t - j - (N + 1))*istride;
174 cA[os] += lpd[l] * inp[is];
175 cD[os] += hpd[l] * inp[is];
176 }
177 else {
178 is = (N - 1)*istride;
179 cA[os] += lpd[l] * inp[is];
180 cD[os] += hpd[l] * inp[N - 1];
181 }
182 }
183
184 }
185 }
186
187 }
188
idwt_per_stride(double * cA,int len_cA,double * cD,double * lpr,double * hpr,int lpr_len,double * X,int istride,int ostride)189 void idwt_per_stride(double *cA, int len_cA, double *cD, double *lpr, double *hpr, int lpr_len, double *X, int istride, int ostride) {
190 int len_avg, i, l, m, n, t, l2;
191 int is, ms, ns;
192
193 len_avg = lpr_len;
194 l2 = len_avg / 2;
195 m = -2;
196 n = -1;
197
198 for (i = 0; i < len_cA + l2 - 1; ++i) {
199 m += 2;
200 n += 2;
201 ms = m * ostride;
202 ns = n * ostride;
203 X[ms] = 0.0;
204 X[ns] = 0.0;
205 for (l = 0; l < l2; ++l) {
206 t = 2 * l;
207 if ((i - l) >= 0 && (i - l) < len_cA) {
208 is = (i - l) * istride;
209 X[ms] += lpr[t] * cA[is] + hpr[t] * cD[is];
210 X[ns] += lpr[t + 1] * cA[is] + hpr[t + 1] * cD[is];
211 }
212 else if ((i - l) >= len_cA && (i - l) < len_cA + len_avg - 1) {
213 is = (i - l - len_cA) * istride;
214 X[ms] += lpr[t] * cA[is] + hpr[t] * cD[is];
215 X[ns] += lpr[t + 1] * cA[is] + hpr[t + 1] * cD[is];
216 }
217 else if ((i - l) < 0 && (i - l) > -l2) {
218 is = (len_cA + i - l) * istride;
219 X[ms] += lpr[t] * cA[is] + hpr[t] * cD[is];
220 X[ns] += lpr[t + 1] * cA[is] + hpr[t + 1] * cD[is];
221 }
222 }
223 }
224 }
225
idwt_sym_stride(double * cA,int len_cA,double * cD,double * lpr,double * hpr,int lpr_len,double * X,int istride,int ostride)226 void idwt_sym_stride(double *cA, int len_cA, double *cD, double *lpr, double *hpr, int lpr_len, double *X, int istride, int ostride) {
227 int len_avg, i, l, m, n, t, v;
228 int ms, ns, is;
229 len_avg = lpr_len;
230 m = -2;
231 n = -1;
232
233 for (v = 0; v < len_cA; ++v) {
234 i = v;
235 m += 2;
236 n += 2;
237 ms = m * ostride;
238 ns = n * ostride;
239 X[ms] = 0.0;
240 X[ns] = 0.0;
241 for (l = 0; l < len_avg / 2; ++l) {
242 t = 2 * l;
243 if ((i - l) >= 0 && (i - l) < len_cA) {
244 is = (i - l) * istride;
245 X[ms] += lpr[t] * cA[is] + hpr[t] * cD[is];
246 X[ns] += lpr[t + 1] * cA[is] + hpr[t + 1] * cD[is];
247 }
248 }
249 }
250 }
251
imodwt_per_stride(int M,double * cA,int len_cA,double * cD,double * filt,int lf,double * X,int istride,int ostride)252 void imodwt_per_stride(int M, double *cA, int len_cA, double *cD, double *filt,int lf,double *X,int istride, int ostride) {
253 int len_avg, i, l, t;
254 int is, os;
255
256 len_avg = lf;
257
258 for (i = 0; i < len_cA; ++i) {
259 t = i;
260 os = i * ostride;
261 is = t *istride;
262 X[os] = (filt[0] * cA[is]) + (filt[len_avg] * cD[is]);
263 for (l = 1; l < len_avg; l++) {
264 t += M;
265 while (t >= len_cA) {
266 t -= len_cA;
267 }
268 while (t < 0) {
269 t += len_cA;
270 }
271 is = t *istride;
272 X[os] += (filt[l] * cA[is]) + (filt[len_avg + l] * cD[is]);
273
274 }
275 }
276
277 }
278
idwt2_shift(int shift,int rows,int cols,double * lpr,double * hpr,int lf,double * A,double * H,double * V,double * D,double * oup)279 void idwt2_shift(int shift, int rows, int cols, double *lpr, double *hpr, int lf, double *A,double *H, double *V,double *D, double *oup) {
280 int i, k, N, ir, ic, J, dim1, dim2;
281 int istride, ostride;
282 double *cL, *cH, *X_lp;
283
284
285 N = rows > cols ? 2 * rows : 2 * cols;
286
287 J = 1;
288 dim1 = 2 * rows;
289 dim2 = 2 * cols;
290
291 X_lp = (double*)malloc(sizeof(double)* (N + 2 * lf - 1));
292 cL = (double*)calloc(dim1*dim2, sizeof(double));
293 cH = (double*)calloc(dim1*dim2, sizeof(double));
294
295 ir = rows;
296 ic = cols;
297 istride = ic;
298 ostride = 1;
299 for (i = 0; i < ic; ++i) {
300 idwt_per_stride(A+i, ir, H+i, lpr, hpr, lf, X_lp, istride, ostride);
301
302 for (k = lf / 2 - 1; k < 2 * ir + lf / 2 - 1; ++k) {
303 cL[(k - lf / 2 + 1)*ic + i] = X_lp[k];
304 }
305
306 idwt_per_stride(V+i, ir, D+i, lpr, hpr, lf, X_lp, istride, ostride);
307
308 for (k = lf / 2 - 1; k < 2 * ir + lf / 2 - 1; ++k) {
309 cH[(k - lf / 2 + 1)*ic + i] = X_lp[k];
310 }
311 }
312 ir *= 2;
313 istride = 1;
314 ostride = 1;
315
316 for (i = 0; i < ir; ++i) {
317 idwt_per_stride(cL + i*ic, ic, cH + i*ic, lpr, hpr, lf, X_lp, istride, ostride);
318
319 for (k = lf / 2 - 1; k < 2 * ic + lf / 2 - 1; ++k) {
320 oup[(k - lf / 2 + 1) + i*ic * 2] = X_lp[k];
321 }
322 }
323 ic *= 2;
324
325
326 if (shift == -1) {
327 //Save the last column
328 for (i = 0; i < ir; ++i) {
329 cL[i] = oup[(i + 1)*ic - 1];
330 }
331 // Save the last row
332 memcpy(cH, oup + (ir - 1)*ic, sizeof(double)*ic);
333 for (i = ir - 1; i > 0; --i) {
334 memcpy(oup + i*ic + 1, oup + (i - 1)*ic, sizeof(double)*(ic - 1));
335 }
336 oup[0] = cL[ir - 1];
337 for (i = 1; i < ir; ++i) {
338 oup[i*ic] = cL[i - 1];
339 }
340
341 for (i = 1; i < ic; ++i) {
342 oup[i] = cH[i - 1];
343 }
344 }
345
346
347 free(X_lp);
348 free(cL);
349 free(cH);
350
351 }
352
upsamp(double * x,int lenx,int M,double * y)353 int upsamp(double *x, int lenx, int M, double *y) {
354 int N, i, j, k;
355
356 if (M < 0) {
357 return -1;
358 }
359
360 if (M == 0) {
361 for (i = 0; i < lenx; ++i) {
362 y[i] = x[i];
363 }
364 return lenx;
365 }
366
367 N = M * (lenx - 1) + 1;
368 j = 1;
369 k = 0;
370
371 for (i = 0; i < N; ++i) {
372 j--;
373 y[i] = 0.0;
374 if (j == 0) {
375 y[i] = x[k];
376 k++;
377 j = M;
378 }
379 }
380
381 return N;
382 }
383
upsamp2(double * x,int lenx,int M,double * y)384 int upsamp2(double *x, int lenx, int M, double *y) {
385 int N, i, j, k;
386 // upsamp2 returns even numbered output. Last value is set to zero
387 if (M < 0) {
388 return -1;
389 }
390
391 if (M == 0) {
392 for (i = 0; i < lenx; ++i) {
393 y[i] = x[i];
394 }
395 return lenx;
396 }
397
398 N = M * lenx;
399 j = 1;
400 k = 0;
401
402 for (i = 0; i < N; ++i) {
403 j--;
404 y[i] = 0.0;
405 if (j == 0) {
406 y[i] = x[k];
407 k++;
408 j = M;
409 }
410 }
411
412 return N;
413 }
414
downsamp(double * x,int lenx,int M,double * y)415 int downsamp(double *x, int lenx, int M, double *y) {
416 int N, i;
417
418 if (M < 0) {
419 return -1;
420 }
421 if (M == 0) {
422 for (i = 0; i < lenx; ++i) {
423 y[i] = x[i];
424 }
425 return lenx;
426 }
427
428 N = (lenx - 1) / M + 1;
429
430 for (i = 0; i < N; ++i) {
431 y[i] = x[i*M];
432 }
433
434 return N;
435 }
436 /*
437 int per_ext(double *sig, int len, int a,double *oup) {
438 int i,len2;
439 // oup is of length len + (len%2) + 2 * a
440 for (i = 0; i < len; ++i) {
441 oup[a + i] = sig[i];
442 }
443 len2 = len;
444 if ((len % 2) != 0) {
445 len2 = len + 1;
446 oup[a + len] = sig[len - 1];
447 }
448 for (i = 0; i < a; ++i) {
449 oup[a-1-i] = sig[len - 1 - i];
450 oup[len2 + a + i] = sig[i];
451 }
452
453 return len2;
454
455 }
456 */
457
per_ext(double * sig,int len,int a,double * oup)458 int per_ext(double *sig, int len, int a, double *oup) {
459 int i, len2;
460 double temp1;
461 double temp2;
462 for (i = 0; i < len; ++i) {
463 oup[a + i] = sig[i];
464 }
465 len2 = len;
466 if ((len % 2) != 0) {
467 len2 = len + 1;
468 oup[a + len] = sig[len - 1];
469 }
470 for (i = 0; i < a; ++i) {
471 temp1 = oup[a + i];
472 temp2 = oup[a + len2 - 1 - i];
473 oup[a - 1 - i] = temp2;
474 oup[len2 + a + i] = temp1;
475 }
476 return len2;
477 }
478 /*
479 int symm_ext(double *sig, int len, int a, double *oup) {
480 int i, len2;
481 // oup is of length len + 2 * a
482 for (i = 0; i < len; ++i) {
483 oup[a + i] = sig[i];
484 }
485 len2 = len;
486 for (i = 0; i < a; ++i) {
487 oup[a - 1 - i] = sig[i];
488 oup[len2 + a + i] = sig[len - 1 - i];
489 }
490
491 return len2;
492
493 }
494 */
495
symm_ext(double * sig,int len,int a,double * oup)496 int symm_ext(double *sig, int len, int a, double *oup) {
497 int i, len2;
498 double temp1;
499 double temp2;
500 // oup is of length len + 2 * a
501 for (i = 0; i < len; ++i) {
502 oup[a + i] = sig[i];
503 }
504 len2 = len;
505 for (i = 0; i < a; ++i) {
506 temp1 = oup[a + i];
507 temp2 = oup[a + len2 - 1 - i];
508 oup[a - 1 - i] = temp1;
509 oup[len2 + a + i] = temp2;
510 }
511
512 return len2;
513
514 }
515
isign(int N)516 static int isign(int N) {
517 int M;
518 if (N >= 0) {
519 M = 1;
520 }
521 else {
522 M = -1;
523 }
524
525 return M;
526 }
527
iabs(int N)528 static int iabs(int N) {
529 if (N >= 0) {
530 return N;
531 }
532 else {
533 return -N;
534 }
535 }
536
circshift(double * array,int N,int L)537 void circshift(double *array, int N, int L) {
538 int i;
539 double *temp;
540 if (iabs(L) > N) {
541 L = isign(L) * (iabs(L) % N);
542 }
543 if (L < 0) {
544 L = (N + L) % N;
545 }
546
547 temp = (double*)malloc(sizeof(double) * L);
548
549 for (i = 0; i < L; ++i) {
550 temp[i] = array[i];
551 }
552
553 for (i = 0; i < N - L; ++i) {
554 array[i] = array[i + L];
555 }
556
557 for (i = 0; i < L; ++i) {
558 array[N - L + i] = temp[i];
559 }
560
561 free(temp);
562 }
563
testSWTlength(int N,int J)564 int testSWTlength(int N, int J) {
565 int ret,div,i;
566 ret = 1;
567
568 div = 1;
569 for (i = 0; i < J; ++i) {
570 div *= 2;
571 }
572
573 if (N % div) {
574 ret = 0;
575 }
576
577 return ret;
578
579 }
580
wmaxiter(int sig_len,int filt_len)581 int wmaxiter(int sig_len, int filt_len) {
582 int lev;
583 double temp;
584
585 temp = log((double)sig_len / ((double)filt_len - 1.0)) / log(2.0);
586 lev = (int)temp;
587
588 return lev;
589 }
590
entropy_s(double * x,int N)591 static double entropy_s(double *x,int N) {
592 int i;
593 double val,x2;
594
595 val = 0.0;
596
597 for(i = 0; i < N; ++i) {
598 if (x[i] != 0) {
599 x2 = x[i] * x[i];
600 val -= x2 * log(x2);
601 }
602 }
603 return val;
604 }
605
entropy_t(double * x,int N,double t)606 static double entropy_t(double *x,int N, double t) {
607 int i;
608 double val,x2;
609 if (t < 0) {
610 printf("Threshold value must be >= 0");
611 exit(1);
612 }
613 val = 0.0;
614
615 for(i = 0; i < N; ++i) {
616 x2 = fabs(x[i]);
617 if (x2 > t) {
618 val += 1;
619 }
620
621 }
622
623 return val;
624
625 }
626
entropy_n(double * x,int N,double p)627 static double entropy_n(double *x,int N,double p) {
628 int i;
629 double val,x2;
630 if (p < 1) {
631 printf("Norm power value must be >= 1");
632 exit(1);
633 }
634 val = 0.0;
635 for(i = 0; i < N; ++i) {
636 x2 = fabs(x[i]);
637 val += pow(x2,(double)p);
638
639 }
640
641 return val;
642 }
643
entropy_l(double * x,int N)644 static double entropy_l(double *x,int N) {
645 int i;
646 double val,x2;
647
648 val = 0.0;
649
650 for(i = 0; i < N; ++i) {
651 if (x[i] != 0) {
652 x2 = x[i] * x[i];
653 val += log(x2);
654 }
655 }
656 return val;
657 }
658
costfunc(double * x,int N,char * entropy,double p)659 double costfunc(double *x, int N ,char *entropy,double p) {
660 double val;
661
662 if (!strcmp(entropy, "shannon")) {
663 val = entropy_s(x, N);
664 }
665 else if (!strcmp(entropy, "threshold")) {
666 val = entropy_t(x, N,p);
667 }
668 else if (!strcmp(entropy, "norm")) {
669 val = entropy_n(x, N,p);
670 }
671 else if (!strcmp(entropy, "logenergy") || !strcmp(entropy, "log energy") || !strcmp(entropy, "energy")) {
672 val = entropy_l(x, N);
673 }
674 else {
675 printf("Entropy must be one of shannon, threshold, norm or energy");
676 exit(-1);
677 }
678
679 return val;
680 }
681