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