Initial support for OpenMP in Hessian calculation
[openmx:openmx.git] / src / npsolWrap.c
1 /*
2  *  Copyright 2007-2009 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 #include "R.h"
18 #include <Rinternals.h>
19 #include <Rdefines.h>
20 #include <R_ext/Rdynload.h>
21 #include <R_ext/BLAS.h>
22 #include <R_ext/Lapack.h>
23 #include "omxDefines.h"
24 #include "npsolWrap.h"
25
26 #ifdef SUPPORT_OPENMP
27 #include <omp.h>
28 #endif
29
30 #include <stdio.h>
31 #include <sys/types.h>
32 #include <errno.h>
33 #include "omxState.h"
34 #include "omxMatrix.h"
35 #include "omxAlgebra.h"
36 #include "omxObjective.h"
37 #include "omxNPSOLSpecific.h"
38 #include "omxImportFrontendState.h"
39 #include "omxExportBackendState.h"
40 //#include "omxSymbolTable.h"
41
42 /* NPSOL-related functions */
43 extern void F77_SUB(npsol)(int *n, int *nclin, int *ncnln, int *ldA, int *ldJ, int *ldR, double *A,
44                                                         double *bl,     double *bu, void* funcon, void* funobj, int *inform, int *iter, 
45                                                         int *istate, double *c, double *cJac, double *clambda, double *f, double *g, double *R,
46                                                         double *x, int *iw,     int *leniw, double *w, int *lenw);
47
48 /* Objective Function */
49 void F77_SUB(objectiveFunction) ( int* mode, int* n, double* x, double* f, double* g, int* nstate );                            // For general computation
50 void F77_SUB(limitObjectiveFunction)(   int* mode, int* n, double* x, double* f, double* g, int* nstate );                      // For limit computations
51
52 /* Constraint Function */
53 void F77_SUB(constraintFunction)(int *mode, int *ncnln, int *n, int *ldJ, int *needc, double *x, double *c, double *cJac, int *nstate); // Constraints in the style of old Mx
54
55 /* Helper functions */
56 void handleFreeVarList(omxState *os, double* x, int numVars);   // Locates and inserts elements from the optimizer into matrices.
57 SEXP getListElement(SEXP list, const char *str);                                // Gets the value named str from SEXP list.  From "Writing R Extensions"
58 SEXP getVar(SEXP str, SEXP env);                                                                // Gets the object named str from environment env.  From "Writing R Extensions"
59 unsigned short omxEstimateHessian(int numHessians, double functionPrecision, int r, omxState* currentState);
60
61 struct hess_struct {
62         int     numParams;
63         double* freeParams;
64         double* Haprox;
65         double* Gaprox;
66         omxMatrix* objectiveMatrix;
67         double  f0;
68         double  functionPrecision;
69         int r;
70 };
71
72 /* Set up R .Call info */
73 R_CallMethodDef callMethods[] = {
74 {"callNPSOL", (void*(*)())&callNPSOL, 11},
75 {"omxCallAlgebra", (void*(*)())&omxCallAlgebra, 3},
76 {"findIdenticalRowsData", (void*(*)())&findIdenticalRowsData, 5},
77 {NULL, NULL, 0}
78 };
79
80 void R_init_mylib(DllInfo *info) {
81 /* Register routines, allocate resources. */
82 R_registerRoutines(info, NULL, callMethods, NULL, NULL);
83 }
84
85 void R_unload_mylib(DllInfo *info) {
86 /* Release resources. */
87 }
88
89 /* Globals for function evaluation */
90 SEXP RObjFun, RConFun;                  // Pointers to the functions NPSOL calls
91 SEXP env;                                               // Environment for evaluation and object hunting
92
93 /* Made global for objective functions that want them */
94 int n, nclin, ncnln;                    // Number of Free Params, linear, and nonlinear constraints
95 double f;                                               // Objective Function Value
96 double *g;                                              // Gradient Pointer
97 double *R, *cJac;                               // Hessian (Approx) and Jacobian
98 int *istate;                                    // Current state of constraints (0 = no, 1 = lower, 2 = upper, 3 = both (equality))
99
100 omxState* currentState;                 // Current State of optimization
101
102 /* Main functions */
103 SEXP omxCallAlgebra(SEXP matList, SEXP algNum, SEXP options) {
104
105         if(OMX_DEBUG) { Rprintf("-----------------------------------------------------------------------\n");}
106         if(OMX_DEBUG) { Rprintf("Explicit call to algebra %d.\n", INTEGER(algNum));}
107
108         int j,k,l;
109         omxMatrix* algebra;
110         int algebraNum = INTEGER(algNum)[0];
111         SEXP ans, nextMat;
112         char output[250];
113         int errOut = 0;
114
115         /* Create new omxState for current state storage and initialize it. */
116         currentState = (omxState*) R_alloc(1, sizeof(omxState));
117         omxInitState(currentState);
118         currentState->numFreeParams = n;
119         if(OMX_DEBUG) { Rprintf("Created state object at 0x%x.\n", currentState);}
120
121         /* Retrieve All Matrices From the MatList */
122
123         if(OMX_DEBUG) { Rprintf("Processing %d matrix(ces).\n", length(matList));}
124         currentState->numMats = length(matList);
125         currentState->matrixList = (omxMatrix**) R_alloc(length(matList), sizeof(omxMatrix*));
126
127         for(k = 0; k < length(matList); k++) {
128                 PROTECT(nextMat = VECTOR_ELT(matList, k));      // This is the matrix + populations
129                 currentState->matrixList[k] = omxNewMatrixFromRPrimitive(nextMat, currentState);
130                 if(OMX_DEBUG) {
131                         Rprintf("Matrix initialized at 0x%0xd = (%d x %d).\n",
132                                 currentState->matrixList[k], currentState->matrixList[k]->rows, currentState->matrixList[k]->cols);
133                 }
134                 UNPROTECT(1); // nextMat
135         }
136
137         algebra = omxNewAlgebraFromOperatorAndArgs(algebraNum, currentState->matrixList, currentState->numMats, currentState);
138
139         if(algebra==NULL) {
140                 error(currentState->statusMsg);
141         }
142
143         if(OMX_DEBUG) {Rprintf("Completed Algebras and Matrices.  Beginning Initial Compute.\n");}
144         omxStateNextEvaluation(currentState);
145
146         omxRecompute(algebra);
147
148         PROTECT(ans = allocMatrix(REALSXP, algebra->rows, algebra->cols));
149         for(l = 0; l < algebra->rows; l++)
150                 for(j = 0; j < algebra->cols; j++)
151                         REAL(ans)[j * algebra->rows + l] =
152                                 omxMatrixElement(algebra, l, j);
153
154         UNPROTECT(1);   /* algebra */
155
156         if(OMX_DEBUG) { Rprintf("All Algebras complete.\n"); }
157
158         if(currentState->statusCode != 0) {
159                 errOut = currentState->statusCode;
160                 strncpy(output, currentState->statusMsg, 250);
161         }
162
163         omxFreeState(currentState);
164
165         if(errOut != 0) {
166                 error(output);
167         }
168
169         return ans;
170 }
171
172 SEXP callNPSOL(SEXP objective, SEXP startVals, SEXP constraints,
173         SEXP matList, SEXP varList, SEXP algList,
174         SEXP data, SEXP intervalList, SEXP checkpointList, SEXP options, SEXP state) {
175
176         /* NPSOL Arguments */
177         void (*funcon)(int*, int*, int*, int*, int*, double*, double*, double*, int*);
178
179         int ldA, ldJ, ldR, inform, iter, leniw, lenw; // nclin, ncnln
180         int *iw = NULL; // , istate;
181
182 //      double f;
183         double *A=NULL, *bl=NULL, *bu=NULL, *c=NULL, *clambda=NULL, *x = NULL, *w=NULL; //  *g, *R, *cJac,
184         double *est, *grad, *hess;
185
186         /* Helpful variables */
187
188         int k, l;                                       // Index Vars
189
190         int nctotl, nlinwid, nlnwid;    // Helpful side variables.
191         
192         int errOut = 0;                 // Error state: Clear
193
194         SEXP nextLoc;
195
196         int calculateStdErrors = FALSE;
197         int numHessians = 0;
198         int ciMaxIterations = 5;
199         int disableOptimizer = 0;
200
201         /* Sanity Check and Parse Inputs */
202         /* TODO: Need to find a way to account for nullness in these.  For now, all checking is done on the front-end. */
203 //      if(!isVector(startVals)) error ("startVals must be a vector");
204 //      if(!isVector(matList)) error ("matList must be a list");
205 //      if(!isVector(algList)) error ("algList must be a list");
206
207         n = length(startVals);
208
209         /* Create new omxState for current state storage and initialize it. */
210         currentState = (omxState*) R_alloc(1, sizeof(omxState));
211         omxInitState(currentState);
212         currentState->numFreeParams = n;
213         if(OMX_DEBUG) { Rprintf("Created state object at 0x%x.\n", currentState);}
214
215         /* Retrieve Data Objects */
216         if(!errOut) errOut = omxProcessMxDataEntities(data);
217     
218         /* Retrieve All Matrices From the MatList */
219         if(!errOut) errOut = omxProcessMxMatrixEntities(matList);
220
221         /* Process Algebras Here */
222         if(!errOut) errOut = omxProcessMxAlgebraEntities(algList);
223
224         /* Initial Matrix and Algebra Calculations */
225         if(!errOut) errOut = omxInitialMatrixAlgebraCompute();
226         
227         /* Process Objective Function */
228         if(!errOut) errOut = omxProcessObjectiveFunction(objective, &n);
229
230         // TODO: Make calculateHessians an option instead.
231
232   if(!errOut) { // In the event of an initialization error, skip all this.
233
234         /* Process Matrix and Algebra Population Function */
235         /*
236          Each matrix is a list containing a matrix and the other matrices/algebras that are
237          populated into it at each iteration.  The first element is already processed, above.
238          The rest of the list will be processed here.
239         */
240         for(int j = 0; j < currentState->numMats; j++) {
241                 PROTECT(nextLoc = VECTOR_ELT(matList, j));              // This is the matrix + populations
242                 omxProcessMatrixPopulationList(currentState->matrixList[j], nextLoc);
243                 UNPROTECT(1);
244         }
245
246         /* Process Free Var List */
247         omxProcessFreeVarList(varList, n);
248
249         /* Processing Constraints */
250         ncnln = omxProcessConstraints(constraints);
251         funcon = F77_SUB(constraintFunction);
252
253         /* Process Confidence Interval List */
254         omxProcessConfidenceIntervals(intervalList);
255
256         /* Process Checkpoint List */
257         omxProcessCheckpointOptions(checkpointList);
258
259   } else { // End if(errOut)
260     error(currentState->statusMsg);
261   }
262
263         /* Set up Optimization Memory Allocations */
264         if(n == 0) {                    // Special Case for the evaluation-only condition
265
266                 if(OMX_DEBUG) { Rprintf("No free parameters.  Avoiding Optimizer Entirely.\n"); }
267                 int mode = 0, nstate = -1;
268                 f = 0;
269                 double* x = NULL, *g = NULL;
270
271                 if(currentState->objectiveMatrix != NULL) {
272                         F77_SUB(objectiveFunction)(&mode, &n, x, &f, g, &nstate);
273                 };
274                 numHessians = 0;                                        // No hessian if there's no free params
275                 currentState->numIntervals = 0;         // No intervals if there's no free params
276                 inform = 0;
277                 iter = 0;
278
279                 omxStateNextEvaluation(currentState);   // Advance for a final evaluation.
280
281         } else {
282                 /* Initialize Scalar Variables. */
283                 nclin = 0;                                              // No linear constraints.
284
285                 /* Set boundaries and widths. */
286                 if(nclin <= 0) {
287                         nclin = 0;                                      // This should never matter--nclin should always be non-negative.
288                         nlinwid = 1;                            // For memory allocation purposes, nlinwid > 0
289                 } else {                                                // nlinwid is  used to calculate ldA, and eventually the size of A.
290                         nlinwid = nclin;
291                 }
292
293                 if(ncnln <= 0) {
294                         ncnln = 0;                                      // This should never matter--ncnln should always be non-negative.
295                         nlnwid = 1;                                     // For memory allocation purposes nlnwid > 0
296                 } else {                                                // nlnwid is used to calculate ldJ, and eventually the size of J.
297                         nlnwid = ncnln;
298                 }
299
300                 nctotl = n + nlinwid + nlnwid;
301
302                 leniw = 3 * n + nclin + 2 * ncnln;
303                 lenw = 2 * n * n + n * nclin + 2 * n * ncnln + 20 * n + 11 * nclin + 21 * ncnln;
304
305                 ldA = nlinwid;                  // NPSOL specifies this should be either 1 or nclin, whichever is greater
306                 ldJ = nlnwid;                   // NPSOL specifies this should be either 1 or nclin, whichever is greater
307                 ldR = n;                                // TODO: Test alternative versions of the size of R to see what's best.
308
309         /* Allocate arrays */
310                 A               = (double*) R_alloc (ldA * n, sizeof ( double )  );
311                 bl              = (double*) R_alloc ( nctotl, sizeof ( double ) );
312                 bu              = (double*) R_alloc (nctotl, sizeof ( double ) );
313                 c               = (double*) R_alloc (nlnwid, sizeof ( double ));
314                 cJac    = (double*) R_alloc (ldJ * n, sizeof ( double ) );
315                 clambda = (double*) R_alloc (nctotl, sizeof ( double )  );
316                 g               = (double*) R_alloc (n, sizeof ( double ) );
317                 R               = (double*) R_alloc (ldR * n, sizeof ( double ));
318                 x               = (double*) R_alloc ((n+1), sizeof ( double ));
319                 w               = (double*) R_alloc (lenw, sizeof ( double ));
320
321                 istate  = (int*) R_alloc (nctotl, sizeof ( int ) );
322                 iw              = (int*) R_alloc (leniw, sizeof ( int ));
323
324                 /* Set up actual run */
325
326                 omxSetupBoundsAndConstraints(bl, bu, n, nclin);
327                 
328                 /*      Set NPSOL options */
329                 omxSetNPSOLOpts(options, &numHessians, &calculateStdErrors, &ciMaxIterations, &disableOptimizer);
330
331                 /* Initialize Starting Values */
332                 if(OMX_VERBOSE) {
333                         Rprintf("--------------------------\n");
334                         Rprintf("Starting Values (%d) are:\n", n);
335                 }
336                 for(k = 0; k < n; k++) {
337                         x[k] = REAL(startVals)[k];
338                         if((x[k] == 0.0) && !disableOptimizer) {
339                                 x[k] += 0.1;
340                         }
341                         if(OMX_VERBOSE) { Rprintf("%d: %f\n", k, x[k]); }
342                 }
343                 if(OMX_DEBUG) {
344                         Rprintf("--------------------------\n");
345                         Rprintf("Setting up optimizer...");
346                 }
347
348         /*  F77_CALL(npsol)
349                 (       int *n,                                 -- Number of variables
350                         int *nclin,                             -- Number of linear constraints
351                         int *ncnln,                             -- Number of nonlinear constraints
352                         int *ldA,                               -- Row dimension of A (Linear Constraints)
353                         int *ldJ,                               -- Row dimension of cJac (Jacobian)
354                         int *ldR,                               -- Row dimension of R (Hessian)
355                         double *A,                              -- Linear Constraints Array A (in Column-major order)
356                         double *bl,                             -- Lower Bounds Array (at least n + nclin + ncnln long)
357                         double *bu,                             -- Upper Bounds Array (at least n + nclin + ncnln long)
358                         function funcon,                -- Nonlinear constraint function
359                         function funobj,                -- Objective function
360                         int *inform,                    -- Used to report state.  Need not be initialized.
361                         int *iter,                              -- Used to report number of major iterations performed.  Need not be initialized.
362                         int *istate,                    -- Initial State.  Need not be initialized unless using Warm Start.
363                         double *c,                              -- Array of length ncnln.  Need not be initialized.  Reports nonlinear constraints at final iteration.
364                         double *cJac,                   -- Array of Row-length ldJ.  Unused if ncnln = 0. Generally need not be initialized.
365                         double *clambda,                -- Array of length n+nclin+ncnln.  Need not be initialized unless using Warm Start. Reports final QP multipliers.
366                         double *f,                              -- Used to report final objective value.  Need not be initialized.
367                         double *g,                              -- Array of length n. Used to report final objective gradient.  Need not be initialized.
368                         double *R,                              -- Array of length ldR.  Need not be intialized unless using Warm Start.
369                         double *x,                              -- Array of length n.  Contains initial solution estimate.
370                         int *iw,                                -- Array of length leniw. Need not be initialized.  Provides workspace.
371                         int *leniw,                             -- Length of iw.  Must be at least 3n + nclin + ncnln.
372                         double *w,                              -- Array of length lenw. Need not be initialized.  Provides workspace.
373                         int *lenw                               -- Length of w.  Must be at least 2n^2 + n*nclin + 2*n*ncnln + 20*n + 11*nclin +21*ncnln
374                 )
375
376                 bl, bu, istate, and clambda are all length n+nclin+ncnln.
377                         First n elements refer to the vars, in order.
378                         Next nclin elements refer to bounds on Ax
379                         Last ncnln elements refer to bounds on c(x)
380
381                 All arrays must be in column-major order.
382
383                 */
384
385                 if(OMX_DEBUG) {
386                         Rprintf("Set.\n");
387                 }
388
389                 if (disableOptimizer) {
390                         int mode = 0, nstate = -1;              
391                         if(currentState->objectiveMatrix != NULL) {
392                                 F77_SUB(objectiveFunction)(&mode, &n, x, &f, g, &nstate);
393                         };
394
395                         inform = 0;
396                         iter = 0;
397
398                         omxStateNextEvaluation(currentState);   // Advance for a final evaluation.              
399                 } else {
400                         F77_CALL(npsol)(&n, &nclin, &ncnln, &ldA, &ldJ, &ldR, A, bl, bu, (void*)funcon,
401                                                         (void*) F77_SUB(objectiveFunction), &inform, &iter, istate, c, cJac,
402                                                         clambda, &f, g, R, x, iw, &leniw, w, &lenw);
403                 }
404                 if(OMX_DEBUG) { Rprintf("Final Objective Value is: %f.\n", f); }
405                 
406                 handleFreeVarList(currentState, x, n);
407                 
408         } // END OF PERFORM OPTIMIZATION CASE
409
410         SEXP minimum, estimate, gradient, hessian, code, status, statusMsg, iterations;
411         SEXP evaluations, ans=NULL, names=NULL, algebras, matrices, optimizer;
412         SEXP intervals, NAmat, intervalCodes, calculatedHessian, stdErrors;
413
414         int numReturns = 13;
415
416         PROTECT(minimum = NEW_NUMERIC(1));
417         PROTECT(code = NEW_NUMERIC(1));
418         PROTECT(status = allocVector(VECSXP, 3));
419         PROTECT(iterations = NEW_NUMERIC(1));
420         PROTECT(evaluations = NEW_NUMERIC(2));
421         PROTECT(matrices = NEW_LIST(currentState->numMats));
422         PROTECT(algebras = NEW_LIST(currentState->numAlgs));
423
424         /* N-dependent SEXPs */
425         PROTECT(estimate = allocVector(REALSXP, n));
426         PROTECT(optimizer = allocVector(VECSXP, 2));
427         PROTECT(gradient = allocVector(REALSXP, n));
428         PROTECT(hessian = allocMatrix(REALSXP, n, n));
429         PROTECT(calculatedHessian = allocMatrix(REALSXP, n, n));
430         PROTECT(stdErrors = allocMatrix(REALSXP, n, 1)); // for optimizer
431         PROTECT(names = allocVector(STRSXP, 2)); // for optimizer
432         PROTECT(intervals = allocMatrix(REALSXP, currentState->numIntervals, 2)); // for optimizer
433         PROTECT(intervalCodes = allocMatrix(INTSXP, currentState->numIntervals, 2)); // for optimizer
434         PROTECT(NAmat = allocMatrix(REALSXP, 1, 1)); // In case of missingness
435         REAL(NAmat)[0] = R_NaReal;
436
437         /* Store outputs for return */
438         if(objective != NULL) {
439                 REAL(minimum)[0] = f;
440                 currentState->optimum = f;
441         } else {
442                 REAL(minimum)[0] = R_NaReal;
443         }
444
445         est = REAL(estimate);  // Aliases to avoid repeated function calls.
446         grad = REAL(gradient);
447         hess = REAL(hessian);
448
449         for(k = 0; k < n; k++) {                // Do these with memcpy instead, probably.
450                 est[k] = x[k];
451                 grad[k] = g[k];
452                 for(l = 0; l < n; l++) {                        // Save the NPSOL hessian, in case somebody wants it
453                         hess[k*n + l] = R[k*n + l];             // This is ok, because they're both in Col-Major already.
454                 }
455         }
456
457         omxSaveState(currentState, x, f);               // Keep the current values for the currentState.
458
459         /* Fill in details from the optimizer */
460         SET_VECTOR_ELT(optimizer, 0, gradient);
461         SET_VECTOR_ELT(optimizer, 1, hessian);
462
463         SET_STRING_ELT(names, 0, mkChar("minimum"));
464         SET_STRING_ELT(names, 1, mkChar("estimate"));
465         namesgets(optimizer, names);
466
467         REAL(code)[0] = inform;
468         REAL(iterations)[0] = iter;
469         REAL(evaluations)[0] = currentState->computeCount;
470
471         /* Fill Status code. */
472         SET_VECTOR_ELT(status, 0, code);
473         PROTECT(code = NEW_NUMERIC(1));
474         REAL(code)[0] = currentState->statusCode;
475         SET_VECTOR_ELT(status, 1, code);
476         PROTECT(statusMsg = allocVector(STRSXP, 1));
477         SET_STRING_ELT(statusMsg, 0, mkChar(currentState->statusMsg));
478         SET_VECTOR_ELT(status, 2, statusMsg);
479
480         if(numHessians && currentState->optimumStatus >= 0) {           // No hessians or standard errors if the optimum is invalid
481                 if(currentState->numConstraints == 0) {
482                         if(OMX_DEBUG) { Rprintf("Calculating Hessian for Objective Function.\n");}
483                         int gotHessians = omxEstimateHessian(numHessians, .0001, 4, currentState);
484                         if(gotHessians) {
485                                 if(calculateStdErrors) {
486                                         for(int j = 0; j < numHessians; j++) {          //TODO: Fix Hessian calculation to allow more if requested
487                                                 if(OMX_DEBUG) { Rprintf("Calculating Standard Errors for Objective Function.\n");}
488                                                 omxObjective* oo = currentState->objectiveMatrix->objective;
489                                                 if(oo->getStandardErrorFun != NULL) {
490                                                         oo->getStandardErrorFun(oo);
491                                                 } else {
492                                                         omxCalculateStdErrorFromHessian(2.0, oo);
493                                                 }
494                                         }
495                                 }
496                         } else {
497                                 numHessians = 0;
498                         }
499                 } else {
500                         numHessians = 0;
501                 }
502         }
503
504         /* Likelihood-based Confidence Interval Calculation */
505         if(currentState->numIntervals) {
506                 if(inform == 0 || inform == 1 || inform == 6) {
507                         if(OMX_DEBUG) { Rprintf("Calculating likelihood-based confidence intervals.\n"); }
508                         currentState->optimizerState = (omxOptimizerState*) R_alloc(1, sizeof(omxOptimizerState));
509                         for(int i = 0; i < currentState->numIntervals; i++) {
510
511                                 memcpy(x, currentState->optimalValues, n * sizeof(double)); // Reset to previous optimum
512                                 currentState->currentInterval = i;
513                                 omxConfidenceInterval *currentCI = &(currentState->intervalList[i]);
514                                 currentCI->lbound += currentState->optimum;                     // Convert from offsets to targets
515                                 currentCI->ubound += currentState->optimum;                     // Convert from offsets to targets
516
517                                 /* Set up for the lower bound */
518                                 inform = -1;
519                                 // Number of times to keep trying.
520                                 int cycles = ciMaxIterations;
521                                 double value = INF;
522                                 double objDiff = 1.e-4;         // TODO : Use function precision to determine CI jitter?
523                                 while(inform != 0 && cycles > 0) {
524                                         /* Find lower limit */
525                                         currentCI->calcLower = TRUE;
526                                         F77_CALL(npsol)(&n, &nclin, &ncnln, &ldA, &ldJ, &ldR, A, bl, bu, (void*)funcon,
527                                                                         (void*) F77_SUB(limitObjectiveFunction), &inform, &iter, istate, c, cJac,
528                                                                         clambda, &f, g, R, x, iw, &leniw, w, &lenw);
529
530                                         currentCI->lCode = inform;
531                                         if(f < value) {
532                                                 currentCI->min = omxMatrixElement(currentCI->matrix, currentCI->row, currentCI->col);
533                                                 value = f;
534                                         }
535
536                                         if(inform != 0 && OMX_DEBUG) {
537                                                 Rprintf("Calculation of lower interval %d failed: Bad inform value of %d\n",
538                                                         i, inform);
539                                         }
540                                         cycles--;
541                                         if(inform != 0) {
542                                                 unsigned int jitter = TRUE;
543                                                 for(int j = 0; j < n; j++) {
544                                                         if(fabs(x[j] - currentState->optimalValues[j]) > objDiff){
545                                                                 jitter = FALSE;
546                                                                 break;
547                                                         }
548                                                 }
549                                                 if(jitter) {
550                                                         for(int j = 0; j < n; j++) {
551                                                                 x[j] = currentState->optimalValues[j] + objDiff;
552                                                         }
553                                                 }
554                                         }
555                                 }
556
557                                 if(OMX_DEBUG) { Rprintf("Found lower bound %d.  Seeking upper.\n", i); }
558                                 // TODO: Repopulate original optimizer state in between CI calculations
559
560                                 memcpy(x, currentState->optimalValues, n * sizeof(double));
561
562                                 /* Reset for the upper bound */
563                                 value = INF;
564                                 inform = -1;
565                                 cycles = ciMaxIterations;
566
567                                 while(inform != 0 && cycles >= 0) {
568                                         /* Find upper limit */
569                                         currentCI->calcLower = FALSE;
570                                         F77_CALL(npsol)(&n, &nclin, &ncnln, &ldA, &ldJ, &ldR, A, bl, bu, (void*)funcon,
571                                                                         (void*) F77_SUB(limitObjectiveFunction), &inform, &iter, istate, c, cJac,
572                                                                         clambda, &f, g, R, x, iw, &leniw, w, &lenw);
573
574                                         currentCI->uCode = inform;
575                                         if(f < value) {
576                                                 currentCI->max = omxMatrixElement(currentCI->matrix, currentCI->row, currentCI->col);
577                                         }
578
579                                         if(inform != 0 && OMX_DEBUG) {
580                                                 Rprintf("Calculation of upper interval %d failed: Bad inform value of %d\n",
581                                                         i, inform);
582                                         }
583                                         cycles--;
584                                         if(inform != 0) {
585                                                 unsigned int jitter = TRUE;
586                                                 for(int j = 0; j < n; j++) {
587                                                         if(fabs(x[j] - currentState->optimalValues[j]) > objDiff){
588                                                                 jitter = FALSE;
589                                                                 break;
590                                                         }
591                                                 }
592                                                 if(jitter) {
593                                                         for(int j = 0; j < n; j++) {
594                                                                 x[j] = currentState->optimalValues[j] + objDiff;
595                                                         }
596                                                 }
597                                         }
598                                 }
599                                 if(OMX_DEBUG) {Rprintf("Found Upper bound %d.\n", i);}
600                         }
601                 } else {                                        // Improper code. No intervals calculated.
602                                                                         // TODO: Throw a warning, allow force()
603                         warning("Not calculating confidence intervals because of error status.");
604                         if(OMX_DEBUG) {
605                                 Rprintf("Calculation of all intervals failed: Bad inform value of %d", inform);
606                         }
607                 }
608         }
609
610         handleFreeVarList(currentState, currentState->optimalValues, n);  // Restore to optima for final compute
611         if(!errOut) omxFinalAlgebraCalculation(matrices, algebras); 
612
613         omxPopulateObjectiveFunction(numReturns, &ans, &names);
614
615         if(numHessians) {
616                 omxPopulateHessians(numHessians, currentState->objectiveMatrix, 
617                         calculatedHessian, stdErrors, calculateStdErrors, n);
618         }
619
620         if(currentState->numIntervals) {        // Populate CIs
621                 omxPopulateConfidenceIntervals(intervals, intervalCodes);
622         }
623         
624         REAL(evaluations)[1] = currentState->computeCount;
625
626         int nextEl = 0;
627
628         SET_STRING_ELT(names, nextEl++, mkChar("minimum"));
629         SET_STRING_ELT(names, nextEl++, mkChar("estimate"));
630         SET_STRING_ELT(names, nextEl++, mkChar("gradient"));
631         SET_STRING_ELT(names, nextEl++, mkChar("hessianCholesky"));
632         SET_STRING_ELT(names, nextEl++, mkChar("status"));
633         SET_STRING_ELT(names, nextEl++, mkChar("iterations"));
634         SET_STRING_ELT(names, nextEl++, mkChar("evaluations"));
635         SET_STRING_ELT(names, nextEl++, mkChar("matrices"));
636         SET_STRING_ELT(names, nextEl++, mkChar("algebras"));
637         SET_STRING_ELT(names, nextEl++, mkChar("confidenceIntervals"));
638         SET_STRING_ELT(names, nextEl++, mkChar("confidenceIntervalCodes"));
639         SET_STRING_ELT(names, nextEl++, mkChar("calculatedHessian"));
640         SET_STRING_ELT(names, nextEl++, mkChar("standardErrors"));
641
642         nextEl = 0;
643
644         SET_VECTOR_ELT(ans, nextEl++, minimum);
645         SET_VECTOR_ELT(ans, nextEl++, estimate);
646         SET_VECTOR_ELT(ans, nextEl++, gradient);
647         SET_VECTOR_ELT(ans, nextEl++, hessian);
648         SET_VECTOR_ELT(ans, nextEl++, status);
649         SET_VECTOR_ELT(ans, nextEl++, iterations);
650         SET_VECTOR_ELT(ans, nextEl++, evaluations);
651         SET_VECTOR_ELT(ans, nextEl++, matrices);
652         SET_VECTOR_ELT(ans, nextEl++, algebras);
653         SET_VECTOR_ELT(ans, nextEl++, intervals);
654         SET_VECTOR_ELT(ans, nextEl++, intervalCodes);
655         if(numHessians == 0) {
656                 SET_VECTOR_ELT(ans, nextEl++, NAmat);
657         } else {
658                 SET_VECTOR_ELT(ans, nextEl++, calculatedHessian);
659         }
660         if(!calculateStdErrors) {
661                 SET_VECTOR_ELT(ans, nextEl++, NAmat);
662         } else {
663                 SET_VECTOR_ELT(ans, nextEl++, stdErrors);
664         }
665         namesgets(ans, names);
666
667         if(OMX_VERBOSE) {
668                 Rprintf("Inform Value: %d\n", currentState->optimumStatus);
669                 Rprintf("--------------------------\n");
670         }
671
672         /* Free data memory */
673         omxFreeState(currentState);
674
675         UNPROTECT(numReturns);                                          // Unprotect Output Parameters
676         UNPROTECT(8);                                                           // Unprotect internals
677
678         if(OMX_DEBUG) {Rprintf("All vectors freed.\n");}
679
680         return(ans);
681
682 }
683
684 /****** Objective Function *********/
685 void F77_SUB(objectiveFunction)
686         (       int* mode, int* n, double* x,
687                 double* f, double* g, int* nstate )
688 {
689         unsigned short int checkpointNow = FALSE;
690
691         if(OMX_DEBUG) {Rprintf("Starting Objective Run.\n");}
692
693         if(*mode == 1) {
694                 currentState->majorIteration++;
695                 currentState->minorIteration = 0;
696                 checkpointNow = TRUE;                                   // Only checkpoint at major iterations.
697         } else currentState->minorIteration++;
698
699         omxMatrix* objectiveMatrix = currentState->objectiveMatrix;
700         char errMsg[5] = "";
701         omxRaiseError(currentState, 0, errMsg);                                         // Clear Error State
702         /* Interruptible? */
703         R_CheckUserInterrupt();
704     /* This allows for abitrary repopulation of the free parameters.
705      * Typically, the default is for repopulateFun to be NULL,
706      * and then handleFreeVarList is invoked */
707         if (objectiveMatrix->objective->repopulateFun != NULL) {
708                 objectiveMatrix->objective->repopulateFun(objectiveMatrix->objective, x, *n);
709         } else {
710                 handleFreeVarList(currentState, x, *n);
711         }
712         omxRecompute(objectiveMatrix);
713
714         /* Derivative Calculation Goes Here. */
715
716         if(isnan(objectiveMatrix->data[0])) {
717                 if(OMX_DEBUG) {
718                         Rprintf("Objective value is NaN.\n");
719                 }
720                 omxRaiseError(currentState, -1, "Objective function returned a value of NaN.");
721                 *mode = -1;
722         }
723
724         if(isinf(objectiveMatrix->data[0])) {
725                 if(OMX_DEBUG) {
726                         Rprintf("Objective Value is infinite.\n");
727                 }
728                 omxRaiseError(currentState, -1, "Objective function returned an infinite value.");
729                 *mode = -1;
730         }
731
732         if(currentState->statusCode <= -1) {            // At some point, we'll add others
733                 if(OMX_DEBUG) {
734                         Rprintf("Error status reported.\n");
735                 }
736                 *mode = -1;
737         }
738
739         *f = objectiveMatrix->data[0];
740         if(OMX_VERBOSE) {
741                 Rprintf("Objective Value is: %f, Mode is %d.\n", objectiveMatrix->data[0], *mode);
742         }
743
744         if(OMX_DEBUG) { Rprintf("-======================================================-\n"); }
745
746         if(checkpointNow && currentState->numCheckpoints != 0) {        // If it's a new major iteration
747                 omxSaveCheckpoint(currentState, x, f);          // Check about saving a checkpoint
748         }
749
750 }
751
752 /* (Non)Linear Constraint Functions */
753 void F77_SUB(constraintFunction)
754         (       int *mode, int *ncnln, int *n,
755                 int *ldJ, int *needc, double *x,
756                 double *c, double *cJac, int *nstate)
757 {
758
759         if(OMX_DEBUG) { Rprintf("Constraint function called.\n");}
760
761         if(*mode==1) {
762                 if(OMX_DEBUG) {
763                         Rprintf("But only gradients requested.  Returning.\n");
764                         Rprintf("-=====================================================-\n");
765                 }
766                 return;
767         }
768
769         int j, k, l = 0, size;
770
771         handleFreeVarList(currentState, x, *n);
772
773         for(j = 0; j < currentState->numConstraints; j++) {
774                 omxRecompute(currentState->conList[j].result);
775                 size = currentState->conList[j].result->rows * currentState->conList[j].result->cols;
776                 if(OMX_VERBOSE) { omxPrint(currentState->conList[j].result, "Constraint evaluates as:"); }
777                 for(k = 0; k < currentState->conList[j].size; k++){
778                         c[l++] = currentState->conList[j].result->data[k];
779                 }
780         }
781
782         if(OMX_DEBUG) { Rprintf("-=======================================================-\n"); }
783
784         return;
785
786 }
787
788 /****** HELPER FUNCTIONS ******* (integrate into omxOptimizer) */
789 /* Sub Free Vars Into Appropriate Slots */
790 void handleFreeVarList(omxState* os, double* x, int numVars) {
791
792         if(OMX_DEBUG) {
793                 Rprintf("Processing Free Parameter Estimates.\n");
794                 Rprintf("Number of free parameters is %d.\n", numVars);
795         }
796
797         if(numVars == 0) return;
798
799         omxFreeVar* freeVarList = os->freeVarList;
800         omxMatrix** matrixList = os->matrixList;
801
802         os->computeCount++;
803
804         if(OMX_VERBOSE) {
805                 Rprintf("--------------------------\n");
806                 Rprintf("Call: %d.%d (%d)\n", os->majorIteration, os->minorIteration, os->computeCount);
807                 Rprintf("Estimates: [");
808                 for(int k = 0; k < numVars; k++) {
809                         Rprintf(" %f", x[k]);
810                 }
811                 Rprintf("] \n");
812                 Rprintf("--------------------------\n");
813         }
814
815         /* Fill in Free Var Estimates */
816         for(int k = 0; k < numVars; k++) {
817                 // if(OMX_DEBUG) { Rprintf("%d: %f - %d\n", k,  x[k], freeVarList[k].numLocations); }
818                 for(int l = 0; l < freeVarList[k].numLocations; l++) {
819                         *(freeVarList[k].location[l]) = x[k];
820                         if(OMX_DEBUG) {
821                                 Rprintf("Setting location:%ld to value %f for var %d\n",
822                                         freeVarList[k].location[l], x[k], k);
823                         }
824                         omxMarkDirty(matrixList[freeVarList[k].matrices[l]]);
825                 }
826         }
827 }
828
829 /* get the list element named str, or return NULL */
830 SEXP getListElement(SEXP list, const char *str) {
831 /* Attribution: modified from the code given in Writing R Extensions */
832         SEXP elmt = R_NilValue, names = getAttrib(list, R_NamesSymbol);
833         int i;
834         for (i = 0; i < length(list); i++)
835                 if(strcmp(CHAR(STRING_ELT(names, i)), str) == 0) {
836                         elmt = VECTOR_ELT(list, i);
837                         break;
838                 }
839         return elmt;
840 }
841
842 SEXP getVar(SEXP str, SEXP env) {
843 /* Attribution: modified from the code given in Writing R Extensions */
844    SEXP ans;
845    if(!isString(str) || length(str) != 1)
846         error("getVar: variable name is not a single string");
847    if(!isEnvironment(env))
848         error("getVar: env should be an environment");
849    ans = findVar(install(CHAR(STRING_ELT(str, 0))), env);
850    return(ans);
851 }
852
853 /* Objective function for confidence interval limit finding. 
854  * Replaces the standard objective function when finding confidence intervals. */
855 void F77_SUB(limitObjectiveFunction)
856         (       int* mode, int* n, double* x, double* f, double* g, int* nstate ) {
857                 
858                 if(OMX_VERBOSE) Rprintf("Calculating interval %d, %s boundary:", currentState->currentInterval, (currentState->intervalList[currentState->currentInterval].calcLower?"lower":"upper"));
859
860                 F77_CALL(objectiveFunction)(mode, n, x, f, g, nstate);  // Standard objective function call
861
862                 omxConfidenceInterval *oCI = &(currentState->intervalList[currentState->currentInterval]);
863                 
864                 omxRecompute(oCI->matrix);
865                 
866                 double CIElement = omxMatrixElement(oCI->matrix, oCI->row, oCI->col);
867
868                 if(OMX_DEBUG) {
869                         Rprintf("Finding Confidence Interval Likelihoods: lbound is %f, ubound is %f, estimate likelihood is %f, and element current value is %f.\n",
870                                 oCI->lbound, oCI->ubound, *f, CIElement);
871                 }
872
873                 /* Catch boundary-passing condition */
874                 if(isnan(CIElement) || isinf(CIElement)) {
875                         omxRaiseError(currentState, -1, 
876                                 "Confidence interval is in a range that is currently incalculable. Add constraints to keep the value in the region where it can be calculated.");
877                         *mode = -1;
878                         return;
879                 }
880
881                 if(oCI->calcLower) {
882                         double diff = oCI->lbound - *f;         // Offset - likelihood
883                         *f = diff*diff + CIElement;
884                                 // Minimize element for lower bound.
885                 } else {
886                         double diff = oCI->ubound - *f;                 // Offset - likelihood
887                         *f = diff*diff - CIElement;
888                                 // Maximize element for upper bound.
889                 }
890
891                 if(OMX_DEBUG) {
892                         Rprintf("Interval Objective in previous iteration was calculated to be %f.\n", *f);
893                 }
894 }
895
896 void omxPopulateHessianWork(struct hess_struct *hess_work, double functionPrecision, int r, omxState* childState) {
897
898         omxObjective* oo = childState->objectiveMatrix->objective;
899         int numParams = childState->numFreeParams;
900
901         double* optima = childState->optimalValues;
902         double *freeParams = (double*) Calloc(numParams, double);
903
904         hess_work->numParams = numParams;
905         hess_work->functionPrecision = functionPrecision;
906         hess_work->r = r;
907         hess_work->Haprox = (double*) Calloc(r, double);                // Hessian Workspace
908         hess_work->Gaprox = (double*) Calloc(r, double);                // Gradient Workspace
909         hess_work->freeParams = freeParams;
910         for(int i = 0; i < numParams; i++) {
911                 freeParams[i] = optima[i];
912         }
913
914         omxMatrix *objectiveMatrix = oo->matrix;
915         hess_work->objectiveMatrix = objectiveMatrix;
916
917         if (objectiveMatrix->objective->repopulateFun != NULL) {        //  Just in case
918                 objectiveMatrix->objective->repopulateFun(objectiveMatrix->objective, freeParams, numParams);
919         } else {
920                 handleFreeVarList(childState, freeParams, numParams);
921         }
922         omxRecompute(objectiveMatrix);          // Initial recompute in case it matters.        
923         hess_work->f0 = omxMatrixElement(objectiveMatrix, 0, 0);
924 }
925
926 /**
927   @params i              parameter number
928   @params hess_work      local copy
929   @params optima         shared read-only variable
930   @params gradient       shared write-only variable
931   @params hessian        shared write-only variable
932  */
933 void omxEstimateHessianSingleParameter(int i, struct hess_struct* hess_work, 
934         double *optima, double *gradient, double *hessian) {
935
936         static const double v = 2.0; //Note: NumDeriv comments that this could be a parameter, but is hard-coded in the algorithm
937         static const double eps = 1E-4; // Kept here for access purposes.
938
939         int     numParams          = hess_work->numParams;         // read-only
940         double *Haprox             = hess_work->Haprox;
941         double *Gaprox             = hess_work->Gaprox;
942         double *freeParams         = hess_work->freeParams;
943         omxMatrix* objectiveMatrix = hess_work->objectiveMatrix; 
944         double functionPrecision   = hess_work->functionPrecision; // read-only
945         double f0                  = hess_work->f0;                // read-only
946         int    r                   = hess_work->r;                 // read-only
947
948
949         /* Part the first: Gradient and diagonal */
950         double iOffset = fabs(functionPrecision * optima[i]);
951         if(fabs(iOffset) < eps) iOffset += eps;
952         if(OMX_DEBUG) {Rprintf("Hessian estimation: iOffset: %f.\n", iOffset);}
953         for(int k = 0; k < r; k++) {                    // Decreasing step size, starting at k == 0
954                 if(OMX_DEBUG) {Rprintf("Hessian estimation: Parameter %d at refinement level %d (%f). One Step Forward.\n", i, k, iOffset);}
955                 freeParams[i] = optima[i] + iOffset;
956                 if (objectiveMatrix->objective->repopulateFun != NULL) {        //  Just in case
957                         objectiveMatrix->objective->repopulateFun(objectiveMatrix->objective, freeParams, numParams);
958                 } else {
959                         handleFreeVarList(currentState, freeParams, numParams);
960                 }
961                 omxRecompute(objectiveMatrix);
962                 double f1 = omxMatrixElement(objectiveMatrix, 0, 0);
963
964                 if(OMX_DEBUG) {Rprintf("Hessian estimation: One Step Back.\n");}
965
966                 freeParams[i] = optima[i] - iOffset;
967                 if (objectiveMatrix->objective->repopulateFun != NULL) {        // Just in case
968                         objectiveMatrix->objective->repopulateFun(objectiveMatrix->objective, freeParams, numParams);
969                 } else {
970                         handleFreeVarList(currentState, freeParams, numParams);
971                 }
972                 omxRecompute(objectiveMatrix);
973                 double f2 = omxMatrixElement(objectiveMatrix, 0, 0);
974
975                 Gaprox[k] = (f1 - f2) / (2.0*iOffset);                                          // This is for the gradient
976                 Haprox[k] = (f1 - 2.0 * f0 + f2) / (iOffset * iOffset);         // This is second derivative
977                 freeParams[i] = optima[i];                                                                      // Reset parameter value
978                 iOffset /= v;
979                 if(OMX_DEBUG) {Rprintf("Hessian estimation: (%d, %d)--Calculating F1: %f F2: %f, Haprox: %f...\n", i, i, f1, f2, Haprox[k]);}
980         }
981
982         for(int m = 1; m < r; m++) {                                            // Richardson Step
983                 for(int k = 0; k < (r - m); k++) {
984                         Gaprox[k] = (Gaprox[k+1] * pow(4.0, m) - Gaprox[k])/(pow(4.0, m)-1); // NumDeriv Hard-wires 4s for r here. Why?
985                         Haprox[k] = (Haprox[k+1] * pow(4.0, m) - Haprox[k])/(pow(4.0, m)-1); // NumDeriv Hard-wires 4s for r here. Why?
986                 }
987         }
988
989         if(OMX_DEBUG) { Rprintf("Hessian estimation: Populating Hessian (0x%x) at ([%d, %d] = %d) with value %f...\n", hessian, i, i, i*numParams+i, Haprox[0]); }
990         gradient[i] = Gaprox[0];                                                // NPSOL reports a gradient that's fine.  Why report two?
991         hessian[i*numParams + i] = Haprox[0];
992
993         /* Part the Second: Off-diagonals */                    // These go here because they depend on their associated diagonals
994         // for(int i = 0; i < numParams; i++) {                 // Which parameter (Again).
995         for(int l = (i-1); l >= 0; l--) {                               // Crossed with which parameter? (Start with diagonal)
996                 double iOffset = fabs(functionPrecision*optima[i]);
997                 if(fabs(iOffset) < eps) iOffset += eps;
998                 double lOffset = fabs(functionPrecision*optima[l]);
999                 if(fabs(lOffset) < eps) lOffset += eps;
1000
1001                 for(int k = 0; k < r; k++) {
1002                         freeParams[i] = optima[i] + iOffset;
1003                         freeParams[l] = optima[l] + lOffset;
1004                         if (objectiveMatrix->objective->repopulateFun != NULL) {        //  Just in case
1005                                 objectiveMatrix->objective->repopulateFun(objectiveMatrix->objective, freeParams, numParams);
1006                         } else {
1007                                 handleFreeVarList(currentState, freeParams, numParams);
1008                         }
1009                         omxRecompute(objectiveMatrix);
1010                         double f1 = omxMatrixElement(objectiveMatrix, 0, 0);
1011
1012                         if(OMX_DEBUG) {Rprintf("Hessian estimation: One Step Back.\n");}
1013
1014                         freeParams[i] = optima[i] - iOffset;
1015                         freeParams[l] = optima[l] - lOffset;
1016                         if (objectiveMatrix->objective->repopulateFun != NULL) {        // Just in case
1017                                 objectiveMatrix->objective->repopulateFun(objectiveMatrix->objective, freeParams, numParams);
1018                         } else {
1019                                 handleFreeVarList(currentState, freeParams, numParams);
1020                         }
1021                         omxRecompute(objectiveMatrix);
1022                         double f2 = omxMatrixElement(objectiveMatrix, 0, 0);
1023
1024                         Haprox[k] = (f1 - 2.0 * f0 + f2 - hessian[i*numParams+i]*iOffset*iOffset -
1025                                                 hessian[l*numParams+l]*lOffset*lOffset)/(2.0*iOffset*lOffset);
1026                         if(OMX_DEBUG) {Rprintf("Hessian first off-diagonal calculation: Haprox = %f, iOffset = %f, lOffset=%f from params %f, %f and %f, %f and %d (also: %f, %f and %f).\n", Haprox[k], iOffset, lOffset, f1, hessian[i*numParams+i], hessian[l*numParams+l], v, k, pow(v, k), functionPrecision*optima[i], functionPrecision*optima[l]);}
1027
1028                         freeParams[i] = optima[i];                              // Reset parameter values
1029                         freeParams[l] = optima[l];
1030
1031                         iOffset = iOffset / v;                                  //  And shrink step
1032                         lOffset = lOffset / v;
1033                 }
1034
1035                 for(int m = 1; m < r; m++) {                                            // Richardson Step
1036                         for(int k = 0; k < (r - m); k++) {
1037                                 if(OMX_DEBUG) {Rprintf("Hessian off-diagonal calculation: Haprox = %f, iOffset = %f, lOffset=%f from params %f, %f and %f, %f and %d (also: %f, %f and %f, and %f).\n", Haprox[k], iOffset, lOffset, functionPrecision, optima[i], optima[l], v, m, pow(4.0, m), functionPrecision*optima[i], functionPrecision*optima[l], k);}
1038                                 Haprox[k] = (Haprox[k+1] * pow(4.0, m) - Haprox[k]) / (pow(4.0, m)-1);
1039                         }
1040                 }
1041
1042                 if(OMX_DEBUG) {Rprintf("Hessian estimation: Populating Hessian (0x%x) at ([%d, %d] = %d and %d) with value %f...", hessian, i, l, i*numParams+l, l*numParams+i, Haprox[0]);}
1043                 hessian[i*numParams+l] = Haprox[0];
1044                 hessian[l*numParams+i] = Haprox[0];
1045         }
1046
1047         if(OMX_DEBUG) {Rprintf("Done with parameter %d.\n", i);}
1048
1049 }
1050
1051 /*************************************************************************************
1052  *
1053  *   omxEstimateHessian
1054  *
1055  *  author: tbrick, 2010-02-04
1056  *
1057  *  Based on code in the numDeriv library for R <citation needed> // TODO: Find NumDeriv Citation
1058  *
1059  *  @params functionPrecision   functional precision for the calculation
1060  *  @params r                                   number of repetitions for Richardson approximation
1061  *  @params currentState                the current omxState
1062  *
1063  ************************************************************************************/
1064 unsigned short omxEstimateHessian(int numHessians, double functionPrecision, int r, omxState* parentState) {
1065
1066         // TODO: Check for nonlinear constraints and adjust algorithm accordingly.
1067         // TODO: Allow more than one hessian value for calculation
1068
1069         if(numHessians > 1) {
1070                 error("NYI: Cannot yet calculate more than a single hessian per optimization.\n");
1071         }
1072
1073         if(numHessians == 0) return FALSE;
1074         int numChildren = parentState->numChildren;
1075         int numParams = parentState->numFreeParams;
1076         int i;
1077
1078         omxObjective* parent_oo = parentState->objectiveMatrix->objective;
1079         double* parent_optima = parentState->optimalValues;
1080
1081     struct hess_struct* hess_work;
1082         if (numChildren < 2) {
1083                 hess_work = Calloc(1, struct hess_struct);
1084                 omxPopulateHessianWork(hess_work, functionPrecision, r, parentState);
1085         } else {
1086                 hess_work = Calloc(numChildren, struct hess_struct);
1087                 for(int i = 0; i < numChildren; i++) {
1088                         omxPopulateHessianWork(hess_work + i, functionPrecision, r, parentState->childList[i]);
1089                 }
1090         }
1091
1092         if(parent_oo->hessian == NULL) {
1093                 parent_oo->hessian = (double*) R_alloc(numParams * numParams, sizeof(double));
1094                 if(OMX_DEBUG) {Rprintf("Generated hessian memory, (%d x %d), at 0x%x.\n", numParams, numParams, parent_oo->hessian);}
1095         }
1096
1097         if(parent_oo->gradient == NULL) {
1098                 parent_oo->gradient = (double*) R_alloc(numParams, sizeof(double));
1099                 if(OMX_DEBUG) {Rprintf("Generated gradient memory, (%d), at 0x%x.\n", numParams, parent_oo->gradient);}
1100         }
1101   
1102         double* parent_gradient = parent_oo->gradient;
1103         double* parent_hessian = parent_oo->hessian;
1104
1105 #ifndef SUPPORT_OPENMP
1106         for(i = 0; i < numParams; i++) {
1107                 omxEstimateHessianSingleParameter(i, hess_work, 
1108                         parent_optima, parent_gradient, parent_hessian);
1109         }
1110 #else
1111     int parallelism = (numChildren == 0) ? 1 : numChildren;
1112
1113         #pragma omp parallel for num_threads(parallelism) 
1114         for(i = 0; i < numParams; i++) {
1115                 int threadId = (numChildren < 2) ? 0 : omp_get_thread_num();
1116                 omxEstimateHessianSingleParameter(i, hess_work + threadId, 
1117                         parent_optima, parent_gradient, parent_hessian);
1118         }
1119 #endif
1120
1121         if(OMX_DEBUG) {Rprintf("Hessian Computation complete.\n");}
1122
1123         if (numChildren < 2) {
1124                 Free(hess_work->Haprox);
1125                 Free(hess_work->Gaprox);
1126                 Free(hess_work->freeParams);
1127             Free(hess_work);
1128         } else {
1129                 for(int i = 0; i < numChildren; i++) {
1130                         Free((hess_work + i)->Haprox);
1131                         Free((hess_work + i)->Gaprox);
1132                         Free((hess_work + i)->freeParams);
1133                 }
1134                 Free(hess_work);
1135         }
1136         return TRUE;
1137
1138 }