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