utf: module signal_processing
[scilab.git] / scilab / modules / signal_processing / sci_gateway / cpp / sci_rpem.cpp
1 /*
2 * Scilab ( http://www.scilab.org/ ) - This file is part of Scilab
3 * Copyright (C) 2011 - DIGITEO - 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
14 #include "signal_gw.hxx"
15 #include "list.hxx"
16 #include "double.hxx"
17 #include "function.hxx"
18
19 extern "C"
20 {
21 #include "sci_malloc.h"
22 #include "localization.h"
23 #include "Scierror.h"
24
25
26     extern void C2F(rpem)(double theta[], double p[],
27                           int *order,
28                           double *u, double *y, double *lambda, double *k, double *c,
29                           int *istab2,
30                           double *v, double *eps, double *eps1,
31                           int *dimension,
32                           double phi[], double psi[], double tstab[],
33                           double work[], double f[], double g[], double l[]);
34 }
35 /*--------------------------------------------------------------------------*/
36 types::Function::ReturnValue sci_rpem(types::typed_list &in, int _iRetCount, types::typed_list &out)
37 {
38     double* data    = NULL;
39     double* u       = NULL;
40     double* y       = NULL;
41     double* tstab   = NULL;
42     double* work    = NULL;
43     double* f       = NULL;
44     double* g       = NULL;
45
46     double v        = 0;
47     double eps      = 0;
48     double eps1     = 0;
49
50     double lambda   = 0.950l;
51     double alpha    = 0.990l;
52     double beta     = 0.01l;
53     double kappa    = 0.000l;
54     double mu       = 0.980l;
55     double nu       = 0.020l;
56     double c        = 1000.0l;
57
58     int order       = 0;
59     int dimension   = 0;
60     int istab2      = 0;
61     int u_length    = 0;
62
63     types::Double* dTheta   = NULL;
64     types::Double* dP       = NULL;
65     types::Double* dPhi     = NULL;
66     types::Double* dPsi     = NULL;
67     types::Double* dL       = NULL;
68
69     if (in.size() < 3 || in.size() > 6)
70     {
71         Scierror(77, _("%s: Wrong number of input argument(s): %d to %d expected.\n"), "rpem", 3, 6);
72         return types::Function::Error;
73     }
74
75     /* arg1: w0 = list(theta, p, l, phi, psi) */
76     if ((in[0]->isList() == false) || in[0]->getAs<types::List>()->getSize() != 5)
77     {
78         Scierror(999, _("%s: Wrong type for input argument #%d: %d-element list expected.\n"), "rpem", 1, 5);
79         return types::Function::Error;
80     }
81
82     types::List* w0 = in[0]->getAs<types::List>();
83     for (int i = 0; i < 5; i++)
84     {
85         if (!w0->get(i)->isDouble() || w0->get(i)->getAs<types::Double>()->isComplex())
86         {
87             Scierror(77, _("%s: Wrong type for element %d of input argument #%d: A matrix of real expected.\n"), "rpem", i + 1, 1);
88             return types::Function::Error;
89         }
90         types::Double* current = w0->get(i)->getAs<types::Double>();
91         switch (i)
92         {
93             case 0:  /* theta: 3n real ranged row vector */
94             {
95                 if (current->getRows() != 1)
96                 {
97                     Scierror(77, _("%s: Wrong size for element %d of input argument #%d: A row vector expected.\n"), "rpem", i + 1, 1);
98                     return types::Function::Error;
99                 }
100                 if (current->getCols() % 3 != 0)
101                 {
102                     Scierror(77, _("%s: Wrong size for element %d of input argument #%d: Size must be multiple of %d.\n"), "rpem", i + 1, 1, 3);
103                     return types::Function::Error;
104                 }
105                 dimension = current->getCols();
106                 order = dimension / 3;
107                 dTheta = new types::Double(1, dimension);
108                 dTheta->set(current->get());
109             }
110             break;
111             case 1:  /* p: 3n x 3n real ranged matrix */
112             {
113                 if (current->getRows() != dimension || current->getCols() != dimension)
114                 {
115                     Scierror(77, _("%s: Wrong size for element %d of input argument #%d: A square matrix expected.\n"), "rpem", i + 1, 1);
116                     return types::Function::Error;
117                 }
118                 dP = new types::Double(dimension, dimension);
119                 dP->set(current->get());
120             }
121             break;
122             case 2:  /* l: 3n real ranged row vector */
123             case 3:  /* phi: 3n real ranged row vector */
124             case 4:  /* psi: 3n real ranged row vector */
125             {
126                 if (current->getRows() != 1 || current->getCols() != dimension)
127                 {
128                     Scierror(77, _("%s: Wrong size for element %d of input argument #%d: Same sizes of element %d expected.\n"), "rpem", i + 1, 1, 1);
129                     return types::Function::Error;
130                 }
131             }
132         }
133     }
134
135     dL      = new types::Double(1, dimension);
136     dPhi    = new types::Double(1, dimension);
137     dPsi    = new types::Double(1, dimension);
138
139     dL->set(w0->get(2)->getAs<types::Double>()->get());
140     dPhi->set(w0->get(3)->getAs<types::Double>()->get());
141     dPsi->set(w0->get(4)->getAs<types::Double>()->get());
142
143     /* arg2: u0: real ranged row vector */
144     if ((in[1]->isDouble() == false) || in[1]->getAs<types::Double>()->getRows() != 1)
145     {
146         Scierror(999, _("%s: Wrong size for input argument #%d: A row vector expected.\n"), "rpem", 2);
147         return types::Function::Error;
148     }
149     u = in[1]->getAs<types::Double>()->get();
150     u_length = in[1]->getAs<types::Double>()->getCols();
151
152     /* arg3: y0: real ranged row vector of same length as u0 */
153     if ((in[2]->isDouble() == false) || in[2]->getAs<types::Double>()->getRows() != 1)
154     {
155         Scierror(999, _("%s: Wrong size for input argument #%d: A row vector expected.\n"), "rpem", 3);
156         return types::Function::Error;
157     }
158     if (in[2]->getAs<types::Double>()->getCols() != u_length)
159     {
160         Scierror(999, _("%s: Incompatible input arguments #%d and #%d: Same column dimensions expected.\n"), "rpem", 2, 3);
161         return types::Function::Error;
162     }
163     y = in[2]->getAs<types::Double>()->get();
164
165     /* optional arguments */
166     switch (in.size())
167     {
168         case 6: /* c */
169         {
170             if ((in[5]->isDouble() == false) || !in[5]->getAs<types::Double>()->isScalar())
171             {
172                 Scierror(999, _("%s: Wrong size for input argument #%d: A scalar expected.\n"), "rpem", 6);
173                 return types::Function::Error;
174             }
175             c = in[5]->getAs<types::Double>()->get(0);
176         }
177         case 5: /* [kappa, mu, nu] */
178         {
179             if ((in[4]->isDouble() == false) || (in[4]->getAs<types::Double>()->getRows() != 1) || (in[4]->getAs<types::Double>()->getCols() != 3))
180             {
181                 Scierror(999, _("%s: Wrong size for input argument #%d: A %d-by-%d matrix expected.\n"), "rpem", 5, 1, 3);
182                 return types::Function::Error;
183             }
184             data    = in[4]->getAs<types::Double>()->get();
185             kappa   = data[0];
186             mu      = data[1];
187             nu      = data[2];
188         }
189         case 4: /* [lambda, alpha, beta] */
190         {
191             if ((in[3]->isDouble() == false) || (in[3]->getAs<types::Double>()->getRows() != 1) || (in[3]->getAs<types::Double>()->getCols() != 3))
192             {
193                 Scierror(999, _("%s: Wrong size for input argument #%d: A %d-by-%d matrix expected.\n"), "rpem", 4, 1, 3);
194                 return types::Function::Error;
195             }
196             data    = in[3]->getAs<types::Double>()->get();
197             lambda  = data[0];
198             alpha   = data[1];
199             beta    = data[2];
200         }
201     }
202
203     /*** algorithm call ***/
204
205     /* references provided to justify allocation with code relying on it */
206     f = (double *) MALLOC((dimension) * sizeof(double));        /* rpem.f l.168 */
207     memset(f, 0x00, (dimension) * sizeof(double));
208     g = (double *) MALLOC((dimension) * sizeof(double));        /* rpem.f l.169 */
209     memset(g, 0x00, (dimension) * sizeof(double));
210     tstab = (double *) MALLOC((order + 1) * sizeof(double));    /* rpem.f l.105 */
211     memset(tstab, 0x00, (order + 1) * sizeof(double));
212     work = (double *) MALLOC((2 * order + 2) * sizeof(double)); /* nstabl.f */
213     memset(work, 0x00, (2 * order + 2) * sizeof(double));
214     /* (tip: bound variables to determine required memory: nk1 <= ordre + 1) */
215
216     for (int i = 1 ; i < u_length ; ++i)
217     {
218         C2F(rpem)(  dTheta->get(), dP->get(), &order, &(u[i - 1]), &(y[i]), &lambda, &kappa, &c,
219                     &istab2, &v, &eps, &eps1, //output
220                     &dimension, dPhi->get(), dPsi->get(),
221                     tstab, work, //output
222                     f, g, dL->get());
223
224         lambda = alpha * lambda + beta;
225         kappa = mu * kappa + nu;
226     }
227
228     FREE(work);
229     FREE(tstab);
230     FREE(g);
231     FREE(f);
232
233     /*** output formatting ***/
234
235     types::List* resultList = new types::List();
236     resultList->append(dTheta);
237
238     resultList->append(dP);
239     resultList->append(dL);
240     resultList->append(dPhi);
241     resultList->append(dPsi);
242
243     out.push_back(resultList);
244
245     if (_iRetCount == 2)
246     {
247         types::Double* dV = new types::Double(1, 1);
248         dV->set(&v);
249         out.push_back(dV);
250     }
251
252     return types::Function::OK;
253 }
254 /*--------------------------------------------------------------------------*/
255