signal_processing plugged.
[scilab.git] / scilab / modules / signal_processing / sci_gateway / cpp / sci_fft.cpp
1 /*
2 * Scilab ( http://www.scilab.org/ ) - This file is part of Scilab
3 * Copyright (C) 2011 - DIGITEO - Antoine ELIAS
4 *
5 * This file must be used under the terms of the CeCILL.
6 * This source file is licensed as described in the file COPYING, which
7 * you should have received as part of this distribution.  The terms
8 * are also available at
9 * http://www.cecill.info/licences/Licence_CeCILL_V2-en.txt
10 *
11 */
12 /*--------------------------------------------------------------------------*/
13
14 #include "signal_gw.hxx"
15 #include "double.hxx"
16 #include "function.hxx"
17
18 extern "C"
19 {
20 #include "localization.h"
21 #include "Scierror.h"
22 #include "elem_common.h"
23
24 extern void C2F(fft842)(int *inverse, int *signal_length, double *signal_real, double *signal_imaginary, int *error);
25 extern void C2F(dfft2)(double *signal_real, double *signal_imaginary, int *nseg, int *n, int *nspn, int *inverse, int *error, int *buffer_data, int *buffer_size);
26 }
27
28 #define POW2_15     32768
29
30 bool isPowerOf2(int _iVal);
31
32 int maxfactor(int n);
33 int fft_1dim(double *signal_real, double *signal_imaginary, int signal_length, int inverse, int *buffer_data, int buffer_size);
34 int fft_2dim(double *signal_real, double *signal_imaginary, int signal_rows, int signal_cols, int inverse, int *buffer_data, int buffer_size);
35 int fft_ndim(double *signal_real, double *signal_imaginary, int signal_length, int dimensions_length, int dimension_stride, int inverse, int *buffer_data, int buffer_size);
36
37 /*--------------------------------------------------------------------------*/
38 types::Function::ReturnValue sci_fft(types::typed_list &in, int _iRetCount, types::typed_list &out)
39 {
40     int iDimLength          = 0;
41     int iDimCount           = 0;
42     int iInc                = 0;
43     int iWay                = -1;
44     int iSize               = 0;
45     int iOne                = 1;
46     int iErr                = 0;
47     double dblZero          = 0;
48
49     //workspace
50     int iWS                 = 0;
51     int* piWS               = NULL;
52     types::Double* pIn1     = NULL;
53
54     //check input parameters
55     if(in.size() != 1 && in.size() != 2 && in.size() != 4)
56     {
57         ScierrorW(77, _W("%ls: Wrong number of input argument(s): %d or %d expected.\n"), L"fft", 1, 4);
58         return types::Function::Error;
59     }
60
61     switch(in.size())
62     {
63     case 4 :
64         //check fourth input parameter : inc
65         if(in[3]->isDouble() == false || in[3]->getAs<types::Double>()->isScalar() == false)
66         {
67             ScierrorW(999, _W("%ls: Wrong type for input argument #%d: Scalar expected.\n"), L"fft", 4);
68             return types::Function::Error;
69         }
70
71         iInc = (int)in[3]->getAs<types::Double>()->get(0);
72
73         //check third input parameter : dim
74         if(in[2]->isDouble() == false || in[2]->getAs<types::Double>()->isScalar() == false)
75         {
76             ScierrorW(999, _W("%ls: Wrong type for input argument #%d: Scalar expected.\n"), L"fft", 3);
77             return types::Function::Error;
78         }
79
80         iDimLength = (int)in[2]->getAs<types::Double>()->get(0);
81         iDimCount = 3; //any value > 2 (used as a flag) 
82
83     case 2 : 
84         //check third input parameter : way
85         if(in[1]->isDouble() == false || in[1]->getAs<types::Double>()->isScalar() == false)
86         {
87             ScierrorW(999, _W("%ls: Wrong type for input argument #%d: Scalar expected.\n"), L"fft", 2);
88             return types::Function::Error;
89         }
90
91         iWay = (int)in[1]->getAs<types::Double>()->get(0);
92         if(iWay != -1 && iWay != 1)
93         {
94             ScierrorW(999, _W("%ls: Wrong value for input argument #%d: Must be in the set {%ls}.\n"), L"fft", 2, "-1 1");
95             return types::Function::Error;
96         }
97
98     case 1:
99         if(in[0]->isDouble() == false)
100         {
101             ScierrorW(999, _W("%ls: Wrong type for input argument #%d: Scalar expected.\n"), L"fft", 1);
102             return types::Function::Error;
103         }
104
105         pIn1 = in[0]->getAs<types::Double>();
106
107         iDimCount = Max(iDimCount, ((pIn1->getRows() == 1 || pIn1->getCols() == 1) ? 1 : 2));
108         iSize = pIn1->getSize();
109         break;
110     default :
111         {
112             ScierrorW(77, _W("%ls: Wrong number of input argument(s): %d eor %d xpected.\n"), L"fft", 1, 4);
113             return types::Function::Error;
114         }
115     }
116
117     types::Double* pOut = pIn1->clone()->getAs<types::Double>();
118     pOut->setComplex(true);
119
120     //alloc workspace required by dfft2
121     iWS = 8 * maxfactor(iSize) + 24;
122     piWS = (int*)MALLOC(iWS * sizeof(int));
123     if (piWS == NULL)
124     {
125         ScierrorW(999, _W("%ls : Memory allocation error.\n"), L"fft");
126         return types::Function::Error;
127     }
128
129     switch(iDimCount)
130     {
131     case 1 :
132         iErr = fft_1dim(pOut->getReal(), pOut->getImg(), iSize, iWay, piWS, iWS);
133         break;
134     case 2 :
135         iErr = fft_2dim(pOut->getReal(), pOut->getImg(), pOut->getRows(), pOut->getCols(), iWay, piWS, iWS);
136         if(iErr == 1)
137         {
138             ScierrorW(999, _W("%ls : Memory allocation error.\n"), L"fft");
139             return types::Function::Error;
140         }
141         break;
142     default :
143         iErr = fft_ndim(pOut->getReal(), pOut->getImg(), iSize, iDimLength, iInc, iWay, piWS, iWS);
144         break;
145     }
146
147     FREE(piWS);
148
149     out.push_back(pOut);
150     return types::Function::OK;
151 }
152
153 int fft_1dim(double *signal_real, double *signal_imaginary, int signal_length, int inverse, int *buffer_data, int buffer_size)
154 {
155     int error = 0;
156     int one = 1;
157
158     if(isPowerOf2(signal_length) && signal_length < POW2_15)
159     {
160         C2F(fft842)(&inverse, &signal_length, signal_real, signal_imaginary, &error);
161     }
162     else
163     {
164         C2F(dfft2)(signal_real, signal_imaginary, &one, &signal_length, &one, &inverse, &error, buffer_data, &buffer_size);
165     }
166
167     return error;
168 }
169
170 int fft_2dim(double *signal_real, double *signal_imaginary, int signal_rows, int signal_cols, int inverse, int *buffer_data, int buffer_size)
171 {
172     int error = 0;
173     int one = 1;
174
175     if(isPowerOf2(signal_rows) && signal_rows < POW2_15)
176     {
177         for (int i = 0 ; i < signal_cols ; i++)
178         {
179             C2F(fft842)(&inverse, &signal_rows, &(signal_real[signal_rows * i]), &(signal_imaginary[signal_rows * i]), &error);
180         }
181     }
182     else
183     {
184         C2F(dfft2)(signal_real, signal_imaginary, &signal_cols, &signal_rows, &one, &inverse, &error, buffer_data, &buffer_size);
185     }
186
187     if(isPowerOf2(signal_cols) && signal_cols < POW2_15)
188     {
189         double* temp_real = NULL;
190         double* temp_imaginary = NULL;
191
192         temp_real = (double *)MALLOC(signal_cols * sizeof(double));
193         temp_imaginary = (double *)MALLOC(signal_cols * sizeof(double));
194
195         if(temp_real == NULL || temp_imaginary == NULL)
196         {
197             return 1;
198         }
199
200         for (int i = 0 ; i < signal_rows ; i++)
201         {
202             C2F(dcopy)(&signal_cols, &(signal_real[i]),&signal_rows, temp_real, &one);
203             C2F(dcopy)(&signal_cols, &(signal_imaginary[i]), &signal_rows, temp_imaginary, &one);
204             C2F(fft842)(&inverse, &signal_cols, temp_real, temp_imaginary, &error);
205             C2F(dcopy)(&signal_cols, temp_real, &one, &(signal_real[i]), &signal_rows);
206             C2F(dcopy)(&signal_cols, temp_imaginary, &one, &(signal_imaginary[i]), &signal_rows);
207         }
208
209         FREE(temp_imaginary);
210         temp_imaginary = NULL;
211         free(temp_real);
212         temp_real = NULL;
213     }
214     else
215     { /* erroneous implementation suspected */
216         C2F(dfft2)(signal_real, signal_imaginary, &one, &signal_cols, &signal_rows, &inverse, &error, buffer_data, &buffer_size);
217     }
218
219     return error;
220 }
221
222 int fft_ndim(double *signal_real, double *signal_imaginary, int signal_length, int dimensions_length, int dimension_stride, int inverse, int *buffer_data, int buffer_size)
223 {
224     /* translated litterally from Fortran... but... wtf ?! */
225     int error = 0;
226     int nseg = signal_length / dimensions_length / dimension_stride;
227     C2F(dfft2)(signal_real, signal_imaginary, &nseg, &dimensions_length, &dimension_stride, &inverse, &error, buffer_data, &buffer_size);
228     return error;
229 }
230
231
232 int maxfactor(int n)
233 {
234     int nfac[15];
235     int m = 0, j = 0, jj = 0, k = 0, kt = 0, max = 0;
236
237     for (k = abs(n) ; k % 16 == 0 ; k /= 16)
238     {
239         m++;
240         nfac[m - 1] = 4;
241     }
242     
243     for (j = 3, jj = 9 ; jj <= k ; j += 2, jj = j*j)
244     {
245         if(k % jj != 0)
246         {
247             continue;
248         }
249         m++;
250         nfac[m - 1] = j;
251         k /= jj;
252     }
253
254     if(k <= 4)
255     {
256         kt = m;
257         nfac[m] = k;
258         if(k != 1)
259         {
260             m++;
261         }
262     }
263     else
264     {
265         if(k % 4 == 0)
266         {
267             m++;
268             nfac[m - 1] = 2;
269             k /= 4;
270         }
271         
272         kt = m;
273         for (j = 2 ; j <= k ; j = ((j + 1) / 2) * 2 + 1)
274         {
275             if(k % j != 0)
276             {
277                 continue;
278             }
279
280             m++;
281             nfac[m - 1] = j;
282             k /= j;
283         }
284     }
285
286     if(kt != 0)
287     {
288         for (j = kt ; j > 0 ; j--)
289         {
290             m++;
291             nfac[m - 1] = nfac[j - 1];
292         }
293     }
294
295     /* get nfac maximum */
296     for (j = 0, max = nfac[0]; j < m ; j++)
297     {
298         max = (nfac[j] > max) ? nfac[j] : max;
299     }
300
301     return max;
302 }
303
304 bool isPowerOf2(int _iVal)
305 {
306     return (_iVal & (_iVal - 1)) == 0;
307 }