1 /**
2 * Smarc
3 *
4 * Copyright (c) 2009-2011 Institut Télécom - Télécom Paristech
5 * Télécom ParisTech / dept. TSI
6 *
7 * Authors : Benoit Mathieu, Jacques Prado
8 *
9 * This file is part of Smarc.
10 *
11 * Smarc is free software: you can redistribute it and/or modify
12 * it under the terms of the GNU Lesser General Public License as published by
13 * the Free Software Foundation, either version 3 of the License, or
14 * (at your option) any later version.
15 *
16 * Smarc is distributed in the hope that it will be useful,
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 * GNU Lesser General Public License for more details.
20 *
21 * You should have received a copy of the GNU Lesser General Public License
22 * along with this program. If not, see <http://www.gnu.org/licenses/>.
23 */
24
25 #include "polyfilt.h"
26
27 #include <stdlib.h>
28 #include <stdio.h>
29 #include <string.h>
30
polyfiltLM(struct PSFilter * pfilt,struct PSState * pstate,const double * signal,int signalLen,int * nbRead,double * output,int outputLen,int * nbWritten)31 void polyfiltLM(struct PSFilter* pfilt, struct PSState* pstate,
32 const double* signal, int signalLen, int* nbRead, double* output,
33 int outputLen, int* nbWritten) {
34 const int M = pfilt->M;
35 const int L = pfilt->L;
36 const int K = pfilt->K;
37
38 int signalPos = 0;
39 int outPos = 0;
40 int phase = pstate->phase;
41
42 // skip first sample for delays
43 if (pstate->skip>0)
44 {
45 const int maxAdvance = (M + L - 1) / L;
46 while (pstate->skip>0 && ((signalPos+maxAdvance)<signalLen)) {
47 pstate->skip--;
48 phase += M;
49 signalPos += phase / L;
50 phase = phase % L;
51 }
52 }
53
54 // process filtering
55 while ((signalPos+K<=signalLen) && (outPos<outputLen))
56 {
57 // compute value
58 output[outPos++] = filter(pfilt->filters + phase*K,signal + signalPos, K);
59
60 // consume samples
61 phase += M;
62 signalPos += phase / L;
63 phase = phase % L;
64 }
65
66 // report state values
67 pstate->phase = phase;
68 *nbRead = signalPos;
69 *nbWritten = outPos;
70 }
71
72
polyfiltM(struct PSFilter * pfilt,struct PSState * pstate,const double * SMARC_RESTRICT signal,const int signalLen,int * SMARC_RESTRICT nbConsume,double * SMARC_RESTRICT output,const int outputLen,int * SMARC_RESTRICT nbWritten)73 void polyfiltM(struct PSFilter* pfilt, struct PSState* pstate,
74 const double* SMARC_RESTRICT signal, const int signalLen, int* SMARC_RESTRICT nbConsume,
75 double* SMARC_RESTRICT output, const int outputLen, int* SMARC_RESTRICT nbWritten) {
76 const int M = pfilt->M;
77 const int K = pfilt->K;
78 const double* filt = pfilt->filters;
79
80 int signalPos = 0;
81 int outPos = 0;
82
83 // skip first sample for delays
84 while (pstate->skip>0 && ((signalPos+M)<signalLen)) {
85 pstate->skip--;
86 signalPos += M;
87 }
88
89 // process filtering
90 while (((signalPos+K)<=signalLen) && (outPos<outputLen))
91 {
92 // compute value
93 // double v = 0.0;
94 // const double* inPtr = signal + signalPos;
95 // for (int k=0;k<K;k++)
96 // v += inPtr[k] * filt[k];
97 // output[outPos++] = v;
98 output[outPos++] = filter(filt,signal+signalPos,K);
99
100 // consume samples
101 signalPos += M;
102 }
103
104 // report state values
105 *nbWritten = outPos;
106 *nbConsume = signalPos;
107 }
108
polyfiltL(struct PSFilter * pfilt,struct PSState * pstate,const double * signal,int signalLen,int * nbRead,double * output,int outputLen,int * nbWritten)109 void polyfiltL(struct PSFilter* pfilt, struct PSState* pstate,
110 const double* signal, int signalLen, int* nbRead, double* output,
111 int outputLen, int* nbWritten) {
112 const int L = pfilt->L;
113 const int K = pfilt->K;
114
115 int signalPos = 0;
116 int outPos = 0;
117 int phase = pstate->phase;
118
119 // skip first sample for delays
120 while (pstate->skip>0 && signalPos<signalLen) {
121 pstate->skip--;
122 phase++;
123 if (phase==L) {
124 signalPos++;
125 phase = 0;
126 }
127 }
128
129 // compute first output to reach phase 0
130 while (signalPos+K<=signalLen && outPos<outputLen)
131 {
132 // double v=0;
133 // const double* inPtr = signal + signalPos;
134 // const double* filtPtr = pfilt->filters + phase*K;
135 // for (int k=0;k<K;++k)
136 // v += inPtr[k] * filtPtr[k];
137 // output[outPos++] = v;
138 output[outPos++] = filter(pfilt->filters + phase*K, signal+signalPos, K);
139 phase++;
140 if (phase==L) {
141 signalPos++;
142 phase=0;
143 }
144 }
145
146 // report state values
147 pstate->phase = phase;
148 *nbRead = signalPos;
149 *nbWritten = outPos;
150 }
151