Enable R_NO_REMAP for a cleaner namespace
[openmx:openmx.git] / src / omxWLSFitFunction.cpp
1  /*
2  *  Copyright 2007-2014 The OpenMx Project
3  *
4  *  Licensed under the Apache License, Version 2.0 (the "License");
5  *  you may not use this file except in compliance with the License.
6  *  You may obtain a copy of the License at
7  *
8  *       http://www.apache.org/licenses/LICENSE-2.0
9  *
10  *   Unless required by applicable law or agreed to in writing, software
11  *   distributed under the License is distributed on an "AS IS" BASIS,
12  *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  *  See the License for the specific language governing permissions and
14  *  limitations under the License.
15  */
16
17 #include "omxAlgebraFunctions.h"
18 #include "omxWLSFitFunction.h"
19
20 void flattenDataToVector(omxMatrix* cov, omxMatrix* means, omxThresholdColumn* thresholds, int nThresholds, omxMatrix* vector) {
21     // TODO: vectorize data flattening
22     // if(OMX_DEBUG) { mxLog("Flattening out data vectors: cov 0x%x, mean 0x%x, thresh 0x%x[n=%d] ==> 0x%x", 
23     //         cov, means, thresholds, nThresholds, vector); }
24     
25     int nextLoc = 0;
26     for(int j = 0; j < cov->rows; j++) {
27         for(int k = 0; k <= j; k++) {
28             omxSetVectorElement(vector, nextLoc, omxMatrixElement(cov, k, j)); // Use upper triangle in case of SYMM-style mat.
29             nextLoc++;
30         }
31     }
32     if (means != NULL) {
33         for(int j = 0; j < cov->rows; j++) {
34             omxSetVectorElement(vector, nextLoc, omxVectorElement(means, j));
35             nextLoc++;
36         }
37     }
38     if (thresholds != NULL) {
39         for(int j = 0; j < nThresholds; j++) {
40             omxThresholdColumn* thresh = thresholds + j;
41             for(int k = 0; k < thresh->numThresholds; k++) {
42                 omxSetVectorElement(vector, nextLoc, omxMatrixElement(thresh->matrix, k, thresh->column));
43                 nextLoc++;
44             }
45         }
46     }
47 }
48
49 void omxDestroyWLSFitFunction(omxFitFunction *oo) {
50
51         if(OMX_DEBUG) {mxLog("Freeing WLS FitFunction.");}
52     if(oo->argStruct == NULL) return;
53 //Introduce memory leak by not destroying the P matrix.  Try and Rf_error found that freeing P caused an Rf_error.  Without freeing P sometimes the return to R finishes successfully and sometimes not.
54         omxWLSFitFunction* owo = ((omxWLSFitFunction*)oo->argStruct);
55     omxFreeMatrixData(owo->observedFlattened);
56     omxFreeMatrixData(owo->expectedFlattened);
57     omxFreeMatrixData(owo->weights);
58     omxFreeMatrixData(owo->B);
59     //omxFreeMatrixData(owo->P);
60 }
61
62 static void omxCallWLSFitFunction(omxFitFunction *oo, int want, FitContext *) {
63         if (want & (FF_COMPUTE_PREOPTIMIZE)) return;
64
65         if(OMX_DEBUG) { mxLog("Beginning WLS Evaluation.");}
66         // Requires: Data, means, covariances.
67
68         double sum = 0.0;
69
70         omxMatrix *oCov, *oMeans, *eCov, *eMeans, *P, *B, *weights, *oFlat, *eFlat;
71         
72     omxThresholdColumn *oThresh, *eThresh;
73
74         omxWLSFitFunction *owo = ((omxWLSFitFunction*)oo->argStruct);
75         
76     /* Locals for readability.  Compiler should cut through this. */
77         oCov            = owo->observedCov;
78         oMeans          = owo->observedMeans;
79         oThresh         = owo->observedThresholds;
80         eCov            = owo->expectedCov;
81         eMeans          = owo->expectedMeans;
82         eThresh         = owo->expectedThresholds;
83         oFlat           = owo->observedFlattened;
84         eFlat           = owo->expectedFlattened;
85         weights         = owo->weights;
86         B                       = owo->B;
87         P                       = owo->P;
88     int nThresh = owo->nThresholds;
89     int onei    = 1;
90         
91         omxExpectation* expectation = oo->expectation;
92
93     /* Recompute and recopy */
94         if(OMX_DEBUG) { mxLog("WLSFitFunction Computing expectation"); }
95         omxExpectationCompute(expectation, NULL);
96
97     // TODO: Flatten data only once.
98         flattenDataToVector(oCov, oMeans, oThresh, nThresh, oFlat);
99         flattenDataToVector(eCov, eMeans, eThresh, nThresh, eFlat);
100
101         omxCopyMatrix(B, oFlat);
102
103         omxDAXPY(-1.0, eFlat, B);
104         
105     if(weights != NULL) {
106         omxDGEMV(TRUE, 1.0, weights, B, 0.0, P);
107     } else {
108         // ULS Case: Memcpy faster than dgemv.
109         // TODO: Better to use an omxMatrix duplicator here.
110         memcpy(P, B, B->cols*sizeof(double)); //omxCopyMatrix()
111     }
112
113     sum = F77_CALL(ddot)(&(P->cols), P->data, &onei, B->data, &onei);
114
115     oo->matrix->data[0] = sum;
116
117         if(OMX_DEBUG) { mxLog("WLSFitFunction value comes to: %f.", oo->matrix->data[0]); }
118
119 }
120
121 void omxPopulateWLSAttributes(omxFitFunction *oo, SEXP algebra) {
122     if(OMX_DEBUG) { mxLog("Populating WLS Attributes."); }
123
124         omxWLSFitFunction *argStruct = ((omxWLSFitFunction*)oo->argStruct);
125         omxMatrix *expCovInt = argStruct->expectedCov;                  // Expected covariance
126         omxMatrix *expMeanInt = argStruct->expectedMeans;                       // Expected means
127         omxMatrix *weightInt = argStruct->weights;                      // Expected means
128
129         SEXP expCovExt, expMeanExt, weightExt, gradients;
130         Rf_protect(expCovExt = Rf_allocMatrix(REALSXP, expCovInt->rows, expCovInt->cols));
131         for(int row = 0; row < expCovInt->rows; row++)
132                 for(int col = 0; col < expCovInt->cols; col++)
133                         REAL(expCovExt)[col * expCovInt->rows + row] =
134                                 omxMatrixElement(expCovInt, row, col);
135
136         if (expMeanInt != NULL) {
137                 Rf_protect(expMeanExt = Rf_allocMatrix(REALSXP, expMeanInt->rows, expMeanInt->cols));
138                 for(int row = 0; row < expMeanInt->rows; row++)
139                         for(int col = 0; col < expMeanInt->cols; col++)
140                                 REAL(expMeanExt)[col * expMeanInt->rows + row] =
141                                         omxMatrixElement(expMeanInt, row, col);
142         } else {
143                 Rf_protect(expMeanExt = Rf_allocMatrix(REALSXP, 0, 0));         
144         }
145         
146         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(weightInt, "...WLS Weight Matrix: W"); }
147         Rf_protect(weightExt = Rf_allocMatrix(REALSXP, weightInt->rows, weightInt->cols));
148         for(int row = 0; row < weightInt->rows; row++)
149                 for(int col = 0; col < weightInt->cols; col++)
150                         REAL(weightExt)[col * weightInt->rows + row] = weightInt->data[col * weightInt->rows + row];
151                                 //omxMatrixElement(weightInt, row, col);
152         
153         
154         if(0) {  /* TODO fix for new internal API
155                 int nLocs = Global->numFreeParams;
156                 double gradient[Global->numFreeParams];
157                 for(int loc = 0; loc < nLocs; loc++) {
158                         gradient[loc] = NA_REAL;
159                 }
160                 //oo->gradientFun(oo, gradient);
161                 Rf_protect(gradients = Rf_allocMatrix(REALSXP, 1, nLocs));
162
163                 for(int loc = 0; loc < nLocs; loc++)
164                         REAL(gradients)[loc] = gradient[loc];
165                  */
166         } else {
167                 Rf_protect(gradients = Rf_allocMatrix(REALSXP, 0, 0));
168         }
169         
170         if(OMX_DEBUG) { mxLog("Installing populated WLS Attributes."); }
171         Rf_setAttrib(algebra, Rf_install("expCov"), expCovExt);
172         Rf_setAttrib(algebra, Rf_install("expMean"), expMeanExt);
173         Rf_setAttrib(algebra, Rf_install("weights"), weightExt);
174         Rf_setAttrib(algebra, Rf_install("gradients"), gradients);
175         
176         Rf_setAttrib(algebra, Rf_install("SaturatedLikelihood"), Rf_ScalarReal(0));
177         Rf_setAttrib(algebra, Rf_install("IndependenceLikelihood"), Rf_ScalarReal(0));
178         Rf_setAttrib(algebra, Rf_install("ADFMisfit"), Rf_ScalarReal(omxMatrixElement(oo->matrix, 0, 0)));
179         
180         if(OMX_DEBUG) { mxLog("Unprotecting WLS Attributes."); }
181         Rf_unprotect(4);
182 }
183
184 void omxSetWLSFitFunctionCalls(omxFitFunction* oo) {
185         
186         /* Set FitFunction Calls to WLS FitFunction Calls */
187         oo->computeFun = omxCallWLSFitFunction;
188         oo->destructFun = omxDestroyWLSFitFunction;
189         oo->populateAttrFun = omxPopulateWLSAttributes;
190 }
191
192 void omxInitWLSFitFunction(omxFitFunction* oo) {
193     
194         omxMatrix *cov, *means, *weights;
195         
196     if(OMX_DEBUG) { mxLog("Initializing WLS FitFunction function."); }
197         
198     int vectorSize = 0;
199         
200         omxSetWLSFitFunctionCalls(oo);
201         
202         if(OMX_DEBUG) { mxLog("Retrieving expectation.\n"); }
203         if (!oo->expectation) { Rf_error("%s requires an expectation", oo->fitType); }
204         
205         if(OMX_DEBUG) { mxLog("Retrieving data.\n"); }
206     omxData* dataMat = oo->expectation->data;
207         
208         if(strncmp(omxDataType(dataMat), "acov", 4) != 0 && strncmp(omxDataType(dataMat), "cov", 3) != 0) {
209                 char *errstr = (char*) calloc(250, sizeof(char));
210                 sprintf(errstr, "WLS FitFunction unable to handle data type %s.  Data must be of type 'acov'.\n", omxDataType(dataMat));
211                 omxRaiseError(oo->matrix->currentState, -1, errstr);
212                 free(errstr);
213                 if(OMX_DEBUG) { mxLog("WLS FitFunction unable to handle data type %s.  Aborting.", omxDataType(dataMat)); }
214                 return;
215         }
216
217         omxWLSFitFunction *newObj = (omxWLSFitFunction*) R_alloc(1, sizeof(omxWLSFitFunction));
218         oo->argStruct = (void*)newObj;
219         
220     /* Get Expectation Elements */
221         newObj->expectedCov = omxGetExpectationComponent(oo->expectation, oo, "cov");
222         newObj->expectedMeans = omxGetExpectationComponent(oo->expectation, oo, "means");
223     newObj->nThresholds = oo->expectation->numOrdinal;
224     newObj->expectedThresholds = oo->expectation->thresholds;
225     // FIXME: threshold structure should be asked for by omxGetExpectationComponent
226
227         /* Read and set expected means, variances, and weights */
228     cov = omxDataMatrix(dataMat, NULL);
229     means = omxDataMeans(dataMat, NULL, NULL);
230     weights = omxDataAcov(dataMat, NULL);
231         newObj->observedThresholds  = omxDataThresholds(dataMat);
232
233     newObj->observedCov = cov;
234     newObj->observedMeans = means;
235     newObj->weights = weights;
236     newObj->n = omxDataNumObs(dataMat);
237     newObj->nThresholds = omxDataNumFactor(dataMat);
238         //Rf_unprotect(1); //MDH: Why is this here?!?
239         
240         // Error Checking: Observed/Expected means must agree.  
241         // ^ is XOR: true when one is false and the other is not.
242         if((newObj->expectedMeans == NULL) ^ (newObj->observedMeans == NULL)) {
243             if(newObj->expectedMeans != NULL) {
244                     omxRaiseError(oo->matrix->currentState, OMX_ERROR,
245                             "Observed means not detected, but an expected means matrix was specified.\n  If you  wish to model the means, you must provide observed means.\n");
246                     return;
247             } else {
248                     omxRaiseError(oo->matrix->currentState, OMX_ERROR,
249                             "Observed means were provided, but an expected means matrix was not specified.\n  If you provide observed means, you must specify a model for the means.\n");
250                     return;             
251             }
252         }
253 /*
254         if((newObj->expectedThresholds == NULL) ^ (newObj->observedThresholds == NULL)) {
255             if(newObj->expectedThresholds != NULL) {
256                     omxRaiseError(oo->matrix->currentState, OMX_ERROR,
257                             "Observed thresholds not detected, but an expected thresholds matrix was specified.\n   If you wish to model the thresholds, you must provide observed thresholds.\n ");
258                     return;
259             } else {
260                     omxRaiseError(oo->matrix->currentState, OMX_ERROR,
261                             "Observed thresholds were provided, but an expected thresholds matrix was not specified.\nIf you provide observed thresholds, you must specify a model for the thresholds.\n");
262                     return;             
263             }
264         }
265 */
266     /* Error check weight matrix size */
267     int ncol = newObj->observedCov->cols;
268     vectorSize = (ncol * (ncol + 1) ) / 2;
269     if(newObj->expectedMeans != NULL) {
270         vectorSize = vectorSize + ncol;
271     }
272     if(newObj->observedThresholds != NULL) {
273         for(int i = 0; i < newObj->nThresholds; i++) {
274             vectorSize = vectorSize + newObj->observedThresholds[i].numThresholds;
275         }
276     }
277         if(OMX_DEBUG) { mxLog("Intial WLSFitFunction vectorSize comes to: %d.", vectorSize); }
278
279     if(weights != NULL && (weights->rows != weights->cols || weights->cols != vectorSize)) {
280         omxRaiseError(oo->matrix->currentState, OMX_DEVELOPER_ERROR,
281          "Developer Error in WLS-based FitFunction object: WLS-based expectation specified an incorrectly-sized weight matrix.\nIf you are not developing a new expectation type, you should probably post this to the OpenMx forums.");
282      return;
283     }
284
285         
286         // FIXME: More Rf_error checking for incoming Fit Functions
287
288         /* Temporary storage for calculation */
289         newObj->observedFlattened = omxInitMatrix(NULL, vectorSize, 1, TRUE, oo->matrix->currentState);
290         newObj->expectedFlattened = omxInitMatrix(NULL, vectorSize, 1, TRUE, oo->matrix->currentState);
291         newObj->P = omxInitMatrix(NULL, 1, vectorSize, TRUE, oo->matrix->currentState);
292         newObj->B = omxInitMatrix(NULL, vectorSize, 1, TRUE, oo->matrix->currentState);
293
294     flattenDataToVector(newObj->observedCov, newObj->observedMeans, newObj->observedThresholds, newObj->nThresholds, newObj->observedFlattened);
295     flattenDataToVector(newObj->expectedCov, newObj->expectedMeans, newObj->expectedThresholds, newObj->nThresholds, newObj->expectedFlattened);
296
297     //oo->argStruct = (void*)newObj; //MDH: move this earlier?
298
299 }