Add thread-safe logging functions
[openmx:openmx.git] / src / omxAlgebraFunctions.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 *  omxAlgebraFunctions.c
20 *
21 *  Created: Timothy R. Brick    Date: 2008-11-13 12:33:06
22 *
23 *       Includes the functions required for omxAlgebra statements.
24 *   These functions should take a number of values that
25 *   evenly matches the number of args requested by the
26 *   omxSymbolTable.
27 *
28 **********************************************************/
29
30 #include "omxAlgebraFunctions.h"
31 #include "omxMatrix.h"
32 #include "merge.h"
33 #include "omxBLAS.h"
34 #include "omxOpenmpWrap.h"
35 #include "omxSadmvnWrapper.h"
36
37 void omxStandardizeCovMatrix(omxMatrix* cov, double* corList, double* weights) {
38         // Maybe coerce this into an algebra or sequence of algebras?
39
40         if(OMX_DEBUG) { mxLog("Standardizing matrix."); }
41
42         int rows = cov->rows;
43
44         for(int i = 0; i < rows; i++) {
45                 weights[i] = sqrt(omxMatrixElement(cov, i, i));
46         }
47
48         for(int i = 0; i < rows; i++) {
49                 for(int j = 0; j < i; j++) {
50                         corList[((i*(i-1))/2) + j] = omxMatrixElement(cov, i, j) / (weights[i] * weights[j]);
51                 }
52         }
53 }
54
55 void checkIncreasing(omxMatrix* om, int column) {
56         double previous = - INFINITY;
57         double current;
58         for(int j = 0; j < om->rows; j++ ) {
59                 current = omxMatrixElement(om, j, column);
60                 if(isnan(current) || current == NA_INTEGER) {
61                         continue;
62                 }
63                 if(current <= previous) {
64                         char *errstr = (char*) calloc(250, sizeof(char));
65                         sprintf(errstr, "Thresholds are not strictly increasing.");
66                         //TODO: Count 'em all, then throw an error that lists which ones.
67                         omxRaiseError(om->currentState, -1, errstr);
68                         free(errstr);
69                 }
70         }
71 }
72
73
74
75 // TODO: Implement wrappers for BLAS functions used here.
76
77 /* omxAlgebraFunction Wrappers */
78
79 void omxMatrixTranspose(omxMatrix** matList, int numArgs, omxMatrix* result) {
80
81         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Transpose.");}
82
83         omxMatrix* inMat = matList[0];
84
85         omxCopyMatrix(result, inMat);
86         result->colMajor = !result->colMajor;
87         int rowtemp = result->rows;
88         result->rows = result->cols;
89         result->cols = rowtemp;
90         int *populateTemp = result->populateToCol;
91         result->populateToCol = result->populateToRow;
92         result->populateToRow = populateTemp;
93         omxMatrixLeadingLagging(result);
94 }
95
96 void omxMatrixInvert(omxMatrix** matList, int numArgs, omxMatrix* result)
97 {
98
99         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Invert.");}
100
101         omxMatrix* inMat = matList[0];
102
103         int lwork = 4 * inMat->rows * inMat->cols;
104         int l = 0;
105
106         int*    ipiv = (int*) malloc(inMat->rows * sizeof(int));
107         double* work = (double*) malloc(lwork * sizeof(double));
108
109         omxCopyMatrix(result, inMat);
110         F77_CALL(dgetrf)(&(result->cols), &(result->rows), result->data, &(result->leading), ipiv, &l);
111         if(l != 0) {
112                 char *errstr = (char*) calloc(250, sizeof(char));
113                 sprintf(errstr, "Attempted to invert non-invertable matrix.");
114                 omxRaiseError(result->currentState, -1, errstr);
115                 free(errstr);
116         } else {
117                 F77_CALL(dgetri)(&(result->cols), result->data, &(result->leading), ipiv, work, &lwork, &l);
118         }
119
120         free(ipiv);
121         free(work);
122 }
123
124 void omxMatrixMult(omxMatrix** matList, int numArgs, omxMatrix* result)
125 {
126
127         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Multiply.");}
128
129         omxMatrix* preMul = matList[0];
130         omxMatrix* postMul = matList[1];
131
132         if(preMul == NULL || postMul == NULL) {
133                 char *errstr = (char*) calloc(250, sizeof(char));
134                 sprintf(errstr, "Null matrix pointer detected.\n");
135                 free(errstr);
136                 return;
137         }
138
139         static double zero = 0.0;
140         static double one = 1.0;
141
142         /* Conformability Check! */
143         if(preMul->cols != postMul->rows) {
144                 char *errstr = (char*) calloc(250, sizeof(char));
145                 sprintf(errstr, "Non-conformable matrices [(%d x %d) and (%d x %d)] in Matrix Multiply.", preMul->rows, preMul->cols, postMul->rows, postMul->cols);
146                 omxRaiseError(result->currentState, -1, errstr);
147                 free(errstr);
148                 return;
149         }
150
151         if(result->rows != preMul->rows || result->cols != postMul->cols)
152                 omxResizeMatrix(result, preMul->rows, postMul->cols, FALSE);
153
154         /* For debugging */
155         if(OMX_DEBUG_ALGEBRA) {
156                 omxPrint(result, "NewMatrix");
157                 mxLog("DGEMM: %c, %c, %d, %d, %d, %f, %0x %d %0x %d %f %0x %d", *(preMul->majority), *(postMul->majority), (preMul->rows), (postMul->cols), (preMul->cols), one, (preMul->data), (preMul->leading), (postMul->data), (postMul->leading), zero, (result->data), (result->leading));
158         }
159
160         /* The call itself */
161         omxDGEMM(FALSE, FALSE, 1.0, preMul, postMul, 0.0, result);
162         // F77_CALL(omxunsafedgemm)((preMul->majority), (postMul->majority), &(preMul->rows), &(postMul->cols), &(preMul->cols), &one, preMul->data, &(preMul->leading), postMul->data, &(postMul->leading), &zero, result->data, &(result->leading));
163         result->colMajor = TRUE;
164
165         omxMatrixLeadingLagging(result);
166 }
167
168 void omxElementPower(omxMatrix** matList, int numArgs, omxMatrix* result)
169 {
170
171         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Element Powering.");}
172
173         omxMatrix* first = matList[0];
174         omxMatrix* second = matList[1];
175
176         if(first->cols != second->cols || first->rows != second->rows) {
177                 char *errstr = (char*) calloc(250, sizeof(char));
178                 sprintf(errstr, "Non-conformable matrices in Element Powering.");
179                 omxRaiseError(result->currentState, -1, errstr);
180                 free(errstr);
181                 return;
182         }
183
184         int rows = first->rows;
185         int cols = first->cols;
186         int size = rows * cols;
187
188         if((rows != result->rows) || (cols != result->cols)) {
189                 omxResizeMatrix(result, rows, cols, FALSE);
190         }
191         
192         if (first->colMajor == second->colMajor) {
193                 for(int i = 0; i < size; i++) {
194                         omxSetVectorElement(result, i,
195                                 pow(omxVectorElement(first, i),
196                                         omxVectorElement(second, i)));
197                 }
198                 result->colMajor = first->colMajor;
199                 omxMatrixLeadingLagging(result);
200         } else {
201                 for(int i = 0; i < rows; i++) {
202                         for(int j = 0; j < cols; j++) {
203                                 omxSetMatrixElement(result, i, j,
204                                         pow(omxMatrixElement(first, i, j),
205                                                 omxMatrixElement(second, i, j)));
206                         }
207                 }
208         }
209 }
210
211 void omxMatrixElementMult(omxMatrix** matList, int numArgs, omxMatrix* result)
212 {
213         omxMatrix* first = matList[0];
214         omxMatrix* second = matList[1];
215
216         /* Conformability Check! */
217         if(first->cols != second->cols || first->rows != second->rows) {
218                 char *errstr = (char*) calloc(250, sizeof(char));
219                 sprintf(errstr, "Non-conformable matrices in Element Multiplication.");
220                 omxRaiseError(result->currentState, -1, errstr);
221                 free(errstr);
222                 return;
223         }
224
225         int rows = first->rows;
226         int cols = first->cols;
227         int size = rows * cols;
228
229         if((rows != result->rows) || (cols != result->cols)) {
230                 omxResizeMatrix(result, rows, cols, FALSE);
231         }
232         
233         if (first->colMajor == second->colMajor) {
234                 for(int i = 0; i < size; i++) {
235                         omxSetVectorElement(result, i,
236                                 omxVectorElement(first, i) *
237                                 omxVectorElement(second, i));
238                 }
239                 result->colMajor = first->colMajor;
240                 omxMatrixLeadingLagging(result);
241         } else {
242                 for(int i = 0; i < rows; i++) {
243                         for(int j = 0; j < cols; j++) {
244                                 omxSetMatrixElement(result, i, j,
245                                         omxMatrixElement(first, i, j) *
246                                         omxMatrixElement(second, i, j));
247                         }
248                 }
249         }
250 }
251
252
253 void omxKroneckerProd(omxMatrix** matList, int numArgs, omxMatrix* result)
254 {
255
256         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Kronecker Product.");}
257
258         omxMatrix* preMul = matList[0];
259         omxMatrix* postMul = matList[1];
260
261         int preMulRows = preMul->rows;
262         int preMulCols = preMul->cols;
263         int postMulRows = postMul->rows;
264         int postMulCols = postMul->cols;
265         int rows = preMulRows * postMulRows;
266         int cols = preMulCols * postMulCols;
267
268         if(result->rows != rows || result->cols != cols)
269                 omxResizeMatrix(result, rows, cols, FALSE);
270
271         for(int preRow = 0; preRow < preMulRows; preRow++)
272                 for(int postRow = 0; postRow < postMulRows; postRow++)
273                         for(int preCol = 0; preCol < preMulCols; preCol++)
274                                 for(int postCol = 0; postCol < postMulCols; postCol++)
275                                         omxSetMatrixElement(result, preRow * postMulRows + postRow,
276                                                 preCol * postMulCols + postCol,
277                                                 omxMatrixElement(preMul, preRow, preCol) * omxMatrixElement(postMul, postRow, postCol));
278 }
279
280 void omxKroneckerPower(omxMatrix** matList, int numArgs, omxMatrix* result)
281 {
282
283         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Kronecker Power.");}
284
285         omxMatrix* preMul = matList[0];
286         omxMatrix* postMul = matList[1];
287
288         int rows = preMul->rows * postMul->rows;
289         int cols = preMul->cols * postMul->cols;
290
291         if(result->rows != rows || result->cols != cols)
292                 omxResizeMatrix(result, rows, cols, FALSE);
293
294         for(int preRow = 0; preRow < preMul->rows; preRow++)
295                 for(int postRow = 0; postRow < postMul->rows; postRow++)
296                         for(int preCol = 0; preCol < preMul->cols; preCol++)
297                                 for(int postCol = 0; postCol < postMul->cols; postCol++)
298                                         omxSetMatrixElement(result, preRow*postMul->rows + postRow,
299                                                 preCol*postMul->cols + postCol,
300                                                 pow(omxMatrixElement(preMul, preRow, preCol), omxMatrixElement(postMul, postRow, postCol)));
301 }
302
303 void omxQuadraticProd(omxMatrix** matList, int numArgs, omxMatrix* result)
304 {
305
306         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Quadratic Product.");}
307
308         omxMatrix* preMul = matList[0];
309         omxMatrix* postMul = matList[1];
310         /* A %&% B = ABA' */
311
312         static double zero = 0.0;
313         static double one = 1.0;
314
315         /* Conformability Check! */
316         if(preMul->cols != postMul->rows || postMul->rows != postMul->cols) {
317                 omxRaiseError(preMul->currentState, -1, "Non-conformable matrices in Matrix Quadratic Product.");
318                 return;
319         }
320
321         omxMatrix* intermediate = NULL;
322         intermediate = omxInitTemporaryMatrix(NULL, preMul->rows, postMul->cols, TRUE, preMul->currentState);
323
324         if(OMX_DEBUG_ALGEBRA) { mxLog("Quadratic: os = 0x%x, step = %d.", result->currentState, intermediate->currentState->computeCount);}
325
326         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Quadratic Product: Readying result matrix.(%x, %x)", result->algebra, result->fitFunction);}
327
328         if(result->rows != preMul->rows || result->cols != preMul->rows)
329                 omxResizeMatrix(result, preMul->rows, preMul->rows, FALSE);
330
331         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Quadratic Product: Readying intermediate Matrix.(%x, %x)", intermediate->algebra, intermediate->fitFunction);}
332
333         /* The call itself */
334         if(OMX_DEBUG_ALGEBRA) { mxLog("Quadratic: premul.");}
335         F77_CALL(omxunsafedgemm)((preMul->majority), (postMul->majority), &(preMul->rows), &(postMul->cols), &(preMul->cols), &one, preMul->data, &(preMul->leading), postMul->data, &(postMul->leading), &zero, intermediate->data, &(intermediate->leading));
336
337         if(OMX_DEBUG_ALGEBRA) { mxLog("Quadratic: postmul.");}
338 //      if(OMX_DEBUG_ALGEBRA) { mxLog("Quadratic postmul: result is (%d x %d), %d leading, inter is (%d x %d), prem is (%d x %d), post is (%d x %d).", result->rows, result->cols, result->leading, intermediate->rows, intermediate->cols, preMul->rows, preMul->cols, postMul->rows, postMul->cols);}
339         F77_CALL(omxunsafedgemm)((intermediate->majority), (preMul->minority), &(intermediate->rows), &(preMul->rows), &(intermediate->cols), &one, intermediate->data, &(intermediate->leading), preMul->data, &(preMul->leading), &zero, result->data, &(result->leading));
340         if(OMX_DEBUG_ALGEBRA) { mxLog("Quadratic: clear.");}
341
342         omxFreeAllMatrixData(intermediate);
343
344 }
345
346 void omxElementDivide(omxMatrix** matList, int numArgs, omxMatrix* result)
347 {
348
349         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Element Division.");}
350
351         omxMatrix* first = matList[0];
352         omxMatrix* second = matList[1];
353
354         /* Conformability Check! */
355         if(first->cols != second->cols || first->rows != second->rows) {
356                 char *errstr = (char*) calloc(250, sizeof(char));
357                 sprintf(errstr, "Non-conformable matrices in Element Division.");
358                 omxRaiseError(result->currentState, -1, errstr);
359                 free(errstr);
360                 return;
361         }
362
363         int rows = first->rows;
364         int cols = first->cols;
365         int size = rows * cols;
366
367         if((rows != result->rows) || (cols != result->cols)) {
368                 omxResizeMatrix(result, rows, cols, FALSE);
369         }
370         
371         if (first->colMajor == second->colMajor) {
372                 for(int i = 0; i < size; i++) {
373                         omxSetVectorElement(result, i,
374                                 omxVectorElement(first, i) /
375                                 omxVectorElement(second, i));
376                 }
377                 result->colMajor = first->colMajor;
378                 omxMatrixLeadingLagging(result);
379         } else {
380                 for(int i = 0; i < rows; i++) {
381                         for(int j = 0; j < cols; j++) {
382                                 omxSetMatrixElement(result, i, j,
383                                         omxMatrixElement(first, i, j) /
384                                         omxMatrixElement(second, i, j));
385                         }
386                 }
387         }
388 }
389
390 void omxUnaryNegation(omxMatrix** matList, int numArgs, omxMatrix* result)
391 {
392         if (OMX_DEBUG_ALGEBRA) {mxLog("ALGEBRA: Unary Negation.");}
393
394         omxMatrix* inMat = matList[0];
395
396         int rows = inMat->rows;
397         int cols = inMat->cols;
398
399         if((rows != result->rows) || (cols != result->cols)){
400                 omxResizeMatrix(result, rows, cols, FALSE);
401         }
402
403         int vec_length = rows * cols;
404         for (int i=0; i < vec_length; i++){
405                 double ith_value = omxVectorElement(inMat, i);
406                 if (ith_value == 0.0){
407                         omxSetVectorElement(result, i, 1.0);
408                 }
409                 else {
410                         omxSetVectorElement(result, i, 0.0);
411                 }
412         }
413         result->colMajor = inMat->colMajor;
414         omxMatrixLeadingLagging(result);
415 }
416
417 void omxBinaryOr(omxMatrix** matList, int numArgs, omxMatrix* result){
418                 if (OMX_DEBUG_ALGEBRA) {mxLog("ALGEBRA: Binary Or.");}
419                 omxMatrix* first = matList[0];
420                     omxMatrix* second = matList[1];
421
422                 if(first->cols != second->cols || first->rows != second->rows) {
423                         char *errstr = (char*) calloc(250, sizeof(char));
424                         sprintf(errstr, "Non-conformable matrices in Binary Or.");
425                         omxRaiseError(result->currentState, -1, errstr);
426                         free(errstr);
427                         return;
428                 }
429
430                 int rows = first->rows;
431                 int cols = first->cols;
432                 int size = rows * cols;
433
434             if((rows != result->rows) || (cols != result->cols)){
435                         omxResizeMatrix(result, rows, cols, FALSE);
436             }
437
438                 if (first->colMajor == second->colMajor) {
439                         for(int i = 0; i < size; i++) {
440                                         double ith_first  = omxVectorElement(first, i);
441                                         double ith_second =omxVectorElement(second, i);
442                                         if ((ith_first == 0.0) && (ith_second == 0.0)){
443                                                 omxSetVectorElement(result, i, 0.0);
444                                         }
445                                         else {
446                                                 omxSetVectorElement(result, i, 1.0);
447                                         }
448                         }
449                 result->colMajor = first->colMajor;
450                 omxMatrixLeadingLagging(result);
451                 } else {
452                         for(int i = 0; i < rows; i++) {
453                                 for(int j = 0; j < cols; j++) {
454                                                         double ith_first  = omxMatrixElement(first, i, j);
455                                                         double ith_second = omxMatrixElement(second, i, j);
456                                         if ((ith_first == 0.0) && (ith_second == 0.0)){
457                                                 omxSetMatrixElement(result, i, j, 0.0);
458                                         }
459                                         else {
460                                                 omxSetMatrixElement(result, i, j, 1.0);
461                                         }
462                                 }
463                         }
464                 }
465 }
466
467 void omxBinaryAnd(omxMatrix** matList, int numArgs, omxMatrix* result){
468                 if (OMX_DEBUG_ALGEBRA) {mxLog("ALGEBRA: Binary And.");}
469                 omxMatrix* first = matList[0];
470                     omxMatrix* second = matList[1];
471
472                 if(first->cols != second->cols || first->rows != second->rows) {
473                         char *errstr = (char*) calloc(250, sizeof(char));
474                         sprintf(errstr, "Non-conformable matrices in Binary And.");
475                         omxRaiseError(result->currentState, -1, errstr);
476                         free(errstr);
477                                 return;
478                 }
479
480                 int rows = first->rows;
481                 int cols = first->cols;
482                 int size = rows * cols;
483
484             if((rows != result->rows) || (cols != result->cols)){
485                      omxResizeMatrix(result, rows, cols, FALSE);
486             }
487
488                 if (first->colMajor == second->colMajor) {
489                         for(int i = 0; i < size; i++) {
490                                         double ith_first  = omxVectorElement(first, i);
491                                         double ith_second =omxVectorElement(second, i);
492                                         if ((ith_first == 0.0) || (ith_second == 0.0)){
493                                                 omxSetVectorElement(result, i, 0.0);
494                                         }
495                                         else {
496                                                 omxSetVectorElement(result, i, 1.0);
497                                         }
498                         }
499                 result->colMajor = first->colMajor;
500                 omxMatrixLeadingLagging(result);
501                 } else {
502                         for(int i = 0; i < rows; i++) {
503                                 for(int j = 0; j < cols; j++) {
504                                                         double ith_first  = omxMatrixElement(first, i, j);
505                                                         double ith_second = omxMatrixElement(second, i, j);
506                                         if ((ith_first == 0.0) || (ith_second == 0.0)){
507                                                 omxSetMatrixElement(result, i, j, 0.0);
508                                         }
509                                         else {
510                                                 omxSetMatrixElement(result, i, j, 1.0);
511                                         }
512                                 }
513                         }
514                 }
515 }
516
517 void omxBinaryLessThan(omxMatrix** matList, int numArgs, omxMatrix* result){
518                 if (OMX_DEBUG_ALGEBRA) {mxLog("ALGEBRA: Binary Less Than.");}
519                 omxMatrix* first = matList[0];
520                     omxMatrix* second = matList[1];
521
522                 if(first->cols != second->cols || first->rows != second->rows) {
523                         char *errstr = (char*) calloc(250, sizeof(char));
524                         sprintf(errstr, "Non-conformable matrices in Binary Less Than.");
525                         omxRaiseError(result->currentState, -1, errstr);
526                         free(errstr);
527                                 return;
528                 }
529
530                 int rows = first->rows;
531                 int cols = first->cols;
532                 int size = rows * cols;
533
534             if((rows != result->rows) || (cols != result->cols)){
535                      omxResizeMatrix(result, rows, cols, FALSE);
536             }
537
538                 if (first->colMajor == second->colMajor) {
539                         for(int i = 0; i < size; i++) {
540                                 double ith_value = omxVectorElement(first, i) -
541                                                    omxVectorElement(second, i);
542                                                 if (ith_value < 0.0){
543                                                         omxSetVectorElement(result, i, 1.0);
544                                                 }
545                                                 else {
546                                                         omxSetVectorElement(result, i, 0.0);
547                                                 }
548                         }
549                 result->colMajor = first->colMajor;
550                 omxMatrixLeadingLagging(result);
551                 } else {
552                         for(int i = 0; i < rows; i++) {
553                                 for(int j = 0; j < cols; j++) {
554                                         double ith_value = omxMatrixElement(first, i, j) -
555                                            omxMatrixElement(second, i, j);
556
557                                         if (ith_value < 0.0){
558                                                 omxSetMatrixElement(result, i, j, 1.0);
559                                         }
560                                         else {
561                                                 omxSetMatrixElement(result, i, j, 0.0);
562                                         }
563                                 }
564                         }
565                 }
566 }
567
568 void omxBinaryGreaterThan(omxMatrix** matList, int numArgs, omxMatrix* result)
569 {
570         if (OMX_DEBUG_ALGEBRA) {mxLog("ALGEBRA: Binary Greater Than.");}
571  
572         omxMatrix* first = matList[0];
573             omxMatrix* second = matList[1];
574
575         if(first->cols != second->cols || first->rows != second->rows) {
576                 char *errstr = (char*) calloc(250, sizeof(char));
577                 sprintf(errstr, "Non-conformable matrices in Binary Greater Than.");
578                 omxRaiseError(result->currentState, -1, errstr);
579                 free(errstr);
580                         return;
581         }
582
583         int rows = first->rows;
584         int cols = first->cols;
585         int size = rows * cols;
586
587         if((rows != result->rows) || (cols != result->cols)){
588                 omxResizeMatrix(result, rows, cols, FALSE);
589         }
590
591         if (first->colMajor == second->colMajor) {
592                 for(int i = 0; i < size; i++) {
593                         double ith_value = omxVectorElement(first, i) -
594                                            omxVectorElement(second, i);
595                         if (ith_value > 0.0){
596                                 omxSetVectorElement(result, i, 1.0);
597                         }
598                         else {
599                                 omxSetVectorElement(result, i, 0.0);
600                         }
601                 }
602         result->colMajor = first->colMajor;
603         omxMatrixLeadingLagging(result);
604         } else {
605                 for(int i = 0; i < rows; i++) {
606                         for(int j = 0; j < cols; j++) {
607                                 double ith_value = omxMatrixElement(first, i, j) -
608                                                    omxMatrixElement(second, i, j);
609
610                                 if (ith_value > 0.0){
611                                         omxSetMatrixElement(result, i, j, 1.0);
612                                 }
613                                 else {
614                                         omxSetMatrixElement(result, i, j, 0.0);
615                                 }
616                         }
617                 }
618         }
619 }
620
621 void omxBinaryApproxEquals(omxMatrix** matList, int numArgs, omxMatrix* result)
622 {
623         
624        if (OMX_DEBUG_ALGEBRA) {mxLog("ALGEBRA: Binary Approx. Equals.");}
625  
626         omxMatrix* first  = matList[0];
627             omxMatrix* second = matList[1];
628                 omxMatrix* epsilon = matList[2]; 
629                 
630         if(first->cols != second->cols  || first->rows != second->rows || 
631            first->cols != epsilon->cols || first->rows != epsilon->rows) {
632                 char *errstr = (char*) calloc(250, sizeof(char));
633                 sprintf(errstr, "Non-conformable matrices in Binary Approx Equals.");
634                 omxRaiseError(result->currentState, -1, errstr);
635                 free(errstr);
636                         return;
637         }
638
639         int rows = first->rows;
640         int cols = first->cols;
641         int size = rows * cols;
642         double negativeOne = -1.0;
643
644     if((rows != result->rows) || (cols != result->cols)){
645                 omxResizeMatrix(result, rows, cols, FALSE);
646     }
647
648         if (first->colMajor == second->colMajor && second->colMajor == epsilon->colMajor) {
649                 for(int i = 0; i < size; i++) {
650                 double ith_value = omxVectorElement(first, i) -
651                                            omxVectorElement(second, i);
652                                 double epsilon_value = omxVectorElement(epsilon, i);
653                                 
654                                 if (ith_value < 0.0){
655                                         ith_value = ith_value * negativeOne;
656                                 }
657                                 if (ith_value < epsilon_value){
658                                         omxSetVectorElement(result, i, 1.0);
659                                 }
660                                 else {
661                                         omxSetVectorElement(result, i, 0.0);
662                                 }
663                 }
664         result->colMajor = first->colMajor;
665         omxMatrixLeadingLagging(result);
666         } else {
667                 for(int i = 0; i < rows; i++) {
668                         for(int j = 0; j < cols; j++) {
669                                                     double ith_value = omxMatrixElement(first, i, j) -
670                                                    omxMatrixElement(second, i, j);
671
672                                                         double epsilon_value = omxMatrixElement(epsilon, i, j);
673                                                         if (ith_value < 0.0){
674                                                                 ith_value = ith_value * negativeOne;
675                                                         }
676                                 if (ith_value < epsilon_value){
677                                         omxSetMatrixElement(result, i, j, 1.0);
678                                 }
679                                 else {
680                                         omxSetMatrixElement(result, i, j, 0.0);
681                                 }
682                         }
683                 }
684         }
685
686 }
687
688 void omxMatrixAdd(omxMatrix** matList, int numArgs, omxMatrix* result)
689 {
690
691         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Addition.");}
692
693         omxMatrix* first = matList[0];
694         omxMatrix* second = matList[1];
695
696         /* Conformability Check! */
697         if(first->cols != second->cols || first->rows != second->rows) {
698                 char *errstr = (char*) calloc(250, sizeof(char));
699                 sprintf(errstr, "Non-conformable matrices in Matrix Addition.");
700                 omxRaiseError(result->currentState, -1, errstr);
701                 free(errstr);
702                 return;
703         }
704
705         int rows = first->rows;
706         int cols = first->cols;
707         int size = rows * cols;
708
709         if((rows != result->rows) || (cols != result->cols)) {
710                 omxResizeMatrix(result, rows, cols, FALSE);
711         }
712         
713         if (first->colMajor == second->colMajor) {
714                 for(int i = 0; i < size; i++) {
715                         omxSetVectorElement(result, i,
716                                 omxVectorElement(first, i) +
717                                 omxVectorElement(second, i));
718                 }
719                 result->colMajor = first->colMajor;
720                 omxMatrixLeadingLagging(result);
721         } else {
722                 for(int i = 0; i < rows; i++) {
723                         for(int j = 0; j < cols; j++) {
724                                 omxSetMatrixElement(result, i, j,
725                                         omxMatrixElement(first, i, j) +
726                                         omxMatrixElement(second, i, j));
727                         }
728                 }
729         }
730 }
731
732 int matrixExtractIndices(omxMatrix *source, int dimLength, int **indices, omxMatrix *result) {
733
734         int *retval;
735         /* Case 1: the source vector contains no elements */
736         if (source->rows == 0 || source->cols == 0) {
737                 retval = (int*) calloc(dimLength, sizeof(int));
738                 for(int i = 0; i < dimLength; i++) {
739                         retval[i] = i;
740                 }
741                 *indices = retval;
742                 return(dimLength);
743         }
744         int zero = 0, positive = 0, negative = 0;
745         /* Count the number of zero, positive, and negative elements */
746         for(int i = 0; i < source->rows * source->cols; i++) {
747                 double delement = omxVectorElement(source, i);
748                 if (!R_finite(delement)) {
749                         char *errstr = (char*) calloc(250, sizeof(char));
750                         sprintf(errstr, "non-finite value in '[' operator.\n");
751                         omxRaiseError(result->currentState, -1, errstr);
752                         free(errstr);
753                         return(0);
754                 }
755                 int element = (int) delement;
756                 if (element < 0) {
757                         /* bounds checking */
758                         if (element < - dimLength) {
759                                 char *errstr = (char*) calloc(250, sizeof(char));
760                                 sprintf(errstr, "index %d is out of bounds in '[' operator.", element);
761                                 omxRaiseError(result->currentState, -1, errstr);
762                                 free(errstr);
763                                 return(0);
764                         }
765                         negative++;
766                 } else if (element == 0) {
767                         zero++;
768                 } else {
769                         /* bounds checking */
770                         if (element > dimLength) {
771                                 char *errstr = (char*) calloc(250, sizeof(char));
772                                 sprintf(errstr, "index %d is out of bounds in '[' operator.", element);
773                                 omxRaiseError(result->currentState, -1, errstr);
774                                 free(errstr);
775                                 return(0);
776                         }
777                         positive++;
778                 }
779         }
780         /* It is illegal to mix positive and negative elements */
781         if (positive > 0 && negative > 0) {
782                 char *errstr = (char*) calloc(250, sizeof(char));
783                 sprintf(errstr, "Positive and negative indices together in '[' operator.");
784                 omxRaiseError(result->currentState, -1, errstr);
785                 free(errstr);
786                 return(0);
787         }
788         /* convert negative indices into a list of positive indices */
789         if (negative > 0) {
790                 int *track = (int*) calloc(dimLength, sizeof(int));
791                 int length = dimLength;
792                 for(int i = 0; i < source->rows * source->cols; i++) {
793                         int element = (int) omxVectorElement(source, i);
794                         if (element < 0) {
795                                 if (!track[-element - 1]) length--;
796                                 track[-element - 1]++;
797                         }
798                 }
799                 if (length == 0) {
800                         free(track);
801                         return(0);
802                 }
803                 retval = (int*) calloc(length, sizeof(int));
804                 int j = 0;
805                 for(int i = 0; i < dimLength; i++) {
806                         if(!track[i]) {
807                                 retval[j++] = i;
808                         }
809                 }
810                 free(track);
811                 *indices = retval;
812                 return(length);
813         }
814         /* convert positive indices with offset of zero instead of one */
815         if (positive > 0) {
816                 int length = positive - zero;
817                 retval = (int*) calloc(length, sizeof(int));
818                 int j = 0;
819                 for(int i = 0; i < source->rows * source->cols; i++) {
820                         int element = (int) omxVectorElement(source, i);
821                         if (element > 0) {
822                                 retval[j++] = element - 1;
823                         }
824                 }
825                 *indices = retval;
826                 return(length);
827         }
828         /* return zero length if no positive or negative elements */
829         return(0);
830 }
831
832 void omxMatrixExtract(omxMatrix** matList, int numArgs, omxMatrix* result) {
833
834         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Extract.");}
835
836         omxMatrix* inMat = matList[0];
837         omxMatrix* rowMatrix = matList[1];
838         omxMatrix* colMatrix = matList[2];
839
840         if(OMX_DEBUG_ALGEBRA) { omxPrint(rowMatrix, "Row matrix: "); }
841         if(OMX_DEBUG_ALGEBRA) { omxPrint(colMatrix, "Col matrix: "); }
842
843         int *rowIndices, *colIndices;
844         int rowIndexLength, colIndexLength;
845
846         rowIndexLength = matrixExtractIndices(rowMatrix, inMat->rows, &rowIndices, result);
847         colIndexLength = matrixExtractIndices(colMatrix, inMat->cols, &colIndices, result);
848
849         if (result->rows != rowIndexLength || result->cols != colIndexLength) {
850                 omxResizeMatrix(result, rowIndexLength, colIndexLength, FALSE);
851         }
852
853         for(int row = 0; row < rowIndexLength; row++) {
854                 for(int col = 0; col < colIndexLength; col++) {
855                         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Extract: (%d, %d)[%d, %d] <- (%d, %d)[%d,%d].", result->rows, result->cols, row, col, rowIndexLength, colIndexLength, rowIndices[row], colIndices[col]);}
856                         double element = omxMatrixElement(inMat, rowIndices[row], colIndices[col]);
857                         omxSetMatrixElement(result, row, col, element);
858                 }
859         }
860
861         if (rowIndexLength > 0) free(rowIndices);
862         if (colIndexLength > 0) free(colIndices);
863
864 }
865
866 void omxMatrixSubtract(omxMatrix** matList, int numArgs, omxMatrix* result)
867 {
868
869         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Subtraction.");}
870
871         omxMatrix* first = matList[0];
872         omxMatrix* second = matList[1];
873
874         if(first->cols != second->cols || first->rows != second->rows) {
875                 char *errstr = (char*) calloc(250, sizeof(char));
876                 sprintf(errstr, "Non-conformable matrices in Matrix Subtract.");
877                 omxRaiseError(result->currentState, -1, errstr);
878                 free(errstr);
879                 return;
880         }
881
882         int rows = first->rows;
883         int cols = first->cols;
884         int size = rows * cols;
885
886         if((rows != result->rows) || (cols != result->cols)) {
887                 omxResizeMatrix(result, rows, cols, FALSE);
888         }
889         
890         if (first->colMajor == second->colMajor) {
891                 for(int i = 0; i < size; i++) {
892                         omxSetVectorElement(result, i,
893                                 omxVectorElement(first, i) -
894                                 omxVectorElement(second, i));
895                 }
896                 result->colMajor = first->colMajor;
897                 omxMatrixLeadingLagging(result);
898         } else {
899                 for(int i = 0; i < rows; i++) {
900                         for(int j = 0; j < cols; j++) {
901                                 omxSetMatrixElement(result, i, j,
902                                         omxMatrixElement(first, i, j) -
903                                         omxMatrixElement(second, i, j));
904                         }
905                 }
906         }
907 }
908
909 void omxUnaryMinus(omxMatrix** matList, int numArgs, omxMatrix* result)
910 {
911
912         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Unary Minus.");}
913
914         omxMatrix* inMat = matList[0];
915
916         int rows = inMat->rows;
917         int cols = inMat->cols;
918         int size = rows * cols;
919
920         if((rows != result->rows) || (cols != result->cols)) {
921                 omxResizeMatrix(result, rows, cols, FALSE);
922         }
923
924         for(int i = 0; i < size; i++) {
925                 omxSetVectorElement(result, i,
926                         - omxVectorElement(inMat, i));
927         }
928         result->colMajor = inMat->colMajor;
929         omxMatrixLeadingLagging(result);
930
931 }
932
933 void omxMatrixHorizCat(omxMatrix** matList, int numArgs, omxMatrix* result) {
934
935         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Horizontal Matrix Concatenation.");}
936
937         int totalRows = 0, totalCols = 0, currentCol=0;
938
939         if(numArgs == 0) return;
940
941         totalRows = matList[0]->rows;                   // Assumed constant.  Assert this below.
942
943         for(int j = 0; j < numArgs; j++) {
944                 if(totalRows != matList[j]->rows) {
945                         char *errstr = (char*) calloc(250, sizeof(char));
946                         sprintf(errstr, "Non-conformable matrices in horizontal concatenation (cbind). First argument has %d rows, and argument #%d has %d rows.", totalRows, j + 1, matList[j]->rows);
947                         omxRaiseError(result->currentState, -1, errstr);
948                         free(errstr);
949                         return;
950                 }
951                 totalCols += matList[j]->cols;
952         }
953
954         if(result->rows != totalRows || result->cols != totalCols) {
955                 if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: HorizCat: resizing result.");}
956                 omxResizeMatrix(result, totalRows, totalCols, FALSE);
957         }
958
959         int allArgumentsColMajor = result->colMajor;
960         for(int j = 0; j < numArgs && allArgumentsColMajor; j++) {
961                 if (!matList[j]->colMajor) allArgumentsColMajor = 0;
962         }
963
964         if (allArgumentsColMajor) {
965                 int offset = 0;
966                 for(int j = 0; j < numArgs; j++) {      
967                         omxMatrix* current = matList[j];
968                         int size = current->rows * current->cols;
969                         memcpy(result->data + offset, current->data, size * sizeof(double));
970                         offset += size;
971                 }
972         } else {
973                 for(int j = 0; j < numArgs; j++) {
974                         for(int k = 0; k < matList[j]->cols; k++) {
975                                 for(int l = 0; l < totalRows; l++) {            // Gotta be a faster way to do this.
976                                         omxSetMatrixElement(result, l, currentCol, omxMatrixElement(matList[j], l, k));
977                                 }
978                                 currentCol++;
979                         }
980                 }
981         }
982
983 }
984
985 void omxMatrixVertCat(omxMatrix** matList, int numArgs, omxMatrix* result) {
986
987         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Vertical Matrix Concatenation.");}
988
989         int totalRows = 0, totalCols = 0, currentRow=0;
990
991         if(numArgs == 0) return;
992
993         totalCols = matList[0]->cols;                   // Assumed constant.  Assert this below.
994
995         for(int j = 0; j < numArgs; j++) {
996                 if(totalCols != matList[j]->cols) {
997                         char *errstr = (char*) calloc(250, sizeof(char));
998                         sprintf(errstr, "Non-conformable matrices in vertical concatenation (rbind). First argument has %d cols, and argument #%d has %d cols.", totalCols, j + 1, matList[j]->cols);
999                         omxRaiseError(result->currentState, -1, errstr);
1000                         free(errstr);
1001                         return;
1002                 }
1003                 totalRows += matList[j]->rows;
1004         }
1005
1006         if(result->rows != totalRows || result->cols != totalCols) {
1007                 omxResizeMatrix(result, totalRows, totalCols, FALSE);
1008         }
1009
1010         int allArgumentsRowMajor = !result->colMajor;
1011         for(int j = 0; j < numArgs && allArgumentsRowMajor; j++) {
1012                 if (matList[j]->colMajor) allArgumentsRowMajor = 0;
1013         }
1014
1015         if (allArgumentsRowMajor) {
1016                 int offset = 0;
1017                 for(int j = 0; j < numArgs; j++) {      
1018                         omxMatrix* current = matList[j];
1019                         int size = current->rows * current->cols;       
1020                         memcpy(result->data + offset, current->data, size * sizeof(double));
1021                         offset += size;
1022                 }
1023         } else {
1024                 for(int j = 0; j < numArgs; j++) {
1025                         for(int k = 0; k < matList[j]->rows; k++) {
1026                                 for(int l = 0; l < totalCols; l++) {            // Gotta be a faster way to do this.
1027                                         omxSetMatrixElement(result, currentRow, l, omxMatrixElement(matList[j], k, l));
1028                                 }
1029                                 currentRow++;
1030                         }
1031                 }
1032         }
1033
1034 }
1035
1036 void omxMatrixDeterminant(omxMatrix** matList, int numArgs, omxMatrix* result)
1037 {
1038
1039         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Determinant.");}
1040
1041         omxMatrix* inMat = matList[0];
1042         omxMatrix* calcMat;                                     // This should be preallocated.
1043
1044         int rows = inMat->rows;
1045         int cols = inMat->cols;
1046         double det = 1;
1047         int info;
1048
1049         if(rows != cols) {
1050                 char *errstr = (char*) calloc(250, sizeof(char));
1051                 sprintf(errstr, "Determinant of non-square matrix cannot be found.\n");
1052                 omxRaiseError(result->currentState, -1, errstr);
1053                 free(errstr);
1054                 return;
1055         }
1056
1057         if(result->rows != 1 || result->cols != 1) {
1058                 omxResizeMatrix(result, 1, 1, FALSE);
1059         }
1060
1061         calcMat = omxInitTemporaryMatrix(NULL, rows, cols, TRUE, inMat->currentState);
1062         omxCopyMatrix(calcMat, inMat);
1063
1064         int* ipiv = (int*) calloc(inMat->rows, sizeof(int));
1065
1066         F77_CALL(dgetrf)(&(calcMat->rows), &(calcMat->cols), calcMat->data, &(calcMat->cols), ipiv, &info);
1067
1068         if(info != 0) {
1069                 char *errstr = (char*) calloc(250, sizeof(char));
1070                 sprintf(errstr, "Determinant Calculation: Nonsingular matrix (at row %d) on LUP decomposition.", info);
1071                 omxRaiseError(result->currentState, -1, errstr);
1072                 free(errstr);
1073                 free(ipiv);
1074                 omxFreeAllMatrixData(calcMat);
1075                 return;
1076         }
1077
1078         if(OMX_DEBUG_ALGEBRA) {
1079                 omxPrint(calcMat, "LU Decomp");
1080                 mxLog("info is %d.", info);
1081         }
1082
1083         for(int i = 0; i < rows; i++) {
1084                 det *= omxMatrixElement(calcMat, i, i);
1085                 if(ipiv[i] != (i+1)) det *= -1;
1086         }
1087
1088         if(OMX_DEBUG_ALGEBRA) {
1089                 mxLog("det is %d.", det);
1090         }
1091
1092         omxFreeAllMatrixData(calcMat);
1093
1094         omxSetMatrixElement(result, 0, 0, det);
1095
1096         free(ipiv);
1097 }
1098
1099 void omxMatrixTrace(omxMatrix** matList, int numArgs, omxMatrix* result)
1100 {
1101         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Trace.");}
1102
1103         /* Consistency check: */
1104         if(result->rows != numArgs && result->cols != numArgs) {
1105                 omxResizeMatrix(result, numArgs, 1, FALSE);
1106         }
1107
1108     for(int i = 0; i < numArgs; i++) {
1109         double trace = 0.0;
1110         omxMatrix* inMat = matList[i];
1111         double* values = inMat->data;
1112         int nrow  = inMat->rows;
1113         int ncol  = inMat->cols;
1114
1115         if(nrow != ncol) {
1116                 char *errstr = (char*) calloc(250, sizeof(char));
1117                 sprintf(errstr, "Non-square matrix in Trace().\n");
1118                 omxRaiseError(result->currentState, -1, errstr);
1119                 free(errstr);
1120             return;
1121         }
1122
1123         /* Note: This algorithm is numerically unstable.  Sorry, dudes. */
1124         for(int j = 0; j < nrow; j++)
1125            trace += values[j * nrow + j];
1126
1127         omxSetVectorElement(result, i, trace);
1128         }
1129 };
1130
1131 void omxMatrixTotalSum(omxMatrix** matList, int numArgs, omxMatrix* result) {
1132
1133         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Total Sum.");}
1134
1135         /* Consistency check: */
1136         if(result->rows != 1 || result->cols != 1) {
1137                 omxResizeMatrix(result, 1, 1, FALSE);
1138         }
1139
1140         double sum = 0.0;
1141
1142         /* Note: This algorithm is numerically unstable.  Sorry, dudes. */
1143         for(int j = 0; j < numArgs; j++) {
1144                 double* data = matList[j]->data;
1145                 int matlength = matList[j]->rows * matList[j]->cols;
1146                 for(int k = 0; k < matlength; k++) {
1147                         sum += data[k];
1148                 }
1149         }
1150
1151         omxSetMatrixElement(result, 0, 0, sum);
1152 }
1153
1154 void omxMatrixTotalProduct(omxMatrix** matList, int numArgs, omxMatrix* result) {
1155
1156         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Total Product.");}
1157
1158         /* Consistency check: */
1159         if(result->rows != 1 || result->cols != 1) {
1160                 omxResizeMatrix(result, 1, 1, FALSE);
1161         }
1162
1163         double product = 1.0;
1164
1165         /* Note: This algorithm is numerically unstable.  Sorry, dudes. */
1166         for(int j = 0; j < numArgs; j++) {
1167                 double* data = matList[j]->data;
1168                 int matlength = matList[j]->rows * matList[j]->cols;
1169                 for(int k = 0; k < matlength; k++) {
1170                         product *= data[k];
1171                 }
1172         }
1173
1174         omxSetMatrixElement(result, 0, 0, product);
1175 }
1176
1177 void omxMatrixArithmeticMean(omxMatrix** matList, int numArgs, omxMatrix* result) {
1178
1179         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Minimum Element.");}
1180         
1181         /* Consistency check: */
1182         if(result->rows != 1 || result->cols != 1) {
1183                 omxResizeMatrix(result, 1, 1, FALSE);
1184         }
1185
1186         omxMatrix *input = matList[0];
1187         int matLength = input->rows * input->cols;
1188         if (matLength == 0) return;
1189         double mean = omxVectorElement(input, 0);
1190         for(int i = 1; i < matLength; i++) {
1191                 double val = omxVectorElement(input, i);
1192                 mean += (val - mean) / (i + 1); 
1193         }
1194
1195         omxSetMatrixElement(result, 0, 0, mean);
1196 }
1197
1198 void omxMatrixMinimum(omxMatrix** matList, int numArgs, omxMatrix* result) {
1199
1200         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Minimum Element.");}
1201
1202         /* Consistency check: */
1203         if(result->rows != 1 || result->cols != 1) {
1204                 omxResizeMatrix(result, 1, 1, FALSE);
1205         }
1206
1207         double min = DBL_MAX; // DBL_MAX is the maximum possible DOUBLE value, usually 10e37.
1208                                                   // We could change this to use NPSOL's INFINITY, but why bother?
1209
1210         for(int j = 0; j < numArgs; j++) {
1211                 double* data = matList[j]->data;
1212                 int matlength = matList[j]->rows * matList[j]->cols;
1213                 for(int k = 0; k < matlength; k++) {
1214                         if(data[k] < min) min = data[k];
1215                 }
1216         }
1217
1218         omxSetMatrixElement(result, 0, 0, min);
1219 }
1220
1221 void omxMatrixMaximum(omxMatrix** matList, int numArgs, omxMatrix* result){
1222
1223         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Maximum Element.");}
1224
1225         /* Consistency check: */
1226         if(result->rows != 1 || result->cols != 1) {
1227                 omxResizeMatrix(result, 1, 1, FALSE);
1228         }
1229
1230         double max = -DBL_MAX;
1231
1232         for(int j = 0; j < numArgs; j++) {
1233                 double* data = matList[j]->data;
1234                 int matlength = matList[j]->rows * matList[j]->cols;
1235                 for(int k = 0; k < matlength; k++) {
1236                         if(data[k] > max) max = data[k];
1237                 }
1238         }
1239
1240         omxSetMatrixElement(result, 0, 0, max);
1241 }
1242
1243 void omxMatrixAbsolute(omxMatrix** matList, int numArgs, omxMatrix* result)
1244 {
1245
1246         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Absolute Value.");}
1247
1248         omxMatrix* inMat = matList[0];
1249
1250         int max = inMat->cols * inMat->rows;
1251
1252         omxCopyMatrix(result, inMat);
1253
1254         double* data = result->data;
1255         for(int j = 0; j < max; j++) {
1256                 data[j] = fabs(data[j]);
1257         }
1258
1259 }
1260
1261 void omxMatrixDiagonal(omxMatrix** matList, int numArgs, omxMatrix* result) {
1262
1263         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: diag2vec.");}
1264
1265         omxMatrix* inMat = matList[0];
1266         int diags = inMat->cols;
1267         if(inMat->cols > inMat->rows) {
1268                 diags = inMat->rows;
1269         }
1270
1271         if (result->cols != 1 || result->rows != diags) {
1272                 omxResizeMatrix(result, diags, 1, FALSE);
1273         }
1274
1275         for(int j = 0; j < diags; j++) {
1276                 omxSetMatrixElement(result, j, 0, omxMatrixElement(inMat, j, j));
1277         }
1278
1279 }
1280
1281 void omxMatrixFromDiagonal(omxMatrix** matList, int numArgs, omxMatrix* result) {
1282
1283         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: vec2diag.");}
1284
1285         omxMatrix* inMat = matList[0];
1286         int diags = inMat->cols;
1287
1288         if(inMat->cols < inMat->rows) {
1289                 diags = inMat->rows;
1290         }
1291
1292         if(inMat->cols != 1 && inMat->rows != 1) {
1293                 char *errstr = (char*) calloc(250, sizeof(char));
1294                 sprintf(errstr, "To generate a matrix from a diagonal that is not 1xN or Nx1.");
1295                 omxRaiseError(result->currentState, -1, errstr);
1296                 free(errstr);
1297                 return;
1298         }
1299
1300         if (result->cols != diags || result->rows != diags) {
1301                         omxResizeMatrix(result, diags, diags, FALSE);
1302         }
1303
1304         for(int j = 0; j < diags; j++) {
1305                 for(int k = 0; k < diags; k++) {
1306                         if(j == k) {
1307                                 omxSetMatrixElement(result, j, k, omxVectorElement(inMat, j));
1308                         } else {
1309                                 omxSetMatrixElement(result, j, k, 0);
1310                         }
1311                 }
1312         }
1313 }
1314
1315 void omxElementCosine(omxMatrix** matList, int numArgs, omxMatrix* result)
1316 {
1317
1318         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Element Cosine.");}
1319
1320         omxMatrix* inMat = matList[0];
1321
1322         int max = inMat->cols * inMat->rows;
1323
1324         omxCopyMatrix(result, inMat);
1325
1326         double* data = result->data;
1327         for(int j = 0; j < max; j++) {
1328                 data[j] = cos(data[j]);
1329         }
1330
1331 }
1332
1333 void omxElementCosh(omxMatrix** matList, int numArgs, omxMatrix* result)
1334 {
1335
1336         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Element Hyperbolic Cosine.");}
1337
1338         omxMatrix* inMat = matList[0];
1339
1340         int max = inMat->cols * inMat->rows;
1341
1342         omxCopyMatrix(result, inMat);
1343
1344         double* data = result->data;
1345         for(int j = 0; j < max; j++) {
1346                 data[j] = cosh(data[j]);
1347         }
1348
1349 }
1350
1351 void omxElementSine(omxMatrix** matList, int numArgs, omxMatrix* result)
1352 {
1353
1354         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Element Sine.");}
1355
1356         omxMatrix* inMat = matList[0];
1357
1358         int max = inMat->cols * inMat->rows;
1359
1360         omxCopyMatrix(result, inMat);
1361
1362         double* data = result->data;
1363         for(int j = 0; j < max; j++) {
1364                 data[j] = sin(data[j]);
1365         }
1366
1367 }
1368
1369 void omxElementSinh(omxMatrix** matList, int numArgs, omxMatrix* result)
1370 {
1371
1372         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Element Hyperbolic Sine.");}
1373
1374         omxMatrix* inMat = matList[0];
1375
1376         int max = inMat->cols * inMat->rows;
1377
1378         omxCopyMatrix(result, inMat);
1379
1380         double* data = result->data;
1381         for(int j = 0; j < max; j++) {
1382                 data[j] = sinh(data[j]);
1383         }
1384
1385 }
1386
1387 void omxElementTangent(omxMatrix** matList, int numArgs, omxMatrix* result)
1388 {
1389
1390         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Element Tangent.");}
1391
1392         omxMatrix* inMat = matList[0];
1393
1394         int max = inMat->cols * inMat->rows;
1395
1396         omxCopyMatrix(result, inMat);
1397
1398         double* data = result->data;
1399         for(int j = 0; j < max; j++) {
1400                 data[j] = tan(data[j]);
1401         }
1402
1403 }
1404
1405 void omxElementTanh(omxMatrix** matList, int numArgs, omxMatrix* result)
1406 {
1407
1408         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Element Hyperbolic Tangent.");}
1409
1410         omxMatrix* inMat = matList[0];
1411
1412         int max = inMat->cols * inMat->rows;
1413
1414         omxCopyMatrix(result, inMat);
1415
1416         double* data = result->data;
1417         for(int j = 0; j < max; j++) {
1418                 data[j] = tanh(data[j]);
1419         }
1420
1421 }
1422
1423 void omxElementExponent(omxMatrix** matList, int numArgs, omxMatrix* result)
1424 {
1425
1426         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Element Exponent.");}
1427
1428         omxMatrix* inMat = matList[0];
1429
1430         int max = inMat->cols * inMat->rows;
1431
1432         omxCopyMatrix(result, inMat);
1433
1434         double* data = result->data;
1435         for(int j = 0; j < max; j++) {
1436                 data[j] = exp(data[j]);
1437         }
1438
1439 }
1440
1441 void omxElementNaturalLog(omxMatrix** matList, int numArgs, omxMatrix* result)
1442 {
1443
1444         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Element Natural Log.");}
1445
1446         omxMatrix* inMat = matList[0];
1447
1448         int max = inMat->cols * inMat->rows;
1449
1450         omxCopyMatrix(result, inMat);
1451
1452         double* data = result->data;
1453         for(int j = 0; j < max; j++) {
1454                 data[j] = log(data[j]);
1455         }
1456
1457 }
1458
1459 void omxElementSquareRoot(omxMatrix** matList, int numArgs, omxMatrix* result)
1460 {
1461         omxMatrix *inMat = matList[0];
1462
1463         int max = inMat->cols * inMat->rows;
1464
1465         omxCopyMatrix(result, inMat);
1466
1467         double* data = result->data;
1468         for(int j = 0; j < max; j++) {
1469                 data[j] = sqrt(data[j]);
1470         }
1471 }
1472
1473 void omxMatrixVech(omxMatrix** matList, int numArgs, omxMatrix* result) {
1474         omxMatrix *inMat = matList[0];
1475
1476         int size;
1477         if (inMat->rows > inMat->cols) {
1478                 size = inMat->cols * (2 * inMat->rows - inMat->cols + 1) / 2;
1479         } else {
1480                 size = inMat->rows * (inMat->rows + 1) / 2;
1481         }
1482
1483         /* Consistency check: */
1484         if(result->rows != size || result->cols != 1) {
1485                 omxResizeMatrix(result, size, 1, FALSE);
1486         }
1487
1488         int counter = 0;
1489         for(int i = 0; i < inMat->cols; i++) {
1490                 for(int j = i; j < inMat->rows; j++) {
1491                         omxSetMatrixElement(result, counter, 0, omxMatrixElement(inMat, j, i));
1492                         counter++;
1493                 }
1494         }
1495
1496         if(counter != size) {
1497                 char *errstr = (char*) calloc(250, sizeof(char));
1498                 sprintf(errstr, "Internal error in vech().\n");
1499                 omxRaiseError(result->currentState, -1, errstr);
1500                 free(errstr);
1501         }
1502
1503 }
1504
1505 void omxMatrixVechs(omxMatrix** matList, int numArgs, omxMatrix* result) {
1506         omxMatrix *inMat = matList[0];
1507
1508         int size;
1509         if (inMat->rows > inMat->cols) {
1510                 size = inMat->cols * (2 * inMat->rows - inMat->cols + 1) / 2 - inMat->cols;
1511         } else {
1512                 size = inMat->rows * (inMat->rows + 1) / 2 - inMat->rows;
1513         }
1514
1515         /* Consistency check: */
1516         if(result->rows != size || result->cols != 1) {
1517                 omxResizeMatrix(result, size, 1, FALSE);
1518         }
1519
1520         int counter = 0;
1521         for(int i = 0; i < inMat->cols; i++) {
1522                 for(int j = i + 1; j < inMat->rows; j++) {
1523                         omxSetMatrixElement(result, counter, 0, omxMatrixElement(inMat, j, i));
1524                         counter++;
1525                 }
1526         }
1527
1528         if(counter != size) {
1529                 char *errstr = (char*) calloc(250, sizeof(char));
1530                 sprintf(errstr, "Internal error in vechs().\n");
1531                 omxRaiseError(result->currentState, -1, errstr);
1532                 free(errstr);
1533         }
1534
1535 }
1536
1537 void omxRowVectorize(omxMatrix** matList, int numArgs, omxMatrix* result) {
1538         if(OMX_DEBUG_ALGEBRA) {
1539                 mxLog("Row Vectorize %s.", result->name);
1540         }
1541
1542         omxMatrix *inMat = matList[0];
1543
1544         int size = (inMat->rows * inMat->cols);
1545
1546         /* Consistency Check */
1547         if(result->rows != size || result->cols != 1)
1548                 omxResizeMatrix(result, size, 1, FALSE);
1549
1550         if(!inMat->colMajor) {          // Special case: we can just memcpy.
1551                 memcpy(result->data, inMat->data, size*sizeof(double));
1552         } else {
1553                 int next = 0;
1554                 for(int i = 0; i < inMat->rows; i++) {
1555                         for(int j = 0; j < inMat->cols; j++) {
1556                                 omxSetMatrixElement(result, next++, 0, omxMatrixElement(inMat, i, j));
1557                         }
1558                 }
1559         }
1560 }
1561
1562 void omxColVectorize(omxMatrix** matList, int numArgs, omxMatrix* result) {
1563         if(OMX_DEBUG_ALGEBRA) {
1564                  mxLog("Column Vectorize %s.", result->name);
1565         }
1566
1567         omxMatrix *inMat = matList[0];
1568
1569         int size = (inMat->rows * inMat->cols);
1570
1571         /* Consistency Check */
1572         if(result->rows != size || result->cols != 1)
1573                 omxResizeMatrix(result, size, 1, FALSE);
1574         if(inMat->colMajor) {           // Special case: we can just memcpy.
1575                 memcpy(result->data, inMat->data, size * sizeof(double));
1576         } else {
1577                 int next = 0;
1578                 for(int i = 0; i < inMat->cols; i++) {
1579                         for(int j = 0; j < inMat->rows; j++) {
1580                                 omxSetMatrixElement(result, next++, 0, omxMatrixElement(inMat, j, i));
1581                         }
1582                 }
1583         }
1584 }
1585
1586
1587 void omxSequenceGenerator(omxMatrix** matList, int numArgs, omxMatrix* result) {
1588
1589         double start = omxVectorElement(matList[0], 0);
1590         double stop = omxVectorElement(matList[1], 0);
1591
1592         if (!R_finite(start)) {
1593                 char *errstr = (char*) calloc(250, sizeof(char));
1594                 sprintf(errstr, "Non-finite start value in ':' operator.\n");
1595                 omxRaiseError(result->currentState, -1, errstr);
1596                 free(errstr);
1597                 return;
1598         }
1599
1600         if (!R_finite(stop)) {
1601                 char *errstr = (char*) calloc(250, sizeof(char));
1602                 sprintf(errstr, "Non-finite stop value in ':' operator.\n");
1603                 omxRaiseError(result->currentState, -1, errstr);
1604                 free(errstr);
1605                 return;
1606         }
1607
1608         double difference = stop - start;
1609         if (difference < 0) difference = - difference;
1610
1611         int size = ((int) difference) + 1;
1612
1613         /* Consistency check: */
1614         if(result->rows != size || result->cols != 1) {
1615                 omxResizeMatrix(result, size, 1, FALSE);
1616         }
1617
1618         /* Sanity-checking.  This loop can be eliminated */
1619         for(int i = 0; i < size; i++) {
1620                 omxSetVectorElement(result, i, 0);
1621         }
1622
1623         int count = 0;
1624         if ((stop - start) >= 0) {
1625                 while (start <= stop) {
1626                         omxSetVectorElement(result, count, start);
1627                         start = start + 1.0;
1628                         count++;
1629                 }
1630         } else {
1631                 while (start >= stop) {
1632                         omxSetVectorElement(result, count, start);
1633                         start = start - 1.0;
1634                         count++;
1635                 }
1636         }
1637 }
1638
1639 void omxMultivariateNormalIntegration(omxMatrix** matList, int numArgs, omxMatrix* result) {
1640
1641         omxMatrix* cov = matList[0];
1642         omxMatrix* means = matList[1];
1643         omxMatrix* lBoundMat = matList[2];
1644         omxMatrix* uBoundMat = matList[3];
1645
1646         /* Conformance checks: */
1647         if (result->rows != 1 || result->cols != 1) omxResizeMatrix(result, 1, 1, FALSE);
1648
1649         if (cov->rows != cov->cols) {
1650                 char *errstr = (char*) calloc(250, sizeof(char));
1651                 sprintf(errstr, "covariance is not a square matrix");
1652                 omxRaiseError(result->currentState, -1, errstr);
1653                 free(errstr);
1654                 return;
1655         }
1656
1657         if (means->rows > 1 && means->cols > 1) {
1658                 char *errstr = (char*) calloc(250, sizeof(char));
1659                 sprintf(errstr, "means is neither row nor column vector");
1660                 omxRaiseError(result->currentState, -1, errstr);
1661                 free(errstr);
1662                 return;
1663         }
1664
1665         if (lBoundMat->rows > 1 && lBoundMat->cols > 1) {
1666                 char *errstr = (char*) calloc(250, sizeof(char));
1667                 sprintf(errstr, "lbound is neither row nor column vector");
1668                 omxRaiseError(result->currentState, -1, errstr);
1669                 free(errstr);
1670                 return;
1671         }
1672
1673         if (uBoundMat->rows > 1 && uBoundMat->cols > 1) {
1674                 char *errstr = (char*) calloc(250, sizeof(char));
1675                 sprintf(errstr, "ubound is neither row nor column vector");
1676                 omxRaiseError(result->currentState, -1, errstr);
1677                 free(errstr);
1678                 return;
1679         }
1680
1681         int nElements = (cov->cols > 1) ? cov->cols : cov->rows;
1682         double *lBounds, *uBounds;
1683         double *weights;
1684         double *corList;
1685         lBounds = (double*) malloc(nElements * sizeof(double));
1686         uBounds = (double*) malloc(nElements * sizeof(double));
1687         weights = (double*) malloc(nElements * sizeof(double));
1688         corList = (double*) malloc((nElements * (nElements + 1) / 2) * sizeof(double));
1689
1690         omxStandardizeCovMatrix(cov, corList, weights);
1691
1692         // SADMVN calls Alan Genz's sadmvn.f--see appropriate file for licensing info.
1693         // TODO: Check with Genz: should we be using sadmvn or sadmvn?
1694         // Parameters are:
1695         //      N               int                     # of vars
1696         //      Lower   double*         Array of lower bounds
1697         //      Upper   double*         Array of upper bounds
1698         //      Infin   int*            Array of flags: <0 = (-Inf, Inf) 0 = (-Inf, upper] 1 = [lower, Inf), 2 = [lower, upper]
1699         //      Correl  double*         Array of correlation coeffs: in row-major lower triangular order
1700         //      MaxPts  int                     Maximum # of function values (use 1000*N or 1000*N*N)
1701         //      Abseps  double          Absolute error tolerance.  Yick.
1702         //      Releps  double          Relative error tolerance.  Use EPSILON.
1703         //      Error   &double         On return: absolute real error, 99% confidence
1704         //      Value   &double         On return: evaluated value
1705         //      Inform  &int            On return: 0 = OK; 1 = Rerun, increase MaxPts; 2 = Bad input
1706         // TODO: Separate block diagonal covariance matrices into pieces for integration separately
1707         double Error;
1708         double absEps = 1e-3;
1709         double relEps = 0;
1710         int MaxPts = OMX_DEFAULT_MAX_PTS(cov->rows);
1711         double likelihood;
1712         int inform;
1713         int numVars = cov->rows;
1714         int Infin[cov->rows];
1715         int fortranThreadId = omx_absolute_thread_num() + 1;
1716
1717         for(int i = 0; i < nElements; i++) {
1718                 lBounds[i] = (omxVectorElement(lBoundMat, i) - omxVectorElement(means, i))/weights[i];
1719                 uBounds[i] = (omxVectorElement(uBoundMat, i) - omxVectorElement(means, i))/weights[i];
1720                 Infin[i] = 2; // Default to both thresholds
1721                 if(uBounds[i] <= lBounds[i]) {
1722                         char *errstr = (char*) calloc(250, sizeof(char));
1723                         sprintf(errstr, "Thresholds are not strictly increasing: %3.3f >= %3.3f.", lBounds[i], uBounds[i]);
1724                         omxRaiseError(result->currentState, -1, errstr);
1725                         free(errstr);
1726                         free(corList);
1727                         free(weights);
1728                         free(uBounds);
1729                         free(lBounds);
1730                         return;
1731                 }
1732                 if(!R_finite(lBounds[i]) ) {
1733                         Infin[i] -= 2;  // NA or INF or -INF means no lower threshold.
1734                 } else {
1735
1736                 }
1737                 if(!R_finite(uBounds[i]) ) {
1738                         Infin[i] -= 1; // NA or INF or -INF means no upper threshold.
1739                 }
1740
1741         }
1742
1743
1744         F77_CALL(sadmvn)(&numVars, &(lBounds[0]), &(*uBounds), Infin, corList, 
1745                 &MaxPts, &absEps, &relEps, &Error, &likelihood, &inform, &fortranThreadId);
1746
1747         if(OMX_DEBUG_ALGEBRA) { mxLog("Output of sadmvn is %f, %f, %d.", Error, likelihood, inform); }
1748
1749         if(inform == 2) {
1750                 char *errstr = (char*) calloc(250, sizeof(char));
1751                 sprintf(errstr, "Improper input to sadmvn.");
1752                 omxRaiseError(result->currentState, -1, errstr);
1753                 free(errstr);
1754                 free(corList);
1755                 free(weights);
1756                 free(uBounds);
1757                 free(lBounds);
1758                 return;
1759         }
1760
1761         free(corList);
1762         free(weights);
1763         free(uBounds);
1764         free(lBounds);
1765
1766         omxSetMatrixElement(result, 0, 0, likelihood);
1767
1768 }
1769
1770 void omxAllIntegrationNorms(omxMatrix** matList, int numArgs, omxMatrix* result) {
1771
1772         if(OMX_DEBUG_ALGEBRA) {
1773                 mxLog("All-part multivariate normal integration.");
1774         }
1775
1776         omxMatrix* cov = matList[0];
1777         omxMatrix* means = matList[1];
1778         int nCols = cov->cols;
1779         int i,j,k;
1780
1781         int totalLevels = 1;
1782         omxMatrix **thresholdMats = (omxMatrix **) malloc(nCols * sizeof(omxMatrix*));
1783         int *numThresholds = (int*) malloc(nCols * sizeof(int));
1784         int *matNums = (int*) malloc(nCols * sizeof(int));
1785         int *thresholdCols = (int*) malloc(nCols * sizeof(int));
1786         int *currentThresholds = (int*) malloc(nCols * sizeof(int));
1787
1788         int currentMat = 0;
1789
1790         for(i = currentMat; i < nCols;) {                                                       // Map out the structure of levels.
1791         if(OMX_DEBUG_ALGEBRA) {
1792                 mxLog("All-part multivariate normal integration: Examining threshold column %d.", i);
1793         }
1794                 thresholdMats[currentMat] = matList[currentMat+2];              // Get the thresholds for this covariance column
1795
1796                 for(j = 0; j < thresholdMats[currentMat]->cols; j++) {  // We walk along the columns of this threshold matrix
1797                         double ubound, lbound = omxMatrixElement(thresholdMats[currentMat], 0, j);
1798                         if(ISNA(lbound)) {
1799                                 char *errstr = (char*) calloc(250, sizeof(char));
1800                                 sprintf(errstr, "Invalid lowest threshold for dimension %d of Allint.", j);
1801                                 omxRaiseError(result->currentState, -1, errstr);
1802                                 free(errstr);
1803                                 return;
1804                         }
1805
1806                         thresholdCols[i] = j;
1807
1808                         for(k = 1; k < thresholdMats[currentMat]->rows; k++) {
1809                                 ubound = omxMatrixElement(thresholdMats[currentMat], k, j);
1810                                 if(ISNA(ubound)) {
1811                                         numThresholds[i] = k-1;
1812                                         totalLevels *= numThresholds[i];
1813                                         break;
1814                                 }
1815
1816                                 if(!(ubound > lbound)) {
1817                                         char *errstr = (char*) calloc(250, sizeof(char));
1818                                         sprintf(errstr, "Thresholds (%f and %f) are not strictly increasing for dimension %d of Allint.", lbound, ubound, j+1);
1819                                         omxRaiseError(result->currentState, -1, errstr);
1820                                         free(errstr);
1821                                         return;
1822                                 }
1823
1824                                 if(!R_finite(ubound)) {                                 // Infinite bounds must be last.
1825                                         numThresholds[i] = k;
1826                                         totalLevels *= numThresholds[i];
1827                                         break;
1828                                 }
1829
1830                                 if(k == (thresholdMats[currentMat]->rows -1)) { // In case the highest threshold isn't Infinity
1831                                         numThresholds[i] = k;
1832                                         totalLevels *= numThresholds[i];
1833                                 }
1834                         }
1835                         currentThresholds[i] = 1;
1836                         matNums[i] = currentMat;
1837                         if(++i >= nCols) {                                                      // We have all we need
1838                                 break;
1839                         }
1840                 }
1841                 currentMat++;
1842         }
1843
1844         /* Conformance checks: */
1845         if(result->rows != totalLevels || result->cols != 1) omxResizeMatrix(result, totalLevels, 1, FALSE);
1846
1847         double *weights = (double*) malloc(nCols * sizeof(double));
1848         double *corList = (double*) malloc((nCols * (nCols + 1) / 2) * sizeof(double));
1849
1850         omxStandardizeCovMatrix(cov, &(*corList), &(*weights));
1851
1852         // SADMVN calls Alan Genz's sadmvn.f--see appropriate file for licensing info.
1853         // TODO: Check with Genz: should we be using sadmvn or sadmvn?
1854         // Parameters are:
1855         //      N               int                     # of vars
1856         //      Lower   double*         Array of lower bounds
1857         //      Upper   double*         Array of upper bounds
1858         //      Infin   int*            Array of flags: <0 = (-Inf, Inf) 0 = (-Inf, upper] 1 = [lower, Inf), 2 = [lower, upper]
1859         //      Correl  double*         Array of correlation coeffs: in row-major lower triangular order
1860         //      MaxPts  int                     Maximum # of function values (use 1000*N or 1000*N*N)
1861         //      Abseps  double          Absolute error tolerance.  Yick.
1862         //      Releps  double          Relative error tolerance.  Use EPSILON.
1863         //      Error   &double         On return: absolute real error, 99% confidence
1864         //      Value   &double         On return: evaluated value
1865         //      Inform  &int            On return: 0 = OK; 1 = Rerun, increase MaxPts; 2 = Bad input
1866         // TODO: Separate block diagonal covariance matrices into pieces for integration separately
1867         double Error;
1868         double absEps = 1e-3;
1869         double relEps = 0;
1870         int MaxPts = OMX_DEFAULT_MAX_PTS(cov->rows);
1871         double likelihood;
1872         int inform;
1873         int numVars = nCols;
1874         int* Infin = (int*) malloc(nCols * sizeof(int));
1875         double* lBounds = (double*) malloc(nCols * sizeof(double));
1876         double* uBounds = (double*) malloc(nCols * sizeof(double));
1877         int fortranThreadId = omx_absolute_thread_num() + 1;
1878
1879         /* Set up first row */
1880         for(j = (nCols-1); j >= 0; j--) {                                       // For each threshold set, starting from the fastest
1881
1882                 Infin[j] = 2;                                                                   // Default to using both thresholds
1883                 lBounds[j] = (omxMatrixElement(thresholdMats[matNums[j]], currentThresholds[j]-1, thresholdCols[j]) - omxVectorElement(means, j))/weights[j];
1884                 if(!R_finite(lBounds[j])) {                                     // Inifinite lower bounds = -Inf to ?
1885                                 Infin[j] -= 2;
1886                 }
1887
1888                 uBounds[j] = (omxMatrixElement(thresholdMats[matNums[j]], currentThresholds[j], thresholdCols[j]) - omxVectorElement(means, j))/weights[j];
1889
1890                 if(!R_finite(uBounds[j])) {                                     // Inifinite lower bounds = -Inf to ?
1891                                 Infin[j] -= 1;
1892                 }
1893
1894                 if(Infin[j] < 0) { Infin[j] = 3; }                      // Both bounds infinite.
1895         }
1896
1897         F77_CALL(sadmvn)(&numVars, &(lBounds[0]), &(*uBounds), Infin, corList, 
1898                 &MaxPts, &absEps, &relEps, &Error, &likelihood, &inform, &fortranThreadId);
1899
1900         if(OMX_DEBUG_ALGEBRA) { mxLog("Output of sadmvn is %f, %f, %d.", Error, likelihood, inform); }
1901
1902         if(inform == 2) {
1903                 char *errstr = (char*) calloc(250, sizeof(char));
1904                 sprintf(errstr, "Improper input to sadmvn.");
1905                 omxRaiseError(result->currentState, -1, errstr);
1906                 free(errstr);
1907                 goto AllIntCleanup;
1908         }
1909
1910         omxSetMatrixElement(result, 0, 0, likelihood);
1911
1912
1913         /* And repeat with increments for all other rows. */
1914         for(i = 1; i < totalLevels; i++) {
1915                 for(j = (nCols-1); j >= 0; j--) {                                                       // For each threshold set, starting from the fastest
1916                         currentThresholds[j]++;                                                                 // Move to the next threshold set.
1917                         if(currentThresholds[j] > numThresholds[j]) {                   // Hit the end; cycle to the next.
1918                                 currentThresholds[j] = 1;
1919                         }
1920
1921                         /* Update only the rows that need it. */
1922                         Infin[j] = 2; // Default to both thresholds
1923                         lBounds[j] = (omxMatrixElement(thresholdMats[matNums[j]], currentThresholds[j]-1, thresholdCols[j]) - omxVectorElement(means, j))/weights[j];
1924                         if(!R_finite(lBounds[j])) {                                                             // Inifinite lower bounds = -Inf to ?
1925                                 Infin[j] -= 2;
1926                         }
1927                         uBounds[j] = (omxMatrixElement(thresholdMats[matNums[j]], currentThresholds[j], thresholdCols[j]) - omxVectorElement(means, j))/weights[j];
1928
1929                         if(!R_finite(uBounds[j])) {                                                     // Inifinite lower bounds = -Inf to ?
1930                                 Infin[j] -= 1;
1931                         }
1932
1933                         if(Infin[j] < 0) { Infin[j] = 3; }                                              // Both bounds infinite.
1934
1935                         if(currentThresholds[j] != 1) {                                                 // If we just cycled, we need to see the next set.
1936                                 break;
1937                         }
1938
1939                 }
1940
1941                 F77_CALL(sadmvn)(&numVars, &(lBounds[0]), &(*uBounds), Infin, corList,
1942                         &MaxPts, &absEps, &relEps, &Error, &likelihood, &inform, &fortranThreadId);
1943
1944                 if(OMX_DEBUG_ALGEBRA) { mxLog("Output of sadmvn is %f, %f, %d.", Error, likelihood, inform); }
1945
1946                 if(inform == 2) {
1947                         char *errstr = (char*) calloc(250, sizeof(char));
1948                         sprintf(errstr, "Improper input to sadmvn.");
1949                         omxRaiseError(result->currentState, -1, errstr);
1950                         free(errstr);
1951                         goto AllIntCleanup;
1952                 }
1953
1954                 omxSetMatrixElement(result, i, 0, likelihood);
1955         }
1956
1957 AllIntCleanup:
1958         free(Infin);
1959         free(lBounds);
1960         free(uBounds);
1961         free(weights);
1962         free(corList);
1963         free(thresholdMats);
1964         free(numThresholds);
1965         free(matNums);
1966         free(thresholdCols);
1967         free(currentThresholds);
1968 }
1969
1970 int omxCompareDoubleHelper(const void* one, const void* two, void *ign) {
1971         double diff = *(double*) two - *(double*) one;
1972         if(diff > EPSILON) {
1973                 return 1;
1974         } else if(diff < -EPSILON) {
1975                 return -1;
1976         } else return 0;
1977 }
1978
1979
1980 int omxComparePointerContentsHelper(const void* one, const void* two, void *ign) {
1981         double diff = (*(*(double**) two)) - (*(*(double**) one));
1982         if(diff > EPSILON) {
1983                 return 1;
1984         } else if(diff < -EPSILON) {
1985                 return -1;
1986         } else return 0;
1987 }
1988
1989 void omxSortHelper(double* sortOrder, omxMatrix* original, omxMatrix* result) {
1990         /* Sorts the columns of a matrix or the rows of a column vector
1991                                         in decreasing order of the elements of sortOrder. */
1992
1993         if(OMX_DEBUG) {mxLog("SortHelper:Original is (%d x %d), result is (%d x %d).", original->rows, original->cols, result->rows, result->cols);}
1994
1995         if(!result->colMajor || !original->colMajor
1996                 || result->cols != original->cols || result->rows != original->rows) {
1997                 char *errstr = (char*) calloc(250, sizeof(char));
1998                 sprintf(errstr, "Incorrect input to omxRowSortHelper: %d %d %d %d", result->cols, original->cols, result->rows, original->rows);
1999                 omxRaiseError(result->currentState, -1, errstr);
2000                 free(errstr);
2001                 return;
2002         }
2003
2004         double* sortArray[original->rows];
2005         int numElements = original->cols;
2006         int numRows = original->rows;
2007
2008         if(numElements == 1)  numElements = numRows;            // Special case for column vectors
2009
2010         for(int i = 0; i < numElements; i++) {
2011                 sortArray[i] = sortOrder + i;
2012         }
2013
2014         freebsd_mergesort(sortArray, numElements, sizeof(double*), omxComparePointerContentsHelper, NULL);
2015
2016         if(OMX_DEBUG) {mxLog("Original is (%d x %d), result is (%d x %d).", original->rows, original->cols, result->rows, result->cols);}
2017
2018
2019         for(int i = 0; i < numElements; i++) {
2020                 if(original->cols == 1) {
2021                         omxSetMatrixElement(result, i, 0, omxMatrixElement(original, (sortArray[i] - sortOrder), 0));
2022                 } else {
2023                         memcpy(omxLocationOfMatrixElement(result, 0, i), omxLocationOfMatrixElement(original, 0, sortArray[i]-sortOrder), numRows * sizeof(double));
2024                 }
2025         }
2026
2027         return;
2028 }
2029
2030 void omxRealEigenvalues(omxMatrix** matList, int numArgs, omxMatrix* result) {
2031
2032
2033         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Eigenvalues.");}
2034
2035         omxMatrix* A = omxInitMatrix(NULL, 0, 0, TRUE, result->currentState);
2036         omxMatrix* B = omxInitMatrix(NULL, 0, 0, TRUE, result->currentState);
2037         omxCopyMatrix(B, matList[0]);
2038         omxResizeMatrix(A, B->rows, 1, FALSE);
2039
2040         /* Conformability Check! */
2041         if(B->cols != B->rows) {
2042                 char *errstr = (char*) calloc(250, sizeof(char));
2043                 sprintf(errstr, "Non-square matrix in eigenvalue decomposition.\n");
2044                 omxRaiseError(B->currentState, -1, errstr);
2045                 free(errstr);
2046                 omxFreeMatrixData(A);
2047                 omxFreeMatrixData(B);
2048                 return;
2049         }
2050
2051         if(result->rows != B->rows || result->cols != 1)
2052                 omxResizeMatrix(result, B->rows, 1, FALSE);
2053
2054         char N = 'N';                                           // Indicators for BLAS
2055         // char V = 'V';                                                // Indicators for BLAS
2056
2057         int One = 1;
2058         int lwork = 10*B->rows;
2059
2060         int info;
2061
2062         double* work = (double*) malloc(lwork * sizeof(double));
2063         double* WI = (double*) malloc(B->cols * sizeof(double));
2064
2065         /* For debugging */
2066         if(OMX_DEBUG_ALGEBRA) {
2067                 omxPrint(result, "NewMatrix");
2068                 mxLog("DGEEV: %c, %c, %d, %x, %d, %x, %x, %x, %d, %x, %d, %x, %d, %d", N, N, (result->rows), result->data, (result->leading), A->data, WI, NULL, One, NULL, One, work, &lwork, &info);
2069         }
2070
2071         /* The call itself */
2072         F77_CALL(dgeev)(&N, &N, &(B->rows), B->data, &(B->leading), A->data, WI, NULL, &One, NULL, &One, work, &lwork, &info);
2073         if(info != 0) {
2074                 char *errstr = (char*) calloc(250, sizeof(char));
2075                 sprintf(errstr, "DGEEV returned %d in (real) eigenvalue decomposition:", info);
2076                 if(info > 0)
2077                         sprintf(errstr, "%s argument %d had an illegal value.  Post this to the OpenMx wiki.\n", errstr, info);
2078                 else
2079                         sprintf(errstr, "%s Unable to decompose matrix: Not of full rank.\n", errstr);
2080                 omxRaiseError(result->currentState, -1, errstr);
2081                 free(errstr);
2082                 goto RealEigenValCleanup;
2083         }
2084
2085         result->colMajor = TRUE;
2086
2087         // Calculate Eigenvalue modulus.
2088         for(int i = 0; i < A->rows; i++) {
2089                 double value = omxMatrixElement(A, i, 0);
2090                 if(WI[i] != 0) {                                // FIXME: Might need to be abs(WI[i] > EPSILON)
2091                         value = sqrt(WI[i]*WI[i] + value*value);                                // Sort by eigenvalue modulus
2092                         WI[i] = value;
2093                         WI[++i] = value;                                                                                // Conjugate pair.
2094                 } else {
2095                         WI[i] = fabs(value);                                                                    // Modulus of a real is its absolute value
2096                 }
2097         }
2098
2099         omxSortHelper(WI, A, result);
2100
2101 RealEigenValCleanup:
2102         omxFreeMatrixData(A);                           // FIXME: State-keeping for algebras would save significant time in memory allocation/deallocation
2103         omxFreeMatrixData(B);
2104         omxMatrixLeadingLagging(result);
2105
2106         free(work);
2107         free(WI);
2108 }
2109
2110 void omxRealEigenvectors(omxMatrix** matList, int numArgs, omxMatrix* result) {
2111         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Eigenvalues.");}
2112
2113         omxMatrix* A = omxInitMatrix(NULL, 0, 0, TRUE, result->currentState);
2114         omxCopyMatrix(result, matList[0]);
2115         omxResizeMatrix(A, result->rows, result->cols, FALSE);
2116
2117
2118         if(A == NULL) {
2119                 char *errstr = (char*) calloc(250, sizeof(char));
2120                 sprintf(errstr, "Null matrix pointer detected.\n");
2121                 omxRaiseError(result->currentState, -1, errstr);
2122                 free(errstr);
2123                 return;
2124         }
2125
2126         /* Conformability Check! */
2127         if(A->cols != A->rows) {
2128                 char *errstr = (char*) calloc(250, sizeof(char));
2129                 sprintf(errstr, "Non-square matrix in (real) eigenvalue decomposition.\n");
2130                 omxRaiseError(result->currentState, -1, errstr);
2131                 free(errstr);
2132                 omxFreeMatrixData(A);
2133                 return;
2134         }
2135
2136         char N = 'N';                                           // Indicators for BLAS
2137         char V = 'V';                                           // Indicators for BLAS
2138
2139         int One = 1;
2140         int lwork = 10*A->rows;
2141
2142         int info;
2143
2144         double *WR = (double*) malloc(A->cols * sizeof(double));
2145         double *WI = (double*) malloc(A->cols * sizeof(double));
2146         double *work = (double*) malloc(lwork * sizeof(double));
2147
2148         /* For debugging */
2149         if(OMX_DEBUG_ALGEBRA) {
2150                 omxPrint(result, "NewMatrix");
2151                 mxLog("DGEEV: %c, %c, %d, %x, %d, %x, %x, %x, %x, %d, %x, %d, %d, %d", N, V, (result->rows), result->data, (result->leading), WR, WI, NULL, A->data, &(A->leading), work, lwork, info);
2152         }
2153
2154         /* The call itself */
2155         F77_CALL(dgeev)(&N, &V, &(result->rows), result->data, &(result->leading), WR, WI, NULL, &One, A->data, &(A->leading), work, &lwork, &info);
2156         if(info != 0) {
2157                 char *errstr = (char*) calloc(250, sizeof(char));
2158                 sprintf(errstr, "DGEEV returned %d in eigenvalue decomposition:", info);
2159                 if(info > 0)
2160                         sprintf(errstr, "%s argument %d had an illegal value.  Post this to the OpenMx wiki.\n", errstr, info);
2161                 else
2162                         sprintf(errstr, "%s Unable to decompose matrix: Not of full rank.\n", errstr);
2163                 omxRaiseError(result->currentState, -1, errstr);
2164                 free(errstr);
2165                 goto RealEigenVecCleanup;
2166         }
2167
2168         // Filter real and imaginary eigenvectors.  Real ones have no WI.
2169         for(int i = 0; i < A->cols; i++) {
2170                 if(fabs(WI[i]) > EPSILON) {                                                                     // If this is part of a conjugate pair
2171                         memcpy(omxLocationOfMatrixElement(A, 0, i+1), omxLocationOfMatrixElement(A, 0, i), A->rows * sizeof(double));
2172                                 // ^^ This is column-major, so we can clobber columns over one another.
2173                         WR[i] = sqrt(WR[i] *WR[i] + WI[i]*WI[i]);                               // Sort by eigenvalue modulus
2174                         WR[i+1] = WR[i];                                                                                // Identical--conjugate pair
2175                         i++;    // Skip the next one; we know it's the conjugate pair.
2176                 } else {
2177                         WR[i] = fabs(WR[i]);                                                                    // Modulus of a real is its absolute value
2178                 }
2179         }
2180
2181         result->colMajor = TRUE;
2182
2183         // Sort results
2184         omxSortHelper(WR, A, result);
2185
2186 RealEigenVecCleanup:
2187         omxFreeMatrixData(A);           // FIXME: State-keeping for algebras would save significant time in memory allocation/deallocation
2188         omxMatrixLeadingLagging(result);
2189
2190         free(WR);
2191         free(WI);
2192         free(work);     
2193 }
2194
2195 void omxImaginaryEigenvalues(omxMatrix** matList, int numArgs, omxMatrix* result) {
2196
2197         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Eigenvalues.");}
2198
2199         omxMatrix* A = omxInitMatrix(NULL, 0, 0, TRUE, result->currentState);
2200         omxMatrix* B = omxInitMatrix(NULL, 0, 0, TRUE, result->currentState);
2201         omxCopyMatrix(B, matList[0]);
2202         omxResizeMatrix(A, B->rows, 1, FALSE);
2203
2204         /* Conformability Check! */
2205         if(B->cols != B->rows) {
2206                 char *errstr = (char*) calloc(250, sizeof(char));
2207                 sprintf(errstr, "Non-square matrix in eigenvalue decomposition.\n");
2208                 omxRaiseError(result->currentState, -1, errstr);
2209                 free(errstr);
2210                 omxFreeMatrixData(A);
2211                 omxFreeMatrixData(B);
2212                 return;
2213         }
2214
2215         if(result->cols != 1 || result->rows != A->rows)
2216                 omxResizeMatrix(result, B->rows, 1, FALSE);
2217
2218         char N = 'N';                                           // Indicators for BLAS
2219         char V = 'V';                                           // Indicators for BLAS
2220
2221         int One = 1;
2222         int lwork = 10*B->rows;
2223
2224         int info;
2225
2226         double *WR = (double*) malloc(B->cols * sizeof(double));
2227         double *VR = (double*) malloc(B->rows * B->cols * sizeof(double));
2228         double *work = (double*) malloc(lwork * sizeof(double));
2229
2230         /* For debugging */
2231         if(OMX_DEBUG_ALGEBRA) {
2232                 omxPrint(result, "NewMatrix");
2233                 mxLog("DGEEV: %c, %c, %d, %x, %d, %x, %x, %x, %d, %x, %d, %x, %d, %d", N, V, B->data, (B->rows), A->data, WR, NULL, One, VR, (A->rows), work, lwork, info);
2234         }
2235
2236         /* The call itself */
2237         F77_CALL(dgeev)(&N, &N, &(B->rows), B->data, &(B->leading), WR, A->data, NULL, &One, NULL, &One, work, &lwork, &info);
2238         if(info != 0) {
2239                 char *errstr = (char*) calloc(250, sizeof(char));
2240                 sprintf(errstr, "DGEEV returned %d in (real) eigenvalue decomposition:", info);
2241                 if(info > 0)
2242                         sprintf(errstr, "%s argument %d had an illegal value.  Post this to the OpenMx wiki.\n", errstr, info);
2243                 else
2244                         sprintf(errstr, "%s Unable to decompose matrix: Not of full rank.\n", errstr);
2245                 omxRaiseError(result->currentState, -1, errstr);
2246                 free(errstr);
2247                 goto ImagEigenValCleanup;
2248         }
2249
2250         // Calculate Eigenvalue modulus.
2251         for(int i = 0; i < result->rows; i++) {
2252                 double value = omxMatrixElement(A, i, 0);                                       // A[i] is the ith imaginary eigenvalue
2253                 value *= value;                                                                                         // Squared imaginary part
2254                 if(value > EPSILON) {
2255                         value = sqrt(WR[i] *WR[i] + value);                             // Sort by eigenvalue modulus
2256                         WR[i] = value;
2257                         WR[++i] = value;                                                                                // Conjugate pair.
2258                 } else {
2259                         WR[i] = fabs(WR[i]);
2260                 }
2261         }
2262
2263         result->colMajor = TRUE;
2264
2265         // Sort results
2266         omxSortHelper(WR, A, result);
2267
2268 ImagEigenValCleanup:
2269         omxFreeMatrixData(A);           // FIXME: State-keeping for algebras would save significant time in memory allocation/deallocation
2270         omxFreeMatrixData(B);
2271         omxMatrixLeadingLagging(result);
2272
2273         free(WR);
2274         free(VR);
2275         free(work);
2276 }
2277
2278 void omxImaginaryEigenvectors(omxMatrix** matList, int numArgs, omxMatrix* result) {
2279         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Eigenvalues.");}
2280
2281         omxMatrix* A = omxInitMatrix(NULL, 0, 0, TRUE, result->currentState);
2282         omxCopyMatrix(result, matList[0]);
2283         omxResizeMatrix(A, result->rows, result->cols, FALSE);
2284
2285         /* Conformability Check! */
2286         if(A->cols != A->rows) {
2287                 char *errstr = (char*) calloc(250, sizeof(char));
2288                 sprintf(errstr, "Non-square matrix in (imaginary) eigenvalue decomposition.\n");
2289                 omxRaiseError(result->currentState, -1, errstr);
2290                 free(errstr);
2291                 omxFreeMatrixData(A);
2292                 return;
2293         }
2294
2295         char N = 'N';                                           // Indicators for BLAS
2296         char V = 'V';                                           // Indicators for BLAS
2297
2298         int One = 1;
2299         int lwork = 10*A->rows;
2300
2301         int info;
2302
2303         double *WR = (double*) malloc(A->cols * sizeof(double));
2304         double *WI = (double*) malloc(A->cols * sizeof(double));
2305         double *work = (double*) malloc(lwork * sizeof(double));
2306
2307         if(result->rows != A->rows || result->cols != A->cols)
2308                 omxResizeMatrix(result, A->rows, A->cols, FALSE);
2309
2310         /* For debugging */
2311         if(OMX_DEBUG_ALGEBRA) {
2312                 omxPrint(result, "NewMatrix");
2313                 mxLog("DGEEV: %c, %c, %d, %x, %d, %x, %x, %x, %d, %x, %d, %x, %d, %d", N, V, (A->rows), A->data, (A->leading), WR, WI, NULL, One, result->data, (result->leading), work, lwork, info);
2314         }
2315
2316         /* The call itself */
2317         F77_CALL(dgeev)(&N, &V, &(result->rows), result->data, &(result->leading), WR, WI, NULL, &One, A->data, &(A->leading), work, &lwork, &info);
2318         if(info != 0) {
2319                 char *errstr = (char*) calloc(250, sizeof(char));
2320                 sprintf(errstr, "DGEEV returned %d in eigenvalue decomposition:", info);
2321                 if(info > 0)
2322                         sprintf(errstr, "%s argument %d had an illegal value.  Post this to the OpenMx wiki.\n", errstr, info);
2323                 else
2324                         sprintf(errstr, "%s Unable to decompose matrix: Not of full rank.\n", errstr);
2325                 omxRaiseError(result->currentState, -1, errstr);
2326                 free(errstr);
2327                 goto ImagEigenVecCleanup;
2328         }
2329
2330         // Filter real and imaginary eigenvectors.  Imaginary ones have a WI.
2331         for(int i = 0; i < result->cols; i++) {
2332                 if(WI[i] != 0) {                                // FIXME: Might need to be abs(WI[i] > EPSILON)
2333                         // memcpy(omxLocationOfMatrixElement(A, 0, i), omxLocationOfMatrixElement(A, 0, i+1), A->rows * sizeof(double));
2334                         for(int j = 0; j < result->rows; j++) {
2335                                 double value = omxMatrixElement(A, j, i+1);                     // Conjugate pair
2336                                 omxSetMatrixElement(A, j, i, value);                            // Positive first,
2337                                 omxSetMatrixElement(A, j, i+1, -value);                         // Negative second
2338                         }
2339                         WR[i] = sqrt(WR[i] *WR[i] + WI[i]*WI[i]);                               // Sort by eigenvalue modulus
2340                         WR[i+1] = WR[i];                                                                                // Identical--conjugate pair
2341                         i++;    // Skip the next one; we know it's the conjugate pair.
2342                 } else {                                                // If it's not imaginary, it's zero.
2343                         for(int j = 0; j < A->rows; j++) {
2344                                 omxSetMatrixElement(A, j, i, 0.0);
2345                         }
2346                         WR[i] = fabs(WR[i]);                                                                    // Modulus of a real is its absolute value
2347
2348                 }
2349         }
2350
2351         result->colMajor = TRUE;
2352
2353         omxSortHelper(WR, A, result);
2354
2355 ImagEigenVecCleanup:
2356         omxFreeMatrixData(A);                   // FIXME: State-keeping for algebras would save significant time in memory allocation/deallocation
2357         omxMatrixLeadingLagging(result);
2358
2359         free(WR);
2360         free(WI);
2361         free(work);
2362
2363 }
2364
2365 void omxSelectRows(omxMatrix** matList, int numArgs, omxMatrix* result) {
2366     if (OMX_DEBUG_ALGEBRA) {mxLog("ALGEBRA: Select Rows.");}
2367
2368         omxMatrix* inMat = matList[0];
2369         omxMatrix* selector = matList[1];
2370
2371         int rows = inMat->rows;
2372     int selectLength = selector->rows * selector->cols;
2373     int toRemove[rows];
2374     int numRemoves = 0;
2375     
2376     if((selector->cols != 1) && selector->rows !=1) {
2377                 char *errstr = (char*) calloc(250, sizeof(char));
2378                 sprintf(errstr, "Selector must have a single row or a single column.\n");
2379         omxRaiseError(result->currentState, -1, errstr);
2380                 free(errstr);
2381                 return;
2382     }
2383
2384         if(selectLength != rows) {
2385                 char *errstr = (char*) calloc(250, sizeof(char));
2386                 sprintf(errstr, "Non-conformable matrices for row selection.\n");
2387         omxRaiseError(result->currentState, -1, errstr);
2388                 free(errstr);
2389                 return;
2390         }
2391         
2392         if(result->aliasedPtr != inMat) {
2393         omxAliasMatrix(result, inMat);
2394         }
2395         
2396     omxResetAliasedMatrix(result);
2397         
2398     for(int index = 0; index < selectLength; index++) {
2399         if(omxVectorElement(selector, index) == 0) {
2400             numRemoves++;
2401             toRemove[index] = 1;
2402         } else {
2403             toRemove[index] = 0;
2404         }
2405     }
2406     
2407     if(numRemoves >= rows) {
2408                 char *errstr = (char*) calloc(250, sizeof(char));
2409                 sprintf(errstr, "Attempted to select zero columns.\n");
2410         omxRaiseError(result->currentState, -1, errstr);
2411                 free(errstr);        
2412                 return;
2413     }
2414     
2415     int zeros[inMat->cols];
2416     memset(zeros, 0, sizeof(*zeros) * inMat->cols);
2417     omxRemoveRowsAndColumns(result, numRemoves, 0, toRemove, zeros);
2418
2419 }
2420
2421 void omxSelectCols(omxMatrix** matList, int numArgs, omxMatrix* result) {
2422     if (OMX_DEBUG_ALGEBRA) {mxLog("ALGEBRA: Select Columns.");}
2423
2424         omxMatrix* inMat = matList[0];
2425         omxMatrix* selector = matList[1];
2426
2427         int cols = inMat->cols;
2428     int selectLength = selector->rows * selector->cols;
2429     int toRemove[cols];
2430     int numRemoves = 0;
2431
2432     if((selector->cols != 1) && selector->rows !=1) {
2433                 char *errstr = (char*) calloc(250, sizeof(char));
2434                 sprintf(errstr, "Selector must have a single row or a single column.\n");
2435         omxRaiseError(result->currentState, -1, errstr);
2436                 free(errstr);        
2437                 return;
2438     }
2439
2440         if(selectLength != cols) {
2441                 char *errstr = (char*) calloc(250, sizeof(char));
2442                 sprintf(errstr, "Non-conformable matrices for row selection.\n");
2443         omxRaiseError(result->currentState, -1, errstr);
2444                 free(errstr);
2445                 return;
2446         }
2447         
2448         if(result->aliasedPtr != inMat) {
2449         omxAliasMatrix(result, inMat);
2450         }
2451         
2452     omxResetAliasedMatrix(result);
2453         
2454     for(int index = 0; index < selectLength; index++) {
2455         if(omxVectorElement(selector, index) == 0) {
2456             numRemoves++;
2457             toRemove[index] = 1;
2458         } else {
2459             toRemove[index] = 0;
2460         }
2461     }
2462     
2463     if(numRemoves >= cols) {
2464                 char *errstr = (char*) calloc(250, sizeof(char));
2465                 sprintf(errstr, "Attempted to select zero columns.\n");
2466         omxRaiseError(result->currentState, -1, errstr);
2467                 free(errstr);        
2468                 return;
2469     }
2470     
2471     int zeros[inMat->rows];
2472     memset(zeros, 0, sizeof(*zeros) * inMat->rows);
2473     omxRemoveRowsAndColumns(result, 0, numRemoves, zeros, toRemove);
2474     
2475 }
2476
2477 void omxSelectRowsAndCols(omxMatrix** matList, int numArgs, omxMatrix* result) {
2478     if (OMX_DEBUG_ALGEBRA) {mxLog("ALGEBRA: Select Rows And Cols.");}
2479
2480         omxMatrix* inMat = matList[0];
2481         omxMatrix* selector = matList[1];
2482
2483         int rows = inMat->rows;
2484         int cols = inMat->cols;
2485     int selectLength = selector->rows * selector->cols;
2486     int toRemove[cols];
2487     int numRemoves = 0;
2488
2489     if((selector->cols != 1) && selector->rows !=1) {
2490                 char *errstr = (char*) calloc(250, sizeof(char));
2491                 sprintf(errstr, "Selector must have a single row or a single column.\n");
2492         omxRaiseError(result->currentState, -1, errstr);
2493                 free(errstr);        
2494                 return;
2495     }
2496
2497         if(rows != cols) {
2498                 char *errstr = (char*) calloc(250, sizeof(char));
2499                 sprintf(errstr, "Can only select rows and columns from square matrices.\n");
2500         omxRaiseError(result->currentState, -1, errstr);
2501                 free(errstr);
2502                 return;
2503         }
2504
2505         if(selectLength != cols) {
2506                 char *errstr = (char*) calloc(250, sizeof(char));
2507                 sprintf(errstr, "Non-conformable matrices for row selection.\n");
2508         omxRaiseError(result->currentState, -1, errstr);
2509                 free(errstr);
2510                 return;
2511         }
2512         
2513         if(result->aliasedPtr != inMat) {
2514         omxAliasMatrix(result, inMat);
2515         }
2516         
2517     omxResetAliasedMatrix(result);
2518         
2519     for(int index = 0; index < selectLength; index++) {
2520         if(omxVectorElement(selector, index) == 0) {
2521             numRemoves++;
2522             toRemove[index] = 1;
2523         } else {
2524             toRemove[index] = 0;
2525         }
2526     }
2527     
2528     if(numRemoves >= cols) {
2529                 char *errstr = (char*) calloc(250, sizeof(char));
2530                 sprintf(errstr, "Attempted to select zero columns.\n");
2531         omxRaiseError(result->currentState, -1, errstr);
2532                 free(errstr);        
2533                 return;
2534     }
2535     
2536     omxRemoveRowsAndColumns(result, numRemoves, numRemoves, toRemove, toRemove);
2537
2538 }
2539
2540 void omxAddOwnTranspose(omxMatrix** matlist, int numArgs, omxMatrix* result) {
2541     omxMatrix* M = *matlist;
2542
2543     if(M->rows != M->cols || M->rows != result->cols || result->rows != result->cols) {
2544         char *errstr = (char*) calloc(250, sizeof(char));
2545         sprintf(errstr, "A + A^T attempted on asymmetric matrix.\n");
2546         omxRaiseError(result->currentState, -1, errstr);
2547         free(errstr);
2548                 return;
2549     }
2550     
2551     double total;
2552     
2553     for(int i = 0; i < result->rows; i++) {
2554         for(int j = 0; j < i; j++) {
2555             total = omxMatrixElement(M, i, j);
2556             total += omxMatrixElement(M, j, i);
2557             omxSetMatrixElement(result, i, j, total);
2558             omxSetMatrixElement(result, j, i, total);
2559         }
2560         omxSetMatrixElement(result, i, i, 2 * omxMatrixElement(M, i, i));
2561     }
2562     
2563 }
2564
2565 void omxCovToCor(omxMatrix** matList, int numArgs, omxMatrix* result)
2566 {
2567
2568     omxMatrix* inMat = matList[0];
2569     int rows = inMat->rows;
2570
2571         omxMatrix* intermediate;
2572
2573     if(inMat->rows != inMat->cols) {
2574         char *errstr = (char*) calloc(250, sizeof(char));
2575         sprintf(errstr, "cov2cor of non-square matrix cannot even be attempted\n");
2576         omxRaiseError(result->currentState, -1, errstr);
2577         free(errstr);
2578                 return;
2579         }
2580
2581         if(result->rows != rows || result->cols != rows) {
2582         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: cov2cor resizing result.");}
2583         omxResizeMatrix(result, rows, rows, FALSE);
2584         }
2585
2586     intermediate = omxInitTemporaryMatrix(NULL, 1, rows, TRUE, inMat->currentState);
2587
2588     for(int i = 0; i < rows; i++) {
2589         intermediate->data[i] = sqrt(1.0 / omxMatrixElement(inMat, i, i));
2590     }
2591
2592     if (inMat->colMajor) {
2593         for(int col = 0; col < rows; col++) {
2594             for(int row = 0; row < rows; row++) {
2595                 result->data[col * rows + row] = inMat->data[col * rows + row] * 
2596                     intermediate->data[row] * intermediate->data[col];
2597             }
2598         }
2599     } else {
2600         for(int col = 0; col < rows; col++) {
2601             for(int row = 0; row < rows; row++) {
2602                 result->data[col * rows + row] = inMat->data[row * rows + col] * 
2603                     intermediate->data[row] * intermediate->data[col];
2604             }
2605         }
2606     }
2607
2608     for(int i = 0; i < rows; i++) {
2609         result->data[i * rows + i] = 1.0;
2610     }
2611
2612     omxFreeAllMatrixData(intermediate);
2613 }
2614
2615 void omxCholesky(omxMatrix** matList, int numArgs, omxMatrix* result)
2616 {
2617
2618         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Cholesky Decomposition.");}
2619