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