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