Change fit function init API
[openmx:openmx.git] / src / omxMLFitFunction.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 "omxExpectation.h"
25 #include "omxMLFitFunction.h"
26 #include "omxFIMLFitFunction.h"
27 #include "omxRAMExpectation.h"
28
29 void omxDestroyMLFitFunction(omxFitFunction *oo) {
30
31         if(OMX_DEBUG) {Rprintf("Freeing ML Fit Function.");}
32         omxMLFitFunction* omlo = ((omxMLFitFunction*)oo->argStruct);
33
34         if(omlo->localCov != NULL)      omxFreeMatrixData(omlo->localCov);
35         if(omlo->localProd != NULL)     omxFreeMatrixData(omlo->localProd);
36         if(omlo->P != NULL)                     omxFreeMatrixData(omlo->P);
37         if(omlo->C != NULL)                     omxFreeMatrixData(omlo->C);
38         if(omlo->I != NULL)                     omxFreeMatrixData(omlo->I);
39 }
40
41 omxRListElement* omxSetFinalReturnsMLFitFunction(omxFitFunction *oo, int *numReturns) {
42         *numReturns = 3;
43         omxRListElement* retVal = (omxRListElement*) R_alloc(*numReturns, sizeof(omxRListElement));
44         double det = 0.0;
45         omxMatrix* cov = ((omxMLFitFunction*)oo->argStruct)->observedCov;
46         int ncols = ((omxMLFitFunction*)oo->argStruct)->observedCov->cols;
47     
48         retVal[0].numValues = 1;
49         retVal[0].values = (double*) R_alloc(1, sizeof(double));
50         strncpy(retVal[0].label, "Minus2LogLikelihood", 20);
51         retVal[0].values[0] = omxMatrixElement(oo->matrix, 0, 0);
52
53         retVal[1].numValues = 1;
54         retVal[1].values = (double*) R_alloc(1, sizeof(double));
55         strncpy(retVal[1].label, "SaturatedLikelihood", 20);
56         retVal[1].values[0] = (((omxMLFitFunction*)oo->argStruct)->logDetObserved + ncols) * (((omxMLFitFunction*)oo->argStruct)->n - 1);
57
58         retVal[2].numValues = 1;
59         retVal[2].values = (double*) R_alloc(1, sizeof(double));
60         strncpy(retVal[2].label, "IndependenceLikelihood", 23);
61         // Independence model assumes all-zero manifest covariances.
62         // (det(expected) + tr(observed * expected^-1)) * (n - 1);
63         // expected is the diagonal of the observed.  Inverse expected is 1/each diagonal value.
64         // Therefore the diagonal elements of observed * expected^-1 are each 1.
65         // So the trace of the matrix is the same as the number of columns.
66         // The determinant of a diagonal matrix is the product of the diagonal elements.
67         // Since these are the same in the expected as in the observed, we can get 'em direct.
68         for(int i = 0; i < ncols; i++) {
69                 // We sum logs instead of logging the product.
70                 det += log(omxMatrixElement(cov, i, i));
71         }
72         if(OMX_DEBUG) { Rprintf("det: %f, tr: %f, n= %d, total:%f\n", det, ncols, ((omxMLFitFunction*)oo->argStruct)->n, (ncols + det) * (((omxMLFitFunction*)oo->argStruct)->n - 1)); }
73         if(OMX_DEBUG) { omxPrint(cov, "Observed:"); }
74         retVal[2].values[0] = (ncols + det) * (((omxMLFitFunction*)oo->argStruct)->n - 1);
75
76         return retVal;
77 }
78
79 static void omxCallMLFitFunction(omxFitFunction *oo, int want, double *gradient) {      // TODO: Figure out how to give access to other per-iteration structures.
80
81         if(OMX_DEBUG) { Rprintf("Beginning ML Evaluation.\n");}
82         // Requires: Data, means, covariances.
83
84         double sum = 0.0, det = 0.0;
85         char u = 'U';
86         char r = 'R';
87         int info = 0;
88         double oned = 1.0;
89         double zerod = 0.0;
90         double minusoned = -1.0;
91         int onei = 1;
92         double fmean = 0.0;
93
94         omxMatrix *scov, *smeans, *cov, *means, *localCov, *localProd, *P, *C;
95
96         omxMLFitFunction *omo = ((omxMLFitFunction*)oo->argStruct);
97         
98     /* Locals for readability.  Compiler should cut through this. */
99         scov            = omo->observedCov;
100         smeans          = omo->observedMeans;
101         cov                     = omo->expectedCov;
102         means           = omo->expectedMeans;
103         localCov        = omo->localCov;
104         localProd       = omo->localProd;
105         P                       = omo->P;
106         C                       = omo->C;
107         double n        = omo->n;
108         double Q        = omo->logDetObserved;
109         omxExpectation* expectation = oo->expectation;
110
111     /* Recompute and recopy */
112         omxExpectationCompute(expectation);
113
114         omxCopyMatrix(localCov, cov);                           // But expected cov is destroyed in inversion
115
116         if(OMX_DEBUG_ALGEBRA) {
117                 omxPrint(scov, "Observed Covariance is");
118                 omxPrint(localCov, "Implied Covariance Is");
119                 omxPrint(cov, "Original Covariance Is");
120         }
121
122         /* Calculate |expected| */
123
124 //      F77_CALL(dgetrf)(&(localCov->cols), &(localCov->rows), localCov->data, &(localCov->cols), ipiv, &info);
125         F77_CALL(dpotrf)(&u, &(localCov->cols), localCov->data, &(localCov->cols), &info);
126
127         if(OMX_DEBUG_ALGEBRA) { Rprintf("Info on LU Decomp: %d\n", info);}
128         if(info > 0) {
129                 char *errstr = (char*) calloc(250, sizeof(char));
130                 sprintf(errstr, "Expected covariance matrix is non-positive-definite");
131                 if(oo->matrix->currentState->computeCount <= 0) {
132                         strncat(errstr, " at starting values", 20);
133                 }
134                 strncat(errstr, ".\n", 3);
135                 omxRaiseError(oo->matrix->currentState, -1, errstr);                                            // Raise error
136                 free(errstr);
137                 return;                                                                                                                                         // Leave output untouched
138         }
139
140         //det = log(det)        // TVO: changed multiplication of det to first log and the summing up; this line should just set det to zero.
141         for(info = 0; info < localCov->cols; info++) {          // |cov| is the square of the product of the diagonal elements of U from the LU factorization.
142                 det += log(fabs(localCov->data[info+localCov->rows*info])); // TVO: changed * to + and added fabs command
143         }
144         det *= 2.0;             // TVO: instead of det *= det;
145
146         if(OMX_DEBUG_ALGEBRA) { Rprintf("Determinant of Expected Cov: %f\n", exp(det)); }
147         // TVO: removed det = log(fabs(det))
148         if(OMX_DEBUG_ALGEBRA) { Rprintf("Log of Determinant of Expected Cov: %f\n", det); }
149
150         /* Calculate Expected^(-1) */
151 //      F77_CALL(dgetri)(&(localCov->rows), localCov->data, &(localCov->cols), ipiv, work, lwork, &info);
152         F77_CALL(dpotri)(&u, &(localCov->rows), localCov->data, &(localCov->cols), &info);
153         if(OMX_DEBUG_ALGEBRA) { Rprintf("Info on Invert: %d\n", info); }
154
155         if(OMX_DEBUG_ALGEBRA) {omxPrint(cov, "Expected Covariance Matrix:");}
156         if(OMX_DEBUG_ALGEBRA) {omxPrint(localCov, "Inverted Matrix:");}
157
158         /* Calculate C = Observed * expected^(-1) */
159
160         if(OMX_DEBUG_ALGEBRA) {Rprintf("Call is: DSYMM(%d, %d, %f, %0x, %d, %0x, %d, %f, %0x, %d)",
161                                         (scov->rows), (localCov->cols), oned, scov->data, (localCov->leading),
162                                         localCov->data, (localCov->leading), zerod, localProd->data, (localProd->leading));}
163
164
165         // Stop gcc from issuing a warning
166         int majority = *(scov->majority) == 'n' ? scov->rows : scov->cols;
167
168         /*  TODO:  Make sure leading edges are being appropriately calculated, and sub them back into this */
169         F77_CALL(dsymm)(&r, &u, &(localCov->rows), &(scov->cols),
170                                         &oned, localCov->data, &(majority),
171                                         scov->data, &(majority),
172                                         &zerod, localProd->data, &(localProd->leading));
173
174     /* And get the trace of the result */
175
176         for(info = 0; info < localCov->cols; info++) {
177                 sum += localProd->data[info*localCov->cols + info];
178         }
179
180 //      for(info = 0; info < (localCov->cols * localCov->rows); info++) {
181 //              sum += localCov->data[info] * scov->data[info];
182 //      }
183
184         if(OMX_DEBUG_ALGEBRA) {omxPrint(scov, "Observed Covariance Matrix:");}
185         if(OMX_DEBUG_ALGEBRA) {omxPrint(localCov, "Inverse Matrix:");}
186         if(OMX_DEBUG_ALGEBRA) {omxPrint(localProd, "Product Matrix:");}
187
188         if(means != NULL) {
189                 if(OMX_DEBUG_ALGEBRA) { Rprintf("Means Likelihood Calculation"); }
190                 omxRecompute(means);
191                 omxCopyMatrix(P, means);
192                 // P = means - smeans
193                 if(OMX_DEBUG_ALGEBRA) {omxPrint(means, "means");}
194                 if(OMX_DEBUG_ALGEBRA) {omxPrint(smeans, "smeans");}
195                 F77_CALL(daxpy)(&(smeans->cols), &minusoned, smeans->data, &onei, P->data, &onei);
196                 if(OMX_DEBUG_ALGEBRA) {omxPrint(P, "means - smeans");}
197                 // C = P * Cov
198                 F77_CALL(dsymv)(&u, &(localCov->rows), &oned, localCov->data, &(localCov->leading), P->data, &onei, &zerod, C->data, &onei);
199                 // P = C * P'
200                 fmean = F77_CALL(ddot)(&(C->cols), P->data, &onei, C->data, &onei);
201
202                 if(OMX_DEBUG_ALGEBRA) { Rprintf("Mean contribution to likelihood is %f per row.\n", fmean); }
203                 if(fmean < 0.0) fmean = 0.0;
204         }
205
206         oo->matrix->data[0] = (sum + det) * (n - 1) + fmean * (n);
207
208         if(OMX_DEBUG) { Rprintf("MLFitFunction value comes to: %f (Chisq: %f).\n", oo->matrix->data[0], (sum + det) - Q - cov->cols); }
209
210 }
211
212 void omxPopulateMLAttributes(omxFitFunction *oo, SEXP algebra) {
213     if(OMX_DEBUG) { Rprintf("Populating ML Attributes.\n"); }
214
215         omxMLFitFunction *argStruct = ((omxMLFitFunction*)oo->argStruct);
216         omxMatrix *expCovInt = argStruct->expectedCov;                  // Expected covariance
217         omxMatrix *expMeanInt = argStruct->expectedMeans;                       // Expected means
218
219         SEXP expCovExt, expMeanExt, gradients;
220         PROTECT(expCovExt = allocMatrix(REALSXP, expCovInt->rows, expCovInt->cols));
221         for(int row = 0; row < expCovInt->rows; row++)
222                 for(int col = 0; col < expCovInt->cols; col++)
223                         REAL(expCovExt)[col * expCovInt->rows + row] =
224                                 omxMatrixElement(expCovInt, row, col);
225
226         if (expMeanInt != NULL) {
227                 PROTECT(expMeanExt = allocMatrix(REALSXP, expMeanInt->rows, expMeanInt->cols));
228                 for(int row = 0; row < expMeanInt->rows; row++)
229                         for(int col = 0; col < expMeanInt->cols; col++)
230                                 REAL(expMeanExt)[col * expMeanInt->rows + row] =
231                                         omxMatrixElement(expMeanInt, row, col);
232         } else {
233                 PROTECT(expMeanExt = allocMatrix(REALSXP, 0, 0));               
234         }   
235
236         if (0) {  // TODO, fix for new gradient internal API
237                 int nLocs = oo->matrix->currentState->numFreeParams;
238                 double gradient[oo->matrix->currentState->numFreeParams];
239                 for(int loc = 0; loc < nLocs; loc++) {
240                         gradient[loc] = NA_REAL;
241                 }
242                 //oo->gradientFun(oo, gradient);
243                 PROTECT(gradients = allocMatrix(REALSXP, 1, nLocs));
244
245                 for(int loc = 0; loc < nLocs; loc++)
246                         REAL(gradients)[loc] = gradient[loc];
247         } else {
248                 PROTECT(gradients = allocMatrix(REALSXP, 0, 0));
249         }
250     
251         setAttrib(algebra, install("expCov"), expCovExt);
252         setAttrib(algebra, install("expMean"), expMeanExt);
253         setAttrib(algebra, install("gradients"), gradients);
254         
255         UNPROTECT(3);
256
257 }
258
259 void omxSetMLFitFunctionCalls(omxFitFunction* oo) {
260         
261         /* Set FitFunction Calls to ML FitFunction Calls */
262         oo->fitType = "omxMLFitFunction";
263         oo->computeFun = omxCallMLFitFunction;
264         oo->destructFun = omxDestroyMLFitFunction;
265         oo->setFinalReturns = omxSetFinalReturnsMLFitFunction;
266         oo->populateAttrFun = omxPopulateMLAttributes;
267         oo->repopulateFun = handleFreeVarList;
268 }
269
270
271 void omxInitMLFitFunction(omxFitFunction* oo) {
272
273         if(OMX_DEBUG) { Rprintf("Initializing ML fit function.\n"); }
274
275         int info = 0;
276         double det = 1.0;
277         char u = 'U';
278         SEXP rObj = oo->rObj;
279         
280         /* Read and set expectation */
281         omxSetMLFitFunctionCalls(oo);
282
283         if (!oo->expectation) { error("%s requires an expectation", oo->fitType); }
284
285         omxData* dataMat = oo->expectation->data;
286
287         if(!(dataMat == NULL) && strncmp(omxDataType(dataMat), "cov", 3) != 0 && strncmp(omxDataType(dataMat), "cor", 3) != 0) {
288                 if(strncmp(omxDataType(dataMat), "raw", 3) == 0) {
289                         if(OMX_DEBUG) { Rprintf("Raw Data: Converting to FIML.\n"); }
290                         omxInitFIMLFitFunction(oo, rObj);
291                         return;
292                 }
293                 char *errstr = (char*) calloc(250, sizeof(char));
294                 sprintf(errstr, "ML FitFunction unable to handle data type %s.\n", omxDataType(dataMat));
295                 omxRaiseError(oo->matrix->currentState, -1, errstr);
296                 free(errstr);
297                 if(OMX_DEBUG) { Rprintf("ML FitFunction unable to handle data type %s.  Aborting.\n", omxDataType(dataMat)); }
298                 return;
299         }
300
301         omxMLFitFunction *newObj = (omxMLFitFunction*) R_alloc(1, sizeof(omxMLFitFunction));
302
303         if(OMX_DEBUG) { Rprintf("Processing Observed Covariance.\n"); }
304         newObj->observedCov = omxDataMatrix(dataMat, NULL);
305         if(OMX_DEBUG) { Rprintf("Processing Observed Means.\n"); }
306         newObj->observedMeans = omxDataMeans(dataMat, NULL, NULL);
307         if(OMX_DEBUG && newObj->observedMeans == NULL) { Rprintf("ML: No Observed Means.\n"); }
308         if(OMX_DEBUG) { Rprintf("Processing n.\n"); }
309         newObj->n = omxDataNumObs(dataMat);
310
311         newObj->expectedCov = omxGetExpectationComponent(oo->expectation, oo, "cov");
312         newObj->expectedMeans = omxGetExpectationComponent(oo->expectation, oo, "means");
313
314         if(newObj->expectedCov == NULL) {
315                 omxRaiseError(oo->matrix->currentState, OMX_DEVELOPER_ERROR,
316                         "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.");
317                 return;
318         }
319
320         // Error Checking: Observed/Expected means must agree.  
321         // ^ is XOR: true when one is false and the other is not.
322         if((newObj->expectedMeans == NULL) ^ (newObj->observedMeans == NULL)) {
323                 if(newObj->expectedMeans != NULL) {
324                         omxRaiseError(oo->matrix->currentState, OMX_ERROR,
325                                 "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");
326                         return;
327                 } else {
328                         omxRaiseError(oo->matrix->currentState, OMX_ERROR,
329                                 "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");
330                         return;         
331                 }
332         }
333
334         /* Temporary storage for calculation */
335         int rows = newObj->observedCov->rows;
336         int cols = newObj->observedCov->cols;
337         newObj->localCov = omxInitMatrix(NULL, rows, cols, TRUE, oo->matrix->currentState);
338         newObj->localProd = omxInitMatrix(NULL, rows, cols, TRUE, oo->matrix->currentState);
339         newObj->P = omxInitMatrix(NULL, 1, cols, TRUE, oo->matrix->currentState);
340         newObj->C = omxInitMatrix(NULL, rows, cols, TRUE, oo->matrix->currentState);
341         newObj->I = omxInitMatrix(NULL, rows, cols, TRUE, oo->matrix->currentState);
342
343         for(int i = 0; i < rows; i++) omxSetMatrixElement(newObj->I, i, i, 1.0);
344
345         omxCopyMatrix(newObj->localCov, newObj->observedCov);
346
347         newObj->lwork = newObj->expectedCov->rows;
348         newObj->work = (double*)R_alloc(newObj->lwork, sizeof(double));
349
350         // TODO: Determine where the saturated model computation should go.
351
352         F77_CALL(dpotrf)(&u, &(newObj->localCov->cols), newObj->localCov->data, &(newObj->localCov->cols), &info);
353
354         if(OMX_DEBUG) { Rprintf("Info on LU Decomp: %d\n", info); }
355         if(info != 0) {
356                 char *errstr = (char*) calloc(250, sizeof(char));
357                 sprintf(errstr, "Observed Covariance Matrix is non-positive-definite.\n");
358                 omxRaiseError(oo->matrix->currentState, -1, errstr);
359                 free(errstr);
360                 return;
361         }
362         for(info = 0; info < newObj->localCov->cols; info++) {
363                 det *= omxMatrixElement(newObj->localCov, info, info);
364         }
365         det *= det;                                     // Product of squares.
366
367         if(OMX_DEBUG) { Rprintf("Determinant of Observed Cov: %f\n", det); }
368         newObj->logDetObserved = log(det);
369         if(OMX_DEBUG) { Rprintf("Log Determinant of Observed Cov: %f\n", newObj->logDetObserved); }
370
371         omxCopyMatrix(newObj->localCov, newObj->expectedCov);
372     oo->argStruct = (void*)newObj;
373
374 }
375
376 void omxSetMLFitFunctionGradient(omxFitFunction* oo, void (*derivativeFun)(omxFitFunction*, double*)) {
377     if(strncmp("omxMLFitFunction", oo->fitType, 16)) {
378         char errorstr[250];
379         sprintf(errorstr, "PROGRAMMER ERROR: Using vanilla-ML gradient with FitFunction of type %s", oo->fitType);
380         omxRaiseError(oo->matrix->currentState, -2, errorstr);
381         return;
382     }
383     
384     if(derivativeFun == NULL) {
385         char errorstr[250];
386         sprintf(errorstr, "Programmer error: ML gradient given NULL gradient function.");
387         omxRaiseError(oo->matrix->currentState, -2, errorstr);
388         return;
389     }
390     
391     //oo->gradientFun = derivativeFun; TODO
392 }
393
394 void omxSetMLFitFunctionGradientComponents(omxFitFunction* oo, void (*derivativeFun)(omxFitFunction*, omxMatrix**, omxMatrix**, int*)) {
395     if(OMX_DEBUG) { Rprintf("Setting up gradient component function for ML FitFunction."); }
396     if(!strncmp("omxFIMLFitFunction", oo->fitType, 16)) {
397         if(OMX_DEBUG) { Rprintf("FIML FitFunction gradients not yet implemented. Skipping."); }
398         return; // ERROR:NYI.
399     }
400     
401     if(derivativeFun == NULL) {
402         char errorstr[250];
403         sprintf(errorstr, "Programmer error: ML gradient components given NULL gradient function.");
404         omxRaiseError(oo->matrix->currentState, -2, errorstr);
405         return;
406     }
407     
408     omxMLFitFunction *omo = ((omxMLFitFunction*) oo->argStruct);
409     int rows = omo->observedCov->rows;
410     int cols = omo->observedCov->cols;
411     int nFreeVars = oo->matrix->currentState->numFreeParams;
412             
413     omo->derivativeFun = derivativeFun;
414     omo->X  = omxInitMatrix(NULL, rows, cols, TRUE, oo->matrix->currentState);
415     omo->Y  = omxInitMatrix(NULL, rows, cols, TRUE, oo->matrix->currentState);
416     omo->Ms = omxInitMatrix(NULL, 1, cols, TRUE, oo->matrix->currentState);
417     omo->Mu = omxInitMatrix(NULL, 1, cols, TRUE, oo->matrix->currentState);
418     omo->dSigma = (omxMatrix**) R_alloc(nFreeVars, sizeof(omxMatrix*));
419     omo->dMu = (omxMatrix**) R_alloc(nFreeVars, sizeof(omxMatrix*));
420     for(int i = 0; i < nFreeVars; i++) {
421         omo->dSigma[i] = omxInitMatrix(NULL, rows, cols, TRUE, oo->matrix->currentState);
422         omo->dMu[i] = omxInitMatrix(NULL, rows, 1, TRUE, oo->matrix->currentState);
423     }
424     //oo->gradientFun = omxCalculateMLGradient; TODO
425 }
426
427 void omxCalculateMLGradient(omxFitFunction* oo, double* gradient) {
428
429     if(OMX_DEBUG) { Rprintf("Beginning ML Gradient Calculation.\n"); }
430     // Rprintf("Beginning ML Gradient Calculation, Iteration %d.%d (%d)\n", 
431         // oo->matrix->currentState->majorIteration, oo->matrix->currentState->minorIteration,
432         // oo->matrix->currentState->computeCount); //:::DEBUG:::
433     // 1) Calculate current Expected Covariance
434     // 2) Calculate eCov, the Inverse Expected Covariance matrix 
435     // 3) Calculate C = I - eCov D, where D is the observed covariance matrix
436     // 4) Calculate b = M - [observed M]
437     // 5) For each location in locs:
438     //   gradient[loc] = tr(eCov^-1 %*% dEdt %*% C) - (b^T %*% eCov^-1 %*% dEdt + 2 dMdt^T))eCov^-1 b)
439
440     omxMLFitFunction *omo = ((omxMLFitFunction*)oo->argStruct);
441     
442     /* Locals for readability.  Compiler should cut through this. */
443     omxMatrix *scov         = omo->observedCov;
444     omxMatrix *smeans       = omo->observedMeans;
445     omxMatrix *cov          = omo->expectedCov;
446     omxMatrix *M            = omo->expectedMeans;
447     omxMatrix *eCov         = omo->localCov;        // TODO: Maybe need to avoid reusing these
448     omxMatrix *I            = omo->I;
449     omxMatrix *C            = omo->C;
450     omxMatrix *X            = omo->X;
451     omxMatrix *Y            = omo->Y;
452     omxMatrix *Mu           = omo->Mu;
453     omxMatrix *Ms           = omo->Ms;
454     omxMatrix *P            = omo->P;
455     double n                = omo->n;
456     omxMatrix** dSigmas     = omo->dSigma;
457     omxMatrix** dMus        = omo->dMu;
458     
459     int gradientSize = oo->matrix->currentState->numFreeParams;
460     
461     char u = 'U';
462     int info;
463     double minusoned = -1.0;
464     int onei = 1;
465     int status[gradientSize];
466     int nLocs = oo->matrix->currentState->numFreeParams;
467     
468     // Calculate current FitFunction values
469     // We can safely assume this has been done
470     // omxFitFunctionCompute(oo);
471     
472     // Calculate current eCov
473     
474     omxCopyMatrix(eCov, cov);                           // But expected cov is destroyed in inversion
475     
476     F77_CALL(dpotrf)(&u, &(eCov->cols), eCov->data, &(eCov->cols), &info);
477
478     if(OMX_DEBUG_ALGEBRA) { Rprintf("Info on LU Decomp: %d\n", info);}
479     if(info > 0) {
480             char *errstr = (char*) calloc(250, sizeof(char));
481         sprintf(errstr, "Expected covariance matrix is non-positive-definite");
482         if(oo->matrix->currentState->computeCount <= 0) {
483             strncat(errstr, " at starting values", 20);
484         }
485         strncat(errstr, ".\n", 3);
486         omxRaiseError(oo->matrix->currentState, -1, errstr);                        // Raise error
487         free(errstr);
488         return;                                                                     // Leave output untouched
489     }
490     
491     F77_CALL(dpotri)(&u, &(eCov->rows), eCov->data, &(eCov->cols), &info);
492     if(info > 0) {
493             char *errstr = (char*) calloc(250, sizeof(char));
494         sprintf(errstr, "Expected covariance matrix is not invertible");
495         if(oo->matrix->currentState->computeCount <= 0) {
496             strncat(errstr, " at starting values", 20);
497         }
498         strncat(errstr, ".\n", 3);
499         omxRaiseError(oo->matrix->currentState, -1, errstr);                        // Raise error
500         free(errstr);
501         return;
502     }
503     // Calculate P = expected means - observed means
504     if(M != NULL) {
505         omxCopyMatrix(P, smeans);
506         F77_CALL(daxpy)(&(smeans->cols), &minusoned, M->data, &onei, P->data, &onei);
507     }
508         
509         // Reset C and Calculate C = I - eCov * oCov
510     omxCopyMatrix(C, I);
511     omxDSYMM(TRUE, -1.0, eCov, scov, 1.0, C);
512     
513     // For means, calculate Ms = eCov-1 %*% P
514     if(M != NULL)
515         omxDSYMM(FALSE, 1.0, eCov, P, 0.0, Ms);
516     
517     // Calculate parameter-level derivatives
518     // TODO: Parallelize Here.
519
520     if(OMX_DEBUG)  { Rprintf("Calling component function.\n"); }
521     omo->derivativeFun(oo, dSigmas, dMus, status);
522     
523     for(int currentLoc = 0; currentLoc < nLocs; currentLoc++) {
524         double meanInfluence, covInfluence;
525         if(status[currentLoc] < 0) continue;  // Failure in computation--skip.
526         //   gradient[loc] = tr(eCov^-1 %*% dEdt %*% C) - 
527         //    (b^T %*% eCov^-1 %*% dEdt + 2 dMdt^T))eCov^-1 b)
528         // omxDGEMM(FALSE, FALSE, 1.0, dSigmas[currentLoc], C, 0.0, Y);
529         omxDSYMM(TRUE, 1.0, dSigmas[currentLoc], C, 0.0, Y);
530         omxDSYMM(TRUE, 1.0, eCov, Y, 0.0, X);
531         gradient[currentLoc] = 0;
532         covInfluence = 0.0;
533         for(int i = 0; i < eCov->cols; i++) 
534             covInfluence += omxMatrixElement(X, i, i);
535         if(M != NULL) {
536             omxCopyMatrix(Mu, dMus[currentLoc]);
537             omxDSYMV(1.0, dSigmas[currentLoc], Ms, 2.0, Mu);
538             meanInfluence = F77_CALL(ddot)(&(eCov->cols), Mu->data, &onei, Ms->data, &onei);
539         } else {
540             meanInfluence = 0;
541         }
542         gradient[currentLoc] = (covInfluence * (n-1)) - (meanInfluence * n);
543         if(OMX_DEBUG) { 
544             Rprintf("Calculation for Gradient value %d: Cov: %3.9f, Mean: %3.9f, total: %3.9f\n",
545             currentLoc, covInfluence, meanInfluence, gradient[currentLoc]); 
546         }
547     }
548 }