Add thread-safe logging functions
[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, omxMatrix* vector) {
27     int nextLoc = 0;
28     for(int j = 0; j < cov->rows; j++) {
29         for(int k = 0; k <= j; k++) {
30             omxSetVectorElement(vector, nextLoc, omxMatrixElement(cov, k, j)); // Use upper triangle in case of SYMM-style mat.
31             nextLoc++;
32         }
33     }
34     if (means != NULL) {
35         for(int j = 0; j < cov->rows; j++) {
36             omxSetVectorElement(vector, nextLoc, omxVectorElement(means, j));
37             nextLoc++;
38         }
39     }
40 }
41
42 void omxDestroyWLSFitFunction(omxFitFunction *oo) {
43
44         if(OMX_DEBUG) {mxLog("Freeing WLS FitFunction.");}
45         omxWLSFitFunction* owo = ((omxWLSFitFunction*)oo->argStruct);
46
47     if(owo->observedFlattened != NULL) omxFreeMatrixData(owo->observedFlattened);
48     if(owo->expectedFlattened != NULL) omxFreeMatrixData(owo->expectedFlattened);
49     if(owo->weights != NULL) omxFreeMatrixData(owo->weights);
50     if(owo->B != NULL) omxFreeMatrixData(owo->B);
51     if(owo->P != NULL) omxFreeMatrixData(owo->P);
52 }
53
54 omxRListElement* omxSetFinalReturnsWLSFitFunction(omxFitFunction *oo, int *numReturns) {
55         *numReturns = 4;
56         omxRListElement* retVal = (omxRListElement*) R_alloc(*numReturns, sizeof(omxRListElement));
57
58         retVal[0].numValues = 1;
59         retVal[0].values = (double*) R_alloc(1, sizeof(double));
60         strncpy(retVal[0].label, "Minus2LogLikelihood", 20);
61         retVal[0].values[0] = omxMatrixElement(oo->matrix, 0, 0);
62
63         retVal[1].numValues = 1;
64         retVal[1].values = (double*) R_alloc(1, sizeof(double));
65         strncpy(retVal[1].label, "SaturatedLikelihood", 20);
66     retVal[1].values[0] = 0;
67
68         retVal[2].numValues = 1;
69         retVal[2].values = (double*) R_alloc(1, sizeof(double));
70         strncpy(retVal[2].label, "IndependenceLikelihood", 23);
71     retVal[2].values[0] = 0;
72
73         retVal[3].numValues = 1;
74         retVal[3].values = (double*) R_alloc(1, sizeof(double));
75         strncpy(retVal[3].label, "ADFMisfit", 20);
76         retVal[3].values[0] = omxMatrixElement(oo->matrix, 0, 0);
77
78     // retVal[1].numValues = 1;
79     // retVal[1].values = (double*) R_alloc(1, sizeof(double));
80     // strncpy(retVal[1].label, "SaturatedADFMisfit", 20);
81     // retVal[1].values[0] = (((omxWLSFitFunction*)oo->argStruct)->logDetObserved + ncols) * (((omxWLSFitFunction*)oo->argStruct)->n - 1);
82     // 
83     // retVal[2].numValues = 1;
84     // retVal[2].values = (double*) R_alloc(1, sizeof(double));
85     // strncpy(retVal[2].label, "IndependenceLikelihood", 23);
86     // // Independence model assumes all-zero manifest covariances.
87     // // (det(expected) + tr(observed * expected^-1)) * (n - 1);
88     // // expected is the diagonal of the observed.  Inverse expected is 1/each diagonal value.
89     // // Therefore the diagonal elements of observed * expected^-1 are each 1.
90     // // So the trace of the matrix is the same as the number of columns.
91     // // The determinant of a diagonal matrix is the product of the diagonal elements.
92     // // Since these are the same in the expected as in the observed, we can get 'em direct.
93     // for(int i = 0; i < ncols; i++) {
94     //  // We sum logs instead of logging the product.
95     //  det += log(omxMatrixElement(cov, i, i));
96     // }
97     // if(OMX_DEBUG) { mxLog("det: %f, tr: %f, n= %d, total:%f", det, ncols, ((omxWLSFitFunction*)oo->argStruct)->n, (ncols + det) * (((omxWLSFitFunction*)oo->argStruct)->n - 1)); }
98     // if(OMX_DEBUG) { omxPrint(cov, "Observed:"); }
99     // retVal[2].values[0] = (ncols + det) * (((omxWLSFitFunction*)oo->argStruct)->n - 1);
100
101         return retVal;
102 }
103
104 static void omxCallWLSFitFunction(omxFitFunction *oo, int want, double *gradient) {     // TODO: Figure out how to give access to other per-iteration structures.
105
106         if(OMX_DEBUG) { mxLog("Beginning WLS Evaluation.");}
107         // Requires: Data, means, covariances.
108
109         double sum = 0.0;
110
111         omxMatrix *oCov, *oMeans, *eCov, *eMeans, *P, *B, *weights, *oFlat, *eFlat;
112
113         omxWLSFitFunction *owo = ((omxWLSFitFunction*)oo->argStruct);
114         
115     /* Locals for readability.  Compiler should cut through this. */
116         oCov            = owo->observedCov;
117         oMeans          = owo->observedMeans;
118         eCov            = owo->expectedCov;
119         eMeans          = owo->expectedMeans;
120         oFlat           = owo->observedFlattened;
121         eFlat           = owo->expectedFlattened;
122         weights         = owo->weights;
123         B                       = owo->B;
124         P                       = owo->P;
125     int onei    = 1;
126         
127         omxExpectation* expectation = oo->expectation;
128
129     /* Recompute and recopy */
130         omxExpectationCompute(expectation);
131
132         flattenDataToVector(oCov, oMeans, oFlat);
133         flattenDataToVector(eCov, eMeans, eFlat);
134
135         omxCopyMatrix(B, oFlat);
136
137         omxDAXPY(-1.0, eFlat, B);
138
139     omxDGEMV(TRUE, 1.0, weights, B, 0.0, P);
140         sum = F77_CALL(ddot)(&(P->cols), P->data, &onei, B->data, &onei);
141
142     oo->matrix->data[0] = sum;
143
144         if(OMX_DEBUG) { mxLog("WLSFitFunction value comes to: %f.", oo->matrix->data[0]); }
145
146 }
147
148 void omxPopulateWLSAttributes(omxFitFunction *oo, SEXP algebra) {
149     if(OMX_DEBUG) { mxLog("Populating WLS Attributes."); }
150
151         omxWLSFitFunction *argStruct = ((omxWLSFitFunction*)oo->argStruct);
152         omxMatrix *expCovInt = argStruct->expectedCov;                  // Expected covariance
153         omxMatrix *expMeanInt = argStruct->expectedMeans;                       // Expected means
154         omxMatrix *weightInt = argStruct->weights;                      // Expected means
155
156         SEXP expCovExt, expMeanExt, weightExt, gradients;
157         PROTECT(expCovExt = allocMatrix(REALSXP, expCovInt->rows, expCovInt->cols));
158         for(int row = 0; row < expCovInt->rows; row++)
159                 for(int col = 0; col < expCovInt->cols; col++)
160                         REAL(expCovExt)[col * expCovInt->rows + row] =
161                                 omxMatrixElement(expCovInt, row, col);
162
163         if (expMeanInt != NULL) {
164                 PROTECT(expMeanExt = allocMatrix(REALSXP, expMeanInt->rows, expMeanInt->cols));
165                 for(int row = 0; row < expMeanInt->rows; row++)
166                         for(int col = 0; col < expMeanInt->cols; col++)
167                                 REAL(expMeanExt)[col * expMeanInt->rows + row] =
168                                         omxMatrixElement(expMeanInt, row, col);
169         } else {
170                 PROTECT(expMeanExt = allocMatrix(REALSXP, 0, 0));               
171         }
172         
173         PROTECT(weightExt = allocMatrix(REALSXP, weightInt->rows, weightInt->cols));
174         for(int row = 0; row < weightInt->rows; row++)
175                 for(int col = 0; col < weightInt->cols; col++)
176                         REAL(weightExt)[col * weightInt->rows + row] =
177                                 omxMatrixElement(weightInt, row, col);
178         
179         
180         if(0) {  // TODO fix for new internal API
181                 int nLocs = oo->matrix->currentState->numFreeParams;
182                 double gradient[oo->matrix->currentState->numFreeParams];
183                 for(int loc = 0; loc < nLocs; loc++) {
184                         gradient[loc] = NA_REAL;
185                 }
186                 //oo->gradientFun(oo, gradient);
187                 PROTECT(gradients = allocMatrix(REALSXP, 1, nLocs));
188
189                 for(int loc = 0; loc < nLocs; loc++)
190                         REAL(gradients)[loc] = gradient[loc];
191         } else {
192                 PROTECT(gradients = allocMatrix(REALSXP, 0, 0));
193         }
194     
195         setAttrib(algebra, install("expCov"), expCovExt);
196         setAttrib(algebra, install("expMean"), expMeanExt);
197         setAttrib(algebra, install("weights"), weightExt);
198         setAttrib(algebra, install("gradients"), gradients);
199         
200         UNPROTECT(4);
201
202 }
203
204 void omxInitWLSFitFunction(omxFitFunction* oo) {
205
206         if(OMX_DEBUG) { mxLog("Initializing WLS FitFunction function."); }
207
208         SEXP rObj = oo->rObj;
209         SEXP nextMatrix;
210         omxMatrix *cov, *means, *weights;
211         
212         /* Read and set expected means, variances, and weights */
213         PROTECT(nextMatrix = GET_SLOT(rObj, install("means")));
214         if(OMX_DEBUG) { mxLog("Processing Expected Means."); }
215         if(!R_FINITE(INTEGER(nextMatrix)[0])) {
216                 if(OMX_DEBUG) {
217                         mxLog("WLS: No Expected Means.");
218                 }
219                 means = NULL;
220         } else {
221                 means = omxMatrixLookupFromState1(nextMatrix, oo->matrix->currentState);
222                 if(OMX_DEBUG) { mxLog("Means matrix created at 0x%x.", means); }
223         }
224         UNPROTECT(1);
225
226         PROTECT(nextMatrix = GET_SLOT(rObj, install("covariance")));
227         if(OMX_DEBUG) { mxLog("Processing Expected Covariance."); }
228         cov = omxMatrixLookupFromState1(nextMatrix, oo->matrix->currentState);
229         UNPROTECT(1);
230         
231         PROTECT(nextMatrix = GET_SLOT(rObj, install("weights")));
232         if(OMX_DEBUG) { mxLog("Processing Expected Weights."); }
233         if(!R_FINITE(INTEGER(nextMatrix)[0])) {
234                 if(OMX_DEBUG) {
235                         mxLog("WLS: No Expected Weights--using ULS.");
236                 }
237                 weights = NULL;
238         } else {
239                 weights = omxMatrixLookupFromState1(nextMatrix, oo->matrix->currentState);
240                 if(OMX_DEBUG) { mxLog("Weights matrix created at 0x%x.", weights); }
241         }
242         UNPROTECT(1);
243         
244         // omxCreateWLSFitFunction(oo, rObj, cov, means, weights); // FIXME: Add WLS Expectation-handling abilities
245         
246 }
247
248 void omxSetWLSFitFunctionCalls(omxFitFunction* oo) {
249         
250         /* Set FitFunction Calls to WLS FitFunction Calls */
251         oo->fitType = "omxWLSFitFunction";
252         oo->computeFun = omxCallWLSFitFunction;
253         oo->destructFun = omxDestroyWLSFitFunction;
254         oo->setFinalReturns = omxSetFinalReturnsWLSFitFunction;
255         oo->populateAttrFun = omxPopulateWLSAttributes;
256         oo->repopulateFun = handleFreeVarList;
257 }
258
259 void omxCreateWLSFitFunction(omxFitFunction* oo, SEXP rObj, omxMatrix* cov, omxMatrix* means, omxMatrix* weights) {
260         
261         SEXP nextMatrix;
262     int vectorSize = 0;
263         
264         omxSetWLSFitFunctionCalls(oo);
265         
266         if(OMX_DEBUG) { mxLog("Retrieving data."); }
267         PROTECT(nextMatrix = GET_SLOT(rObj, install("data")));
268         omxData* dataMat = omxDataLookupFromState(nextMatrix, oo->matrix->currentState);
269         if(strncmp(omxDataType(dataMat), "cov", 3) != 0 && strncmp(omxDataType(dataMat), "cor", 3) != 0) {
270                 char *errstr = (char*) calloc(250, sizeof(char));
271                 sprintf(errstr, "WLS FitFunction unable to handle data type %s.\n", omxDataType(dataMat));
272                 omxRaiseError(oo->matrix->currentState, -1, errstr);
273                 free(errstr);
274                 if(OMX_DEBUG) { mxLog("WLS FitFunction unable to handle data type %s.  Aborting.", omxDataType(dataMat)); }
275                 return;
276         }
277         
278         if(cov == NULL) {
279                 omxRaiseError(oo->matrix->currentState, OMX_DEVELOPER_ERROR,
280                         "Developer Error in WLS-based FitFunction object: WLS-based expectations must specify a model-implied covariance matrix.\nIf you are not developing a new expectation type, you should probably post this to the OpenMx forums.");
281                         // TODO: Revise WLS to accept something besides cov/means
282                 return;
283         }
284
285         omxWLSFitFunction *newObj = (omxWLSFitFunction*) R_alloc(1, sizeof(omxWLSFitFunction));
286
287         newObj->expectedCov = cov;
288         newObj->expectedMeans = means;
289     vectorSize = (cov->rows * (cov->rows + 1) ) / 2;
290     if(means != NULL) {
291         vectorSize += cov->rows;
292     }
293         if(weights == NULL) {
294             // No weights specified--set up ULS weight matrix.
295         weights = omxInitMatrix(NULL, vectorSize, vectorSize, TRUE, oo->matrix->currentState);
296         for(int j = 0; j < vectorSize; j++) {
297             for(int k = 0; k < j; k++) {
298                 omxSetMatrixElement(weights, j, k, 0.0);
299                 omxSetMatrixElement(weights, k, j, 0.0);
300             }
301             omxSetMatrixElement(weights, j, j, 1.0);
302         }
303         } else if(weights->rows != weights->cols && weights->cols != vectorSize) {
304             omxRaiseError(oo->matrix->currentState, OMX_DEVELOPER_ERROR,
305                         "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.");
306                 return;
307         }
308     newObj->weights = weights;
309
310         if(OMX_DEBUG) { mxLog("Processing Observed Covariance."); }
311         newObj->observedCov = omxDataMatrix(dataMat, NULL);
312         if(OMX_DEBUG) { mxLog("Processing Observed Means."); }
313         newObj->observedMeans = omxDataMeans(dataMat, NULL, NULL);
314         if(OMX_DEBUG && newObj->observedMeans == NULL) { mxLog("WLS: No Observed Means."); }
315 //      if(OMX_DEBUG) { mxLog("Processing n."); }
316 //      newObj->n = omxDataNumObs(dataMat);
317         UNPROTECT(1); // nextMatrix
318         
319         // Error Checking: Observed/Expected means must agree.  
320         // ^ is XOR: true when one is false and the other is not.
321         if((newObj->expectedMeans == NULL) ^ (newObj->observedMeans == NULL)) {
322             if(newObj->expectedMeans != NULL) {
323                     omxRaiseError(oo->matrix->currentState, OMX_ERROR,
324                             "Observed means not detected, but an expected means matrix was specified.\n  If you provide observed means, you must specify a model for the means.\n");
325                     return;
326             } else {
327                     omxRaiseError(oo->matrix->currentState, OMX_ERROR,
328                             "Observed means were provided, but an expected means matrix was not specified.\n  If you  wish to model the means, you must provide observed means.\n");
329                     return;             
330             }
331         }
332
333         /* Temporary storage for calculation */
334         newObj->observedFlattened = omxInitMatrix(NULL, vectorSize, 1, TRUE, oo->matrix->currentState);
335         newObj->expectedFlattened = omxInitMatrix(NULL, vectorSize, 1, TRUE, oo->matrix->currentState);
336         newObj->P = omxInitMatrix(NULL, 1, vectorSize, TRUE, oo->matrix->currentState);
337         newObj->B = omxInitMatrix(NULL, vectorSize, 1, TRUE, oo->matrix->currentState);
338
339     flattenDataToVector(newObj->observedCov, newObj->observedMeans, newObj->observedFlattened);
340     flattenDataToVector(newObj->expectedCov, newObj->expectedMeans, newObj->expectedFlattened);
341
342     oo->argStruct = (void*)newObj;
343
344 }