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