bbffeab16f7baa095e3a3b2fb72969cb726e1d89
[scilab.git] / scilab / modules / cacsd / sci_gateway / cpp / sci_freq.cpp
1 /*
2  * Scilab ( http://www.scilab.org/ ) - This file is part of Scilab
3  * Copyright (C) 2014 - Scilab Enterprises - Cedric DELAMARRE
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 #include "cacsd_gw.hxx"
14 #include "function.hxx"
15 #include "double.hxx"
16 #include "polynom.hxx"
17
18 extern "C"
19 {
20 #include "sciprint.h"
21 #include "Scierror.h"
22 #include "localization.h"
23 #include "elem_common.h"
24
25     extern void C2F(dfrmg)( int*, int*, int*, int*, int*, int*, int*,
26                             double*, double*, double*, double*, double*,
27                             double*, double*, double*, double*, int*);
28
29     extern void C2F(dadd)(int*, double*, int*, double*, int*);
30     extern void C2F(horner)(double*, int*, double*, double*, double*, double*);
31 }
32 /*--------------------------------------------------------------------------*/
33 types::Function::ReturnValue freqRational(types::typed_list &in, int _iRetCount, types::typed_list &out);
34 types::Function::ReturnValue freqState(types::typed_list &in, int _iRetCount, types::typed_list &out);
35 /*--------------------------------------------------------------------------*/
36 types::Function::ReturnValue sci_freq(types::typed_list &in, int _iRetCount, types::typed_list &out)
37 {
38     types::Double* pDblA = NULL;
39     types::Double* pDblB = NULL;
40     types::Double* pDblP = NULL;
41
42     if (in.size() < 3 || in.size() > 5)
43     {
44         Scierror(77, _("%s: Wrong number of input argument(s): %d to %d expected.\n"), "freq", 3, 5);
45         return types::Function::Error;
46     }
47
48     if (_iRetCount > 1)
49     {
50         Scierror(78, _("%s: Wrong number of output argument(s): %d expected.\n"), "freq", 1);
51         return types::Function::Error;
52     }
53
54     if (in.size() == 3) // systeme defini par sa representation rationnelle n./d
55     {
56         return freqRational(in, _iRetCount, out);
57     }
58     else // systeme defini par sa representation d'etat
59     {
60         return freqState(in, _iRetCount, out);
61     }
62 }
63
64 types::Function::ReturnValue freqRational(types::typed_list &in, int _iRetCount, types::typed_list &out)
65 {
66     int iRowNum     = 0;
67     int iColNum     = 0;
68     int iRowDen     = 0;
69     int iColDen     = 0;
70     int iSizeF      = 0;
71     int iOne        = 1;
72     int iComplex    = 0;
73     int iError      = 0;
74     double dZero    = 0;
75
76     double** pdblDen    = NULL;
77     double** pdblNum    = NULL;
78     double* pdblF       = NULL;
79     double* pdblFImg    = NULL;
80     double* pdblR       = NULL;
81     double* pdblI       = NULL;
82     int* piRankDen      = NULL;
83     int* piRankNum      = NULL;
84
85     /*** get inputs arguments ***/
86     // get f
87     if (in[2]->isDouble() == false)
88     {
89         Scierror(999, _("%s: Wrong type for input argument #%d: A matrix expected.\n"), "freq", 3);
90         return types::Function::Error;
91     }
92
93     types::Double* pDblF = in[2]->getAs<types::Double>();
94     iSizeF = pDblF->getSize();
95     pdblF = pDblF->get();
96
97     if (pDblF->isComplex())
98     {
99         pdblFImg = pDblF->getImg();
100         iComplex = 1;
101     }
102     else
103     {
104         pdblFImg = &dZero;
105     }
106
107     try
108     {
109         // get DEN
110         if (in[1]->isDouble())
111         {
112             types::Double* pDblDen = in[1]->getAs<types::Double>();
113             double* pdbl = pDblDen->get();
114             if (pDblDen->isComplex())
115             {
116                 Scierror(999, _("%s: Wrong type for input argument #%d: A real matrix expected.\n"), "freq", 2);
117                 return types::Function::Error;
118             }
119
120             iRowDen = pDblDen->getRows();
121             iColDen = pDblDen->getCols();
122
123             piRankDen = new int[pDblDen->getSize()];
124             memset(piRankDen, 0x00, pDblDen->getSize() * sizeof(int));
125
126             pdblDen = new double*[pDblDen->getSize()];
127             for (int i = 0; i < pDblDen->getSize(); i++)
128             {
129                 pdblDen[i] = pdbl + i;
130             }
131         }
132         else if (in[1]->isPoly())
133         {
134             types::Polynom* pPolyDen = in[1]->getAs<types::Polynom>();
135
136             double dblEps = getRelativeMachinePrecision();
137
138             if (pPolyDen->isComplex())
139             {
140                 bool cplx = false;
141
142                 int iSize = pPolyDen->getSize();
143                 for (int i = 0; i < iSize; i++)
144                 {
145                     types::SinglePoly *sp = pPolyDen->get(i);
146                     double *df = sp->getImg();
147
148                     for (int j = 0 ; j <  sp->getSize(); j++)
149                     {
150                         if (abs(df[j]) > dblEps)
151                         {
152                             cplx = true;
153
154                             break;
155                         }
156                     }
157                 }
158
159                 if (cplx)
160                 {
161
162                     Scierror(999, _("%s: Wrong type for input argument #%d: A real polynom expected.\n"), "freq", 2);
163                     return types::Function::Error;
164                 }
165
166             }
167
168             iRowDen = pPolyDen->getRows();
169             iColDen = pPolyDen->getCols();
170
171             piRankDen = new int[pPolyDen->getSize()];
172             pPolyDen->getRank(piRankDen);
173
174             pdblDen = new double*[pPolyDen->getSize()];
175             for (int i = 0; i < pPolyDen->getSize(); i++)
176             {
177                 pdblDen[i] = pPolyDen->get(i)->get();
178             }
179         }
180         else
181         {
182             Scierror(999, _("%s: Wrong type for input argument #%d: A matrix or polynom expected.\n"), "freq", 2);
183             return types::Function::Error;
184         }
185
186         // get NUM
187         if (in[0]->isDouble())
188         {
189             types::Double* pDblNum = in[0]->getAs<types::Double>();
190             double* pdbl = pDblNum->get();
191             if (pDblNum->isComplex())
192             {
193                 Scierror(999, _("%s: Wrong type for input argument #%d: A real matrix expected.\n"), "freq", 1);
194                 throw 1;
195             }
196
197             iRowNum = pDblNum->getRows();
198             iColNum = pDblNum->getCols();
199
200             piRankNum = new int[pDblNum->getSize()];
201             memset(piRankNum, 0x00, pDblNum->getSize() * sizeof(int));
202
203             pdblNum = new double*[pDblNum->getSize()];
204             for (int i = 0; i < pDblNum->getSize(); i++)
205             {
206                 pdblNum[i] = pdbl + i;
207             }
208         }
209         else if (in[0]->isPoly())
210         {
211             types::Polynom* pPolyNum = in[0]->getAs<types::Polynom>();
212
213             double dblEps = getRelativeMachinePrecision();
214             if (pPolyNum->isComplex())
215             {
216                 bool cplx = false;
217
218                 int iSize = pPolyNum->getSize();
219                 for (int i = 0; i < iSize; i++)
220                 {
221                     types::SinglePoly *sp = pPolyNum->get(i);
222                     double *df = sp->getImg();
223
224                     for (int j = 0 ; j <  sp->getSize(); j++)
225                     {
226                         if (abs(df[j]) > dblEps)
227                         {
228                             cplx = true;
229
230                             break;
231                         }
232                     }
233                 }
234
235                 if (cplx)
236                 {
237
238                     Scierror(999, _("%s: Wrong type for input argument #%d: A real polynom expected.\n"), "freq", 1);
239                     return types::Function::Error;
240                 }
241             }
242             iRowNum = pPolyNum->getRows();
243             iColNum = pPolyNum->getCols();
244
245             piRankNum = new int[pPolyNum->getSize()];
246             pPolyNum->getRank(piRankNum);
247
248             pdblNum = new double*[pPolyNum->getSize()];
249             for (int i = 0; i < pPolyNum->getSize(); i++)
250             {
251                 pdblNum[i] = pPolyNum->get(i)->get();
252             }
253         }
254         else
255         {
256             Scierror(999, _("%s: Wrong type for input argument #%d: A matrix or polynom expected.\n"), "freq", 1);
257             return types::Function::Error;
258         }
259
260         if (iRowNum != iRowDen || iColNum != iColDen)
261         {
262             Scierror(60, _("%s: Wrong size for argument: Incompatible dimensions.\n"), "freq");
263             throw 1;
264         }
265
266         /*** perform operations ***/
267         double dVr  = 0;
268         double dVi  = 0;
269         double dUr  = 0;
270         double dUi  = 0;
271         int iSize   = iRowDen * iColDen * iSizeF;
272
273         pdblR = new double[iSize];
274         pdblI = new double[iSize];
275
276         double* pdblRTemp = pdblR;
277         double* pdblITemp = pdblI;
278
279         for (int i = 0; i < iSizeF; i++)
280         {
281             for (int j = 0; j < iRowDen * iColDen; j++)
282             {
283                 C2F(horner)(pdblNum[j], piRankNum + j, pdblF, pdblFImg, &dVr, &dVi);
284                 C2F(horner)(pdblDen[j], piRankDen + j, pdblF, pdblFImg, &dUr, &dUi);
285                 if (dUr * dUr + dUi * dUi == 0)
286                 {
287                     Scierror(27, _("%s: Division by zero...\n"), "freq");
288                     throw 1;
289                 }
290
291                 if (iComplex)
292                 {
293                     C2F(wdiv)(&dVr, &dVi, &dUr, &dUi, pdblRTemp, pdblITemp);
294                 }
295                 else
296                 {
297                     *pdblRTemp = dVr / dUr;
298                 }
299
300                 pdblRTemp++;
301                 pdblITemp++;
302             }
303
304             pdblF++;
305             pdblFImg += iComplex;
306         }
307
308         /*** retrun output arguments ***/
309         types::Double* pDblOut = new types::Double(iRowDen, iColDen * iSizeF, iComplex == 1);
310         double* pdblOut = pDblOut->get();
311         int iSizeOut = pDblOut->getSize();
312         C2F(dcopy)(&iSizeOut, pdblR, &iOne, pdblOut, &iOne);
313
314         if (iComplex)
315         {
316             double* pdblOutImg = pDblOut->getImg();
317             C2F(dcopy)(&iSizeOut, pdblI, &iOne, pdblOutImg, &iOne);
318         }
319
320         out.push_back(pDblOut);
321     }
322     catch (int iErr)
323     {
324         iError = iErr;
325     }
326
327     // free memory
328     delete[] piRankDen;
329     delete[] pdblDen;
330
331     if (pdblR)
332     {
333         delete[] pdblR;
334     }
335     if (pdblI)
336     {
337         delete[] pdblI;
338     }
339
340     if (piRankNum)
341     {
342         delete[] piRankNum;
343     }
344     if (pdblNum)
345     {
346         delete[] pdblNum;
347     }
348
349     if (iError)
350     {
351         return types::Function::Error;
352     }
353
354     return types::Function::OK;
355 }
356
357 types::Function::ReturnValue freqState(types::typed_list &in, int _iRetCount, types::typed_list &out)
358 {
359     types::Double* pDblA = NULL;
360     types::Double* pDblB = NULL;
361     types::Double* pDblC = NULL;
362     types::Double* pDblD = NULL;
363     types::Double* pDblF = NULL;
364
365     double dZero = 0;
366
367     int iRowsA      = 0;
368     int iColsB      = 0;
369     int iRowsC      = 0;
370     int iSizeF      = 0;
371     int iOne        = 1;
372     int iComplex    = 0;
373
374     double* pdblF       = NULL;
375     double* pdblFImg    = NULL;
376
377     /*** get inputs arguments ***/
378     int iNbInputArg = (int)in.size();
379     // get f
380     if (in[iNbInputArg - 1]->isDouble() == false)
381     {
382         Scierror(999, _("%s: Wrong type for input argument #%d: A matrix expected.\n"), "freq", iNbInputArg);
383         return types::Function::Error;
384     }
385
386     pDblF = in[iNbInputArg - 1]->getAs<types::Double>();
387     pdblF = pDblF->get();
388     if (pDblF->isComplex())
389     {
390         pdblFImg = pDblF->getImg();
391         iComplex = 1;
392     }
393     else
394     {
395         pdblFImg = &dZero;
396     }
397
398
399     if (iNbInputArg == 5)
400     {
401         //get D
402         if (in[3]->isDouble() == false)
403         {
404             Scierror(999, _("%s: Wrong type for input argument #%d: A matrix expected.\n"), "freq", 4);
405             return types::Function::Error;
406         }
407
408         pDblD = in[3]->getAs<types::Double>();
409         if (pDblD->isComplex())
410         {
411             Scierror(999, _("%s: Wrong type for input argument #%d: A real matrix expected.\n"), "freq", 4);
412             return types::Function::Error;
413         }
414     }
415
416     // get C
417     if (in[2]->isDouble() == false)
418     {
419         Scierror(999, _("%s: Wrong type for input argument #%d: A matrix expected.\n"), "freq", 3);
420         return types::Function::Error;
421     }
422
423     pDblC = in[2]->getAs<types::Double>();
424
425     if (pDblC->isComplex())
426     {
427         Scierror(999, _("%s: Wrong type for input argument #%d: A real matrix expected.\n"), "freq", 3);
428         return types::Function::Error;
429     }
430
431     // get B
432     if (in[1]->isDouble() == false)
433     {
434         Scierror(999, _("%s: Wrong type for input argument #%d: A matrix expected.\n"), "freq", 2);
435         return types::Function::Error;
436     }
437
438     pDblB = in[1]->getAs<types::Double>();
439
440     if (pDblB->isComplex())
441     {
442         Scierror(999, _("%s: Wrong type for input argument #%d: A real matrix expected.\n"), "freq", 2);
443         return types::Function::Error;
444     }
445
446     // get A
447     if (in[0]->isDouble() == false)
448     {
449         Scierror(999, _("%s: Wrong type for input argument #%d: A matrix expected.\n"), "freq", 1);
450         return types::Function::Error;
451     }
452
453     pDblA = in[0]->getAs<types::Double>();
454
455     if (pDblA->isComplex())
456     {
457         Scierror(999, _("%s: Wrong type for input argument #%d: A real matrix expected.\n"), "freq", 1);
458         return types::Function::Error;
459     }
460
461     if (pDblA->getRows() != pDblA->getCols())
462     {
463         Scierror(999, _("%s: Wrong size for input argument #%d: A square matrix expected.\n"), "freq", 1);
464         return types::Function::Error;
465     }
466
467     iRowsA = pDblA->getRows();
468     iColsB = pDblB->getCols();
469     iRowsC = pDblC->getRows();
470     iSizeF = pDblF->getSize();
471
472     if (iRowsA != pDblB->getRows() || iRowsA != pDblC->getCols())
473     {
474         Scierror(999, _("%s: Wrong size for argument: Incompatible dimensions.\n"), "ppol");
475         return types::Function::Error;
476     }
477
478     if (iNbInputArg == 5 && (pDblD->getRows() != pDblC->getRows() || pDblD->getCols() != pDblB->getCols()))
479     {
480         Scierror(999, _("%s: Wrong size for argument: Incompatible dimensions.\n"), "ppol");
481         return types::Function::Error;
482     }
483
484     /*** perform operations ***/
485     int iJob        = 0;
486     bool bFirst     = true;
487     double dRcond   = 0;
488
489     int* pdblW1     = new int[iRowsA];
490     double* pdblW   = new double[2 * iRowsA * iRowsA + 2 * iRowsA];
491     double* pdblWgr = new double[iColsB * iSizeF * iRowsC];
492     double* pdblWgi = new double[iColsB * iSizeF * iRowsC];
493
494     for (int i = 0; i < iSizeF; i++)
495     {
496         int ig = i * iColsB * iRowsC;
497         C2F(dfrmg)( &iJob, &iRowsA, &iRowsA, &iRowsC, &iRowsC, &iColsB, &iRowsA,
498                     pDblA->get(), pDblB->get(), pDblC->get(), pdblF,
499                     pdblFImg, pdblWgr + ig, pdblWgi + ig, &dRcond, pdblW, pdblW1);
500
501         if (bFirst && dRcond + 1 == 1)
502         {
503             sciprint(_("Warning :\n"));
504             sciprint(_("matrix is close to singular or badly scaled. rcond = %g\n"), dRcond);
505             bFirst = false;
506         }
507
508         if (iNbInputArg == 5)
509         {
510             int iSize = iColsB * iRowsC;
511             C2F(dadd)(&iSize, pDblD->get(), &iOne, pdblWgr + ig, &iOne);
512         }
513
514         pdblF++;
515         pdblFImg += iComplex;
516     }
517
518     /*** retrun output arguments ***/
519     types::Double* pDblOut  = new types::Double(iRowsC, iColsB * iSizeF, iComplex == 1);
520     double* pdblOutReal     = pDblOut->get();
521     double* pdblOutImg      = pDblOut->getImg();
522     int iSizeOut            = pDblOut->getSize();
523
524     C2F(dcopy)(&iSizeOut, pdblWgr, &iOne, pdblOutReal, &iOne);
525     if (iComplex)
526     {
527         C2F(dcopy)(&iSizeOut, pdblWgi, &iOne, pdblOutImg, &iOne);
528     }
529
530     // free memory
531     delete[] pdblW;
532     delete[] pdblW1;
533     delete[] pdblWgr;
534     delete[] pdblWgi;
535
536     out.push_back(pDblOut);
537     return types::Function::OK;
538 }
539 /*--------------------------------------------------------------------------*/