Remove most instances of setFinalReturns
[openmx:openmx.git] / src / omxWLSFitFunction.cpp
1  /*
2  *  Copyright 2007-2013 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 <R.h>
18 #include <Rinternals.h>
19 #include <Rdefines.h>
20 #include <R_ext/Rdynload.h>
21 #include <R_ext/BLAS.h>
22 #include <R_ext/Lapack.h>
23 #include "omxAlgebraFunctions.h"
24 #include "omxWLSFitFunction.h"
25
26 void flattenDataToVector(omxMatrix* cov, omxMatrix* means, omxThresholdColumn* thresholds, int nThresholds, omxMatrix* vector) {
27     // TODO: vectorize data flattening
28     int nextLoc = 0;
29     for(int j = 0; j < cov->rows; j++) {
30         for(int k = 0; k <= j; k++) {
31             omxSetVectorElement(vector, nextLoc, omxMatrixElement(cov, k, j)); // Use upper triangle in case of SYMM-style mat.
32             nextLoc++;
33         }
34     }
35     if (means != NULL) {
36         for(int j = 0; j < cov->rows; j++) {
37             omxSetVectorElement(vector, nextLoc, omxVectorElement(means, j));
38             nextLoc++;
39         }
40     }
41     if (thresholds != NULL) {
42         for(int j = 0; j < nThresholds; j++) {
43             omxThresholdColumn* thresh = thresholds + j;
44             for(int k = 0; k < thresh->numThresholds; k++) {
45                 omxSetVectorElement(vector, nextLoc, omxMatrixElement(thresh->matrix, k, thresh->column));
46                 nextLoc++;
47             }
48         }
49     }
50 }
51
52 void omxDestroyWLSFitFunction(omxFitFunction *oo) {
53
54         if(OMX_DEBUG) {mxLog("Freeing WLS FitFunction.");}
55         omxWLSFitFunction* owo = ((omxWLSFitFunction*)oo->argStruct);
56
57     if(owo->observedFlattened != NULL) omxFreeMatrixData(owo->observedFlattened);
58     if(owo->expectedFlattened != NULL) omxFreeMatrixData(owo->expectedFlattened);
59     if(owo->weights != NULL) omxFreeMatrixData(owo->weights);
60     if(owo->B != NULL) omxFreeMatrixData(owo->B);
61     if(owo->P != NULL) omxFreeMatrixData(owo->P);
62 }
63
64 static void omxCallWLSFitFunction(omxFitFunction *oo, int want, FitContext *) {
65         if (want & FF_COMPUTE_PREOPTIMIZE) return;
66
67         if(OMX_DEBUG) { mxLog("Beginning WLS Evaluation.");}
68         // Requires: Data, means, covariances.
69
70         double sum = 0.0;
71
72         omxMatrix *oCov, *oMeans, *eCov, *eMeans, *P, *B, *weights, *oFlat, *eFlat;
73         
74     omxThresholdColumn *oThresh, *eThresh;
75
76         omxWLSFitFunction *owo = ((omxWLSFitFunction*)oo->argStruct);
77         
78     /* Locals for readability.  Compiler should cut through this. */
79         oCov            = owo->observedCov;
80         oMeans          = owo->observedMeans;
81         oThresh         = owo->observedThresholds;
82         eCov            = owo->expectedCov;
83         eMeans          = owo->expectedMeans;
84         eThresh         = owo->expectedThresholds;
85         oFlat           = owo->observedFlattened;
86         eFlat           = owo->expectedFlattened;
87         weights         = owo->weights;
88         B                       = owo->B;
89         P                       = owo->P;
90     int nThresh = owo->nThresholds;
91     int onei    = 1;
92         
93         omxExpectation* expectation = oo->expectation;
94
95     /* Recompute and recopy */
96         omxExpectationCompute(expectation, NULL);
97
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));
111     }
112     sum = F77_CALL(ddot)(&(P->cols), P->data, &onei, B->data, &onei);
113
114     oo->matrix->data[0] = sum;
115
116         if(OMX_DEBUG) { mxLog("WLSFitFunction value comes to: %f.", oo->matrix->data[0]); }
117
118 }
119
120 void omxPopulateWLSAttributes(omxFitFunction *oo, SEXP algebra) {
121     if(OMX_DEBUG) { mxLog("Populating WLS Attributes."); }
122
123         omxWLSFitFunction *argStruct = ((omxWLSFitFunction*)oo->argStruct);
124         omxMatrix *expCovInt = argStruct->expectedCov;                  // Expected covariance
125         omxMatrix *expMeanInt = argStruct->expectedMeans;                       // Expected means
126         omxMatrix *weightInt = argStruct->weights;                      // Expected means
127
128         SEXP expCovExt, expMeanExt, weightExt, gradients;
129         PROTECT(expCovExt = allocMatrix(REALSXP, expCovInt->rows, expCovInt->cols));
130         for(int row = 0; row < expCovInt->rows; row++)
131                 for(int col = 0; col < expCovInt->cols; col++)
132                         REAL(expCovExt)[col * expCovInt->rows + row] =
133                                 omxMatrixElement(expCovInt, row, col);
134
135         if (expMeanInt != NULL) {
136                 PROTECT(expMeanExt = allocMatrix(REALSXP, expMeanInt->rows, expMeanInt->cols));
137                 for(int row = 0; row < expMeanInt->rows; row++)
138                         for(int col = 0; col < expMeanInt->cols; col++)
139                                 REAL(expMeanExt)[col * expMeanInt->rows + row] =
140                                         omxMatrixElement(expMeanInt, row, col);
141         } else {
142                 PROTECT(expMeanExt = allocMatrix(REALSXP, 0, 0));               
143         }
144         
145         PROTECT(weightExt = allocMatrix(REALSXP, weightInt->rows, weightInt->cols));
146         for(int row = 0; row < weightInt->rows; row++)
147                 for(int col = 0; col < weightInt->cols; col++)
148                         REAL(weightExt)[col * weightInt->rows + row] =
149                                 omxMatrixElement(weightInt, row, col);
150         
151         
152         if(0) {  /* TODO fix for new internal API
153                 int nLocs = Global->numFreeParams;
154                 double gradient[Global->numFreeParams];
155                 for(int loc = 0; loc < nLocs; loc++) {
156                         gradient[loc] = NA_REAL;
157                 }
158                 //oo->gradientFun(oo, gradient);
159                 PROTECT(gradients = allocMatrix(REALSXP, 1, nLocs));
160
161                 for(int loc = 0; loc < nLocs; loc++)
162                         REAL(gradients)[loc] = gradient[loc];
163                  */
164         } else {
165                 PROTECT(gradients = allocMatrix(REALSXP, 0, 0));
166         }
167     
168         setAttrib(algebra, install("expCov"), expCovExt);
169         setAttrib(algebra, install("expMean"), expMeanExt);
170         setAttrib(algebra, install("weights"), weightExt);
171         setAttrib(algebra, install("gradients"), gradients);
172         
173         setAttrib(algebra, install("SaturatedLikelihood"), ScalarReal(0));
174         setAttrib(algebra, install("IndependenceLikelihood"), ScalarReal(0));
175         setAttrib(algebra, install("ADFMisfit"), ScalarReal(omxMatrixElement(oo->matrix, 0, 0)));
176
177         UNPROTECT(4);
178 }
179
180 void omxSetWLSFitFunctionCalls(omxFitFunction* oo) {
181         
182         /* Set FitFunction Calls to WLS FitFunction Calls */
183         oo->computeFun = omxCallWLSFitFunction;
184         oo->destructFun = omxDestroyWLSFitFunction;
185         oo->populateAttrFun = omxPopulateWLSAttributes;
186 }
187
188 void omxInitWLSFitFunction(omxFitFunction* oo) {
189     
190         omxMatrix *cov, *means, *weights;
191         
192     if(OMX_DEBUG) { mxLog("Initializing WLS FitFunction function."); }
193         
194     int vectorSize = 0;
195         
196         omxSetWLSFitFunctionCalls(oo);
197         
198         if(OMX_DEBUG) { mxLog("Retrieving expectation.\n"); }
199         if (!oo->expectation) { error("%s requires an expectation", oo->fitType); }
200         
201         if(OMX_DEBUG) { mxLog("Retrieving data.\n"); }
202     omxData* dataMat = oo->expectation->data;
203         
204         if(strncmp(omxDataType(dataMat), "acov", 4) != 0 && strncmp(omxDataType(dataMat), "cov", 3) != 0) {
205                 char *errstr = (char*) calloc(250, sizeof(char));
206                 sprintf(errstr, "WLS FitFunction unable to handle data type %s.  Data must be of type 'acov'.\n", omxDataType(dataMat));
207                 omxRaiseError(oo->matrix->currentState, -1, errstr);
208                 free(errstr);
209                 if(OMX_DEBUG) { mxLog("WLS FitFunction unable to handle data type %s.  Aborting.", omxDataType(dataMat)); }
210                 return;
211         }
212
213         omxWLSFitFunction *newObj = (omxWLSFitFunction*) R_alloc(1, sizeof(omxWLSFitFunction));
214
215     /* Get Expectation Elements */
216         newObj->expectedCov = omxGetExpectationComponent(oo->expectation, oo, "cov");
217         newObj->expectedMeans = omxGetExpectationComponent(oo->expectation, oo, "means");
218     newObj->nThresholds = oo->expectation->numOrdinal;
219     newObj->expectedThresholds = oo->expectation->thresholds;
220     // FIXME: threshold structure should be asked for by omxGetExpectationComponent
221
222         /* Read and set expected means, variances, and weights */
223     cov = omxDataMatrix(dataMat, NULL);
224     means = omxDataMeans(dataMat, NULL, NULL);
225     weights = omxDataAcov(dataMat, NULL);
226
227     newObj->observedCov = cov;
228     newObj->observedMeans = means;
229     newObj->weights = weights;
230     newObj->n = omxDataNumObs(dataMat);
231     newObj->nThresholds = omxDataNumFactor(dataMat);
232         UNPROTECT(1);
233         
234         // Error Checking: Observed/Expected means must agree.  
235         // ^ is XOR: true when one is false and the other is not.
236         if((newObj->expectedMeans == NULL) ^ (newObj->observedMeans == NULL)) {
237             if(newObj->expectedMeans != NULL) {
238                     omxRaiseError(oo->matrix->currentState, OMX_ERROR,
239                             "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");
240                     return;
241             } else {
242                     omxRaiseError(oo->matrix->currentState, OMX_ERROR,
243                             "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");
244                     return;             
245             }
246         }
247
248         if((newObj->expectedThresholds == NULL) ^ (newObj->observedThresholds == NULL)) {
249             if(newObj->expectedMeans != NULL) {
250                     omxRaiseError(oo->matrix->currentState, OMX_ERROR,
251                             "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 ");
252                     return;
253             } else {
254                     omxRaiseError(oo->matrix->currentState, OMX_ERROR,
255                             "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");
256                     return;             
257             }
258         }
259
260     /* Error check weight matrix size */
261     int ncol = newObj->observedCov->cols;
262     vectorSize = (ncol * (ncol + 1) ) / 2;
263     if(newObj->expectedMeans != NULL) {
264         vectorSize = vectorSize + ncol;
265     }
266     if(newObj->observedThresholds != NULL) {
267         for(int i = 0; i < newObj->nThresholds; i++) {
268             vectorSize = vectorSize + newObj->observedThresholds[i].numThresholds;
269         }
270     }
271
272     if(weights != NULL && weights->rows != weights->cols && weights->cols != vectorSize) {
273         omxRaiseError(oo->matrix->currentState, OMX_DEVELOPER_ERROR,
274          "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.");
275      return;
276     }
277
278         
279         // FIXME: More error checking for incoming Fit Functions
280
281         /* Temporary storage for calculation */
282         newObj->observedFlattened = omxInitMatrix(NULL, vectorSize, 1, TRUE, oo->matrix->currentState);
283         newObj->expectedFlattened = omxInitMatrix(NULL, vectorSize, 1, TRUE, oo->matrix->currentState);
284         newObj->P = omxInitMatrix(NULL, 1, vectorSize, TRUE, oo->matrix->currentState);
285         newObj->B = omxInitMatrix(NULL, vectorSize, 1, TRUE, oo->matrix->currentState);
286
287     flattenDataToVector(newObj->observedCov, newObj->observedMeans, newObj->observedThresholds, newObj->nThresholds, newObj->observedFlattened);
288     flattenDataToVector(newObj->expectedCov, newObj->expectedMeans, newObj->expectedThresholds, newObj->nThresholds, newObj->expectedFlattened);
289
290     oo->argStruct = (void*)newObj;
291
292 }