Minor compile fixes
[openmx:openmx.git] / src / omxMatrix.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 *
19 *  omxMatrix.cc
20 *
21 *  Created: Timothy R. Brick    Date: 2008-11-13 12:33:06
22 *
23 *       Contains code for the omxMatrix class
24 *   omxDataMatrices hold necessary information to simplify
25 *       dealings between the OpenMX back end and BLAS.
26 *
27 **********************************************************/
28 #include "omxMatrix.h"
29 #include "omxOpenmpWrap.h"
30
31 // forward declarations
32 static omxMatrix* fillMatrixHelperFunction(omxMatrix* om, SEXP matrix, omxState* state,
33         unsigned short hasMatrixNumber, int matrixNumber);
34
35 const char omxMatrixMajorityList[3] = "Tn";             // BLAS Column Majority.
36
37 void omxPrintMatrix(omxMatrix *source, const char* header)
38 {
39         std::string buf;
40         buf += string_snprintf("[%d] %s: (%d x %d) [%s-major] SEXP 0x%0x\n%s = matrix(c(",
41                                omx_absolute_thread_num(),
42                                header, source->rows, source->cols, (source->colMajor?"col":"row"),
43                                source->owner, header);
44
45         int first=TRUE;
46         if(source->colMajor) {
47                 for(int j = 0; j < source->rows; j++) {
48                         for(int k = 0; k < source->cols; k++) {
49                                 if (first) first=FALSE;
50                                 else buf += ",";
51                                 buf += string_snprintf(" %3.6f", source->data[k*source->rows+j]);
52                         }
53                 }
54         } else {
55                 for(int j = 0; j < source->cols; j++) {
56                         for(int k = 0; k < source->rows; k++) {
57                                 if (first) first=FALSE;
58                                 else buf += ",";
59                                 buf += string_snprintf(" %3.6f,", source->data[k*source->cols+j]);
60                         }
61                 }
62         }
63         buf += string_snprintf("), byrow=TRUE, nrow=%d, ncol=%d)\n", source->rows, source->cols);
64         mxLogBig(buf);
65 }
66
67 omxMatrix* omxInitMatrix(omxMatrix* om, int nrows, int ncols, unsigned short isColMajor, omxState* os) {
68
69         if(om == NULL) om = (omxMatrix*) R_alloc(1, sizeof(omxMatrix));
70         if(OMX_DEBUG_MATRIX) { mxLog("Initializing matrix 0x%0x to (%d, %d) with state at 0x%x.", om, nrows, ncols, os); }
71
72         om->hasMatrixNumber = 0;
73         om->rows = nrows;
74         om->cols = ncols;
75         om->colMajor = (isColMajor ? 1 : 0);
76
77         om->originalRows = om->rows;
78         om->originalCols = om->cols;
79         om->originalColMajor=om->colMajor;
80
81         om->owner = NULL;
82         if(om->rows == 0 || om->cols == 0) {
83                 om->data = NULL;
84         } else {
85                 om->data = (double*) Calloc(nrows * ncols, double);
86         }
87
88         om->populateFrom = NULL;
89         om->populateFromCol = NULL;
90         om->populateFromRow = NULL;
91         om->populateToCol = NULL;
92         om->populateToRow = NULL;
93
94         om->numPopulateLocations = 0;
95
96         om->aliasedPtr = NULL;
97         om->algebra = NULL;
98         om->fitFunction = NULL;
99
100         om->currentState = os;
101         om->isTemporary = FALSE;
102         om->name = NULL;
103         om->isDirty = FALSE;
104
105         omxMatrixLeadingLagging(om);
106
107         return om;
108
109 }
110
111 omxMatrix* omxInitTemporaryMatrix(omxMatrix* om, int nrows, int ncols, unsigned short isColMajor, omxState* os) {
112
113         if(om == NULL) {
114                 om = (omxMatrix*) Calloc(1, omxMatrix);
115         }
116
117         om = omxInitMatrix(om, nrows, ncols, isColMajor, os);
118         om->isTemporary = TRUE;
119         
120         return(om);
121
122 }
123
124 void omxCopyMatrix(omxMatrix *dest, omxMatrix *orig) {
125         /* Copy a matrix.  NOTE: Matrix maintains its algebra bindings. */
126
127         if(OMX_DEBUG_MATRIX || OMX_DEBUG_ALGEBRA) { mxLog("omxCopyMatrix"); }
128
129         int regenerateMemory = TRUE;
130         int numPopLocs = orig->numPopulateLocations;
131
132         if(!dest->owner && (dest->originalRows == orig->rows && dest->originalCols == orig->cols)) {
133                 regenerateMemory = FALSE;                               // If it's local data and the right size, we can keep memory.
134         }
135
136         dest->rows = orig->rows;
137         dest->cols = orig->cols;
138         dest->colMajor = orig->colMajor;
139         dest->originalRows = dest->rows;
140         dest->originalCols = dest->cols;
141         dest->originalColMajor = dest->colMajor;
142         dest->isDirty = orig->isDirty;
143
144         dest->numPopulateLocations = numPopLocs;
145         if (numPopLocs > 0) {
146                 dest->populateFrom = (int*)R_alloc(numPopLocs, sizeof(int));
147                 dest->populateFromRow = (int*)R_alloc(numPopLocs, sizeof(int));
148                 dest->populateFromCol = (int*)R_alloc(numPopLocs, sizeof(int));
149                 dest->populateToRow = (int*)R_alloc(numPopLocs, sizeof(int));
150                 dest->populateToCol = (int*)R_alloc(numPopLocs, sizeof(int));
151                 
152                 memcpy(dest->populateFrom, orig->populateFrom, numPopLocs * sizeof(int));
153                 memcpy(dest->populateFromRow, orig->populateFromRow, numPopLocs * sizeof(int));
154                 memcpy(dest->populateFromCol, orig->populateFromCol, numPopLocs * sizeof(int));
155                 memcpy(dest->populateToRow, orig->populateToRow, numPopLocs * sizeof(int));
156                 memcpy(dest->populateToCol, orig->populateToCol, numPopLocs * sizeof(int));
157         }
158
159         if(dest->rows == 0 || dest->cols == 0) {
160                 omxFreeMatrixData(dest);
161                 dest->data = NULL;
162         } else {
163                 if(regenerateMemory) {
164                         omxFreeMatrixData(dest);                                                                                        // Free and regenerate memory
165                         dest->data = (double*) Calloc(dest->rows * dest->cols, double);
166                 }
167                 if (dest->data != orig->data) {  // if equal then programmer error? TODO
168                         memcpy(dest->data, orig->data, dest->rows * dest->cols * sizeof(double));
169                 }
170         }
171
172         dest->aliasedPtr = NULL;
173
174         omxMatrixLeadingLagging(dest);
175 }
176
177 void omxAliasMatrix(omxMatrix *dest, omxMatrix *src) {
178         omxCopyMatrix(dest, src);
179         dest->aliasedPtr = src;                                 // Alias now follows back matrix precisely.
180 }
181
182 void omxFreeMatrixData(omxMatrix * om) {
183
184         if(!om->owner && om->data != NULL) {
185                 if(OMX_DEBUG_MATRIX) { mxLog("Freeing matrix data at 0x%0x", om->data); }
186                 Free(om->data);
187         }
188         om->owner = NULL;
189         om->data = NULL;
190 }
191
192 void omxFreeAllMatrixData(omxMatrix *om) {
193     
194     if(om == NULL) return;
195
196         if(OMX_DEBUG) { 
197             mxLog("Freeing matrix at 0x%0x with data = 0x%x, algebra 0x%x, and fit function 0x%x.", 
198                 om, om->data, om->algebra); 
199         }
200
201         omxFreeMatrixData(om);
202
203         if(om->algebra != NULL) {
204                 omxFreeAlgebraArgs(om->algebra);
205                 om->algebra = NULL;
206         }
207
208         if(om->fitFunction != NULL) {
209                 omxFreeFitFunctionArgs(om->fitFunction);
210                 om->fitFunction = NULL;
211         }
212         
213         if(om->isTemporary) {
214                 Free(om);
215                 om = NULL;
216         }
217
218 }
219
220 /**
221  * Copies an omxMatrix to a new R matrix object
222  *
223  * \param om the omxMatrix to copy
224  * \return a PROTECT'd SEXP for the R matrix object
225  */
226 SEXP omxExportMatrix(omxMatrix *om) {
227         SEXP nextMat;
228         PROTECT(nextMat = allocMatrix(REALSXP, om->rows, om->cols));
229         for(int row = 0; row < om->rows; row++) {
230                 for(int col = 0; col < om->cols; col++) {
231                         REAL(nextMat)[col * om->rows + row] =
232                                 omxMatrixElement(om, row, col);
233                 }
234         }
235         return nextMat;
236 }
237
238 void omxZeroByZeroMatrix(omxMatrix *om) {
239         if (om->rows > 0 || om->cols > 0) {
240                 omxResizeMatrix(om, 0, 0, FALSE);
241         }
242 }
243
244 omxMatrix* omxNewIdentityMatrix(int nrows, omxState* state) {
245         omxMatrix* newMat = NULL;
246         int l,k;
247
248         newMat = omxInitMatrix(newMat, nrows, nrows, FALSE, state);
249         for(k =0; k < newMat->rows; k++) {
250                 for(l = 0; l < newMat->cols; l++) {
251                         if(l == k) {
252                                 omxSetMatrixElement(newMat, k, l, 1);
253                         } else {
254                                 omxSetMatrixElement(newMat, k, l, 0);
255                         }
256                 }
257         }
258         return newMat;
259 }
260
261 omxMatrix* omxDuplicateMatrix(omxMatrix* src, omxState* newState) {
262         omxMatrix* newMat;
263     
264         if(src == NULL) return NULL;
265         newMat = omxInitMatrix(NULL, src->rows, src->cols, FALSE, newState);
266         omxCopyMatrix(newMat, src);
267         newMat->hasMatrixNumber = src->hasMatrixNumber;
268         newMat->matrixNumber    = src->matrixNumber;
269     
270     return newMat;    
271 }
272
273 void omxResizeMatrix(omxMatrix *om, int nrows, int ncols, unsigned short keepMemory) {
274         // Always Recompute() before you Resize().
275         if(OMX_DEBUG_MATRIX) { 
276                 mxLog("Resizing matrix from (%d, %d) to (%d, %d) (keepMemory: %d)", 
277                         om->rows, om->cols, 
278                         nrows, ncols, keepMemory);
279         }
280         if((keepMemory == FALSE) && (om->rows != nrows || om->cols != ncols)) {
281                 if(OMX_DEBUG_MATRIX) { mxLog(" and regenerating memory to do it"); }
282                 omxFreeMatrixData(om);
283                 om->data = (double*) Calloc(nrows * ncols, double);
284         } else if(om->originalRows * om->originalCols < nrows * ncols) {
285                 warning("Upsizing an existing matrix may cause undefined behavior.\n"); // TODO: Define this behavior?
286         }
287
288         if(OMX_DEBUG_MATRIX) { mxLog("."); }
289         om->rows = nrows;
290         om->cols = ncols;
291         if(keepMemory == FALSE) {
292                 om->originalRows = om->rows;
293                 om->originalCols = om->cols;
294         }
295
296         omxMatrixLeadingLagging(om);
297 }
298
299 void omxResetAliasedMatrix(omxMatrix *om) {
300         om->rows = om->originalRows;
301         om->cols = om->originalCols;
302         if(om->aliasedPtr != NULL) {
303                 memcpy(om->data, om->aliasedPtr->data, om->rows*om->cols*sizeof(double));
304                 om->colMajor = om->aliasedPtr->colMajor;
305         }
306         omxMatrixLeadingLagging(om);
307 }
308
309 double* omxLocationOfMatrixElement(omxMatrix *om, int row, int col) {
310         int index = 0;
311         if(om->colMajor) {
312                 index = col * om->rows + row;
313         } else {
314                 index = row * om->cols + col;
315         }
316         return om->data + index;
317 }
318
319 void vectorElementError(int index, int numrow, int numcol) {
320         char *errstr = (char*) calloc(250, sizeof(char));
321         if ((numrow > 1) && (numcol > 1)) {
322                 sprintf(errstr, "Requested improper index (%d) from a malformed vector of dimensions (%d, %d).", 
323                         index, numrow, numcol);
324         } else {
325                 int length = (numrow > 1) ? numrow : numcol;
326                 sprintf(errstr, "Requested improper index (%d) from vector of length (%d).", 
327                         index, length);
328         }
329         error(errstr);
330         free(errstr);  // TODO not reached
331 }
332
333 void setMatrixError(omxMatrix *om, int row, int col, int numrow, int numcol) {
334         char *errstr = (char*) calloc(250, sizeof(char));
335         static const char *matrixString = "matrix";
336         static const char *algebraString = "algebra";
337         static const char *fitString = "fit function";
338         const char *typeString;
339         if (om->algebra != NULL) {
340                 typeString = algebraString;
341         } else if (om->fitFunction != NULL) {
342                 typeString = fitString;
343         } else {
344                 typeString = matrixString;
345         }
346         if (om->name == NULL) {
347                 sprintf(errstr, "Attempted to set row and column (%d, %d) in %s with dimensions %d x %d.", 
348                         row, col, typeString, numrow, numcol);
349         } else {
350                 sprintf(errstr, "Attempted to set row and column (%d, %d) in %s \"%s\" with dimensions %d x %d.", 
351                         row, col, typeString, om->name, numrow, numcol);
352         }
353         error(errstr);
354         free(errstr);  // TODO not reached
355 }
356
357 void matrixElementError(int row, int col, int numrow, int numcol) {
358         char *errstr = (char*) calloc(250, sizeof(char));
359         sprintf(errstr, "Requested improper value (%d, %d) from (%d, %d) matrix.",
360                 row, col, numrow, numcol);
361         error(errstr);
362         free(errstr);  // TODO not reached
363 }
364
365 void setVectorError(int index, int numrow, int numcol) {
366         char *errstr = (char*) calloc(250, sizeof(char));
367         if ((numrow > 1) && (numcol > 1)) {
368                 sprintf(errstr, "Attempting to set improper index (%d) from a malformed vector of dimensions (%d, %d).", 
369                         index, numrow, numcol);
370         } else {
371                 int length = (numrow > 1) ? numrow : numcol;
372                 sprintf(errstr, "Setting improper index (%d) from vector of length %d.", 
373                         index, length);
374         }
375         error(errstr);
376         free(errstr);  // TODO not reached
377 }
378
379 double omxAliasedMatrixElement(omxMatrix *om, int row, int col) {
380         int index = 0;
381         if(row >= om->originalRows || col >= om->originalCols) {
382                 char *errstr = (char*) calloc(250, sizeof(char));
383                 sprintf(errstr, "Requested improper value (%d, %d) from (%d, %d) matrix.", 
384                         row + 1, col + 1, om->originalRows, om->originalCols);
385                 error(errstr);
386                 free(errstr);  // TODO not reached
387         return (NA_REAL);
388         }
389         if(om->colMajor) {
390                 index = col * om->originalRows + row;
391         } else {
392                 index = row * om->originalCols + col;
393         }
394         return om->data[index];
395 }
396
397 void omxMarkDirty(omxMatrix *om) { om->isDirty = TRUE; }
398 void omxMarkClean(omxMatrix *om) { om->isDirty = FALSE; }
399
400 unsigned short omxMatrixNeedsUpdate(omxMatrix *om) {
401         for(int i = 0; i < om->numPopulateLocations; i++) {
402                 int index = om->populateFrom[i];
403                 omxMatrix* sourceMatrix;
404                 if (index < 0) {
405                         sourceMatrix = om->currentState->matrixList[~index];
406                 } else {
407                         sourceMatrix = om->currentState->algebraList[index];
408                 }
409                 if(omxNeedsUpdate(sourceMatrix)) return TRUE;   // Make sure it's up to date
410         }
411     return FALSE;
412 };
413
414 omxMatrix* omxNewMatrixFromRPrimitive(SEXP rObject, omxState* state, 
415         unsigned short hasMatrixNumber, int matrixNumber) {
416 /* Creates and populates an omxMatrix with details from an R matrix object. */
417         omxMatrix *om = NULL;
418         om = omxInitMatrix(NULL, 0, 0, FALSE, state);
419         return omxFillMatrixFromRPrimitive(om, rObject, state, hasMatrixNumber, matrixNumber);
420 }
421
422 omxMatrix* omxFillMatrixFromRPrimitive(omxMatrix* om, SEXP rObject, omxState* state,
423         unsigned short hasMatrixNumber, int matrixNumber) {
424 /* Populates the fields of a omxMatrix with details from an R object. */
425         if(!isMatrix(rObject) && !isVector(rObject)) { // Sanity Check
426                 error("Recieved unknown matrix type in omxFillMatrixFromRPrimitive.");
427         }
428         return(fillMatrixHelperFunction(om, rObject, state, hasMatrixNumber, matrixNumber));
429 }
430
431
432
433 static omxMatrix* fillMatrixHelperFunction(omxMatrix* om, SEXP matrix, omxState* state,
434         unsigned short hasMatrixNumber, int matrixNumber) {
435
436         SEXP matrixDims;
437         int* dimList;
438
439         if(OMX_DEBUG) { mxLog("Filling omxMatrix from R matrix."); }
440
441         if(om == NULL) {
442                 om = omxInitMatrix(NULL, 0, 0, FALSE, state);
443         }
444
445         PROTECT(om->owner = coerceVector(matrix, REALSXP));
446         om->data = REAL(om->owner);
447
448         if(isMatrix(matrix)) {
449                 PROTECT(matrixDims = getAttrib(matrix, R_DimSymbol));
450                 dimList = INTEGER(matrixDims);
451                 om->rows = dimList[0];
452                 om->cols = dimList[1];
453         } else if (isVector(matrix)) {          // If it's a vector, assume it's a row vector. BLAS doesn't care.
454                 if(OMX_DEBUG) { mxLog("Vector discovered.  Assuming rowity."); }
455                 om->rows = 1;
456                 om->cols = length(matrix);
457         }
458         if(OMX_DEBUG) { mxLog("Matrix connected to (%d, %d) matrix or MxMatrix.", om->rows, om->cols); }
459
460         om->colMajor = TRUE;
461         om->originalRows = om->rows;
462         om->originalCols = om->cols;
463         om->originalColMajor = TRUE;
464         om->aliasedPtr = NULL;
465         om->algebra = NULL;
466         om->fitFunction = NULL;
467         om->currentState = state;
468         om->hasMatrixNumber = hasMatrixNumber;
469         om->matrixNumber = matrixNumber;
470         om->isDirty = FALSE;
471
472         if(OMX_DEBUG) { mxLog("Pre-compute call.");}
473         omxMatrixLeadingLagging(om);
474         if(OMX_DEBUG) { mxLog("Post-compute call.");}
475
476         if(OMX_DEBUG) {
477                 omxPrintMatrix(om, "Finished importing matrix");
478         }
479
480         return om;
481 }
482
483 void omxProcessMatrixPopulationList(omxMatrix* matrix, SEXP matStruct) {
484
485         if(OMX_DEBUG) { mxLog("Processing Population List: %d elements.", length(matStruct) - 1); }
486         SEXP subList;
487
488         if(length(matStruct) > 1) {
489                 int numPopLocs = length(matStruct) - 1;
490                 matrix->numPopulateLocations = numPopLocs;
491                 matrix->populateFrom = (int*)R_alloc(numPopLocs, sizeof(int));
492                 matrix->populateFromRow = (int*)R_alloc(numPopLocs, sizeof(int));
493                 matrix->populateFromCol = (int*)R_alloc(numPopLocs, sizeof(int));
494                 matrix->populateToRow = (int*)R_alloc(numPopLocs, sizeof(int));
495                 matrix->populateToCol = (int*)R_alloc(numPopLocs, sizeof(int));
496         }
497
498         for(int i = 0; i < length(matStruct)-1; i++) {
499                 PROTECT(subList = AS_INTEGER(VECTOR_ELT(matStruct, i+1)));
500
501                 int* locations = INTEGER(subList);
502                 if(OMX_DEBUG) { mxLog("."); } //:::
503                 matrix->populateFrom[i] = locations[0];
504                 matrix->populateFromRow[i] = locations[1];
505                 matrix->populateFromCol[i] = locations[2];
506                 matrix->populateToRow[i] = locations[3];
507                 matrix->populateToCol[i] = locations[4];
508         }
509 }
510
511 void omxToggleRowColumnMajor(omxMatrix *mat) {
512
513         int i, j;
514         int nrows = mat->rows;
515         int ncols = mat->cols;
516         
517         double *newdata = (double*) Calloc(nrows * ncols, double);
518         double *olddata = mat->data;
519
520         if (mat->colMajor) {
521                 for(i = 0; i < ncols; i++) {
522                         for(j = 0; j < nrows; j++) {
523                                 newdata[i + ncols * j] = olddata[i * nrows + j];
524                         }
525                 }
526         } else {
527                 for(i = 0; i < nrows; i++) {
528                         for(j = 0; j < ncols; j++) {
529                                 newdata[i + nrows * j] = olddata[i * ncols + j];
530                         }
531                 }
532         }
533
534         omxFreeMatrixData(mat);
535         mat->data = newdata;
536         mat->colMajor = !mat->colMajor;
537 }
538
539 void omxTransposeMatrix(omxMatrix *mat) {
540         mat->colMajor = !mat->colMajor;
541         
542         if(mat->rows != mat->cols){
543         int mid = mat->rows;
544         mat->rows = mat->cols;
545         mat->cols = mid;
546         }
547         
548         omxMatrixLeadingLagging(mat);
549 }
550
551 void omxRemoveElements(omxMatrix *om, int numRemoved, int removed[]) {
552
553         if(numRemoved < 1) { return; }
554
555         int oldElements;
556
557         if (om->rows > 1) {
558                 if(om->aliasedPtr == NULL) {
559                         if(om->originalRows == 0) {
560                                 om->originalRows = om->rows;
561                         }
562                         oldElements = om->originalRows;
563                 } else {
564                         oldElements = om->aliasedPtr->rows;
565                 }
566                 om->rows = oldElements - numRemoved;
567         } else {
568                 if(om->aliasedPtr == NULL) {
569                         if(om->originalCols == 0) {
570                                 om->originalCols = om->cols;
571                         }
572                         oldElements = om->originalCols;
573                 } else {
574                         oldElements = om->aliasedPtr->cols;
575                 }
576                 om->cols = oldElements - numRemoved;
577         }
578
579         int nextElement = 0;
580
581         for(int j = 0; j < oldElements; j++) {
582                 if(!removed[j]) {
583                         if(om->aliasedPtr == NULL) {
584                                 omxUnsafeSetVectorElement(om, nextElement, omxUnsafeVectorElement(om, j));
585                         } else {
586                                 omxUnsafeSetVectorElement(om, nextElement, omxUnsafeVectorElement(om->aliasedPtr, j));
587                         }
588                         nextElement++;
589                 }
590         }
591
592         omxMatrixLeadingLagging(om);
593 }
594
595 void omxRemoveRowsAndColumns(omxMatrix *om, int numRowsRemoved, int numColsRemoved, int rowsRemoved[], int colsRemoved[])
596 {
597     // TODO: Create short-circuit form of omxRemoveRowsAndCols to remove just rows or just columns.
598 //      if(OMX_DEBUG_MATRIX) { mxLog("Removing %d rows and %d columns from 0x%0x.", numRowsRemoved, numColsRemoved, om);}
599
600         if(numRowsRemoved < 1 && numColsRemoved < 1) { return; }
601
602         int oldRows, oldCols;
603
604         if(om->aliasedPtr == NULL) {
605                 if(om->originalRows == 0 || om->originalCols == 0) {
606                         om->originalRows = om->rows;
607                         om->originalCols = om->cols;
608                 }
609                 oldRows = om->originalRows;
610                 oldCols = om->originalCols;
611         } else {
612                 oldRows = om->aliasedPtr->rows;
613                 oldCols = om->aliasedPtr->cols;
614         }
615
616         int nextCol = 0;
617         int nextRow = 0;
618
619         if(om->rows > om->originalRows || om->cols > om->originalCols) {        // sanity check.
620                 error("Aliased Matrix is too small for alias.");
621         }
622
623         om->rows = oldRows - numRowsRemoved;
624         om->cols = oldCols - numColsRemoved;
625
626         // Note:  This really aught to be done using a matrix multiply.  Why isn't it?
627         for(int j = 0; j < oldCols; j++) {
628                 if(OMX_DEBUG_MATRIX || OMX_DEBUG_ALGEBRA) { mxLog("Handling column %d/%d...", j, oldCols);}
629                 if(colsRemoved[j]) {
630                         if(OMX_DEBUG_MATRIX || OMX_DEBUG_ALGEBRA) { mxLog("Removed.");}
631                         continue;
632                 } else {
633                         nextRow = 0;
634                         if(OMX_DEBUG_MATRIX || OMX_DEBUG_ALGEBRA) { mxLog("Rows (max %d): ", oldRows); }
635                         for(int k = 0; k < oldRows; k++) {
636                                 if(rowsRemoved[k]) {
637                                         if(OMX_DEBUG_MATRIX || OMX_DEBUG_ALGEBRA) { mxLog("%d removed....", k);}
638                                         continue;
639                                 } else {
640                                         if(OMX_DEBUG_MATRIX || OMX_DEBUG_ALGEBRA) { mxLog("%d kept....", k);}
641                                         if(om->aliasedPtr == NULL) {
642                                                 if(OMX_DEBUG_MATRIX || OMX_DEBUG_ALGEBRA) { mxLog("Self-aliased matrix access.");}
643                                                 omxSetMatrixElement(om, nextRow, nextCol, omxAliasedMatrixElement(om, k, j));
644                                         } else {
645                                                 if(OMX_DEBUG_MATRIX || OMX_DEBUG_ALGEBRA) { mxLog("Matrix 0x%x re-aliasing to 0x%x.", om, om->aliasedPtr);}
646                                                 omxSetMatrixElement(om, nextRow, nextCol, omxMatrixElement(om->aliasedPtr, k,  j));
647                                         }
648                                         nextRow++;
649                                 }
650                         }
651                         if(OMX_DEBUG_MATRIX || OMX_DEBUG_ALGEBRA) { mxLog("");}
652                         nextCol++;
653                 }
654         }
655
656         omxMatrixLeadingLagging(om);
657 }
658
659 /* Function wrappers that switch based on inclusion of algebras */
660 void omxPrint(omxMatrix *source, const char* d) {                                       // Pretty-print a (small) matrix
661     if(source == NULL) mxLog("%s is NULL.", d);
662         else if(source->algebra != NULL) omxAlgebraPrint(source->algebra, d);
663         else if(source->fitFunction != NULL) omxFitFunctionPrint(source->fitFunction, d);
664         else omxPrintMatrix(source, d);
665 }
666
667 void omxPopulateSubstitutions(omxMatrix *om) {
668         for(int i = 0; i < om->numPopulateLocations; i++) {
669                 int index = om->populateFrom[i];
670                 omxMatrix* sourceMatrix;
671                 if (index < 0) {
672                         sourceMatrix = om->currentState->matrixList[~index];
673                 } else {
674                         sourceMatrix = om->currentState->algebraList[index];
675                 }
676                 if (sourceMatrix != NULL) {
677                         omxRecompute(sourceMatrix);                             // Make sure it's up to date
678                         double value = omxMatrixElement(sourceMatrix, om->populateFromRow[i], om->populateFromCol[i]);
679                         omxSetMatrixElement(om, om->populateToRow[i], om->populateToCol[i], value);
680                 }
681         }
682 }
683
684 void omxMatrixLeadingLagging(omxMatrix *om) {
685         om->majority = &(omxMatrixMajorityList[(om->colMajor?1:0)]);
686         om->minority = &(omxMatrixMajorityList[(om->colMajor?0:1)]);
687         om->leading = (om->colMajor?om->rows:om->cols);
688         om->lagging = (om->colMajor?om->cols:om->rows);
689 }
690
691 unsigned short omxNeedsUpdate(omxMatrix *matrix) {
692         if (OMX_DEBUG_ALGEBRA) 
693                 mxLog("Matrix 0x%0x needs update: ", matrix);
694         if (matrix->hasMatrixNumber && !matrix->isDirty) {
695                 if (OMX_DEBUG_ALGEBRA) mxLog("No");
696                 return(FALSE);
697         } else {
698                 if (OMX_DEBUG_ALGEBRA) mxLog("Yes");
699                 return(TRUE);
700         }
701 }
702
703 void omxRecompute(omxMatrix *matrix) {
704         if(matrix->numPopulateLocations > 0) omxPopulateSubstitutions(matrix);
705         else if(!omxNeedsUpdate(matrix)) /* do nothing */;
706         else if(matrix->algebra != NULL) omxAlgebraRecompute(matrix->algebra);
707         else if(matrix->fitFunction != NULL) {
708                 omxFitFunctionCompute(matrix->fitFunction, 0, NULL);
709         }
710 }
711
712 void omxForceCompute(omxMatrix *matrix) {
713         if(matrix->numPopulateLocations > 0) omxPopulateSubstitutions(matrix);
714         else if (matrix->algebra != NULL) omxAlgebraCompute(matrix->algebra);
715         else if(matrix->fitFunction != NULL) {
716                 omxFitFunctionCompute(matrix->fitFunction, 0, NULL);
717         }
718 }
719
720 /*
721  * omxShallowInverse
722  *                      Calculates the inverse of (I-A) using an n-step Neumann series
723  * Assumes that A reduces to all zeros after numIters steps
724  *
725  * params:
726  * omxMatrix *A                         : The A matrix.  I-A will be inverted.  Size MxM.
727  * omxMatrix *Z                         : On output: Computed (I-A)^-1. MxM.
728  * omxMatrix *Ax                        : Space for computation. MxM.
729  * omxMatrix *I                         : Identity matrix. Will not be changed on exit. MxM.
730  */
731
732 void omxShallowInverse(int numIters, omxMatrix* A, omxMatrix* Z, omxMatrix* Ax, omxMatrix* I ) {
733
734         omxMatrix* origZ = Z;
735     double oned = 1, minusOned = -1.0;
736
737         if(numIters == NA_INTEGER) {
738                 int ipiv[A->rows], lwork = 4 * A->rows * A->cols, k;            // TODO: Speedups can be made by preallocating this.
739                 double work[lwork];
740                 if(OMX_DEBUG_ALGEBRA) { mxLog("RAM Algebra (I-A) inversion using standard (general) inversion."); }
741
742                 /* Z = (I-A)^-1 */
743                 if(I->colMajor != A->colMajor) {
744                         omxTransposeMatrix(I);
745                 }
746                 omxCopyMatrix(Z, A);
747
748                 /* Z = (I-A)^-1 */
749                 // F77_CALL(omxunsafedgemm)(I->majority, Z->majority, &(I->cols), &(I->rows), &(Z->rows), &oned, I->data, &(I->cols), I->data, &(I->cols), &minusOned, Z->data, &(Z->cols));
750                 //omxDGEMM(FALSE, FALSE, oned, I, Z, minusOned, Z); //Tim, I think this is incorrect: 1.0*I*Z-Z = Z-Z = 0 but you want I-Z.  So this should be omxDGEMM(FALSE, FALSE, oned, I, I, minusOned, Z). -MDH
751                 omxDGEMM(FALSE, FALSE, oned, I, I, minusOned, Z);
752
753                 // F77_CALL(dgetrf)(&(Z->rows), &(Z->cols), Z->data, &(Z->leading), ipiv, &k);
754                 k = omxDGETRF(Z, ipiv);
755                 if(OMX_DEBUG) { mxLog("Info on LU Decomp: %d", k); }
756                 if(k > 0) {
757                         char errStr[250];
758                         strncpy(errStr, "(I-A) is exactly singular.", 100);
759                         omxRaiseError(A->currentState, -1, errStr);                    // Raise Error
760                         return;
761                 }
762                 // F77_CALL(dgetri)(&(Z->rows), Z->data, &(Z->leading), ipiv, work, &lwork, &k);
763                 k = omxDGETRI(Z, ipiv, work, lwork);
764                 if(OMX_DEBUG_ALGEBRA) { mxLog("Info on Invert: %d", k); }
765
766                 if(OMX_DEBUG_ALGEBRA) {omxPrint(Z, "Z");}
767
768         } else {
769
770                 if(OMX_DEBUG_ALGEBRA) { mxLog("RAM Algebra (I-A) inversion using optimized expansion."); }
771
772                 /* Taylor Expansion optimized I-A calculation */
773                 if(I->colMajor != A->colMajor) {
774                         omxTransposeMatrix(I);
775                 }
776
777                 if(I->colMajor != Ax->colMajor) {
778                         omxTransposeMatrix(Ax);
779                 }
780
781                 omxCopyMatrix(Z, A);
782
783                 /* Optimized I-A inversion: Z = (I-A)^-1 */
784                 // F77_CALL(omxunsafedgemm)(I->majority, A->majority, &(I->cols), &(I->rows), &(A->rows), &oned, I->data, &(I->cols), I->data, &(I->cols), &oned, Z->data, &(Z->cols));  // Z = I + A = A^0 + A^1
785                 // omxDGEMM(FALSE, FALSE, 1.0, I, I, 1.0, Z); // Z == A + I
786
787                 for(int i = 0; i < A->rows; i++)
788                         omxSetMatrixElement(Z, i, i, 1);
789
790                 for(int i = 1; i <= numIters; i++) { // TODO: Efficiently determine how many times to do this
791                         // The sequence goes like this: (I + A), I + (I + A) * A, I + (I + (I + A) * A) * A, ...
792                         // Which means only one DGEMM per iteration.
793                         if(OMX_DEBUG_ALGEBRA) { mxLog("....RAM: Iteration #%d/%d", i, numIters);}
794                         omxCopyMatrix(Ax, I);
795                         // F77_CALL(omxunsafedgemm)(A->majority, A->majority, &(Z->cols), &(Z->rows), &(A->rows), &oned, Z->data, &(Z->cols), A->data, &(A->cols), &oned, Ax->data, &(Ax->cols));  // Ax = Z %*% A + I
796                         omxDGEMM(FALSE, FALSE, oned, A, Z, oned, Ax);
797                         omxMatrix* m = Z; Z = Ax; Ax = m;       // Juggle to make Z equal to Ax
798                 }
799                 if(origZ != Z) {        // Juggling has caused Ax and Z to swap
800                         omxCopyMatrix(Z, Ax);
801                 }
802         }
803 }