Fix/remove improper printf style formats
[openmx:openmx.git] / src / omxFIMLSingleIteration.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
18 #include <R.h>
19 #include <Rinternals.h>
20 #include <Rdefines.h>
21 #include <R_ext/Rdynload.h>
22 #include <R_ext/BLAS.h>
23 #include <R_ext/Lapack.h>
24 #include "omxDefines.h"
25 #include "omxAlgebraFunctions.h"
26 #include "omxSymbolTable.h"
27 #include "omxData.h"
28 #include "omxFIMLFitFunction.h"
29 #include "omxSadmvnWrapper.h"
30
31
32 void omxFIMLAdvanceRow(int *keepCov, int *keepInverse, int *row,
33         omxData *data, int numIdentical) {
34         int rowVal = *row;
35         if(*keepCov <= 0) *keepCov = omxDataNumIdenticalDefs(data, rowVal);
36         if(*keepInverse  <= 0) *keepInverse = omxDataNumIdenticalMissingness(data, rowVal);
37         // mxLog("Incrementing Row."); //:::DEBUG:::
38         *row += numIdentical;
39         *keepCov -= numIdentical;
40         *keepInverse -= numIdentical;
41 }
42
43 void omxFIMLAdvanceJointRow(int *row, int *numIdenticalDefs, 
44         int *numIdenticalContinuousMissingness,
45         int *numIdenticalOrdinalMissingness, 
46         int *numIdenticalContinuousRows,
47         int *numIdenticalOrdinalRows,
48         omxData *data, int numDefs, int numIdentical) {
49
50         int rowVal = *row;
51
52     if(numDefs != 0 && *numIdenticalDefs <= 0) *numIdenticalDefs = 
53                 omxDataNumIdenticalDefs(data, rowVal);
54         if(*numIdenticalContinuousMissingness <= 0) *numIdenticalContinuousMissingness =
55                 omxDataNumIdenticalContinuousMissingness(data, rowVal);
56         if(*numIdenticalOrdinalMissingness <= 0) *numIdenticalOrdinalMissingness = 
57                 omxDataNumIdenticalOrdinalMissingness(data, rowVal);
58         if(*numIdenticalContinuousRows <= 0) *numIdenticalContinuousRows = 
59                 omxDataNumIdenticalContinuousRows(data, rowVal);
60         if(*numIdenticalOrdinalRows <= 0) *numIdenticalOrdinalRows = 
61                 omxDataNumIdenticalOrdinalRows(data, rowVal);
62
63         *row += numIdentical;
64         *numIdenticalDefs -= numIdentical;
65         *numIdenticalContinuousMissingness -= numIdentical;
66         *numIdenticalContinuousRows -= numIdentical;
67         *numIdenticalOrdinalMissingness -= numIdentical;
68         *numIdenticalOrdinalRows -= numIdentical;
69 }
70
71
72 /**
73  * The localobj reference is used to access read-only variables,
74  * or variables that can be modified but whose state cannot be
75  * accessed from other threads.
76  *
77  * The sharedobj reference is used to access write-only variables,
78  * where the memory writes of any two threads are non-overlapping.
79  * No synchronization mechanisms are employed to maintain consistency
80  * of sharedobj references.
81  *
82  *
83  * Because (1) these functions may be invoked with arbitrary 
84  * rowbegin and rowcount values, and (2) the log-likelihood
85  * values for all data rows must be calculated (even in cases
86  * of errors), this function is forbidden from return()-ing early.
87  *
88  * As another consequence of (1) and (2), if "rowbegin" is in
89  * the middle of a sequence of identical rows, then defer
90  * move "rowbegin" to after the sequence of identical rows.
91  * Grep for "[[Comment 4]]" in source code.
92  */
93 void omxFIMLSingleIterationJoint(omxFitFunction *localobj, omxFitFunction *sharedobj, int rowbegin, int rowcount) {
94
95     omxFIMLFitFunction* ofo = ((omxFIMLFitFunction*) localobj->argStruct);
96     omxFIMLFitFunction* shared_ofo = ((omxFIMLFitFunction*) sharedobj->argStruct);
97
98         double Q = 0.0;
99         int numDefs;
100         int numOrdRemoves = 0, numContRemoves=0;
101         int returnRowLikelihoods = 0;
102     int numIdenticalDefs = 0, numIdenticalOrdinalMissingness = 0, numIdenticalOrdinalRows = 0, numOrdinal = 1,
103                             numIdenticalContinuousMissingness = 0, numIdenticalContinuousRows = 0, numContinuous = 1;
104
105         omxMatrix *cov, *means, *smallRow, *smallCov, *smallMeans, *RCX, *dataColumns;
106         omxMatrix *rowLikelihoods, *rowLogLikelihoods;
107     omxMatrix *ordMeans, *ordCov, *ordRow, *contRow;
108     omxMatrix *halfCov, *reduceCov, *ordContCov;
109         omxThresholdColumn *thresholdCols;
110         omxData* data;
111         double *lThresh, *uThresh, *corList, *weights, *oldDefs;
112         int *Infin;
113         omxDefinitionVar* defVars;
114         
115         omxExpectation* expectation;
116         
117
118         // Locals, for readability.  Compiler should cut through this.
119         cov             = ofo->cov;
120         means           = ofo->means;
121         smallRow        = ofo->smallRow;
122         smallCov        = ofo->smallCov;
123         smallMeans      = ofo->smallMeans;
124     ordMeans    = ofo->ordMeans;
125     ordCov      = ofo->ordCov;
126     ordRow      = ofo->ordRow;
127     contRow     = ofo->contRow;
128     halfCov     = ofo->halfCov;
129     reduceCov   = ofo->reduceCov;
130     ordContCov  = ofo->ordContCov;
131         RCX             = ofo->RCX;
132
133         data            = ofo->data;
134         dataColumns     = ofo->dataColumns;
135         defVars         = ofo->defVars;
136         oldDefs         = ofo->oldDefs;
137         numDefs         = ofo->numDefs;
138
139         corList         = ofo->corList;
140         weights         = ofo->weights;
141         lThresh         = ofo->lThresh;
142         uThresh         = ofo->uThresh;
143         thresholdCols = ofo->thresholdCols;
144         returnRowLikelihoods = ofo->returnRowLikelihoods;
145         rowLikelihoods = shared_ofo->rowLikelihoods;            // write-only
146         rowLogLikelihoods = shared_ofo->rowLogLikelihoods;  // write-only
147
148         Infin                   = ofo->Infin;
149         expectation     = localobj->expectation;
150
151     int ordRemove[cov->cols], contRemove[cov->cols];
152     char u = 'U', l = 'L';
153     int info;
154     double determinant = 0.0;
155     double oned = 1.0, zerod = 0.0, minusoned = -1.0;
156     int onei = 1;
157     double likelihood;
158         int inform;
159
160         int firstRow = 1;
161     int row = rowbegin;
162
163     resetDefinitionVariables(oldDefs, numDefs);
164
165         // [[Comment 4]] moving row starting position
166         if (row > 0) {
167                 int prevIdentical = omxDataNumIdenticalRows(data, row - 1);
168                 row += (prevIdentical - 1);
169         }
170
171         while(row < data->rows && (row - rowbegin) < rowcount) {
172         localobj->matrix->currentState->currentRow = row;               // Set to a new row.
173         int numIdentical = omxDataNumIdenticalRows(data, row);
174         if(numIdentical == 0) numIdentical = 1; 
175         // N.B.: numIdentical == 0 means an error occurred and was not properly handled;
176         // it should never be the case.
177         
178         omxDataRow(data, row, dataColumns, smallRow);                               // Populate data row
179         
180         if(OMX_DEBUG_ROWS(row)) {
181             mxLog("Identicality check. Is %sfirst. Total: %d rows identical, %d defs, %d missingness: Continuous: %d rows, %d missingness; Ordinal: %d rows, %d missingness.", 
182                     (firstRow?"":"not "), numIdentical, numIdenticalDefs, omxDataNumIdenticalRows(data, row), 
183                     numIdenticalContinuousRows, numIdenticalContinuousMissingness, 
184                     numIdenticalOrdinalRows, numIdenticalOrdinalMissingness);
185         }
186
187         if(numIdenticalDefs <= 0 || numIdenticalContinuousMissingness <= 0 || numIdenticalOrdinalMissingness <= 0 || firstRow ) {  // If we're keeping covariance from the previous row, do not populate 
188             // Handle Definition Variables.
189             if((numDefs != 0 && numIdenticalDefs > 0) || firstRow) {
190                                 int numVarsFilled = 0;
191                                 if(OMX_DEBUG_ROWS(row)) { mxLog("Handling Definition Vars."); }
192                                 numVarsFilled = handleDefinitionVarList(data, localobj->matrix->currentState, row, defVars, oldDefs, numDefs);
193                                 if (numVarsFilled < 0) {
194                                         for(int nid = 0; nid < numIdentical; nid++) {
195                                                 if(returnRowLikelihoods) omxSetMatrixElement(sharedobj->matrix, omxDataIndex(data, row+nid), 0, 0.0);
196                                                 omxSetMatrixElement(rowLikelihoods, omxDataIndex(data, row+nid), 0, 0.0);
197                                         }
198                                         omxFIMLAdvanceJointRow(&row, &numIdenticalDefs, 
199                                                 &numIdenticalContinuousMissingness,
200                                                 &numIdenticalOrdinalMissingness, 
201                                                 &numIdenticalContinuousRows,
202                                                 &numIdenticalOrdinalRows,
203                                                 data, numDefs, numIdentical);
204                                         continue;
205                                 } else if (numVarsFilled || firstRow) { 
206                                         // Use firstrow instead of rows == 0 for the case where the first row is all NAs
207                                         // N.B. handling of definition var lists always happens, regardless of firstRow.
208                                         // Recalculate means and covariances.
209                                         omxExpectationCompute(expectation, NULL);
210                                 }
211                         }
212             // Filter down correlation matrix and calculate thresholds.
213             // TODO: If identical ordinal or continuous missingness, ignore only the appropriate columns.
214             numOrdRemoves = 0;
215             numContRemoves = 0;
216                         for(int j = 0; j < dataColumns->cols; j++) {
217                         int var = omxVectorElement(dataColumns, j);
218                         int value = omxIntDataElement(data, row, var);// Indexing correction means this is the index of the upper bound +1.
219                         // TODO: Might save time by preseparating ordinal from continuous.
220                         if(isnan(value) || value == NA_INTEGER) {  // Value is NA, therefore filter.
221                                 numOrdRemoves++;
222                     numContRemoves++;
223                     ordRemove[j] = 1;
224                     contRemove[j] = 1;
225                                 Infin[j] = -1;
226                     if(OMX_DEBUG_ROWS(row)) { 
227                                     mxLog("Row %d, column %d, value %d.  NA.", row, j, value);
228                     }
229                                 continue;
230                         } else if(omxDataColumnIsFactor(data, var)) {             // Ordinal column.
231                     numContRemoves++;
232                     ordRemove[j] = 0;
233                     contRemove[j] = 1;
234                     if(OMX_DEBUG_ROWS(row)) { 
235                                 mxLog("Row %d, column %d, value %d.  Ordinal.", row, j, value);
236                     }
237                         } else {
238                             numOrdRemoves++;
239                     ordRemove[j] = 1;
240                     contRemove[j] = 0;
241                     if(OMX_DEBUG_ROWS(row)) { 
242                                 mxLog("Row %d, column %d, value %d.  Continuous.", row, j, value);
243                     }
244                         }
245                 }
246                 
247             if(OMX_DEBUG_ROWS(row)) {
248                 mxLog("\n\nRemovals: %d ordinal, %d continuous out of %d total.", numOrdRemoves, numContRemoves, dataColumns->cols);
249             }
250                 
251                         for(int j=0; j < dataColumns->cols; j++) {
252                                 int var = omxVectorElement(dataColumns, j);
253                                 if(omxDataColumnIsFactor(data, j) && thresholdCols[var].numThresholds > 0) { // j is an ordinal column
254                                         omxRecompute(thresholdCols[var].matrix); // Only one of these--save time by only doing this once
255                                         checkIncreasing(thresholdCols[var].matrix, thresholdCols[var].column);
256                                 }
257                         }
258             numContinuous = dataColumns->cols - numContRemoves;
259             numOrdinal = dataColumns->cols - numOrdRemoves;
260
261                 }
262
263         // TODO: Possible solution here: Manually record threshold column and index from data 
264         //   during this initial reduction step.  Since all the rest is algebras, it'll filter 
265         //   naturally.  Calculate offsets from continuous data, then dereference actual 
266         //   threshold values from the threshold matrix in its original state.  
267         //   Alternately, rearrange the thresholds matrix (and maybe data matrix) to split
268         //    ordinal and continuous variables.
269         //   Requirement: colNum integer vector
270
271                 if(numContinuous <= 0 && numOrdinal <= 0) {
272                     // All elements missing.  Skip row.
273                         for(int nid = 0; nid < numIdentical; nid++) {   
274                                 if(returnRowLikelihoods) {
275                                         omxSetMatrixElement(sharedobj->matrix, omxDataIndex(data, row+nid), 0, 1.0);
276                                 }
277                                 omxSetMatrixElement(rowLikelihoods, omxDataIndex(data, row+nid), 0, 1.0);
278                         }
279                         omxFIMLAdvanceJointRow(&row, &numIdenticalDefs, 
280                                 &numIdenticalContinuousMissingness,
281                                 &numIdenticalOrdinalMissingness, 
282                                 &numIdenticalContinuousRows,
283                                 &numIdenticalOrdinalRows,
284                                 data, numDefs, numIdentical);
285             continue;
286                 }
287
288                 //  smallCov <- cov[!contRemove, !contRemove] : covariance of continuous elements
289                 //  smallMeans <- means[ALL, !contRemove] : continuous means
290                 //  smallRow <- data[ALL, !contRemove]  : continuous data
291                 //              ordCov <- cov[!ordRemove, !ordRemove]
292                 //              ordMeans <- means[NULL, !ordRemove]
293                 //              ordData <- data[NULL, !ordRemove]
294                 //              ordContCov <- cov[!contRemove, !ordRemove]
295
296         // TODO: Data handling is confusing.  Maybe set two self-aliased row-reduction "datacolumns" elements?
297         
298         // SEPARATION: 
299         // Catch here: If continuous columns are all missing, skip everything except the ordCov calculations
300         //              in this case, log likelihood of the continuous is 1 (likelihood is 0)
301         // Do not recompute ordcov if missingness is identical and no def vars
302
303         // SEPARATION: 
304         //  Unprojected covariances only need to reset and re-filter if there are def vars or the appropriate missingness pattern changes
305         //  Also, if each one is not all-missing.
306
307                 if(numContinuous <= 0) {
308                     // All continuous missingness.  Populate some stuff.
309             Q = 0.0;
310             determinant = 0.0;
311             if(numIdenticalDefs <= 0 || numIdenticalOrdinalRows <= 0 || firstRow) {
312                 // Recalculate Ordinal covariance matrix
313                 omxResetAliasedMatrix(ordCov);                          // Re-sample covariance and means matrices for ordinal
314                 omxRemoveRowsAndColumns(ordCov, numOrdRemoves, numOrdRemoves, ordRemove, ordRemove);
315                 
316                 // Recalculate ordinal fs
317                 omxResetAliasedMatrix(ordMeans);
318                 omxRemoveElements(ordMeans, numOrdRemoves, ordRemove);      // Reduce the row to just ordinal.
319                 
320                 // These values pass through directly without modification by continuous variables
321                 
322                 // Calculate correlation matrix, correlation list, and weights from covariance
323                     omxStandardizeCovMatrix(ordCov, corList, weights);
324             }
325         } else if(numIdenticalDefs <= 0 || numIdenticalContinuousRows <= 0 || firstRow) {
326
327             /* Reset and Resample rows if necessary. */
328             // First Cov and Means (if they've changed)
329             if(numIdenticalDefs <= 0 || numIdenticalContinuousMissingness <= 0 || firstRow) {
330                 omxResetAliasedMatrix(smallMeans);
331                 omxRemoveElements(smallMeans, numContRemoves, contRemove);
332                 omxResetAliasedMatrix(smallCov);                                // Re-sample covariance matrix
333                 omxRemoveRowsAndColumns(smallCov, numContRemoves, numContRemoves, contRemove, contRemove);
334
335                 /* Calculate derminant and inverse of Censored continuousCov matrix */
336                 if(OMX_DEBUG_ROWS(row)) { 
337                     omxPrint(smallCov, "Cont Cov to Invert"); 
338                 }
339                 
340                             F77_CALL(dpotrf)(&u, &(smallCov->rows), smallCov->data, &(smallCov->cols), &info);
341
342                 if(info != 0) {
343                     if(!returnRowLikelihoods) {
344                         for(int nid = 0; nid < numIdentical; nid++) {
345                             omxSetMatrixElement(rowLikelihoods, omxDataIndex(data, row+nid), 0, 0.0);
346                         }
347                         char helperstr[200];
348                         char *errstr = (char*) calloc(250, sizeof(char));
349                         sprintf(helperstr, "Expected covariance matrix for continuous variables is not positive-definite in data row %d", 
350                             omxDataIndex(data, row));
351                         if(localobj->matrix->currentState->computeCount <= 0) {
352                             sprintf(errstr, "%s at starting values.\n", helperstr);
353                         } else {
354                             sprintf(errstr, "%s at major iteration %d.\n", helperstr, localobj->matrix->currentState->majorIteration);
355                         }
356                         omxRaiseError(localobj->matrix->currentState, -1, errstr);
357                         free(errstr);
358                     } 
359                     for(int nid = 0; nid < numIdentical; nid++) {
360                         if (returnRowLikelihoods)
361                                                 omxSetMatrixElement(sharedobj->matrix, omxDataIndex(data, row+nid), 0, 0.0);
362                         omxSetMatrixElement(rowLikelihoods, omxDataIndex(data, row+nid), 0, 0.0);
363                     }
364                     if(OMX_DEBUG) {mxLog("Non-positive-definite covariance matrix in row likelihood.  Skipping Row.");}
365                                         omxFIMLAdvanceJointRow(&row, &numIdenticalDefs, 
366                                                 &numIdenticalContinuousMissingness,
367                                                 &numIdenticalOrdinalMissingness, 
368                                                 &numIdenticalContinuousRows,
369                                                 &numIdenticalOrdinalRows,
370                                                 data, numDefs, numIdentical);
371                     continue;
372                 }
373                 // Calculate determinant: squared product of the diagonal of the decomposition
374                         // For speed, use sum of logs rather than log of product.
375                         
376                 determinant = 0.0;
377                         for(int diag = 0; diag < (smallCov->rows); diag++) {
378                                 determinant += log(fabs(omxMatrixElement(smallCov, diag, diag)));
379                         }
380                 // determinant = determinant * determinant;  // Delayed.
381                         F77_CALL(dpotri)(&u, &(smallCov->rows), smallCov->data, &(smallCov->cols), &info);
382                         if(info != 0) {
383                                 if(!returnRowLikelihoods) {
384                                         char *errstr = (char*) calloc(250, sizeof(char));
385                                         sprintf(errstr, "Cannot invert expected continuous covariance matrix. Error %d.", info);
386                                         omxRaiseError(localobj->matrix->currentState, -1, errstr);
387                                         free(errstr);
388                     }
389                                         for(int nid = 0; nid < numIdentical; nid++) {
390                         if (returnRowLikelihoods)
391                                                     omxSetMatrixElement(sharedobj->matrix, omxDataIndex(data, row+nid), 0, 0.0);
392                                                 omxSetMatrixElement(rowLikelihoods, omxDataIndex(data, row+nid), 0, 0.0);
393                                         }
394                                         omxFIMLAdvanceJointRow(&row, &numIdenticalDefs, 
395                                                 &numIdenticalContinuousMissingness,
396                                                 &numIdenticalOrdinalMissingness, 
397                                                 &numIdenticalContinuousRows,
398                                                 &numIdenticalOrdinalRows,
399                                                 data, numDefs, numIdentical);
400                     continue;
401                         }
402             }
403
404             // Reset continuous data row (always needed)
405             omxResetAliasedMatrix(contRow);                                            // Reset smallRow
406             omxRemoveElements(contRow, numContRemoves, contRemove);     // Reduce the row to just continuous.
407             F77_CALL(daxpy)(&(contRow->cols), &minusoned, smallMeans->data, &onei, contRow->data, &onei);
408
409                     /* Calculate Row Likelihood */
410                     /* Mathematically: (2*pi)^cols * 1/sqrt(determinant(ExpectedCov)) * (dataRow %*% (solve(ExpectedCov)) %*% t(dataRow))^(1/2) */
411                 F77_CALL(dsymv)(&u, &(smallCov->rows), &oned, smallCov->data, &(smallCov->cols), contRow->data, &onei, &zerod, RCX->data, &onei);       // RCX is the continuous-column mahalanobis distance.
412                 Q = F77_CALL(ddot)(&(contRow->cols), contRow->data, &onei, RCX->data, &onei); //Q is the total mahalanobis distance
413                 
414                 if(numOrdinal > 0) {
415
416                 // Precalculate Ordinal things that change with continuous changes
417                 // Reserve: 1) Inverse continuous covariance (smallCov)
418                 //          2) Columnwise Mahalanobis distance (contCov^-1)%*%(Data - Means) (RCX)
419                 //          3) Overall Mahalanobis distance (FIML likelihood of data) (Q)
420                 //Calculate:4) Cont/ord covariance %*% Mahalanobis distance  (halfCov)
421                 //          5) ordCov <- ordCov - Cont/ord covariance %*% Inverse continuous cov
422
423                 if(numIdenticalContinuousMissingness <= 0 || firstRow) {
424                     // Re-sample covariance between ordinal and continuous only if the continuous missingness changes.
425                     omxResetAliasedMatrix(ordContCov);
426                     omxRemoveRowsAndColumns(ordContCov, numContRemoves, numOrdRemoves, contRemove, ordRemove);
427
428                     // TODO: Make this less of a hack.
429                     halfCov->rows = smallCov->rows;
430                     halfCov->cols = ordContCov->cols;
431                     omxMatrixLeadingLagging(halfCov);
432                     reduceCov->rows = ordContCov->cols;
433                     reduceCov->cols = ordContCov->cols;
434                     omxMatrixLeadingLagging(reduceCov);
435
436                     F77_CALL(dsymm)(&l, &u, &(smallCov->rows), &(ordContCov->cols), &oned, smallCov->data, &(smallCov->leading), ordContCov->data, &(ordContCov->leading), &zerod, halfCov->data, &(halfCov->leading));          // halfCov is inverse continuous %*% cont/ord covariance
437                     F77_CALL(dgemm)((ordContCov->minority), (halfCov->majority), &(ordContCov->cols), &(halfCov->cols), &(ordContCov->rows), &oned, ordContCov->data, &(ordContCov->leading), halfCov->data, &(halfCov->leading), &zerod, reduceCov->data, &(reduceCov->leading));      // reduceCov is cont/ord^T %*% (contCov^-1 %*% cont/ord)
438                 }
439
440                 if(numIdenticalOrdinalMissingness <= 0 || firstRow) {
441                     // Means, projected covariance, and Columnwise mahalanobis distance must be recalculated
442                     //   unless there are no ordinal variables or the continuous variables are identical
443                     
444                     // Recalculate Ordinal and Ordinal/Continuous covariance matrices.
445                     if(OMX_DEBUG_ROWS(row)) {
446                         mxLog("Resetting Ordinal Covariance Matrix.");
447                         omxPrint(ordCov, "Was:");
448                     }
449
450                     omxResetAliasedMatrix(ordCov);                              // Re-sample covariance and means matrices for ordinal
451                     if(OMX_DEBUG_ROWS(row)) {
452                         mxLog("Resetting/Filtering Ordinal Covariance Matrix.");
453                         omxPrint(ordCov, "Reset to:");
454                     }
455
456                     omxRemoveRowsAndColumns(ordCov, numOrdRemoves, numOrdRemoves, ordRemove, ordRemove);
457                     if(OMX_DEBUG_ROWS(row)) {
458                         mxLog("Resetting/Filtering Ordinal Covariance Matrix.");
459                         omxPrint(ordCov, "Filtered to:");
460                     }
461                 
462                     // FIXME: This assumes that ordCov and reducCov have the same row/column majority.
463                     int vlen = reduceCov->rows * reduceCov->cols;
464                     F77_CALL(daxpy)(&vlen, &minusoned, reduceCov->data, &onei, ordCov->data, &onei); // ordCov <- (ordCov - reduceCov) %*% cont/ord
465             
466                 }
467                 
468                 // Projected means must be recalculated if the continuous variables change at all.
469                 omxResetAliasedMatrix(ordMeans);
470                 omxRemoveElements(ordMeans, numOrdRemoves, ordRemove);      // Reduce the row to just ordinal.
471                 F77_CALL(dgemv)((smallCov->minority), &(halfCov->rows), &(halfCov->cols), &oned, halfCov->data, &(halfCov->leading), contRow->data, &onei, &oned, ordMeans->data, &onei);                      // ordMeans += halfCov %*% contRow
472             }
473             
474         } // End of continuous likelihood values calculation
475         
476         if(numOrdinal <= 0) {       // No Ordinal Vars at all.
477             likelihood = 1;
478         } else {  
479             // There are ordinal vars, and not everything is identical, so we're recalculating
480             // Calculate correlation matrix, correlation list, and weights from covariance
481             if(numIdenticalDefs <=0 || numIdenticalContinuousMissingness <= 0 || numIdenticalOrdinalMissingness <= 0 || firstRow) {
482                 // if(OMX_DEBUG_ROWS(row)) {omxPrint(ordCov, "Ordinal cov matrix for standardization."); } //:::DEBUG:::
483                 omxStandardizeCovMatrix(ordCov, corList, weights);
484             }
485             
486             omxResetAliasedMatrix(ordRow);                                              // Propagate to ordinal row
487             omxRemoveElements(ordRow, numOrdRemoves, ordRemove);            // Reduce the row to just ordinal.
488
489             // omxPrint(ordMeans, "Ordinal Projected Means"); //:::DEBUG:::
490             // omxPrint(ordRow, "Filtered Ordinal Row"); //:::DEBUG:::
491
492
493             // Inspect elements, reweight, and set to 
494             int count = 0;
495                 for(int j = 0; j < dataColumns->cols; j++) {
496                 if(ordRemove[j]) continue;         // NA or non-ordinal
497                 int var = omxVectorElement(dataColumns, j);
498                         int value = omxIntDataElement(data, row, var); //  TODO: Compare with extraction from dataRow.
499                 // mxLog("Row %d, Column %d, value %d+1\n", row, j, value); // :::DEBUG:::
500                 value--;                // Correct for C indexing: value is now the index of the upper bound.
501                 // mxLog("Row %d, Column %d, value %d+1\n", row, j, value); // :::DEBUG:::
502                         double offset;
503                         if(means == NULL) offset = 0;
504                         else offset = omxVectorElement(ordMeans, count);
505                         double weight = weights[count];
506                         if(value == 0) {                                                                        // Lowest threshold = -Inf
507                                 lThresh[count] = (omxMatrixElement(thresholdCols[j].matrix, 0, thresholdCols[j].column) - offset) / weight;
508                                 uThresh[count] = lThresh[count];
509                                 Infin[count] = 0;
510                         } else {
511                                 lThresh[count] = (omxMatrixElement(thresholdCols[j].matrix, value-1, thresholdCols[j].column) - offset) / weight;
512                                 if(thresholdCols[j].numThresholds > value) {    // Highest threshold = Inf
513                                         double tmp = (omxMatrixElement(thresholdCols[j].matrix, value, thresholdCols[j].column) - offset) / weight;
514                                         uThresh[count] = tmp;
515                                         Infin[count] = 2;
516                                 } else {
517                                         uThresh[count] = NA_INTEGER; // NA is a special to indicate +Inf
518                                         Infin[count] = 1;
519                                 }
520                         }
521
522                         if(uThresh[count] == NA_INTEGER || isnan(uThresh[count])) { // for matrix-style specification.
523                                 uThresh[count] = lThresh[count];
524                                 Infin[count] = 1;
525                         }
526                         if(OMX_DEBUG) { 
527                             mxLog("Row %d, column %d.  Thresholds for data column %d and threshold column %d are %f -> %f. (Infin=%d).  Offset is %f and weight is %f",
528                                     row, count, j, value, lThresh[count], uThresh[count], Infin[count], offset, weight);
529                             mxLog("       Thresholds were %f -> %f, scaled by weight %f and shifted by mean %f and total offset %f.",
530                                     omxMatrixElement(thresholdCols[j].matrix, (Infin[count]==0?0:value-1), thresholdCols[j].column), 
531                                     omxMatrixElement(thresholdCols[j].matrix, (Infin[count]==1?value-1:value), thresholdCols[j].column), 
532                             weight, (means==NULL?0:omxVectorElement(ordMeans, count)), offset);
533                         }
534                 count++;
535                 }
536
537                         omxSadmvnWrapper(localobj, cov, ordCov, corList, lThresh, uThresh, Infin, &likelihood, &inform);
538
539                 if(inform == 2) {
540                         if(!returnRowLikelihoods) {
541                                 char helperstr[200];
542                                 char *errstr = (char*) calloc(250, sizeof(char));
543                                 sprintf(helperstr, "Improper value detected by integration routine in data row %d: Most likely the maximum number of ordinal variables (20) has been exceeded.  \n Also check that expected covariance matrix is not positive-definite", omxDataIndex(data, row));
544                                 if(localobj->matrix->currentState->computeCount <= 0) {
545                                         sprintf(errstr, "%s at starting values.\n", helperstr);
546                                 } else {
547                                         sprintf(errstr, "%s at major iteration %d.\n", helperstr, localobj->matrix->currentState->majorIteration);
548                                 }
549                                 omxRaiseError(localobj->matrix->currentState, -1, errstr);
550                                 free(errstr);
551                                 }
552                                 for(int nid = 0; nid < numIdentical; nid++) {
553                     if (returnRowLikelihoods)
554                                                 omxSetMatrixElement(sharedobj->matrix, omxDataIndex(data, row+nid), 0, 0.0);
555                                         omxSetMatrixElement(rowLikelihoods, omxDataIndex(data, row+nid), 0, 0.0);
556                                 }
557                 if(OMX_DEBUG) {mxLog("Improper input to sadmvn in row likelihood.  Skipping Row.");}
558                                 omxFIMLAdvanceJointRow(&row, &numIdenticalDefs, 
559                                         &numIdenticalContinuousMissingness,
560                                         &numIdenticalOrdinalMissingness, 
561                                         &numIdenticalContinuousRows,
562                                         &numIdenticalOrdinalRows,
563                                         data, numDefs, numIdentical);
564                 continue;
565                 }
566                 }
567
568                 double rowLikelihood = pow(2 * M_PI, -.5 * numContinuous) * (1.0/exp(determinant)) * exp(-.5 * Q) * likelihood;
569
570                 if(returnRowLikelihoods) {
571                     if(OMX_DEBUG_ROWS(row)) {mxLog("Change in Total Likelihood is %3.3f * %3.3f * %3.3f = %3.3f", 
572                         pow(2 * M_PI, -.5 * numContinuous), (1.0/exp(determinant)), exp(-.5 * Q), 
573                         pow(2 * M_PI, -.5 * numContinuous) * (1.0/exp(determinant)) * exp(-.5 * Q));}
574             
575                         if(OMX_DEBUG_ROWS(row)) {mxLog("Row %d likelihood is %3.3f.", row, rowLikelihood);}
576                         for(int j = numIdentical + row - 1; j >= row; j--) {  // Populate each successive identical row
577                                 omxSetMatrixElement(sharedobj->matrix, omxDataIndex(data, j), 0, rowLikelihood);
578                                 omxSetMatrixElement(rowLikelihoods, omxDataIndex(data, j), 0, rowLikelihood);
579                         }
580                 } else {
581                         double logLikelihood = -2 * log(likelihood);       // -2 Log of ordinal likelihood
582             logLikelihood += ((2 * determinant) + Q + (log(2 * M_PI) * numContinuous));    // -2 Log of continuous likelihood
583             logLikelihood *= numIdentical;
584
585                         for(int j = numIdentical + row - 1; j >= row; j--) {  // Populate each successive identical row
586                                 omxSetMatrixElement(rowLikelihoods, omxDataIndex(data, j), 0, rowLikelihood);
587                         }
588                         omxSetMatrixElement(rowLogLikelihoods, row, 0, logLikelihood);
589                         
590                         if(OMX_DEBUG_ROWS(row)) { 
591                                 mxLog("Change in Total log Likelihood for row %ld is %3.3f + %3.3f + %3.3f + %3.3f= %3.3f, ", 
592                                     localobj->matrix->currentState->currentRow, (2.0*determinant), Q, (log(2 * M_PI) * numContinuous), 
593                                     -2  * log(rowLikelihood), (2.0 *determinant) + Q + (log(2 * M_PI) * numContinuous));
594                         } 
595
596         }
597         if(firstRow) firstRow = 0;
598                 omxFIMLAdvanceJointRow(&row, &numIdenticalDefs, 
599                         &numIdenticalContinuousMissingness,
600                         &numIdenticalOrdinalMissingness, 
601                         &numIdenticalContinuousRows,
602                         &numIdenticalOrdinalRows,
603                         data, numDefs, numIdentical);
604         continue;
605
606         }
607
608 }
609
610 /**
611  * The localobj reference is used to access read-only variables,
612  * or variables that can be modified but whose state cannot be
613  * accessed from other threads.
614  *
615  * The sharedobj reference is used to access write-only variables,
616  * where the memory writes of any two threads are non-overlapping.
617  * No synchronization mechanisms are employed to maintain consistency
618  * of sharedobj references.
619  *
620  *
621  * Because (1) these functions may be invoked with arbitrary 
622  * rowbegin and rowcount values, and (2) the log-likelihood
623  * values for all data rows must be calculated (even in cases
624  * of errors), this function is forbidden from return()-ing early.
625  *
626  * As another consequence of (1) and (2), if "rowbegin" is in
627  * the middle of a sequence of identical rows, then defer
628  * move "rowbegin" to after the sequence of identical rows.
629  * Grep for "[[Comment 4]]" in source code.
630  */
631 void omxFIMLSingleIterationOrdinal(omxFitFunction *localobj, omxFitFunction *sharedobj, int rowbegin, int rowcount) {
632
633     omxFIMLFitFunction* ofo = ((omxFIMLFitFunction*) localobj->argStruct);
634     omxFIMLFitFunction* shared_ofo = ((omxFIMLFitFunction*) sharedobj->argStruct);
635
636         double Q = 0.0;
637         double* oldDefs;
638         int numDefs;
639         int numRemoves;
640         int returnRowLikelihoods;
641         int keepCov = 0, keepInverse = 0;
642
643         omxExpectation* expectation;
644         
645         omxMatrix *cov, *means, *smallCov, *dataColumns;//, *oldInverse;
646     omxMatrix *rowLikelihoods, *rowLogLikelihoods;;
647     omxThresholdColumn *thresholdCols;
648     double *lThresh, *uThresh, *corList, *weights;
649         int *Infin;
650         omxDefinitionVar* defVars;
651         omxData *data;
652
653         // Locals, for readability.  Should compile out.
654         cov                  = ofo->cov;
655         means                = ofo->means;
656         smallCov             = ofo->smallCov;
657         oldDefs              = ofo->oldDefs;
658         data                 = ofo->data;                       //  read-only
659         dataColumns          = ofo->dataColumns;                //  read-only
660         defVars              = ofo->defVars;                    //  read-only
661         numDefs              = ofo->numDefs;                    //  read-only
662         returnRowLikelihoods = ofo->returnRowLikelihoods;   //  read-only
663         rowLikelihoods    = shared_ofo->rowLikelihoods;     // write-only
664         rowLogLikelihoods = shared_ofo->rowLogLikelihoods;  // write-only
665
666     corList          = ofo->corList;
667     weights          = ofo->weights;
668     lThresh          = ofo->lThresh;
669     uThresh          = ofo->uThresh;
670     thresholdCols    = ofo->thresholdCols;
671
672     Infin            = ofo->Infin;
673
674         expectation      = localobj->expectation;
675
676         int firstRow = 1;
677     int row = rowbegin;
678
679     resetDefinitionVariables(oldDefs, numDefs);
680
681         // [[Comment 4]] moving row starting position
682         if (row > 0) {
683                 int prevIdentical = omxDataNumIdenticalRows(data, row - 1);
684                 row += (prevIdentical - 1);
685         }
686
687         while(row < data->rows && (row - rowbegin) < rowcount) {
688         if(OMX_DEBUG_ROWS(row)) {mxLog("Row %d.", row);}
689         localobj->matrix->currentState->currentRow = row;               // Set to a new row.
690                 int numIdentical = omxDataNumIdenticalRows(data, row);
691                 if(numIdentical == 0) numIdentical = 1; 
692                 // N.B.: numIdentical == 0 means an error occurred and was not properly handled;
693                 // it should never be the case.
694
695         Q = 0.0;
696
697         // Note:  This next bit really aught to be done using a matrix multiply.  Why isn't it?
698         numRemoves = 0;
699
700         // Handle Definition Variables.
701         if(numDefs != 0) {
702                         if(keepCov <= 0) {  // If we're keeping covariance from the previous row, do not populate 
703                                 int numVarsFilled = 0;
704                                 if(OMX_DEBUG_ROWS(row)) { mxLog("Handling Definition Vars."); }
705                                 numVarsFilled = handleDefinitionVarList(data, localobj->matrix->currentState, row, defVars, oldDefs, numDefs);
706                                 if (numVarsFilled < 0) {
707                                         for(int nid = 0; nid < numIdentical; nid++) {
708                                                 if(returnRowLikelihoods) omxSetMatrixElement(sharedobj->matrix, omxDataIndex(data, row+nid), 0, 0.0);
709                                                 omxSetMatrixElement(rowLikelihoods, omxDataIndex(data, row+nid), 0, 0.0);
710                                         }
711                                         omxFIMLAdvanceRow(&keepCov, &keepInverse, &row, data, numIdentical);
712                                         continue;
713                                 } else if (numVarsFilled || firstRow) {
714                                         // Use firstrow instead of rows == 0 for the case where the first row is all NAs
715                                         // N.B. handling of definition var lists always happens, regardless of firstRow.
716                                         omxExpectationCompute(expectation, NULL);
717                                         for(int j=0; j < dataColumns->cols; j++) {
718                                                 if(thresholdCols[j].numThresholds > 0) { // Actually an ordinal column
719                                                         omxRecompute(thresholdCols[j].matrix);
720                                                         checkIncreasing(thresholdCols[j].matrix, thresholdCols[j].column);
721                                                 }
722                                         }
723                                         // Calculate correlation matrix from covariance
724                                         omxStandardizeCovMatrix(cov, corList, weights);
725                                 }
726                         }
727                 }
728
729                 // Filter down correlation matrix and calculate thresholds
730
731                 for(int j = 0; j < dataColumns->cols; j++) {
732                         int var = omxVectorElement(dataColumns, j);
733                         int value = omxIntDataElement(data, row, var); // Indexing correction means this is the index of the upper bound +1.
734                         if(ISNA(value) || value == NA_INTEGER) {  // Value is NA, therefore filter.
735                                 numRemoves++;
736                                 // toRemove[j] = 1;
737                                 Infin[j] = -1;
738                                 continue;
739                         } else {                        // For joint, check here for continuousness
740                                 value--;                // Correct for C indexing: value is now the index of the upper bound.
741                                 // Note : Tested subsampling of the corList and thresholds for speed. 
742                                 //                      Doesn't look like that's much of a speedup.
743                                 double mean = (means == NULL) ? 0 : omxVectorElement(means, j);
744                                 double weight = weights[j];
745                                 if(OMX_DEBUG_ROWS(row)) { 
746                                         mxLog("Row %d, column %d. Mean is %f and weight is %f", row, j, mean, weight);
747                                 }
748                                 if(value == 0) {                                                                        // Lowest threshold = -Inf
749                                         lThresh[j] = (omxMatrixElement(thresholdCols[j].matrix, 0, thresholdCols[j].column) - mean) / weight;
750                                         uThresh[j] = lThresh[j];
751                                         Infin[j] = 0;
752                                 } else {
753                                         lThresh[j] = (omxMatrixElement(thresholdCols[j].matrix, value-1, thresholdCols[j].column) - mean) / weight;
754                                         if(thresholdCols[j].numThresholds > value) {    // Highest threshold = Inf
755                                                 double tmp = (omxMatrixElement(thresholdCols[j].matrix, value, thresholdCols[j].column) - mean) / weight;
756                                                 uThresh[j] = tmp;
757                                                 Infin[j] = 2;
758                                         } else {
759                                                 uThresh[j] = NA_INTEGER; // NA is a special to indicate +Inf
760                                                 Infin[j] = 1;
761                                         }
762                                 }
763                                 
764                                 if(uThresh[j] == NA_INTEGER || isnan(uThresh[j])) { // for matrix-style specification.
765                                         uThresh[j] = lThresh[j];
766                                         Infin[j] = 1;
767                                 }
768
769                                 if(OMX_DEBUG_ROWS(row)) { 
770                                         mxLog("Row %d, column %d.  Thresholds for data column %d and row %d are %f -> %f. (Infin=%d)", 
771                                                 row, j, var, value, lThresh[j], uThresh[j], Infin[j]);
772                                 }
773                         }
774                 }
775
776                 if(numRemoves >= smallCov->rows) {
777                         for(int nid = 0; nid < numIdentical; nid++) {
778                                 if(returnRowLikelihoods) {
779                                         omxSetMatrixElement(sharedobj->matrix, omxDataIndex(data, row+nid), 0, 1.0);
780                                 }
781                                 omxSetMatrixElement(rowLikelihoods, omxDataIndex(data, row+nid), 0, 1.0);
782                         }
783                         omxFIMLAdvanceRow(&keepCov, &keepInverse, &row, data, numIdentical);
784                         continue;
785                 }
786
787                 double likelihood;
788                 int inform;
789
790                 omxSadmvnWrapper(localobj, cov, smallCov, corList, lThresh, uThresh, Infin, &likelihood, &inform);
791
792                 if(inform == 2) {
793                         if(!returnRowLikelihoods) {
794                                 char helperstr[200];
795                                 char *errstr = (char*) calloc(250, sizeof(char));
796                                 sprintf(helperstr, "Improper value detected by integration routine in data row %d: \n Most likely the maximum number of ordinal variables (20) has been exceeded.  \n Also check that the expected covariance matrix is positive-definite", omxDataIndex(data, row));
797                                 if(localobj->matrix->currentState->computeCount <= 0) {
798                                         sprintf(errstr, "%s at starting values.\n", helperstr);
799                                 } else {
800                                         sprintf(errstr, "%s at major iteration %d.\n", helperstr, localobj->matrix->currentState->majorIteration);
801                                 }
802                                 omxRaiseError(localobj->matrix->currentState, -1, errstr);
803                                 free(errstr);
804                         }
805                         for(int nid = 0; nid < numIdentical; nid++) {
806                                 if (returnRowLikelihoods)
807                                         omxSetMatrixElement(sharedobj->matrix, omxDataIndex(data, row+nid), 0, 0.0);
808                                 omxSetMatrixElement(rowLikelihoods, omxDataIndex(data, row+nid), 0, 0.0);
809                         }
810                         omxFIMLAdvanceRow(&keepCov, &keepInverse, &row, data, numIdentical);
811                         continue;
812                 }
813
814                 if(returnRowLikelihoods) {
815                         if(OMX_DEBUG_ROWS(row)) { 
816                                 mxLog("Row %d likelihood is %3.3f.", row, likelihood);
817                         } 
818                         for(int j = numIdentical + row - 1; j >= row; j--) {  // Populate each successive identical row
819                                 omxSetMatrixElement(sharedobj->matrix, omxDataIndex(data, j), 0, likelihood);
820                                 omxSetMatrixElement(rowLikelihoods, omxDataIndex(data, j), 0, likelihood);
821                         }
822                 } else {
823                         for(int j = numIdentical + row - 1; j >= row; j--) {  // Populate each successive identical row
824                                 omxSetMatrixElement(rowLikelihoods, omxDataIndex(data, j), 0, likelihood);
825                         }
826                         double logDet = -2 * log(likelihood);
827                         logDet *= numIdentical;
828
829                         omxSetMatrixElement(rowLogLikelihoods, row, 0, logDet);
830
831                         if(OMX_DEBUG_ROWS(row)) { 
832                                 mxLog("-2 Log Likelihood this row is %3.3f, total change %3.3f",
833                                     logDet, logDet + Q + (log(2 * M_PI) * (cov->cols - numRemoves)));
834                         }
835                 }
836                 
837                 if(firstRow) firstRow = 0;
838                 omxFIMLAdvanceRow(&keepCov, &keepInverse, &row, data, numIdentical);
839         }
840 }
841
842
843
844 /**
845  * The localobj reference is used to access read-only variables,
846  * or variables that can be modified but whose state cannot be
847  * accessed from other threads.
848  *
849  * The sharedobj reference is used to access write-only variables,
850  * where the memory writes of any two threads are non-overlapping.
851  * No synchronization mechanisms are employed to maintain consistency
852  * of sharedobj references.
853  *
854  * Because (1) these functions may be invoked with arbitrary 
855  * rowbegin and rowcount values, and (2) the log-likelihood
856  * values for all data rows must be calculated (even in cases
857  * of errors), this function is forbidden from return()-ing early.
858  *
859  * As another consequence of (1) and (2), if "rowbegin" is in
860  * the middle of a sequence of identical rows, then defer
861  * move "rowbegin" to after the sequence of identical rows.
862  * Grep for "[[Comment 4]]" in source code.
863  * 
864  */
865 void omxFIMLSingleIteration(omxFitFunction *localobj, omxFitFunction *sharedobj, int rowbegin, int rowcount) {
866     if(OMX_DEBUG_ALGEBRA) {mxLog("Entering FIML Single Iteration"); }
867     omxFIMLFitFunction* ofo = ((omxFIMLFitFunction*) localobj->argStruct);
868     omxFIMLFitFunction* shared_ofo = ((omxFIMLFitFunction*) sharedobj->argStruct);
869
870         char u = 'U';
871         int info = 0;
872         double oned = 1.0;
873         double zerod = 0.0;
874         int onei = 1;
875         double determinant = 0.0;
876         double Q = 0.0;
877         double* oldDefs;
878         int numDefs;
879         int isContiguous, contiguousStart, contiguousLength;
880         int numRemoves;
881         int returnRowLikelihoods;
882         int keepCov = 0, keepInverse = 0;
883
884         omxExpectation* expectation;
885         
886         omxMatrix *cov, *means, *smallRow, *smallCov, *RCX, *dataColumns;//, *oldInverse;
887         omxMatrix *rowLikelihoods, *rowLogLikelihoods;
888         omxDefinitionVar* defVars;
889         omxData *data;
890
891         // Locals, for readability.  Should compile out.
892         cov                  = ofo->cov;
893         means                = ofo->means;
894         smallRow             = ofo->smallRow;
895         smallCov             = ofo->smallCov;
896         oldDefs              = ofo->oldDefs;
897         RCX                  = ofo->RCX;
898         data                 = ofo->data;                       //  read-only
899         dataColumns          = ofo->dataColumns;                //  read-only
900         defVars              = ofo->defVars;                    //  read-only
901         numDefs              = ofo->numDefs;                    //  read-only
902         returnRowLikelihoods = ofo->returnRowLikelihoods;   //  read-only
903         rowLikelihoods   = shared_ofo->rowLikelihoods;      // write-only
904         rowLogLikelihoods = shared_ofo->rowLogLikelihoods;  // write-only
905         isContiguous     = ofo->contiguous.isContiguous;    //  read-only
906         contiguousStart  = ofo->contiguous.start;           //  read-only
907         contiguousLength = ofo->contiguous.length;          //  read-only
908
909         expectation = localobj->expectation;
910
911         int toRemove[cov->cols];
912         int dataColumnCols = dataColumns->cols;
913
914         int firstRow = 1;
915         int row = rowbegin;
916         
917
918         // [[Comment 4]] moving row starting position
919         if (row > 0) {
920                 int prevIdentical = omxDataNumIdenticalRows(data, row - 1);
921                 row += (prevIdentical - 1);
922         }
923         
924         if(row == 0 && !strcmp(expectation->expType, "MxExpectationStateSpace") ) {
925                 if(OMX_DEBUG){ mxLog("Resetting State Space state (x) and error cov (P)."); }
926                 omxSetExpectationComponent(expectation, localobj, "Reset", NULL);
927         }
928
929         resetDefinitionVariables(oldDefs, numDefs);
930
931         while(row < data->rows && (row - rowbegin) < rowcount) {
932         if (OMX_DEBUG_ROWS(row)) {mxLog("Row %d.", row);} //:::DEBUG:::
933                 localobj->matrix->currentState->currentRow = row;               // Set to a new row.
934
935                 int numIdentical = omxDataNumIdenticalRows(data, row);
936                 // N.B.: numIdentical == 0 means an error occurred and was not properly handled;
937                 // it should never be the case.
938                 if (numIdentical == 0) numIdentical = 1; 
939                 
940                 Q = 0.0;
941                 
942                 numRemoves = 0;
943                 omxResetAliasedMatrix(smallRow);                        // Resize smallrow
944                 if (isContiguous) {
945                         omxContiguousDataRow(data, row, contiguousStart, contiguousLength, smallRow);
946                 } else {
947                         omxDataRow(data, row, dataColumns, smallRow);   // Populate data row
948                 }
949                 if(OMX_DEBUG_ROWS(row)){omxPrint(smallRow, "...smallRow"); }
950                 if(!strcmp(expectation->expType, "MxExpectationStateSpace")) {
951                         omxSetExpectationComponent(expectation, localobj, "y", smallRow);
952                 }
953                 //If the expectation is a state space model then
954                 // set the y attribute of the state space expectation to smallRow.
955                 
956                 // Handle Definition Variables.
957                 if(numDefs != 0 || !strcmp(expectation->expType, "MxExpectationStateSpace")) {
958                         if(keepCov <= 0 || !strcmp(expectation->expType, "MxExpectationStateSpace")) {  // If we're keeping covariance from the previous row, do not populate
959                                 int numVarsFilled = 0;
960                                 if(OMX_DEBUG_ROWS(row)) { mxLog("Handling Definition Vars."); }
961                                 numVarsFilled = handleDefinitionVarList(data, localobj->matrix->currentState, row, defVars, oldDefs, numDefs);
962                                 if (numVarsFilled < 0) {
963                                         for(int nid = 0; nid < numIdentical; nid++) {
964                                                 if(returnRowLikelihoods) omxSetMatrixElement(sharedobj->matrix, omxDataIndex(data, row+nid), 0, 0.0);
965                                                 omxSetMatrixElement(rowLikelihoods, omxDataIndex(data, row+nid), 0, 0.0);
966                                         }
967                                         if(keepCov <= 0) keepCov = omxDataNumIdenticalDefs(data, row);
968                                         if(keepInverse  <= 0) keepInverse = omxDataNumIdenticalMissingness(data, row);
969                                         // mxLog("Incrementing Row."); //:::DEBUG:::
970                                         row += numIdentical;
971                                         keepCov -= numIdentical;
972                                         keepInverse -= numIdentical;
973                                         continue;
974                                 } else if (numVarsFilled || firstRow || !strcmp(expectation->expType, "MxExpectationStateSpace")) {
975                                 // Use firstrow instead of rows == 0 for the case where the first row is all NAs
976                                 // N.B. handling of definition var lists always happens, regardless of firstRow.
977                                         omxExpectationCompute(expectation, NULL);
978                                         if(OMX_DEBUG_ROWS(row)){ mxLog("Successfully finished expectation compute."); }
979                                 }
980                         } else if(OMX_DEBUG_ROWS(row)){ mxLog("Identical def vars: Not repopulating"); }
981                 }
982
983                 if(OMX_DEBUG_ROWS(row)) { omxPrint(means, "Local Means"); }
984                 if(OMX_DEBUG_ROWS(row)) {
985                         char note[50];
986                         sprintf(note, "Local Data Row %d", row);
987                         omxPrint(smallRow, note); 
988                 }
989                 
990                 /* Censor row and censor and invert cov. matrix. */
991                 // Determine how many rows/cols to remove.
992                 memset(toRemove, 0, sizeof(int) * dataColumnCols);
993                 for(int j = 0; j < dataColumnCols; j++) {
994                         double dataValue = omxVectorElement(smallRow, j);
995                         int dataValuefpclass = fpclassify(dataValue);
996                         if(dataValuefpclass == FP_NAN || dataValuefpclass == FP_INFINITE) {
997                                 numRemoves++;
998                                 toRemove[j] = 1;
999                         } else if(means != NULL) {
1000                                 omxSetVectorElement(smallRow, j, (dataValue -  omxVectorElement(means, j)));
1001                         }
1002                 }
1003                 
1004                 if(cov->cols <= numRemoves) {
1005                         for(int nid = 0; nid < numIdentical; nid++) {
1006                                 if(returnRowLikelihoods) {
1007                                         omxSetMatrixElement(sharedobj->matrix, omxDataIndex(data, row+nid), 0, 1);
1008                                 }
1009                                 omxSetMatrixElement(rowLikelihoods, omxDataIndex(data, row+nid), 0, 1);
1010                         }
1011                         omxFIMLAdvanceRow(&keepCov, &keepInverse, &row, data, numIdentical);
1012                         continue;
1013                 }
1014                 
1015                 omxRemoveElements(smallRow, numRemoves, toRemove);      // Reduce it.
1016                 
1017                 if(OMX_DEBUG_ROWS(row)) { mxLog("Keeper codes: inverse: %d, cov:%d, identical:%d", keepInverse, keepCov, omxDataNumIdenticalRows(data, row)); }
1018
1019                 if((keepInverse <= 0 || keepCov <= 0 || firstRow) && strcmp(expectation->expType, "MxExpectationStateSpace")) { // If defs and missingness don't change, skip.
1020                         // also skip if this is a state space expectation
1021                         if(OMX_DEBUG_ROWS(row)) { mxLog("Beginning to recompute inverse cov for standard models"); }
1022                         omxResetAliasedMatrix(smallCov);                                // Re-sample covariance matrix
1023                         omxRemoveRowsAndColumns(smallCov, numRemoves, numRemoves, toRemove, toRemove);
1024
1025                         if(OMX_DEBUG_ROWS(row)) { omxPrint(smallCov, "Local Covariance Matrix"); }
1026
1027                         /* Calculate derminant and inverse of Censored Cov matrix */
1028                         // TODO : Speed this up.
1029                         F77_CALL(dpotrf)(&u, &(smallCov->rows), smallCov->data, &(smallCov->cols), &info);
1030                         if(info != 0) {
1031                                 int skip;
1032                                 if(!returnRowLikelihoods) {
1033                                         char helperstr[200];
1034                                         char *errstr = (char*) calloc(250, sizeof(char));
1035                                         sprintf(helperstr, "Expected covariance matrix is not positive-definite in data row %d", omxDataIndex(data, row));
1036                                         if(localobj->matrix->currentState->computeCount <= 0) {
1037                                                 sprintf(errstr, "%s at starting values.\n", helperstr);
1038                                         } else {
1039                                                 sprintf(errstr, "%s at major iteration %d (minor iteration %d).\n", helperstr, 
1040                                                         localobj->matrix->currentState->majorIteration, 
1041                                                         localobj->matrix->currentState->minorIteration);
1042                                         }
1043                                         omxRaiseError(localobj->matrix->currentState, -1, errstr);
1044                                         free(errstr);
1045                                 }
1046                                 if(keepCov <= 0) keepCov = omxDataNumIdenticalDefs(data, row);
1047                                 if(keepInverse <= 0) keepInverse = omxDataNumIdenticalMissingness(data, row);
1048                                 
1049                                 skip = (keepCov < keepInverse) ? keepCov : keepInverse;
1050
1051                                 for(int nid = 0; nid < skip; nid++) {
1052                                         if (returnRowLikelihoods) 
1053                                                 omxSetMatrixElement(sharedobj->matrix, omxDataIndex(data, row+nid), 0, 0.0);
1054                                         omxSetMatrixElement(rowLikelihoods, omxDataIndex(data, row+nid), 0, 0.0);
1055                                 }
1056                                 row += skip;
1057                                 keepCov -= skip;
1058                                 keepInverse -= skip;
1059                                 continue;
1060                         }
1061                         
1062                         // Calculate determinant: squared product of the diagonal of the decomposition
1063                         // For speed, we'll take the sum of the logs, rather than the log of the product
1064                         determinant = 0.0;
1065                         for(int diag = 0; diag < (smallCov->rows); diag++) {
1066                                 determinant += log(fabs(omxMatrixElement(smallCov, diag, diag)));
1067                 // if(OMX_DEBUG_ROWS) { mxLog("Next det is: %3.3d", determinant);} //:::DEBUG:::
1068                         }
1069             // determinant = determinant * determinant; // Delayed for now.
1070                         
1071                         F77_CALL(dpotri)(&u, &(smallCov->rows), smallCov->data, &(smallCov->cols), &info);
1072                         if(info != 0) {
1073                                 if(!returnRowLikelihoods) {
1074                                         char *errstr = (char*) calloc(250, sizeof(char));
1075                                         for(int nid = 0; nid < numIdentical; nid++) {
1076                                                 omxSetMatrixElement(rowLikelihoods, omxDataIndex(data, row+nid), 0, 0.0);
1077                                         }
1078                                         sprintf(errstr, "Cannot invert expected covariance matrix. Error %d.", info);
1079                                         omxRaiseError(localobj->matrix->currentState, -1, errstr);
1080                                         free(errstr);
1081                                 } else {
1082                                         for(int nid = 0; nid < numIdentical; nid++) {
1083                                                 omxSetMatrixElement(sharedobj->matrix, omxDataIndex(data, row+nid), 0, 0.0);
1084                                                 omxSetMatrixElement(rowLikelihoods, omxDataIndex(data, row+nid), 0, 0.0);
1085                                         }
1086                                         omxFIMLAdvanceRow(&keepCov, &keepInverse, &row, data, numIdentical);
1087                                         continue;
1088                                 }
1089                         }
1090                 }
1091                 
1092                 /* If it's a state space expectation, extract the inverse rather than recompute it */
1093                 if(!strcmp(expectation->expType, "MxExpectationStateSpace")) {
1094                         if(OMX_DEBUG_ROWS(row)) { mxLog("Beginning to extract inverse cov for state space models"); }
1095                         
1096                         omxResetAliasedMatrix(smallCov);                                // Re-sample covariance matrix
1097                         omxRemoveRowsAndColumns(smallCov, numRemoves, numRemoves, toRemove, toRemove);
1098                         smallCov = omxGetExpectationComponent(expectation, localobj, "inverse");
1099                         if(OMX_DEBUG_ROWS(row)) { omxPrint(smallCov, "Inverse of Local Covariance Matrix in state space model"); }
1100                         
1101                         determinant = *omxGetExpectationComponent(expectation, localobj, "determinant")->data;
1102                         if(OMX_DEBUG_ROWS(row)) { mxLog("0.5*log(det(Cov)) is: %3.3f", determinant);}
1103                 }
1104
1105                 /* Calculate Row Likelihood */
1106                 /* Mathematically: (2*pi)^cols * 1/sqrt(determinant(ExpectedCov)) * (dataRow %*% (solve(ExpectedCov)) %*% t(dataRow))^(1/2) */
1107                 F77_CALL(dsymv)(&u, &(smallCov->rows), &oned, smallCov->data, &(smallCov->cols), smallRow->data, &onei, &zerod, RCX->data, &onei);
1108                 Q = F77_CALL(ddot)(&(smallRow->cols), smallRow->data, &onei, RCX->data, &onei);
1109
1110                 double likelihood = pow(2 * M_PI, -.5 * smallRow->cols) * (1.0/exp(determinant)) * exp(-.5 * Q);
1111                 if(returnRowLikelihoods) {
1112                         if(OMX_DEBUG_ROWS(row)) {mxLog("Change in Total Likelihood is %3.3f * %3.3f * %3.3f = %3.3f", pow(2 * M_PI, -.5 * smallRow->cols), (1.0/exp(determinant)), exp(-.5 * Q), pow(2 * M_PI, -.5 * smallRow->cols) * (1.0/exp(determinant)) * exp(-.5 * Q));}
1113
1114                         for(int j = numIdentical + row - 1; j >= row; j--) {  // Populate each successive identical row
1115                                 omxSetMatrixElement(sharedobj->matrix, omxDataIndex(data, j), 0, likelihood);
1116                                 omxSetMatrixElement(rowLikelihoods, omxDataIndex(data, j), 0, likelihood);
1117                         }
1118                 } else {
1119                         double logLikelihood = ((2.0*determinant) + Q + (log(2 * M_PI) * smallRow->cols)) * numIdentical;
1120                         for(int j = numIdentical + row - 1; j >= row; j--) {  // Populate each successive identical row
1121                                 omxSetMatrixElement(rowLikelihoods, omxDataIndex(data, j), 0, likelihood);
1122                         }
1123                         omxSetMatrixElement(rowLogLikelihoods, row, 0, logLikelihood);
1124
1125                         if(OMX_DEBUG_ROWS(row)) {
1126                                 mxLog("Change in Total Likelihood for row %ld is %3.3f + %3.3f + %3.3f = %3.3f", localobj->matrix->currentState->currentRow, (2.0*determinant), Q, (log(2 * M_PI) * smallRow->cols), (2.0*determinant) + Q + (log(2 * M_PI) * smallRow->cols));
1127                         }
1128                 }
1129                 if(firstRow) firstRow = 0;
1130                 omxFIMLAdvanceRow(&keepCov, &keepInverse, &row, data, numIdentical);
1131         }
1132 }