030eff27befc05648e707289ba7b23b375e0bf1c
[scilab.git] / scilab / modules / signal_processing / sci_gateway / c / sci_rpem.c
1 /*
2  * Scilab ( http://www.scilab.org/ ) - This file is part of Scilab
3  * Copyright (C) 2006 - INRIA - Allan CORNET
4  * Copyright (C) 2009 - Digiteo - Vincent LIARD
5  *
6  * This file must be used under the terms of the CeCILL.
7  * This source file is licensed as described in the file COPYING, which
8  * you should have received as part of this distribution.  The terms
9  * are also available at
10  * http://www.cecill.info/licences/Licence_CeCILL_V2-en.txt
11  *
12  */
13
14 #include "api_scilab.h"
15 #include "gw_signal.h"
16 #include "MALLOC.h"
17 #include "stack-c.h"
18 #include "Scierror.h"
19 #include "localization.h"
20
21 extern void C2F(rpem)(double theta[], double p[],
22                       int *order,
23                       double *u, double *y, double *lambda, double *k, double *c,
24                       int *istab2,
25                       double *v, double *eps, double *eps1,
26                       int *dimension,
27                       double phi[], double psi[], double tstab[],
28                       double work[], double f[], double g[], double l[]);
29
30 int sci_rpem(char *fname, int* _piKey)
31 {
32   double *data, *theta, *p, *l, *phi, *psi, *u, *y, *tstab, *work, *f, *g;
33   double v, eps, eps1;
34   double lambda = 0.950l, alpha = 0.990l, beta = 0.01l, kappa = 0.000l, mu = 0.980l, nu = 0.020l, c = 1000.0l;
35   int rows, cols, type, order, dimension, istab2, u_length, i;
36   int *arg, *it, *res;
37
38
39   /*** arguments capture ***/
40
41   CheckRhs(3,6);
42   CheckLhs(1,2);
43
44   /* arg1: w0 = list(theta, p, l, phi, psi) */
45   getVarAddressFromPosition(_piKey, 1, &arg);
46   getListItemNumber(_piKey, arg, &rows);
47         getVarType(_piKey, arg, &type);
48   if (type != sci_list || rows != 5) {
49     Scierror(999, _("%s: Wrong size for input argument #%d: %d-element list expected.\n"), fname, 1, 5);
50     return 1;
51   }
52
53   /** @todo type of each matrix in w0 should be checked to real
54    sci_matrix (better placed in matrix retrieval functions) */
55   /** @todo error messages should be more accurate */
56
57   /* theta: 3n real ranged row vector */
58   getListItemAddress(_piKey, arg, 1, &it);
59   getMatrixOfDouble(_piKey, it, &rows, &cols, &theta);
60   if (rows != 1) {
61     Scierror(999, _("%s: Wrong size for input argument #%d: A row vector expected.\n"), fname, 1);
62     return 1;
63   }
64   if (cols % 3 != 0) {
65     Scierror(999, _("%s: Wrong size for input argument #%d: Size must be multiple of %d.\n"), fname, 1, 3);
66     return 1;
67   }
68   dimension = cols;
69   order = dimension / 3;
70
71   /* p: 3n x 3n real ranged matrix */
72   getListItemAddress(_piKey, arg, 2, &it);
73   getMatrixOfDouble(_piKey, it, &rows, &cols, &p);
74   if (rows != dimension || cols != dimension) {
75     Scierror(999, _("%s: Wrong size for input argument #%d: A square matrix expected.\n"), fname, 1, 1);
76     return 1;
77   }
78   /* l: 3n real ranged row vector */
79   getListItemAddress(_piKey, arg, 3, &it);
80   getMatrixOfDouble(_piKey, it, &rows, &cols, &l);
81   if (rows != 1 || cols != dimension) {
82     Scierror(999, _("%s: Incompatible input arguments #%d and #%d': Same sizes expected.\n"), fname, 1, 1);
83     return 1;
84   }
85   /* phi: 3n real ranged row vector */
86   getListItemAddress(_piKey, arg, 4, &it);
87   getMatrixOfDouble(_piKey, it, &rows, &cols, &phi);
88   if (rows != 1 || cols != dimension) {
89     Scierror(999, _("%s: Incompatible input arguments #%d and #%d': Same sizes expected.\n"), fname, 1, 1);
90     return 1;
91   }
92   /* psi: 3n real ranged row vector */
93   getListItemAddress(_piKey, arg, 5, &it);
94   getMatrixOfDouble(_piKey, it, &rows, &cols, &psi);
95   if (rows != 1 || cols != dimension) {
96     Scierror(999, _("%s: Incompatible input arguments #%d and #%d': Same sizes expected.\n"), fname, 1, 1);
97     return 1;
98   }
99
100   /* arg2: u0: real ranged row vector */
101   getVarAddressFromPosition(_piKey, 2, &arg);
102   getMatrixOfDouble(_piKey, arg, &rows, &cols, &u);
103   getVarType(_piKey, arg, &type);
104   if((type != sci_matrix) || (rows != 1)) {
105     Scierror(999, _("%s: Wrong size for input argument #%d: A row vector expected.\n"), fname, 2);
106     return 1;
107   }
108   u_length = cols;
109
110   /* arg3: y0: real ranged row vector of same length as u0 */
111   getVarAddressFromPosition(_piKey, 3, &arg);
112   getMatrixOfDouble(_piKey, arg, &rows, &cols, &y);
113   getVarType(_piKey, arg, &type);
114   if ((type != sci_matrix) || (rows != 1) || (cols != u_length)) {
115     Scierror(999, _("%s: Wrong size for input argument #%d: A row vector expected.\n"), fname, 3);
116     Scierror(999, _("%s: Incompatible input arguments #%d and #%d: Same column dimensions expected.\n"), fname, 2, 3);
117     return 1;
118   }
119
120   /* optional arguments */
121   switch (Rhs) {
122   case 6: /* c */
123     getVarAddressFromPosition(_piKey, 6, &arg);
124     getMatrixOfDouble(_piKey, arg, &rows, &cols, &data);
125     getVarType(_piKey,  arg, &type);
126     if (type != sci_matrix || rows != 1 || cols != 1) {
127       Scierror(999, _("%s: Wrong size for input argument #%d: A scalar expected.\n"), fname, 6);
128       return 1;
129     }
130     c = *data;
131   case 5: /* [kappa, mu, nu] */
132     getVarAddressFromPosition(_piKey, 5, &arg);
133     getMatrixOfDouble(_piKey, arg, &rows, &cols, &data);
134     getVarType(_piKey,  arg, &type);
135     if (type != sci_matrix || rows != 1 || cols != 3) {
136       Scierror(999, _("%s: Wrong size for input argument #%d: A %d-by-%d matrix expected.\n"), fname, 5, 1, 3);
137       return 1;
138     }
139     kappa = data[0];
140     mu = data[1];
141     nu = data[2];
142   case 4: /* [lambda, alpha, beta] */
143     getVarAddressFromPosition(_piKey, 4, &arg);
144     getMatrixOfDouble(_piKey, arg, &rows, &cols, &data);
145     getVarType(_piKey,  arg, &type);
146     if (type != sci_matrix || rows != 1 || cols != 3) {
147       Scierror(999, _("%s: Wrong size for input argument #%d: A %d-by-%d matrix expected.\n"), fname, 4, 1, 3);
148       return 1;
149     }
150                 lambda = data[0];
151                 alpha = data[1];
152                 beta = data[2];
153   }
154
155   /*** algorithm call ***/
156
157   /* references provided to justify allocation with code relying on it */
158   f = (double *) MALLOC((dimension) * sizeof(double));          /* rpem.f l.168 */
159   g = (double *) MALLOC((dimension) * sizeof(double));          /* rpem.f l.169  */
160   tstab = (double *) MALLOC((order + 1) * sizeof(double));      /* rpem.f l.105 */
161   work = (double *) MALLOC((2 * order + 2) * sizeof(double));   /* nstabl.f */
162   /* (tip: bound variables to determine required memory: nk1 <= ordre + 1) */
163
164   for (i = 1 ; i < u_length ; ++i) {
165     C2F(rpem)(theta, p, &order, &(u[i-1]), &(y[i]), &lambda, &kappa, &c,
166               &istab2, &v, &eps, &eps1, //output
167               &dimension, phi, psi,
168               tstab,work, //output
169               f, g, l);
170     lambda = alpha * lambda + beta;
171     kappa = mu * kappa + nu;
172   }
173
174   FREE(work);
175   FREE(tstab);
176   FREE(g);
177   FREE(f);
178
179   /*** output formatting ***/
180
181   createList(_piKey, Rhs + 1, 5, &res);
182   createMatrixOfDoubleInList(_piKey, Rhs + 1, res, 1, 1, dimension, theta);
183   createMatrixOfDoubleInList(_piKey, Rhs + 1, res, 2, dimension, dimension, p);
184   createMatrixOfDoubleInList(_piKey, Rhs + 1, res, 3, 1, dimension, l);
185   createMatrixOfDoubleInList(_piKey, Rhs + 1, res, 4, 1, dimension, phi);
186   createMatrixOfDoubleInList(_piKey, Rhs + 1, res, 5, 1, dimension, psi);
187   LhsVar(1) = Rhs + 1;
188   if (Lhs == 2) {
189     //allocMatrixOfDouble(_piKey, Rhs + 2, 1, 1, &data);
190     createMatrixOfDouble(_piKey, Rhs + 2, 1, 1, &v);
191     LhsVar(2) = Rhs + 2;
192   }
193   PutLhsVar();
194   return 0;
195 }