Revert "Add option to checkpoint every evaluation"
[openmx:openmx.git] / src / omxRAMExpectation.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 "omxExpectation.h"
18 #include "omxFitFunction.h"
19 #include "omxBLAS.h"
20 #include "omxRAMExpectation.h"
21
22 typedef struct {
23
24         omxMatrix *cov, *means; // observed covariance and means
25         omxMatrix *A, *S, *F, *M, *I;
26         omxMatrix *C, *EF, *ZM, *U, *V, *W, *X, *Y, *Z, *Ax, *P, *bCB, *ZSBC, *Mns, *lilI, *eCov, *beCov;
27     omxMatrix *dA, *dS, *dM, *b, *D, *paramVec;
28     // For gradients
29     omxMatrix ***eqnOuts;
30     omxMatrix **dAdts, **dSdts, **dMdts;
31
32     // Each vector keeps track of the number
33     // of non-zero elements in the
34     // corresponding dXdts matrix.
35     int *dAdtsCount, *dSdtsCount, *dMdtsCount;
36
37     int *dAdtsRowCache, *dAdtsColCache;
38     int *dSdtsRowCache, *dSdtsColCache;
39     int *dMdtsRowCache, *dMdtsColCache;
40
41     omxMatrix *tempVec, *bigSum, *lilSum;
42
43     int *pNums, nParam; // For Fast Gradient/Hessian Computation
44         int numIters;
45         double logDetObserved;
46         double n;
47         double *work;
48         int lwork;
49
50 } omxRAMExpectation;
51
52 static void calculateRAMGradientComponents(omxExpectation* oo, omxMatrix**, omxMatrix**, int*);
53 static void sliceCrossUpdate(omxMatrix* A, omxMatrix* B, int row, int col, omxMatrix* result);
54 static void omxCalculateRAMCovarianceAndMeans(omxMatrix* A, omxMatrix* S, omxMatrix* F, 
55     omxMatrix* M, omxMatrix* Cov, omxMatrix* Means, int numIters, omxMatrix* I, 
56     omxMatrix* Z, omxMatrix* Y, omxMatrix* X, omxMatrix* Ax);
57 static void fastRAMGradientML(omxExpectation* oo, omxFitFunction* off, double* result);
58 static omxMatrix* omxGetRAMExpectationComponent(omxExpectation* ox, omxFitFunction* off, const char* component);
59
60 // Speedup Helper
61 static void ADB(omxMatrix** A, omxMatrix** B, int numArgs, omxMatrix** D, int *Dcounts, 
62         int *DrowCache, int *DcolCache, int matNum, std::vector< omxFreeVar* > &varList, 
63         int* pNums, int nParam, omxMatrix*** result) {
64     // Computes Matrix %*% params %*% Matrix in O(K^2) time.  Based on von Oertzen & Brick, in prep.
65     // Also populates the matrices called D if it appears.
66     // Minimal Rf_error checking.
67     if(OMX_DEBUG_ALGEBRA) mxLog("Beginning ADB."); //:::DEBUG:::
68
69     omxFreeVar *var;
70     int paramNo;
71
72     for(int param = 0; param < nParam; param++) {
73         if(pNums != NULL) {
74             paramNo = pNums[param];
75         } else {
76             paramNo = param;
77         }
78         var = varList[paramNo];
79
80         // Set D
81         if(D != NULL) {
82             int nonzero = 0;
83             omxMatrix *thisD = D[param];
84             memset(thisD->data, 0, sizeof(double) * thisD->cols * thisD->rows);
85             // Honestly, this should be calculated only once.
86             for(size_t varLoc = 0; varLoc < var->locations.size(); varLoc++) {
87                 if(~var->locations[varLoc].matrix == matNum) {
88                         int row = var->locations[varLoc].row;
89                         int col = var->locations[varLoc].col;
90                         omxSetMatrixElement(thisD, row, col, 1.0);
91                         if (DrowCache != NULL) DrowCache[param] = row;
92                         if (DcolCache != NULL) DcolCache[param] = col;
93                         nonzero++;
94                 }
95             }
96             Dcounts[param] = nonzero;
97         }
98         for(int eqn = 0; eqn < numArgs; eqn++) {
99             omxMatrix* thisResult = result[eqn][param];
100             memset(thisResult->data, 0, sizeof(double) * thisResult->cols * thisResult->rows);
101             for(size_t varLoc = 0; varLoc < var->locations.size(); varLoc++) {
102                 if(~var->locations[varLoc].matrix == matNum) {
103                     sliceCrossUpdate(A[eqn], B[eqn],
104                                      var->locations[varLoc].row,
105                                      var->locations[varLoc].col, thisResult);
106                 }
107             }
108         }
109     }
110 }
111
112 static void sliceCrossUpdate(omxMatrix* A, omxMatrix* B, int row, int col, omxMatrix* result) {
113     // Performs outer multiply of column col of A by row row of B.
114     // Adds product to beta * result.
115     int nrow = A->rows;
116     int ncol = B->cols;
117     int resultrows = result->rows;
118     int brows = B->rows;
119
120     if (A->colMajor && B->colMajor && result->colMajor) {
121         int aoffset = row * nrow;
122         double* Adata = A->data;
123         double* Bdata = B->data;
124         double* resdata = result->data;
125
126         for(int k = 0; k < ncol; k++) {
127             int offset = k * resultrows;
128             double factor = Bdata[k * brows + col];
129             for(int j = 0; j < nrow; j++) {
130                 resdata[offset + j] += Adata[aoffset + j] * factor;
131             }
132         }
133     } else {
134         for(int k = 0; k < ncol; k++) {
135             for(int j = 0; j < nrow; j++) {
136                 omxAccumulateMatrixElement(result, j, k, 
137                     omxMatrixElement(A, j, row) * omxMatrixElement(B, col, k));
138             }
139         }
140     }
141 }
142
143 static void omxCallRAMExpectation(omxExpectation* oo, const char *, const char *) {
144     if(OMX_DEBUG) { mxLog("RAM Expectation calculating."); }
145         omxRAMExpectation* oro = (omxRAMExpectation*)(oo->argStruct);
146         
147         omxRecompute(oro->A);
148         omxRecompute(oro->S);
149         omxRecompute(oro->F);
150         if(oro->M != NULL)
151             omxRecompute(oro->M);
152             
153         omxCalculateRAMCovarianceAndMeans(oro->A, oro->S, oro->F, oro->M, oro->cov, 
154                 oro->means, oro->numIters, oro->I, oro->Z, oro->Y, oro->X, oro->Ax);
155 }
156
157 static void omxDestroyRAMExpectation(omxExpectation* oo) {
158
159         if(OMX_DEBUG) { mxLog("Destroying RAM Expectation."); }
160         
161         omxRAMExpectation* argStruct = (omxRAMExpectation*)(oo->argStruct);
162
163         omxFreeMatrix(argStruct->cov);
164
165         if(argStruct->means != NULL) {
166                 omxFreeMatrix(argStruct->dM);
167                 omxFreeMatrix(argStruct->ZM);
168                 omxFreeMatrix(argStruct->bCB);
169                 omxFreeMatrix(argStruct->b);
170                 omxFreeMatrix(argStruct->tempVec);
171                 omxFreeMatrix(argStruct->bigSum);
172                 omxFreeMatrix(argStruct->lilSum);
173                 omxFreeMatrix(argStruct->beCov);
174                 omxFreeMatrix(argStruct->means);
175         }
176
177         int nParam = argStruct->nParam;
178         if(nParam >= 0) {
179                 for(int j = 0; j < nParam; j++) {
180                         omxFreeMatrix(argStruct->dAdts[j]);
181                         omxFreeMatrix(argStruct->dSdts[j]);
182                         omxFreeMatrix(argStruct->dMdts[j]);
183                         omxFreeMatrix(argStruct->eqnOuts[0][j]);
184                 }
185                 omxFreeMatrix(argStruct->paramVec);
186         }
187
188         omxFreeMatrix(argStruct->dA);
189         omxFreeMatrix(argStruct->dS);
190
191         omxFreeMatrix(argStruct->I);
192         omxFreeMatrix(argStruct->lilI);
193         omxFreeMatrix(argStruct->X);
194         omxFreeMatrix(argStruct->Y);
195         omxFreeMatrix(argStruct->Z);
196         omxFreeMatrix(argStruct->Ax);
197
198         omxFreeMatrix(argStruct->W);
199         omxFreeMatrix(argStruct->U);
200         omxFreeMatrix(argStruct->EF);
201         omxFreeMatrix(argStruct->V);
202         omxFreeMatrix(argStruct->ZSBC);
203         omxFreeMatrix(argStruct->C);
204         omxFreeMatrix(argStruct->eCov);
205
206 }
207
208 static void omxPopulateRAMAttributes(omxExpectation *oo, SEXP algebra) {
209     if(OMX_DEBUG) { mxLog("Populating RAM Attributes."); }
210
211         omxRAMExpectation* oro = (omxRAMExpectation*) (oo->argStruct);
212         omxMatrix* A = oro->A;
213         omxMatrix* S = oro->S;
214         //omxMatrix* X = oro->X;
215         omxMatrix* Ax= oro->Ax;
216         omxMatrix* Z = oro->Z;
217         omxMatrix* I = oro->I;
218     int numIters = oro->numIters;
219     double oned = 1.0, zerod = 0.0;
220     
221     omxRecompute(A);
222     omxRecompute(S);
223         
224     omxShallowInverse(NULL, numIters, A, Z, Ax, I ); // Z = (I-A)^-1
225         
226         omxDGEMM(FALSE, FALSE, oned, Z, S, zerod, Ax);
227
228         omxDGEMM(FALSE, TRUE, oned, Ax, Z, zerod, Ax);
229         // Ax = ZSZ' = Covariance matrix including latent variables
230         
231         SEXP expCovExt;
232         Rf_protect(expCovExt = Rf_allocMatrix(REALSXP, Ax->rows, Ax->cols));
233         for(int row = 0; row < Ax->rows; row++)
234                 for(int col = 0; col < Ax->cols; col++)
235                         REAL(expCovExt)[col * Ax->rows + row] =
236                                 omxMatrixElement(Ax, row, col);
237         Rf_setAttrib(algebra, Rf_install("UnfilteredExpCov"), expCovExt);
238         Rf_setAttrib(algebra, Rf_install("numStats"), Rf_ScalarReal(omxDataDF(oo->data)));
239         Rf_unprotect(1);
240 }
241
242 /*
243  * omxCalculateRAMCovarianceAndMeans
244  *                      Just like it says on the tin.  Calculates the mean and covariance matrices
245  * for a RAM model.  M is the number of total variables, latent and manifest. N is
246  * the number of manifest variables.
247  *
248  * params:
249  * omxMatrix *A, *S, *F         : matrices as specified in the RAM model.  MxM, MxM, and NxM
250  * omxMatrix *M                         : vector containing model implied means. 1xM
251  * omxMatrix *Cov                       : On output: model-implied manifest covariance.  NxN.
252  * omxMatrix *Means                     : On output: model-implied manifest means.  1xN.
253  * int numIterations            : Precomputed number of iterations of taylor series expansion.
254  * omxMatrix *I                         : Identity matrix.  If left NULL, will be populated.  MxM.
255  * omxMatrix *Z                         : On output: Computed (I-A)^-1. MxM.
256  * omxMatrix *Y, *X, *Ax        : Space for computation. NxM, NxM, MxM.  On exit, populated.
257  */
258
259 static void omxCalculateRAMCovarianceAndMeans(omxMatrix* A, omxMatrix* S, omxMatrix* F, 
260         omxMatrix* M, omxMatrix* Cov, omxMatrix* Means, int numIters, omxMatrix* I, 
261         omxMatrix* Z, omxMatrix* Y, omxMatrix* X, omxMatrix* Ax) {
262         
263         if(OMX_DEBUG) { mxLog("Running RAM computation with numIters is %d\n.", numIters); }
264                 
265         if(Ax == NULL || I == NULL || Z == NULL || Y == NULL || X == NULL) {
266                 Rf_error("Internal Error: RAM Metadata improperly populated.  Please report this to the OpenMx development team.");
267         }
268                 
269         if(Cov == NULL && Means == NULL) {
270                 return; // We're not populating anything, so why bother running the calculation?
271         }
272         
273         // if(   (Cov->rows != Cov->cols)  || (A->rows  != A->cols)  // Conformance check
274         //      || (X->rows  != Cov->cols)  || (X->cols  != A->rows)
275         //      || (Y->rows  != Cov->cols)  || (Y->cols  != A->rows)
276         //      || (Ax->rows != Cov->cols)  || (Ax->cols != A->rows)
277         //      || (I->rows  != Cov->cols)  || (I->cols  != Cov->rows)
278         //      || (Y->rows  != Cov->cols)  || (Y->cols  != A->rows)
279         //      || (M->cols  != Cov->cols)  || (M->rows  != 1)
280         //      || (Means->rows != 1)       || (Means->cols != Cov->cols) ) {
281         //              Rf_error("INTERNAL ERROR: Incorrectly sized matrices being passed to omxRAMExpectation Calculation.\n Please report this to the OpenMx development team.");
282         // }
283         
284         omxShallowInverse(NULL, numIters, A, Z, Ax, I );
285         
286         /* Cov = FZSZ'F' */
287         omxDGEMM(FALSE, FALSE, 1.0, F, Z, 0.0, Y);
288
289         omxDGEMM(FALSE, FALSE, 1.0, Y, S, 0.0, X);
290
291         omxDGEMM(FALSE, TRUE, 1.0, X, Y, 0.0, Cov);
292          // Cov = FZSZ'F' (Because (FZ)' = Z'F')
293         
294         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(Cov, "....RAM: Model-implied Covariance Matrix:");}
295         
296         if(M != NULL && Means != NULL) {
297                 // F77_CALL(omxunsafedgemv)(Y->majority, &(Y->rows), &(Y->cols), &oned, Y->data, &(Y->leading), M->data, &onei, &zerod, Means->data, &onei);
298                 omxDGEMV(FALSE, 1.0, Y, M, 0.0, Means);
299                 if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(Means, "....RAM: Model-implied Means Vector:");}
300         }
301 }
302
303 void omxInitRAMExpectation(omxExpectation* oo) {
304         
305         omxState* currentState = oo->currentState;      
306         SEXP rObj = oo->rObj;
307
308         if(OMX_DEBUG) { mxLog("Initializing RAM expectation."); }
309         
310         int l, k;
311
312         SEXP slotValue;
313         
314         omxRAMExpectation *RAMexp = (omxRAMExpectation*) R_alloc(1, sizeof(omxRAMExpectation));
315         
316         /* Set Expectation Calls and Structures */
317         oo->computeFun = omxCallRAMExpectation;
318         oo->destructFun = omxDestroyRAMExpectation;
319         oo->componentFun = omxGetRAMExpectationComponent;
320         oo->populateAttrFun = omxPopulateRAMAttributes;
321         oo->argStruct = (void*) RAMexp;
322         
323         /* Set up expectation structures */
324         if(OMX_DEBUG) { mxLog("Initializing RAM expectation."); }
325
326         if(OMX_DEBUG) { mxLog("Processing M."); }
327         RAMexp->M = omxNewMatrixFromSlot(rObj, currentState, "M");
328
329         if(OMX_DEBUG) { mxLog("Processing A."); }
330         RAMexp->A = omxNewMatrixFromSlot(rObj, currentState, "A");
331
332         if(OMX_DEBUG) { mxLog("Processing S."); }
333         RAMexp->S = omxNewMatrixFromSlot(rObj, currentState, "S");
334
335         if(OMX_DEBUG) { mxLog("Processing F."); }
336         RAMexp->F = omxNewMatrixFromSlot(rObj, currentState, "F");
337
338         /* Identity Matrix, Size Of A */
339         if(OMX_DEBUG) { mxLog("Generating I."); }
340         RAMexp->I = omxNewIdentityMatrix(RAMexp->A->rows, currentState);
341         omxRecompute(RAMexp->I);
342         RAMexp->lilI = omxNewIdentityMatrix(RAMexp->F->rows, currentState);
343         omxRecompute(RAMexp->lilI);
344         
345
346         if(OMX_DEBUG) { mxLog("Processing expansion iteration depth."); }
347         Rf_protect(slotValue = R_do_slot(rObj, Rf_install("depth")));
348         RAMexp->numIters = INTEGER(slotValue)[0];
349         if(OMX_DEBUG) { mxLog("Using %d iterations.", RAMexp->numIters); }
350         Rf_unprotect(1);
351
352         l = RAMexp->F->rows;
353         k = RAMexp->A->cols;
354
355         if(OMX_DEBUG) { mxLog("Generating internals for computation."); }
356
357         RAMexp->Z =     omxInitMatrix(k, k, TRUE, currentState);
358         RAMexp->Ax =    omxInitMatrix(k, k, TRUE, currentState);
359         RAMexp->W =     omxInitMatrix(k, k, TRUE, currentState);
360         RAMexp->U =     omxInitMatrix(l, k, TRUE, currentState);
361         RAMexp->Y =     omxInitMatrix(l, k, TRUE, currentState);
362         RAMexp->X =     omxInitMatrix(l, k, TRUE, currentState);
363         RAMexp->EF=     omxInitMatrix(k, l, TRUE, currentState);
364         RAMexp->V =     omxInitMatrix(l, k, TRUE, currentState);
365         RAMexp->ZSBC =  omxInitMatrix(k, l, TRUE, currentState);
366         RAMexp->C =     omxInitMatrix(l, l, TRUE, currentState);
367         
368         if(omxIsMatrix(RAMexp->A)) {
369                 RAMexp->dA = omxInitMatrix(k, k, TRUE, currentState);
370         } else {
371                 RAMexp->dA = NULL;
372         }
373         if(omxIsMatrix(RAMexp->S)) {
374                 RAMexp->dS = omxInitMatrix(k, k, TRUE, currentState);
375         } else {
376                 RAMexp->dS = NULL;
377         }
378         if(RAMexp->M != NULL) {
379                 RAMexp->dM =    omxInitMatrix(1, k, TRUE, currentState);
380                 RAMexp->ZM =    omxInitMatrix(k, 1, TRUE, currentState);
381                 RAMexp->bCB =   omxInitMatrix(k, 1, TRUE, currentState);
382                 RAMexp->b =     omxInitMatrix(l, 1, TRUE, currentState);
383                 RAMexp->tempVec =       omxInitMatrix(k, 1, TRUE, currentState);
384                 RAMexp->bigSum =        omxInitMatrix(k, 1, TRUE, currentState);
385                 RAMexp->lilSum =        omxInitMatrix(l, 1, TRUE, currentState);
386                 RAMexp->beCov =         omxInitMatrix(l, 1, TRUE, currentState);
387                 RAMexp->Mns =   NULL;
388     }
389
390         RAMexp->cov =           omxInitMatrix(l, l, TRUE, currentState);
391
392         if(RAMexp->M != NULL) {
393                 RAMexp->means =         omxInitMatrix(1, l, TRUE, currentState);
394         } else {
395             RAMexp->means  =    NULL;
396     }
397
398     // Derivative space:
399         RAMexp->eqnOuts = NULL;
400         RAMexp->dAdts = NULL;
401         RAMexp->dSdts = NULL;
402         RAMexp->dMdts = NULL;
403         RAMexp->paramVec = NULL;
404         RAMexp->D =     NULL;
405         RAMexp->pNums = NULL;
406         RAMexp->nParam = -1;
407         RAMexp->eCov=       omxInitMatrix(l, l, TRUE, currentState);
408
409 }
410
411 /*
412  * fastRAMGradientML
413  *                      Calculates the derivatives of the likelihood for a RAM model.  
414  * Locations of free parameters are extracted from the omxExpecation's state object.
415  *
416  * params:
417  * omxMatrix *A, *S, *F         : matrices as specified in the RAM model.  MxM, MxM, and NxM
418  * omxMatrix *M                         : vector containing model implied means. 1xM
419  * omxMatrix *Cov                       : On output: model-implied manifest covariance.  NxN.
420  * omxMatrix *Means                     : On output: model-implied manifest means.  1xN.
421  * int numIterations            : Precomputed number of iterations of taylor series expansion.
422  * omxMatrix *I                         : Identity matrix.  If left NULL, will be populated.  MxM.
423  * omxMatrix *Z                         : On output: Computed (I-A)^-1. MxM.
424  * omxMatrix *Y, *X, *Ax        : Space for computation. NxM, NxM, MxM.  On exit, populated.
425  */
426
427 static void fastRAMGradientML(omxExpectation* oo, omxFitFunction* off, double* result) {
428     
429     // This calculation is based on von Oertzen and Brick, in prep.
430     //
431     // Steps:
432     // 1) (Re) Calculate current A, S, F, M, and Z (where Z = (I-A)^-1)
433     // 2) cov = current Expected Covariance (fxf)
434     // 3) eCov = cov^(-1) (fxf) *
435     // 4) ZS = Z S  (axa) *
436     // 5) B = F Z   (fxa) *
437     // 6) C = I - eCov D, where D is the observed covariance matrix (fxf) *
438     // 7) BC = B^T C (axf) *
439     // 8) ZSB = ZS (B^T) *
440     // 8) ZSBC = ZS BC (axf) *
441     // 9) eCovB = cov^(-1) B (fxa) *
442     // For Means:
443         // 10) Means = BM (fx1) *
444         // 11) b = Means - d, where d is the observed means matrix (fx1)
445         // 12) beCov = b cov^(-1) (1xf) *
446         // 13) beCovB = b cov^(-1) B (1xa) *
447
448     
449     // 14) For each location in locations: (Note: parallelize, gradients here!)
450     //      1A) Calculate dA/dt and dS/dt by substituting 1s into empty matrices
451     //      1B) eqnList = 2 * ADB(eCovB, dAdt, ZSBC)
452     //      1C) eqnList2 = ADB(eCovB, dSdt, BC)  // Preferably, build
453     //      2) sum = eqnList1 + eqnList2
454     //      If Means:
455     //          3) sumVec = beCovB dAdt ZS
456     //          4) sumVec += beCovB ZS dAdt
457     //          5) sumVec += beCovB dSdt
458     //          6) sumVec *= B^T
459     //          7) sumVec += 2 * B dAdt Means
460     //          8) sumVec += 2 * B dMdt
461     //          9) sum += sumVec beCov
462
463     //  Right.  All the * elements are saved for hessian computation.
464  
465     if(OMX_DEBUG) {mxLog("Calculating fast RAM/ML gradient."); }
466     
467     omxRAMExpectation* oro = (omxRAMExpectation*)(oo->argStruct);
468                                     // Size reference: A is axa, F is fxa
469     omxMatrix* A = oro->A;          // axa
470     omxMatrix* S = oro->S;          // axa
471     omxMatrix* F = oro->F;          // fxa
472     omxMatrix* M = oro->M;          // 1xa
473     omxMatrix* Z = oro->Z;          // axa
474
475     omxMatrix* cov = oro->cov;      // fxf
476     omxMatrix* means = oro->means;  // fx1
477     omxMatrix* eCov = oro->eCov;    // fxf
478
479         omxMatrix* ZS = oro->Ax;        // axa
480         
481     omxMatrix* B = oro->X;          // fxa
482     omxMatrix* C = oro->C;          // fxf
483     omxMatrix* BC = oro->EF;        // fxa
484     omxMatrix* ZSBC = oro->ZSBC;    // fxa
485     omxMatrix* eCovB = oro->U;      // fxa
486
487     omxMatrix* ZM = oro->ZM;        // 1xa
488     omxMatrix* beCov = oro->beCov;  // 1xf
489     omxMatrix* bCB = oro->bCB;      // 1xa
490     omxMatrix* b = oro->b;          // 1xa
491     
492     omxMatrix** dAdts = oro->dAdts;
493     omxMatrix** dSdts = oro->dSdts;
494     omxMatrix** dMdts = oro->dMdts;
495     
496     int* dAdtsCount = oro->dAdtsCount;
497     int* dSdtsCount = oro->dSdtsCount;
498     int* dMdtsCount = oro->dMdtsCount;
499
500     int* dAdtsRowCache = oro->dAdtsRowCache;
501     int* dAdtsColCache = oro->dAdtsColCache;
502     int* dSdtsRowCache = oro->dSdtsRowCache;
503     int* dSdtsColCache = oro->dSdtsColCache;
504     int* dMdtsRowCache = oro->dMdtsRowCache;
505     int* dMdtsColCache = oro->dMdtsColCache;
506
507     omxMatrix*** eqnOuts = oro->eqnOuts;
508     
509     omxMatrix* tempVec = oro->tempVec;
510     omxMatrix* bigSum  = oro->bigSum;
511     omxMatrix* lilSum  = oro->lilSum;
512     omxMatrix* D = oro->D;
513     omxMatrix* Mns = oro->Mns;
514     
515     omxMatrix* lilI = oro->lilI;
516     omxMatrix* paramVec = oro->paramVec;
517     
518     double n = oro->n;
519     
520     int* pNums = oro->pNums;
521     int nParam = oro->nParam;
522     std::vector< omxFreeVar* > &varList = oo->freeVarGroup->vars;
523     
524     int Amat = A->matrixNumber;
525     int Smat = S->matrixNumber;
526     int Mmat = 0;
527     double oned = 1.0, minusoned = -1.0;
528     int onei = 1;
529     int info;
530     char u = 'U';
531     if(M != NULL) Mmat = M->matrixNumber;
532     
533     FreeVarGroup *freeVarGroup = oo->freeVarGroup;
534
535     omxMatrix *eqnList1[1], *eqnList2[1];
536
537     if(nParam < 0) {
538         nParam = 0;
539         size_t nTotalParams = freeVarGroup->vars.size();
540         if(OMX_DEBUG) { mxLog("Planning Memory for Fast Gradient Calculation: Using %d params.", nParam); }
541         unsigned short int calc[nTotalParams]; 
542         // Work out the set of parameters for which we can calculate gradients
543         // TODO: Potential speedup by splitting this to calculate dA, dS, and dM separately
544         for(size_t parm = 0; parm < nTotalParams; parm++) {
545             omxFreeVar *ofv = freeVarGroup->vars[parm];
546             calc[parm] = 0;
547             for(size_t loc = 0; loc < ofv->locations.size(); loc++) {
548                 int varMat = ~(ofv->locations[loc].matrix);
549                 if(varMat == Amat || varMat == Smat || (M != NULL && varMat == Mmat)) {
550                     calc[parm] = 1;
551                 } else {
552                     calc[parm] = 0;
553                     break;
554                 }
555             }
556             if(calc[parm]) {
557                 nParam++;
558             }
559         }
560         oro->nParam = nParam;
561         pNums = (int*)R_alloc(nParam, sizeof(int));
562
563         int nextFree = 0;
564         // Populate pNums with numbers of the parameters to calculate
565         for(size_t parm = 0; parm < nTotalParams && nextFree < nParam; parm++) {
566             if(calc[parm]) {
567                 pNums[nextFree] = parm;
568                 nextFree++;
569
570             }
571         }
572         oro->pNums = pNums;
573         
574         oro->dAdts = (omxMatrix**) R_alloc(nParam, sizeof(omxMatrix*));
575         oro->dSdts = (omxMatrix**) R_alloc(nParam, sizeof(omxMatrix*));
576         oro->dMdts = (omxMatrix**) R_alloc(nParam, sizeof(omxMatrix*));
577         oro->dAdtsCount = (int*) R_alloc(nParam, sizeof(int));
578         oro->dSdtsCount = (int*) R_alloc(nParam, sizeof(int));
579         oro->dMdtsCount = (int*) R_alloc(nParam, sizeof(int));
580         
581         oro->dAdtsRowCache = (int*) R_alloc(nParam, sizeof(int));
582         oro->dAdtsColCache = (int*) R_alloc(nParam, sizeof(int));
583         oro->dSdtsRowCache = (int*) R_alloc(nParam, sizeof(int));
584         oro->dSdtsColCache = (int*) R_alloc(nParam, sizeof(int));
585         oro->dMdtsRowCache = (int*) R_alloc(nParam, sizeof(int));
586         oro->dMdtsColCache = (int*) R_alloc(nParam, sizeof(int));
587
588         oro->eqnOuts = (omxMatrix***) R_alloc(1, sizeof(omxMatrix**));
589         oro->eqnOuts[0] = (omxMatrix**) R_alloc(nParam, sizeof(omxMatrix*));
590         
591         int a = A->rows;
592         omxState* currentState = oo->currentState;
593         for(int i = 0; i < nParam; i++) {
594             oro->dAdts[i] = omxInitMatrix(a, a, TRUE, currentState);
595             oro->dSdts[i] = omxInitMatrix(a, a, TRUE, currentState);
596             oro->dMdts[i] = omxInitMatrix(1, a, TRUE, currentState);
597             
598             oro->eqnOuts[0][i] = omxInitMatrix(a, a, TRUE, currentState);
599         }
600         oro->paramVec = omxInitMatrix(nParam, 1, TRUE, currentState);
601         paramVec = oro->paramVec;
602         dAdts = oro->dAdts;
603         dSdts = oro->dSdts;
604         dMdts = oro->dMdts;
605         dAdtsCount = oro->dAdtsCount;
606         dSdtsCount = oro->dSdtsCount;
607         dMdtsCount = oro->dMdtsCount;
608         
609         dAdtsRowCache = oro->dAdtsRowCache;
610         dAdtsColCache = oro->dAdtsColCache;
611         dSdtsRowCache = oro->dSdtsRowCache;
612         dSdtsColCache = oro->dSdtsColCache;
613         dMdtsRowCache = oro->dMdtsRowCache;
614         dMdtsColCache = oro->dMdtsColCache;
615
616         eqnOuts = oro->eqnOuts;
617     }
618
619     double covInfluence[nParam];
620     double meanInfluence[nParam];
621     
622     // 1) (Re) Calculate current A, S, F, M, and Z (where Z = (I-A)^-1)
623     // 2) cov = current Expected Covariance (fxf)
624     // Theoretically, we should calculate current fit function values 
625     // But we can safely assume this has already been done
626     // That assumption may no longer be valid in FIML.
627
628     // 3) eCov = cov^(-1) (fxf)
629     omxCopyMatrix(eCov, cov);                           // But expected cov is destroyed in inversion
630     
631     F77_CALL(dpotrf)(&u, &(eCov->cols), eCov->data, &(eCov->cols), &info);
632
633     if(OMX_DEBUG_ALGEBRA) { mxLog("Info on LU Decomp: %d", info);}
634     if(info > 0) {
635             char *errstr = (char*) calloc(250, sizeof(char));
636         sprintf(errstr, "Expected covariance matrix is non-positive-definite");
637         if(oo->currentState->computeCount <= 0) {
638             strncat(errstr, " at starting values", 20);
639         }
640         strncat(errstr, ".\n", 3);
641         omxRaiseError(errstr);                        // Raise Rf_error
642         free(errstr);
643         return;                                                                     // Leave output untouched
644     }
645     
646     F77_CALL(dpotri)(&u, &(eCov->rows), eCov->data, &(eCov->cols), &info);
647     if(info > 0) {
648             char *errstr = (char*) calloc(250, sizeof(char));
649         sprintf(errstr, "Expected covariance matrix is not invertible");
650         if(oo->currentState->computeCount <= 0) {
651             strncat(errstr, " at starting values", 20);
652         }
653         strncat(errstr, ".\n", 3);
654         omxRaiseError(errstr);                        // Raise Rf_error
655         free(errstr);
656         return;
657     }
658
659     // 4) ZS = Z S  (axa) *
660     omxDSYMM(FALSE, 1.0, S, Z, 0.0, ZS);
661     omxDGEMM(FALSE, FALSE, 1.0, Z, S, 0.0, ZS);
662     
663     // 5) B = F Z   (fxa) *
664     omxDGEMM(FALSE, FALSE, 1.0, F, Z, 0.0, B);
665     
666     // 6) C = I - eCov D, where D is the observed covariance matrix (fxf)
667     omxCopyMatrix(C, lilI);
668     omxDSYMM(TRUE, -1.0, eCov, D, 1.0, C);
669
670     
671     // 7) BC = B^T C (axf) *
672     omxDGEMM(TRUE, FALSE, 1.0, B, C, 0.0, BC);
673     
674     // 8) ZSBC = ZS BC (axf) *
675     omxDGEMM(FALSE, FALSE, 1.0, ZS, BC, 0.0, ZSBC);
676     
677     // 9) eCovB = cov^(-1) B (fxa) *
678     omxDSYMM(TRUE, 1.0, eCov, B, 0.0, eCovB);
679     
680     if(M != NULL) {
681         // 10) Means = BM (fx1) *
682         // This is calculated during expectation computation.
683         // 11) b = Means - d, where d is the observed means matrix (fx1)
684
685         omxCopyMatrix(b, Mns);
686         F77_CALL(daxpy)(&(means->cols), &minusoned, means->data, &onei, b->data, &onei);
687     
688         // 11) beCov = b cov^(-1) (1xf) *
689         omxDSYMV(1.0, eCov, b, 0.0, beCov);
690         
691         // 12) beCovB = b cov^(-1) B (1xa) *
692         omxDGEMV(TRUE, 1.0, eCovB, b, 0.0, bCB);
693         
694         // 13) ZM = Z %*% M (1xa)
695         omxDGEMV(FALSE, 1.0, Z, M, 0.0, ZM);
696     }
697     
698     // 14) For each location in locations:
699     //      1A) Calculate dA/dt, dS/dt, and dM/dt by substituting 1s into empty matrices
700     //      1B) 1 = 2 * ADB(eCovB, dAdt, ZSBC)
701     
702     eqnList1[0] = eCovB;
703     eqnList2[0] = ZSBC;
704     ADB(eqnList1, eqnList2, 1, dAdts, dAdtsCount, dAdtsRowCache, dAdtsColCache, 
705         Amat, varList, pNums, nParam, eqnOuts);
706     omxMatrixTrace(eqnOuts[0], nParam, paramVec);
707     
708     for(int i = 0; i < nParam; i++)
709         covInfluence[i] = 2 * omxVectorElement(paramVec, i);
710         
711     
712     //      1C) eqnList2 = ADB(eCovB, dSdt, BC)
713     eqnList1[0] = eCovB;
714     eqnList2[0] = BC;
715     ADB(eqnList1, eqnList2, 1, dSdts, dSdtsCount, dSdtsRowCache, dSdtsColCache, 
716         Smat, varList, pNums, nParam, eqnOuts);
717     omxMatrixTrace(eqnOuts[0], nParam, paramVec);
718
719     //      2) sum = eqnList1 + eqnList2
720     F77_CALL(daxpy)(&nParam, &oned, paramVec->data, &onei, covInfluence, &onei);
721
722     //      If Means:
723     if(M != NULL) {
724
725         //      Just populate dMdts
726         ADB(NULL, NULL, 0, dMdts, dMdtsCount, dMdtsRowCache, dMdtsColCache,
727             Mmat, varList, pNums, nParam, NULL);
728
729         //  This needs to be done one-per-param.
730         //  Parallelize here.
731         for(int pNum = 0; pNum < nParam; pNum++) {
732
733             switch(dAdtsCount[pNum]) {
734                 case 0:
735                     switch(dSdtsCount[pNum]) {
736                         case 0:
737                             memset(bigSum->data, 0, sizeof(double) * bigSum->rows);
738                             break;
739                         case 1:
740                         {
741                             int rowCache = dSdtsRowCache[pNum];
742                             int colCache = dSdtsColCache[pNum];
743                             memset(bigSum->data, 0, sizeof(double) * bigSum->rows);
744                             bigSum->data[colCache] = bCB->data[rowCache];                            
745                             break;
746                         }
747                         default:
748                             // 5) sumVec = beCovB dSdt (1xa)
749                             omxDGEMV(TRUE, 1.0, dSdts[pNum], bCB, 0.0, bigSum);
750                     }
751                     break;
752                 case 1:
753                 {
754                     int rowCache = dAdtsRowCache[pNum];
755                     int colCache = dAdtsColCache[pNum];
756                     double value = bCB->data[rowCache];
757
758                     int len = bCB->rows;
759                     int nrow = ZS->rows;
760                     int ncol = ZS->cols;
761
762                     if (ZS->colMajor) 
763                         for(int offset = 0; offset < len; offset++)
764                             bigSum->data[offset] = ZS->data[offset * nrow + colCache] * value;
765                     else
766                         for(int offset = 0; offset < len; offset++)
767                             bigSum->data[offset] = ZS->data[colCache * ncol + offset] * value;
768
769                     // Term with dSigma/dTheta
770                     //          4) sumVec += beCovB ZS dAdt (1xa)
771                     omxDGEMV(TRUE, 1.0, ZS, bCB, 0.0, tempVec);
772                     bigSum->data[colCache] += tempVec->data[rowCache];
773
774                     if (dSdtsCount[pNum]) {
775                         //  5) sumVec = beCovB dSdt (1xa)
776                         omxDGEMV(TRUE, 1.0, dSdts[pNum], bCB, 1.0, bigSum);
777                     }               
778                     break;
779                 }
780                 default:
781                     //          3) sumVec = beCovB dAdt ZS (1xa)
782                     omxDGEMV(TRUE, 1.0, dAdts[pNum], bCB, 0.0, tempVec);
783                     omxDGEMV(TRUE, 1.0, ZS, tempVec, 0.0, bigSum);
784                     // Term with dSigma/dTheta
785                     //          4) sumVec += beCovB ZS dAdt (1xa)
786                     omxDGEMV(TRUE, 1.0, ZS, bCB, 0.0, tempVec);
787                     omxDGEMV(TRUE, 1.0, dAdts[pNum], tempVec, 1.0, bigSum);
788                     if (dSdtsCount[pNum]) {
789                         //  5) sumVec = beCovB dSdt (1xa)
790                         omxDGEMV(TRUE, 1.0, dSdts[pNum], bCB, 1.0, bigSum);
791                     }
792             }
793  
794             //          6) sumVec *= B^T --> 1xf
795             omxDGEMV(FALSE, 1.0, B, bigSum, 0.0, lilSum);
796
797             // Factorizing dMudt
798             //          7) sumVec += 2 * B dAdt ZM (1xa)
799             //              Note: in the paper, expected manifest means are Bm
800             switch(dAdtsCount[pNum]) {
801                 case 0:
802                     break;
803                 case 1:
804                 {
805                     int rowCache = dAdtsRowCache[pNum];
806                     int colCache = dAdtsColCache[pNum];
807                     double value = ZM->data[colCache];
808
809                     int len = B->rows;
810                     int nrow = B->rows;
811                     int ncol = B->cols;
812
813                     if (B->colMajor) 
814                         for(int offset = 0; offset < len; offset++)
815                             lilSum->data[offset] += 2.0 * B->data[rowCache * nrow + offset] * value;
816                     else
817                         for(int offset = 0; offset < len; offset++)
818                             lilSum->data[offset] += 2.0 * B->data[offset * ncol + rowCache] * value;
819
820                     break;
821                 }
822                 default:
823                     omxDGEMV(FALSE, 1.0, dAdts[pNum], ZM, 0.0, tempVec);
824                     omxDGEMV(FALSE, 2.0, B, tempVec, 1.0, lilSum); // :::DEBUG:::<-- Save B %*% tempVec
825             }
826
827             //          8) sumVec += 2 * B dMdt (1xf)
828             switch(dMdtsCount[pNum]) {
829                 case 0:
830                     break;
831                 case 1: 
832                 {
833                     int moffset, boffset, nrow;
834
835                     moffset = dMdtsColCache[pNum];
836                     nrow = B->rows;
837                     boffset = moffset * nrow;
838
839                     for(int row = 0; row < nrow; row++)
840                         lilSum->data[row] += 2.0 * B->data[boffset + row];
841
842                     break;
843                 }
844                 default:
845                     omxDGEMV(FALSE, 2.0, B, dMdts[pNum], 1.0, lilSum); // :::DEBUG::: <-- Save B %*% dM
846             }
847
848             //          9) sum = sumVec beCov (1x1)
849             int len = lilSum->rows * lilSum->cols;
850             meanInfluence[pNum] = F77_CALL(ddot)(&len, lilSum->data, &onei, beCov->data, &onei);
851         }
852             
853     } else { 
854         for(int i = 0; i < nParam; i++)
855             meanInfluence[i] = 0;
856     }
857     
858     for(int pNum = 0; pNum < nParam; pNum++) {
859         int nextP = pNums[pNum]; // Original varList parameter number
860         result[pNum] = (covInfluence[pNum] * (n-1)) - (meanInfluence[pNum] * n);
861         if(OMX_DEBUG) {
862             mxLog("Revised calculation for Gradient value %d: Cov: %3.9f, Mean: %3.9f, total: %3.9f",
863                 nextP, covInfluence[pNum], meanInfluence[pNum], result[pNum]);
864         }
865     }
866     
867     // Phew.
868
869 }
870
871
872 static void calculateRAMGradientComponents(omxExpectation* oo, omxMatrix** dSigmas, omxMatrix** dMus, int* status) {
873
874     // Steps:
875     // 1) (Re) Calculate current A, S, F, and Z (where Z = (I-A)^-1)
876     // 2) E = Z S Z^T
877     // 3) B = F Z
878     // 4) For each location in locations:
879     //      1) Calculate dA/dt, dS/dt, and dM/dt by substituting 1s into empty matrices
880     //      2) C = B dAdt E F^T
881     //      3) dSigma/dT = C + C^T + B dS/dT B^T
882     //      4) dM/dT = B dA/dT Z b + B dM/dT
883
884     omxRAMExpectation* oro = (omxRAMExpectation*)(oo->argStruct);
885                                 // Size reference: A is axa, F is fxf
886     omxMatrix* A = oro->A;      // axa
887     omxMatrix* S = oro->S;      // axa
888     omxMatrix* F = oro->F;      // fxf
889     omxMatrix* Z = oro->Z;      // axa
890     // omxMatrix* I = oro->I;      // axa
891     omxMatrix* M = oro->M;      // 1xf
892     omxMatrix* B = oro->X;      // fxa
893     omxMatrix* U = oro->U;      // fxa
894     omxMatrix* EF = oro->EF;    // fxa
895     omxMatrix* ZM = oro->ZM;    // 1xf
896     omxMatrix* V = oro->V;      // fxa
897     omxMatrix* W = oro->W;      // axa
898         omxMatrix* E = oro->Ax;     // axa
899     
900     omxMatrix* dA = oro->dA;
901     omxMatrix* dS = oro->dS;
902     omxMatrix* dM = oro->dM;
903     
904     // int numIters = oro->numIters;
905     int Amat = A->matrixNumber;
906     int Smat = S->matrixNumber;
907     int Mmat = 0;
908     if(M != NULL) Mmat = M->matrixNumber;
909     
910     std::vector< omxFreeVar* > &varList = oo->freeVarGroup->vars;
911     size_t nLocs = varList.size();
912
913     // Assumed.
914     // if(omxNeedsUpdate(Z)) {
915     //     // (Re)Calculate current A, S, F, and Z
916     //     omxCalculateRAMCovarianceAndMeans(A, S, F, M, cov, means, numIters, I, Z, oro->Y, B, Ax);
917     // }
918     
919     // TODO: Handle repeated-call cases.
920     // E = Z S (Z^T)
921     omxDGEMM(FALSE, FALSE, 1.0, Z, S, 0.0, W);
922     omxDGEMM(FALSE, TRUE, 1.0, W, Z, 0.0, E);
923     
924     //EF = E F^T
925     omxDGEMM(FALSE, TRUE, 1.0, E, F, 0.0, EF);
926
927     // B = F Z
928     omxDGEMM(FALSE, FALSE, 1.0, F, Z, 0.0, B);
929     
930     // ZM = ZM
931     if(M != NULL)
932         omxDGEMV(FALSE, 1.0, Z, M, 0.0, ZM);
933     
934     // Step through free params 
935     // Note: This should be parallelized at the next level up.
936     //  This function itself should serially perform one computation for each location
937     //  given in the sequence.  Always write to the appropriate location so that writes
938     //  can be shared across thread-level parallelism.
939     
940     for(size_t param = 0; param < nLocs; param++) {
941         //  Repeated from above.  For each parameter:
942         //      1) Calculate dA/dt, dS/dt, and dM/dt by substituting 1s into empty matrices
943         
944         omxFreeVar *var = varList[param];
945         
946         // Zero dA, dS, and dM.  // TODO: speed
947         for( int k = 0; k < dA->cols; k++) {
948             for( int j = 0; j < dA->rows; j++) {
949                 omxSetMatrixElement(dA, j, k, 0.0);
950                 omxSetMatrixElement(dS, j, k, 0.0);
951             }
952             if(M != NULL) omxSetMatrixElement(dM, 0, k, 0.0);
953         }
954         status[param] = 0;
955
956         // Create dA, dS, and dM mats for each Free Parameter
957         for(size_t varLoc = 0; varLoc < var->locations.size(); varLoc++) {
958             int varMat = ~var->locations[varLoc].matrix; // Matrices are numbered from ~0 to ~N
959             int row = var->locations[varLoc].row;
960             int col = var->locations[varLoc].col;
961             if(varMat == Amat) {
962                 omxSetMatrixElement(dA, row, col, 1);
963             } else if(varMat == Smat) {
964                 omxSetMatrixElement(dS, row, col, 1);
965             } else if(varMat == Mmat && M != NULL) {
966                 omxSetMatrixElement(dM, row, col, 1);
967             } else {
968                 // This parameter has some outside effect
969                 // We cannot directly estimate its effects on the likelihood
970                 // For now, skip.
971                 // TODO: Find a way to deal with these situations
972                 status[param] = -1;
973                 if(OMX_DEBUG) { mxLog("Skipping parameter %d because %dth element has outside influence %d. Looking for %d, %d, or %d.", (int) param, (int) varLoc, varMat, Amat, Smat, Mmat);}
974                 break;
975             }
976         }
977         
978         if(status[param] < 0) {
979             continue;  // Skip this one.
980         }
981         
982         omxMatrix* C = dSigmas[param];
983         omxMatrix* dMu = dMus[param];
984         
985         //      2) C = B dAdt EF
986         omxDGEMM(FALSE, FALSE, 1.0, B, dA, 0.0, U);
987         omxDGEMM(FALSE, FALSE, 1.0, U, EF, 0.0, C);
988         
989         // Note: U is saved here for calculation of M, below.
990         //      3) dSigma/dT = C + C^T + B dS/dT B^T
991         omxAddOwnTranspose(&C, 0, C);
992         omxDGEMM(FALSE, FALSE, 1.0, B, dS, 0.0, V);
993         omxDGEMM(FALSE, TRUE,  1.0, V, B, 1.0, C);
994         
995         if(M != NULL) {
996             //      4) dM/dT = B dA/dT Z M + B dM/dT
997             omxDGEMV(FALSE, 1.0, U, ZM, 0.0, dMu);
998             omxDGEMV(FALSE, 1.0, B, dM, 1.0, dMu);
999
1000         }
1001
1002     }
1003
1004 }
1005
1006 static omxMatrix* omxGetRAMExpectationComponent(omxExpectation* ox, omxFitFunction* off, const char* component) {
1007         
1008         if(OMX_DEBUG) { mxLog("RAM expectation: %s requested--", component); }
1009
1010         omxRAMExpectation* ore = (omxRAMExpectation*)(ox->argStruct);
1011         omxMatrix* retval = NULL;
1012
1013         if(!strncmp("cov", component, 3)) {
1014                 retval = ore->cov;
1015         } else if(!strncmp("mean", component, 4)) {
1016                 retval = ore->means;
1017         } else if(!strncmp("pvec", component, 4)) {
1018                 // Once implemented, change compute function and return pvec
1019         }
1020         
1021         return retval;
1022 }