Revert "Add option to checkpoint every evaluation"
[openmx:openmx.git] / src / omxMLFitFunction.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 "omxExpectation.h"
19 #include "omxFIMLFitFunction.h"
20 #include "omxRAMExpectation.h"
21 #include "omxBuffer.h"
22 #include "matrix.h"
23
24 static const double MIN_VARIANCE = 1e-6;
25
26 struct MLFitState {
27
28         omxMatrix* observedCov;
29         omxMatrix* observedMeans;
30         omxMatrix* expectedCov;
31         omxMatrix* expectedMeans;
32         omxMatrix* localCov;
33         omxMatrix* localProd;
34         omxMatrix* P;
35         omxMatrix* C;
36         omxMatrix* I;
37
38         double n;
39         double logDetObserved;
40
41         double* work;
42         int lwork;
43
44     // Expectation Storage;
45     omxMatrix** dSigma;         // dSigma/dTheta
46     omxMatrix** dMu;            // dMu/dTheta
47     omxMatrix* Mu;
48     omxMatrix* Ms;
49     omxMatrix* X;
50     omxMatrix* Y;
51
52         // fisher information
53         int haveLatentMap;
54         std::vector<int> latentMap;
55         std::vector<HessianBlock> lhBlocks;
56 };
57
58 static void omxDestroyMLFitFunction(omxFitFunction *oo) {
59
60         if(OMX_DEBUG) {mxLog("Freeing ML Fit Function.");}
61         MLFitState* omlo = ((MLFitState*)oo->argStruct);
62
63         omxFreeMatrix(omlo->localCov);
64         omxFreeMatrix(omlo->localProd);
65         omxFreeMatrix(omlo->P);
66         omxFreeMatrix(omlo->C);
67         omxFreeMatrix(omlo->I);
68
69         delete omlo;
70 }
71
72 static void calcExtraLikelihoods(omxFitFunction *oo, double *saturated_out, double *independence_out)
73 {
74         MLFitState *state = (MLFitState*) oo->argStruct;
75         double det = 0.0;
76         omxMatrix* cov = state->observedCov;
77         int ncols = state->observedCov->cols;
78     
79         *saturated_out = (state->logDetObserved + ncols) * (state->n - 1);
80
81         // Independence model assumes all-zero manifest covariances.
82         // (det(expected) + tr(observed * expected^-1)) * (n - 1);
83         // expected is the diagonal of the observed.  Inverse expected is 1/each diagonal value.
84         // Therefore the diagonal elements of observed * expected^-1 are each 1.
85         // So the trace of the matrix is the same as the number of columns.
86         // The determinant of a diagonal matrix is the product of the diagonal elements.
87         // Since these are the same in the expected as in the observed, we can get 'em direct.
88
89         for(int i = 0; i < ncols; i++) {
90                 // We sum logs instead of logging the product.
91                 det += log(omxMatrixElement(cov, i, i));
92         }
93         if(OMX_DEBUG) { omxPrint(cov, "Observed:"); }
94
95         *independence_out = (ncols + det) * (state->n - 1);
96 }
97
98 static void addOutput(omxFitFunction *oo, MxRList *out)
99 {
100         // DEPRECATED, use omxPopulateMLAttributes
101         double saturated_out;
102         double independence_out;
103         calcExtraLikelihoods(oo, &saturated_out, &independence_out);
104         out->add("SaturatedLikelihood", Rf_ScalarReal(saturated_out));
105         out->add("IndependenceLikelihood", Rf_ScalarReal(independence_out));
106 }
107
108 static void mvnFit(omxFitFunction *oo, FitContext *fc)
109 {
110         double sum = 0.0, det = 0.0;
111         char u = 'U';
112         char r = 'R';
113         int info = 0;
114         double oned = 1.0;
115         double zerod = 0.0;
116         double minusoned = -1.0;
117         int onei = 1;
118         double fmean = 0.0;
119
120         MLFitState *omo = ((MLFitState*)oo->argStruct);
121         omxMatrix *scov, *smeans, *cov, *means, *localCov, *localProd, *P, *C;
122         scov            = omo->observedCov;
123         smeans          = omo->observedMeans;
124         cov                     = omo->expectedCov;
125         means           = omo->expectedMeans;
126         localCov        = omo->localCov;
127         localProd       = omo->localProd;
128         P                       = omo->P;
129         C                       = omo->C;
130         double n        = omo->n;
131         double Q        = omo->logDetObserved;
132
133         omxCopyMatrix(localCov, cov);                           // But expected cov is destroyed in inversion
134
135         if(OMX_DEBUG_ALGEBRA) {
136                 omxPrint(scov, "Observed Covariance is");
137                 omxPrint(localCov, "Implied Covariance Is");
138                 omxPrint(cov, "Original Covariance Is");
139         }
140
141         /* Calculate |expected| */
142
143 //      F77_CALL(dgetrf)(&(localCov->cols), &(localCov->rows), localCov->data, &(localCov->cols), ipiv, &info);
144         F77_CALL(dpotrf)(&u, &(localCov->cols), localCov->data, &(localCov->cols), &info);
145
146         if(OMX_DEBUG_ALGEBRA) { mxLog("Info on LU Decomp: %d", info);}
147         if(info > 0) {
148                 oo->matrix->data[0] = NA_REAL;
149                 if (fc) fc->recordIterationError("Expected covariance matrix is non-positive-definite");
150                 return;
151         }
152
153         //det = log(det)        // TVO: changed multiplication of det to first log and the summing up; this line should just set det to zero.
154         for(info = 0; info < localCov->cols; info++) {          // |cov| is the square of the product of the diagonal elements of U from the LU factorization.
155                 det += log(fabs(localCov->data[info+localCov->rows*info])); // TVO: changed * to + and added fabs command
156         }
157         det *= 2.0;             // TVO: instead of det *= det;
158
159         if(OMX_DEBUG_ALGEBRA) { mxLog("Determinant of Expected Cov: %f", exp(det)); }
160         // TVO: removed det = log(fabs(det))
161         if(OMX_DEBUG_ALGEBRA) { mxLog("Log of Determinant of Expected Cov: %f", det); }
162
163         /* Calculate Expected^(-1) */
164 //      F77_CALL(dgetri)(&(localCov->rows), localCov->data, &(localCov->cols), ipiv, work, lwork, &info);
165         F77_CALL(dpotri)(&u, &(localCov->rows), localCov->data, &(localCov->cols), &info);
166         if(OMX_DEBUG_ALGEBRA) { mxLog("Info on Invert: %d", info); }
167
168         if(OMX_DEBUG_ALGEBRA) {omxPrint(cov, "Expected Covariance Matrix:");}
169         if(OMX_DEBUG_ALGEBRA) {omxPrint(localCov, "Inverted Matrix:");}
170
171         /* Calculate C = Observed * expected^(-1) */
172
173         // Stop gcc from issuing a Rf_warning
174         int majority = *(scov->majority) == 'n' ? scov->rows : scov->cols;
175
176         /*  TODO:  Make sure leading edges are being appropriately calculated, and sub them back into this */
177         F77_CALL(dsymm)(&r, &u, &(localCov->rows), &(scov->cols),
178                                         &oned, localCov->data, &(majority),
179                                         scov->data, &(majority),
180                                         &zerod, localProd->data, &(localProd->leading));
181
182     /* And get the trace of the result */
183
184         for(info = 0; info < localCov->cols; info++) {
185                 sum += localProd->data[info*localCov->cols + info];
186         }
187
188 //      for(info = 0; info < (localCov->cols * localCov->rows); info++) {
189 //              sum += localCov->data[info] * scov->data[info];
190 //      }
191
192         if(OMX_DEBUG_ALGEBRA) {omxPrint(scov, "Observed Covariance Matrix:");}
193         if(OMX_DEBUG_ALGEBRA) {omxPrint(localCov, "Inverse Matrix:");}
194         if(OMX_DEBUG_ALGEBRA) {omxPrint(localProd, "Product Matrix:");}
195
196         if(means != NULL) {
197                 if(OMX_DEBUG_ALGEBRA) { mxLog("Means Likelihood Calculation"); }
198                 omxRecompute(means);
199                 omxCopyMatrix(P, means);
200                 // P = means - smeans
201                 if(OMX_DEBUG_ALGEBRA) {omxPrint(means, "means");}
202                 if(OMX_DEBUG_ALGEBRA) {omxPrint(smeans, "smeans");}
203                 F77_CALL(daxpy)(&(smeans->cols), &minusoned, smeans->data, &onei, P->data, &onei);
204                 if(OMX_DEBUG_ALGEBRA) {omxPrint(P, "means - smeans");}
205                 // C = P * Cov
206                 F77_CALL(dsymv)(&u, &(localCov->rows), &oned, localCov->data, &(localCov->leading), P->data, &onei, &zerod, C->data, &onei);
207                 // P = C * P'
208                 fmean = F77_CALL(ddot)(&(C->cols), P->data, &onei, C->data, &onei);
209
210                 if(OMX_DEBUG_ALGEBRA) { mxLog("Mean contribution to likelihood is %f per row.", fmean); }
211                 if(fmean < 0.0) fmean = 0.0;
212         }
213
214         oo->matrix->data[0] = (sum + det) * (n - 1) + fmean * (n);
215
216         if(OMX_DEBUG) { mxLog("MLFitFunction value comes to: %f (Chisq: %f).", oo->matrix->data[0], (sum + det) - Q - cov->cols); }
217 }
218
219 static void buildLatentParamMap(omxFitFunction* oo, FitContext *fc)
220 {
221         FreeVarGroup *fvg = fc->varGroup;
222         MLFitState *state = (MLFitState*) oo->argStruct;
223         std::vector<int> &latentMap = state->latentMap;
224         int meanNum = ~state->expectedMeans->matrixNumber;
225         int covNum = ~state->expectedCov->matrixNumber;
226         int maxAbilities = state->expectedCov->rows;
227         int numLatents = maxAbilities + triangleLoc1(maxAbilities);
228
229         if (state->haveLatentMap == fvg->id[0]) return;
230         if (0) mxLog("%s: rebuild latent parameter map for var group %d",
231                      oo->matrix->name, fvg->id[0]); // TODO add runtime verbose setting
232
233         latentMap.assign(numLatents + triangleLoc1(numLatents), -1);
234
235         state->lhBlocks.clear();
236         state->lhBlocks.resize(2);  // 0=mean, 1=cov
237
238         int numParam = int(fvg->vars.size());
239         for (int px=0; px < numParam; px++) {
240                 omxFreeVar *fv = fvg->vars[px];
241                 for (size_t lx=0; lx < fv->locations.size(); lx++) {
242                         omxFreeVarLocation *loc = &fv->locations[lx];
243                         int matNum = loc->matrix;
244                         if (matNum == meanNum) {
245                                 latentMap[loc->row + loc->col] = px;
246                                 state->lhBlocks[0].vars.push_back(px);
247                         } else if (matNum == covNum) {
248                                 int a1 = loc->row;
249                                 int a2 = loc->col;
250                                 if (a1 < a2) std::swap(a1, a2);
251                                 int cell = maxAbilities + triangleLoc1(a1) + a2;
252                                 if (latentMap[cell] == -1) {
253                                         latentMap[cell] = px;
254                                         state->lhBlocks[1].vars.push_back(px);
255
256                                         if (a1 == a2 && fv->lbound == NEG_INF) {
257                                                 fv->lbound = MIN_VARIANCE;  // variance must be positive
258                                                 if (fc->est[px] < fv->lbound) {
259                                                         Rf_error("Starting value for variance %s is not positive", fv->name);
260                                                 }
261                                         }
262                                 } else if (latentMap[cell] != px) {
263                                         // doesn't detect similar problems in multigroup constraints TODO
264                                         Rf_error("Covariance matrix must be constrained to preserve symmetry");
265                                 }
266                         }
267                 }
268         }
269         state->haveLatentMap = fvg->id[0];
270
271         for (int p1=0; p1 < maxAbilities; p1++) {
272                 HessianBlock &hb = state->lhBlocks[0];
273                 int at1 = latentMap[p1];
274                 if (at1 < 0) continue;
275                 int hb1 = std::lower_bound(hb.vars.begin(), hb.vars.end(), at1) - hb.vars.begin();
276
277                 for (int p2=0; p2 <= p1; p2++) {
278                         int at2 = latentMap[p2];
279                         if (at2 < 0) continue;
280                         int hb2 = std::lower_bound(hb.vars.begin(), hb.vars.end(), at2) - hb.vars.begin();
281
282                         if (hb1 < hb2) std::swap(hb1, hb2);
283                         int at = numLatents + triangleLoc1(p1) + p2;
284                         latentMap[at] = hb1 * hb.vars.size() + hb2;
285                 }
286         }
287
288         for (int p1=maxAbilities; p1 < numLatents; p1++) {
289                 HessianBlock &hb = state->lhBlocks[1];
290                 int at1 = latentMap[p1];
291                 if (at1 < 0) continue;
292                 int hb1 = std::lower_bound(hb.vars.begin(), hb.vars.end(), at1) - hb.vars.begin();
293
294                 for (int p2=maxAbilities; p2 <= p1; p2++) {
295                         int at2 = latentMap[p2];
296                         if (at2 < 0) continue;
297                         int hb2 = std::lower_bound(hb.vars.begin(), hb.vars.end(), at2) - hb.vars.begin();
298
299                         if (hb1 < hb2) std::swap(hb1, hb2);
300                         int at = numLatents + triangleLoc1(p1) + p2;
301                         latentMap[at] = hb1 * hb.vars.size() + hb2;
302                 }
303         }
304 }
305
306 static void mvnInfo(omxFitFunction *oo, FitContext *fc)
307 {
308         buildLatentParamMap(oo, fc);
309
310         const double Scale = Global->llScale;
311         MLFitState *state = (MLFitState*) oo->argStruct;
312         const int maxAbilities = state->expectedCov->rows;
313         omxMatrix *cov = state->expectedCov;
314         const double numObs = state->n;
315         int numLatents = maxAbilities + triangleLoc1(maxAbilities);
316         std::vector<int> &latentMap = state->latentMap;
317
318         if (0) mxLog("%s: latentHessian", oo->matrix->name);
319
320         omxBuffer<double> icovBuffer(maxAbilities * maxAbilities);
321         memcpy(icovBuffer.data(), cov->data, sizeof(double) * maxAbilities * maxAbilities);
322         Matrix icovMat(icovBuffer.data(), maxAbilities, maxAbilities);
323         int info = InvertSymmetricPosDef(icovMat, 'U');
324         if (info) return;
325
326         for (int m1=0; m1 < maxAbilities; ++m1) {
327                 for (int m2=0; m2 < m1; ++m2) {
328                         icovBuffer[m2 * maxAbilities + m1] = icovBuffer[m1 * maxAbilities + m2];
329                 }
330         }
331
332         {
333                 HessianBlock *hb = state->lhBlocks[0].clone();
334
335                 int px=numLatents;
336                 for (int m1=0; m1 < maxAbilities; ++m1) {
337                         for (int m2=0; m2 <= m1; ++m2) {
338                                 int to = latentMap[px];
339                                 ++px;
340                                 if (to < 0) continue;
341                                 hb->mat.data()[to] = -Scale * numObs * icovBuffer[m1 * maxAbilities + m2];
342                         }
343                 }
344                 fc->queue(hb);
345         }
346
347         HessianBlock *hb = state->lhBlocks[1].clone();
348
349         std::vector<double> term1(maxAbilities * maxAbilities);
350         std::vector<double> term2(maxAbilities * maxAbilities);
351
352         int f1=0;
353         for (int r1=0; r1 < maxAbilities; ++r1) {
354                 for (int c1=0; c1 <= r1; ++c1) {
355                         memcpy(term1.data()      + c1 * maxAbilities,
356                                icovBuffer.data() + r1 * maxAbilities, maxAbilities * sizeof(double));
357                         if (r1 != c1) {
358                                 memcpy(term1.data()      + r1 * maxAbilities,
359                                        icovBuffer.data() + c1 * maxAbilities, maxAbilities * sizeof(double));
360                         }
361                         int f2 = f1;
362                         for (int r2=r1; r2 < maxAbilities; ++r2) {
363                                 for (int c2 = (r1==r2? c1 : 0); c2 <= r2; ++c2) {
364                                         int to = latentMap[numLatents + triangleLoc1(f2 + maxAbilities) + f1 + maxAbilities];
365                                         ++f2;
366                                         if (to < 0) continue;
367
368                                         memcpy(term2.data()      + c2 * maxAbilities,
369                                                icovBuffer.data() + r2 * maxAbilities, maxAbilities * sizeof(double));
370                                         if (r2 != c2) {
371                                                 memcpy(term2.data()      + r2 * maxAbilities,
372                                                        icovBuffer.data() + c2 * maxAbilities, maxAbilities * sizeof(double));
373                                         }
374
375                                         double tr = 0;
376                                         for (int d1=0; d1 < maxAbilities; ++d1) {
377                                                 for (int d2=0; d2 < maxAbilities; ++d2) {
378                                                         tr += term1[d2 * maxAbilities + d1] * term2[d1 * maxAbilities + d2];
379                                                 }
380                                         }
381
382                                         // Simulation suggests the sample size should be
383                                         // numObs-2 but this is tedious to accomodate
384                                         // when there are parameter equality constraints. Whether
385                                         // the sample size is adjusted or not seems to make
386                                         // no detectable difference in tests.
387                                         hb->mat.data()[to] = Scale * numObs * -.5 * tr;
388
389                                         OMXZERO(term2.data() + c2 * maxAbilities, maxAbilities);
390                                         if (c2 != r2) OMXZERO(term2.data() + r2 * maxAbilities, maxAbilities);
391                                 }
392                         }
393                         OMXZERO(term1.data() + c1 * maxAbilities, maxAbilities);
394                         if (c1 != r1) OMXZERO(term1.data() + r1 * maxAbilities, maxAbilities);
395                         ++f1;
396                 }
397         }
398         fc->queue(hb);
399 }
400
401 static void omxCallMLFitFunction(omxFitFunction *oo, int want, FitContext *fc)
402 {
403         if (want & (FF_COMPUTE_PREOPTIMIZE)) return;
404
405         omxExpectation* expectation = oo->expectation;
406         omxExpectationCompute(expectation, NULL);
407
408         if ((want & FF_COMPUTE_INFO) && strcmp(expectation->expType, "MxExpectationNormal")==0) {
409                 if (fc->infoMethod != INFO_METHOD_HESSIAN) {
410                         omxRaiseErrorf("Information matrix approximation method %d is not available",
411                                        fc->infoMethod);
412                         return;
413                 }
414                 mvnInfo(oo, fc);
415         }
416
417         if (want & FF_COMPUTE_FIT) {
418                 mvnFit(oo, fc);
419         }
420 }
421
422 static void omxPopulateMLAttributes(omxFitFunction *oo, SEXP algebra) {
423     if(OMX_DEBUG) { mxLog("Populating ML Attributes."); }
424
425         MLFitState *argStruct = ((MLFitState*)oo->argStruct);
426         omxMatrix *expCovInt = argStruct->expectedCov;                  // Expected covariance
427         omxMatrix *expMeanInt = argStruct->expectedMeans;                       // Expected means
428
429         SEXP expCovExt, expMeanExt;
430         Rf_protect(expCovExt = Rf_allocMatrix(REALSXP, expCovInt->rows, expCovInt->cols));
431         for(int row = 0; row < expCovInt->rows; row++)
432                 for(int col = 0; col < expCovInt->cols; col++)
433                         REAL(expCovExt)[col * expCovInt->rows + row] =
434                                 omxMatrixElement(expCovInt, row, col);
435
436         if (expMeanInt != NULL) {
437                 Rf_protect(expMeanExt = Rf_allocMatrix(REALSXP, expMeanInt->rows, expMeanInt->cols));
438                 for(int row = 0; row < expMeanInt->rows; row++)
439                         for(int col = 0; col < expMeanInt->cols; col++)
440                                 REAL(expMeanExt)[col * expMeanInt->rows + row] =
441                                         omxMatrixElement(expMeanInt, row, col);
442         } else {
443                 Rf_protect(expMeanExt = Rf_allocMatrix(REALSXP, 0, 0));         
444         }   
445
446         Rf_setAttrib(algebra, Rf_install("expCov"), expCovExt);
447         Rf_setAttrib(algebra, Rf_install("expMean"), expMeanExt);
448         
449         double saturated_out;
450         double independence_out;
451         calcExtraLikelihoods(oo, &saturated_out, &independence_out);
452         Rf_setAttrib(algebra, Rf_install("SaturatedLikelihood"), Rf_ScalarReal(saturated_out));
453         Rf_setAttrib(algebra, Rf_install("IndependenceLikelihood"), Rf_ScalarReal(independence_out));
454 }
455
456 void omxInitMLFitFunction(omxFitFunction* oo)
457 {
458         if (!oo->expectation) { Rf_error("%s requires an expectation", oo->fitType); }
459
460         omxExpectation *expectation = oo->expectation;
461         if (strcmp(expectation->expType, "MxExpectationBA81")==0) {
462                 omxInitFitFunctionBA81(oo);
463                 return;
464         }
465
466         if(OMX_DEBUG) { mxLog("Initializing ML fit function."); }
467
468         int info = 0;
469         double det = 1.0;
470         char u = 'U';
471         
472         oo->computeFun = omxCallMLFitFunction;
473         oo->destructFun = omxDestroyMLFitFunction;
474         oo->addOutput = addOutput;
475         oo->populateAttrFun = omxPopulateMLAttributes;
476
477         omxData* dataMat = oo->expectation->data;
478
479         if(!(dataMat == NULL) && strncmp(omxDataType(dataMat), "cov", 3) != 0 && strncmp(omxDataType(dataMat), "cor", 3) != 0) {
480                 if(strncmp(omxDataType(dataMat), "raw", 3) == 0) {
481                         if(OMX_DEBUG) { mxLog("Raw Data: Converting to FIML."); }
482                         omxInitFIMLFitFunction(oo);
483                         return;
484                 }
485                 char *errstr = (char*) calloc(250, sizeof(char));
486                 sprintf(errstr, "ML FitFunction unable to handle data type %s.\n", omxDataType(dataMat));
487                 omxRaiseError(errstr);
488                 free(errstr);
489                 if(OMX_DEBUG) { mxLog("ML FitFunction unable to handle data type %s.  Aborting.", omxDataType(dataMat)); }
490                 return;
491         }
492
493         MLFitState *newObj = new MLFitState;
494         newObj->haveLatentMap = FREEVARGROUP_INVALID;
495         oo->argStruct = (void*)newObj;
496
497         if(OMX_DEBUG) { mxLog("Processing Observed Covariance."); }
498         newObj->observedCov = omxDataCovariance(dataMat);
499         if(OMX_DEBUG) { mxLog("Processing Observed Means."); }
500         newObj->observedMeans = omxDataMeans(dataMat);
501         if(OMX_DEBUG && newObj->observedMeans == NULL) { mxLog("ML: No Observed Means."); }
502         if(OMX_DEBUG) { mxLog("Processing n."); }
503         newObj->n = omxDataNumObs(dataMat);
504
505         newObj->expectedCov = omxGetExpectationComponent(oo->expectation, oo, "cov");
506         newObj->expectedMeans = omxGetExpectationComponent(oo->expectation, oo, "means");
507
508         if(newObj->expectedCov == NULL) {
509                 omxRaiseError("Developer Error in ML-based fit function object: ML's expectation 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.");
510                 return;
511         }
512
513         // Error Checking: Observed/Expected means must agree.  
514         // ^ is XOR: true when one is false and the other is not.
515         if((newObj->expectedMeans == NULL) ^ (newObj->observedMeans == NULL)) {
516                 if(newObj->expectedMeans != NULL) {
517                         omxRaiseError("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");
518                         return;
519                 } else {
520                         omxRaiseError("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");
521                         return;         
522                 }
523         }
524
525         /* Temporary storage for calculation */
526         int rows = newObj->observedCov->rows;
527         int cols = newObj->observedCov->cols;
528         newObj->localCov = omxInitMatrix(rows, cols, TRUE, oo->matrix->currentState);
529         newObj->localProd = omxInitMatrix(rows, cols, TRUE, oo->matrix->currentState);
530         newObj->P = omxInitMatrix(1, cols, TRUE, oo->matrix->currentState);
531         newObj->C = omxInitMatrix(rows, cols, TRUE, oo->matrix->currentState);
532         newObj->I = omxInitMatrix(rows, cols, TRUE, oo->matrix->currentState);
533
534         for(int i = 0; i < rows; i++) omxSetMatrixElement(newObj->I, i, i, 1.0);
535
536         omxCopyMatrix(newObj->localCov, newObj->observedCov);
537
538         newObj->lwork = newObj->expectedCov->rows;
539         newObj->work = (double*)R_alloc(newObj->lwork, sizeof(double));
540
541         // TODO: Determine where the saturated model computation should go.
542
543         F77_CALL(dpotrf)(&u, &(newObj->localCov->cols), newObj->localCov->data, &(newObj->localCov->cols), &info);
544
545         if(OMX_DEBUG) { mxLog("Info on LU Decomp: %d", info); }
546         if(info != 0) {
547                 char *errstr = (char*) calloc(250, sizeof(char));
548                 sprintf(errstr, "Observed Covariance Matrix is non-positive-definite.\n");
549                 omxRaiseError(errstr);
550                 free(errstr);
551                 return;
552         }
553         for(info = 0; info < newObj->localCov->cols; info++) {
554                 det *= omxMatrixElement(newObj->localCov, info, info);
555         }
556         det *= det;                                     // Product of squares.
557
558         if(OMX_DEBUG) { mxLog("Determinant of Observed Cov: %f", det); }
559         newObj->logDetObserved = log(det);
560         if(OMX_DEBUG) { mxLog("Log Determinant of Observed Cov: %f", newObj->logDetObserved); }
561
562         omxCopyMatrix(newObj->localCov, newObj->expectedCov);
563 }
564
565 static void omxSetMLFitFunctionGradientComponents(omxFitFunction* oo, void (*derivativeFun)(omxFitFunction*, omxMatrix**, omxMatrix**, int*)) {
566     if(OMX_DEBUG) { mxLog("Setting up gradient component function for ML FitFunction."); }
567     if(!strncmp("omxFIMLFitFunction", oo->fitType, 16)) {
568         if(OMX_DEBUG) { mxLog("FIML FitFunction gradients not yet implemented. Skipping."); }
569         return; // ERROR:NYI.
570     }
571     
572     if(derivativeFun == NULL) {
573         char Rf_errorstr[250];
574         sprintf(Rf_errorstr, "Programmer Rf_error: ML gradient components given NULL gradient function.");
575         omxRaiseError(Rf_errorstr);
576         return;
577     }
578     
579     MLFitState *omo = ((MLFitState*) oo->argStruct);
580     int rows = omo->observedCov->rows;
581     int cols = omo->observedCov->cols;
582     size_t nFreeVars = oo->freeVarGroup->vars.size();
583             
584     omo->X  = omxInitMatrix(rows, cols, TRUE, oo->matrix->currentState);
585     omo->Y  = omxInitMatrix(rows, cols, TRUE, oo->matrix->currentState);
586     omo->Ms = omxInitMatrix(1, cols, TRUE, oo->matrix->currentState);
587     omo->Mu = omxInitMatrix(1, cols, TRUE, oo->matrix->currentState);
588     omo->dSigma = (omxMatrix**) R_alloc(nFreeVars, sizeof(omxMatrix*));
589     omo->dMu = (omxMatrix**) R_alloc(nFreeVars, sizeof(omxMatrix*));
590     for(size_t i = 0; i < nFreeVars; i++) {
591         omo->dSigma[i] = omxInitMatrix(rows, cols, TRUE, oo->matrix->currentState);
592         omo->dMu[i] = omxInitMatrix(rows, 1, TRUE, oo->matrix->currentState);
593     }
594     //oo->gradientFun = omxCalculateMLGradient; TODO
595 }
596
597 static void omxCalculateMLGradient(omxFitFunction* oo, double* gradient) {
598
599     if(OMX_DEBUG) { mxLog("Beginning ML Gradient Calculation."); }
600     // mxLog("Beginning ML Gradient Calculation, Iteration %d.%d (%d)\n", 
601         // oo->matrix->currentState->majorIteration, oo->matrix->currentState->minorIteration,
602         // oo->matrix->currentState->computeCount); //:::DEBUG:::
603     // 1) Calculate current Expected Covariance
604     // 2) Calculate eCov, the Inverse Expected Covariance matrix 
605     // 3) Calculate C = I - eCov D, where D is the observed covariance matrix
606     // 4) Calculate b = M - [observed M]
607     // 5) For each location in locs:
608     //   gradient[loc] = tr(eCov^-1 %*% dEdt %*% C) - (b^T %*% eCov^-1 %*% dEdt + 2 dMdt^T))eCov^-1 b)
609
610     MLFitState *omo = ((MLFitState*)oo->argStruct);
611     
612     /* Locals for readability.  Compiler should cut through this. */
613     omxMatrix *scov         = omo->observedCov;
614     omxMatrix *smeans       = omo->observedMeans;
615     omxMatrix *cov          = omo->expectedCov;
616     omxMatrix *M            = omo->expectedMeans;
617     omxMatrix *eCov         = omo->localCov;        // TODO: Maybe need to avoid reusing these
618     omxMatrix *I            = omo->I;
619     omxMatrix *C            = omo->C;
620     omxMatrix *X            = omo->X;
621     omxMatrix *Y            = omo->Y;
622     omxMatrix *Mu           = omo->Mu;
623     omxMatrix *Ms           = omo->Ms;
624     omxMatrix *P            = omo->P;
625     double n                = omo->n;
626     omxMatrix** dSigmas     = omo->dSigma;
627     omxMatrix** dMus        = omo->dMu;
628     
629     size_t gradientSize = oo->freeVarGroup->vars.size();
630     
631     char u = 'U';
632     int info;
633     double minusoned = -1.0;
634     int onei = 1;
635     int status[gradientSize];
636     int nLocs = gradientSize;
637     
638     // Calculate current FitFunction values
639     // We can safely assume this has been done
640     // omxFitFunctionCompute(oo);
641     
642     // Calculate current eCov
643     
644     omxCopyMatrix(eCov, cov);                           // But expected cov is destroyed in inversion
645     
646     F77_CALL(dpotrf)(&u, &(eCov->cols), eCov->data, &(eCov->cols), &info);
647
648     if(OMX_DEBUG_ALGEBRA) { mxLog("Info on LU Decomp: %d", info);}
649     if(info > 0) {
650             char *errstr = (char*) calloc(250, sizeof(char));
651         sprintf(errstr, "Expected covariance matrix is non-positive-definite");
652         if(oo->matrix->currentState->computeCount <= 0) {
653             strncat(errstr, " at starting values", 20);
654         }
655         strncat(errstr, ".\n", 3);
656         omxRaiseError(errstr);                        // Raise Rf_error
657         free(errstr);
658         return;                                                                     // Leave output untouched
659     }
660     
661     F77_CALL(dpotri)(&u, &(eCov->rows), eCov->data, &(eCov->cols), &info);
662     if(info > 0) {
663             char *errstr = (char*) calloc(250, sizeof(char));
664         sprintf(errstr, "Expected covariance matrix is not invertible");
665         if(oo->matrix->currentState->computeCount <= 0) {
666             strncat(errstr, " at starting values", 20);
667         }
668         strncat(errstr, ".\n", 3);
669         omxRaiseError(errstr);                        // Raise Rf_error
670         free(errstr);
671         return;
672     }
673     // Calculate P = expected means - observed means
674     if(M != NULL) {
675         omxCopyMatrix(P, smeans);
676         F77_CALL(daxpy)(&(smeans->cols), &minusoned, M->data, &onei, P->data, &onei);
677     }
678         
679         // Reset C and Calculate C = I - eCov * oCov
680     omxCopyMatrix(C, I);
681     omxDSYMM(TRUE, -1.0, eCov, scov, 1.0, C);
682     
683     // For means, calculate Ms = eCov-1 %*% P
684     if(M != NULL)
685         omxDSYMM(FALSE, 1.0, eCov, P, 0.0, Ms);
686     
687     // Calculate parameter-level derivatives
688     // TODO: Parallelize Here.
689
690     if(OMX_DEBUG)  { mxLog("Calling component function."); }
691     // omo->derivativeFun(oo, dSigmas, dMus, status);
692     
693     for(int currentLoc = 0; currentLoc < nLocs; currentLoc++) {
694         double meanInfluence, covInfluence;
695         if(status[currentLoc] < 0) continue;  // Failure in computation--skip.
696         //   gradient[loc] = tr(eCov^-1 %*% dEdt %*% C) - 
697         //    (b^T %*% eCov^-1 %*% dEdt + 2 dMdt^T))eCov^-1 b)
698         // omxDGEMM(FALSE, FALSE, 1.0, dSigmas[currentLoc], C, 0.0, Y);
699         omxDSYMM(TRUE, 1.0, dSigmas[currentLoc], C, 0.0, Y);
700         omxDSYMM(TRUE, 1.0, eCov, Y, 0.0, X);
701         gradient[currentLoc] = 0;
702         covInfluence = 0.0;
703         for(int i = 0; i < eCov->cols; i++) 
704             covInfluence += omxMatrixElement(X, i, i);
705         if(M != NULL) {
706             omxCopyMatrix(Mu, dMus[currentLoc]);
707             omxDSYMV(1.0, dSigmas[currentLoc], Ms, 2.0, Mu);
708             meanInfluence = F77_CALL(ddot)(&(eCov->cols), Mu->data, &onei, Ms->data, &onei);
709         } else {
710             meanInfluence = 0;
711         }
712         gradient[currentLoc] = (covInfluence * (n-1)) - (meanInfluence * n);
713         if(OMX_DEBUG) { 
714             mxLog("Calculation for Gradient value %d: Cov: %3.9f, Mean: %3.9f, total: %3.9f",
715             currentLoc, covInfluence, meanInfluence, gradient[currentLoc]); 
716         }
717     }
718 }