Rename ComputeEstimatedHessian to ComputeNumericDeriv
[openmx:openmx.git] / src / Compute.cpp
1 /*
2  *  Copyright 2013 The OpenMx Project
3  *
4  *  Licensed under the Apache License, Version 2.0 (the "License");
5  *  you may not use this file except in compliance with the License.
6  *  You may obtain a copy of the License at
7  *
8  *       http://www.apache.org/licenses/LICENSE-2.0
9  *
10  *   Unless required by applicable law or agreed to in writing, software
11  *   distributed under the License is distributed on an "AS IS" BASIS,
12  *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  *  See the License for the specific language governing permissions and
14  *  limitations under the License.
15  */
16
17 #include <algorithm>
18
19 #include "omxDefines.h"
20 #include "Compute.h"
21 #include "omxState.h"
22 #include "omxExportBackendState.h"
23 #include "omxRFitFunction.h"
24 #include "matrix.h"
25 #include "omxBuffer.h"
26
27 void pda(const double *ar, int rows, int cols);
28
29 void FitContext::init()
30 {
31         size_t numParam = varGroup->vars.size();
32         wanted = 0;
33         sampleSize = 0;  // remove? TODO
34         mac = parent? parent->mac : 0;
35         fit = parent? parent->fit : 0;
36         caution = parent? parent->caution : 0;
37         est = new double[numParam];
38         flavor = new int[numParam];
39         grad = new double[numParam];
40         hess = new double[numParam * numParam];
41         infoDefinite = NA_LOGICAL;
42         infoCondNum = NA_REAL;
43         infoA = NULL;
44         infoB = NULL;
45         ihess = new double[numParam * numParam];
46         stderrs = NULL;
47         changedEstimates = false;
48         inform = INFORM_UNINITIALIZED;
49         iterations = 0;
50 }
51
52 void FitContext::allocStderrs()
53 {
54         if (stderrs) return;
55
56         size_t numParam = varGroup->vars.size();
57         stderrs = new double[numParam];
58
59         for (size_t px=0; px < numParam; ++px) {
60                 stderrs[px] = NA_REAL;
61         }
62 }
63
64 FitContext::FitContext(std::vector<double> &startingValues)
65 {
66         parent = NULL;
67         varGroup = Global->freeGroup[FREEVARGROUP_ALL];
68         init();
69
70         size_t numParam = varGroup->vars.size();
71         if (startingValues.size() != numParam) {
72                 error("Got %d starting values for %d parameters",
73                       startingValues.size(), numParam);
74         }
75         memcpy(est, startingValues.data(), sizeof(double) * numParam);
76
77         for (size_t v1=0; v1 < numParam; v1++) {
78                 grad[v1] = nan("unset");
79                 for (size_t v2=0; v2 < numParam; v2++) {
80                         hess[v1 * numParam + v2] = nan("unset");
81                 }
82         }
83 }
84
85 FitContext::FitContext(FitContext *parent, FreeVarGroup *varGroup)
86 {
87         this->parent = parent;
88         this->varGroup = varGroup;
89         init();
90
91         FreeVarGroup *src = parent->varGroup;
92         FreeVarGroup *dest = varGroup;
93         size_t svars = parent->varGroup->vars.size();
94         size_t dvars = varGroup->vars.size();
95         if (dvars == 0) return;
96         mapToParent.resize(dvars);
97
98         size_t d1 = 0;
99         for (size_t s1=0; s1 < src->vars.size(); ++s1) {
100                 if (src->vars[s1] != dest->vars[d1]) continue;
101                 mapToParent[d1] = s1;
102                 est[d1] = parent->est[s1];
103
104                 if (parent->wanted & (FF_COMPUTE_GRADIENT | FF_COMPUTE_HESSIAN)) {
105                         grad[d1] = parent->grad[s1];
106
107                         size_t d2 = 0;
108                         for (size_t s2=0; s2 < src->vars.size(); ++s2) {
109                                 if (src->vars[s2] != dest->vars[d2]) continue;
110                                 hess[d1 * dvars + d2] = parent->hess[s1 * svars + s2];
111                                 if (++d2 == dvars) break;
112                         }
113                 }
114
115                 // ihess TODO?
116
117                 if (++d1 == dvars) break;
118         }
119         if (d1 != dvars) error("Parent free parameter group is not a superset");
120
121         wanted = parent->wanted;
122         infoDefinite = parent->infoDefinite;
123         infoCondNum = parent->infoCondNum;
124
125         // pda(parent->est, 1, svars);
126         // pda(est, 1, dvars);
127         // pda(parent->grad, 1, svars);
128         // pda(grad, 1, dvars);
129         // pda(parent->hess, svars, svars);
130         // pda(hess, dvars, dvars);
131 }
132
133 void FitContext::copyParamToModel(omxMatrix *mat)
134 { copyParamToModel(mat->currentState); }
135
136 void FitContext::copyParamToModel(omxMatrix *mat, double *at)
137 { copyParamToModel(mat->currentState, at); }
138
139 void FitContext::updateParent()
140 {
141         FreeVarGroup *src = varGroup;
142         FreeVarGroup *dest = parent->varGroup;
143         size_t svars = varGroup->vars.size();
144         size_t dvars = parent->varGroup->vars.size();
145
146         parent->wanted |= wanted;
147         parent->fit = fit;
148         parent->mac = mac;
149         parent->caution = caution;
150         parent->infoDefinite = infoDefinite;
151         parent->infoCondNum = infoCondNum;
152
153         // rewrite using mapToParent TODO
154
155         if (svars > 0) {
156                 size_t s1 = 0;
157                 for (size_t d1=0; d1 < dest->vars.size(); ++d1) {
158                         if (dest->vars[d1] != src->vars[s1]) continue;
159                         parent->est[d1] = est[s1];
160
161                         if (wanted & (FF_COMPUTE_GRADIENT | FF_COMPUTE_HESSIAN)) {
162                                 parent->grad[d1] = grad[s1];
163
164                                 size_t s2 = 0;
165                                 for (size_t d2=0; d2 < dest->vars.size(); ++d2) {
166                                         if (dest->vars[d2] != src->vars[s2]) continue;
167                                         parent->hess[d1 * dvars + d2] = hess[s1 * svars + s2];
168                                         if (++s2 == svars) break;
169                                 }
170                         }
171
172                         // ihess TODO?
173
174                         if (++s1 == svars) break;
175                 }
176                 if (wanted & FF_COMPUTE_PARAMFLAVOR) {
177                         for (size_t s1=0; s1 < src->vars.size(); ++s1) {
178                                 parent->flavor[mapToParent[s1]] = flavor[s1];
179                         }
180                 }
181                 if (stderrs) {
182                         parent->allocStderrs();
183                         for (size_t s1=0; s1 < src->vars.size(); ++s1) {
184                                 parent->stderrs[mapToParent[s1]] = stderrs[s1];
185                         }
186                 }
187         }
188         
189         // pda(est, 1, svars);
190         // pda(parent->est, 1, dvars);
191         // pda(grad, 1, svars);
192         // pda(parent->grad, 1, dvars);
193         // pda(hess, svars, svars);
194         // pda(parent->hess, dvars, dvars);
195 }
196
197 void FitContext::updateParentAndFree()
198 {
199         updateParent();
200         delete this;
201 }
202
203 void FitContext::log(const char *where)
204 {
205         log(where, wanted);
206 }
207
208 void FitContext::log(const char *where, int what)
209 {
210         size_t count = varGroup->vars.size();
211         std::string buf(where);
212         buf += " ---\n";
213         if (what & FF_COMPUTE_MAXABSCHANGE) buf += string_snprintf("MAC: %.5f\n", mac);
214         if (what & FF_COMPUTE_FIT) buf += string_snprintf("fit: %.5f\n", fit);
215         if (what & FF_COMPUTE_ESTIMATE) {
216                 buf += string_snprintf("est %lu: c(", count);
217                 for (size_t vx=0; vx < count; ++vx) {
218                         buf += string_snprintf("%.5f", est[vx]);
219                         if (vx < count - 1) buf += ", ";
220                 }
221                 buf += ")\n";
222         }
223         if (what & FF_COMPUTE_GRADIENT) {
224                 buf += string_snprintf("grad %lu: c(", count);
225                 for (size_t vx=0; vx < count; ++vx) {
226                         buf += string_snprintf("%.5f", grad[vx]);
227                         if (vx < count - 1) buf += ", ";
228                 }
229                 buf += ")\n";
230         }
231         if (what & (FF_COMPUTE_HESSIAN)) {
232                 buf += string_snprintf("hess %lux%lu: c(", count, count);
233                 for (size_t v1=0; v1 < count; ++v1) {
234                         for (size_t v2=0; v2 < count; ++v2) {
235                                 buf += string_snprintf("%.5f", hess[v1 * count + v2]);
236                                 if (v1 < count-1 || v2 < count-1) buf += ", ";
237                         }
238                         buf += "\n";
239                 }
240                 buf += ")\n";
241         }
242         if (what & FF_COMPUTE_IHESSIAN) {
243                 buf += string_snprintf("ihess %lux%lu: c(", count, count);
244                 for (size_t v1=0; v1 < count; ++v1) {
245                         for (size_t v2=0; v2 < count; ++v2) {
246                                 buf += string_snprintf("%.5f", ihess[v1 * count + v2]);
247                                 if (v1 < count-1 || v2 < count-1) buf += ", ";
248                         }
249                         buf += "\n";
250                 }
251                 buf += ")\n";
252         }
253         if (what & FF_COMPUTE_HGPROD) {
254                 buf += string_snprintf("ihess %%*%% grad %lu: list(", hgProd.size());
255                 for (size_t px=0; px < hgProd.size(); ++px) {
256                         buf += string_snprintf("c(%d, %d, %d)", hgProd[px].hentry,
257                                                hgProd[px].gentry, hgProd[px].dest);
258                         if (px < hgProd.size() - 1) buf += ", ";
259                 }
260                 buf += ")\n";
261         }
262         mxLogBig(buf);
263 }
264
265 static void _fixSymmetry(const char *name, double *mat, size_t numParam, bool force)
266 {
267         for (size_t h1=1; h1 < numParam; h1++) {
268                 for (size_t h2=0; h2 < h1; h2++) {
269                         if (!force && mat[h2 * numParam + h1] != 0) {
270                                 omxRaiseErrorf(globalState, "%s is not upper triangular", name);
271                                 break;
272                         }
273                         mat[h2 * numParam + h1] = mat[h1 * numParam + h2];
274                 }
275         }
276 }
277
278 void FitContext::fixHessianSymmetry(int want, bool force)
279 {
280         size_t numParam = varGroup->vars.size();
281
282         if (want & (FF_COMPUTE_HESSIAN)) {
283                 _fixSymmetry("Hessian/information", hess, numParam, force);
284         }
285
286         if (want & FF_COMPUTE_IHESSIAN) {
287                 _fixSymmetry("Inverse Hessian", ihess, numParam, force);
288         }
289 }
290
291 static void omxRepopulateRFitFunction(omxFitFunction* oo, double* x, int n)
292 {
293         omxRFitFunction* rFitFunction = (omxRFitFunction*)oo->argStruct;
294
295         SEXP theCall, estimate;
296
297         PROTECT(estimate = allocVector(REALSXP, n));
298         double *est = REAL(estimate);
299         for(int i = 0; i < n ; i++) {
300                 est[i] = x[i];
301         }
302
303         PROTECT(theCall = allocVector(LANGSXP, 4));
304
305         SETCAR(theCall, install("imxUpdateModelValues"));
306         SETCADR(theCall, rFitFunction->model);
307         SETCADDR(theCall, rFitFunction->flatModel);
308         SETCADDDR(theCall, estimate);
309
310         REPROTECT(rFitFunction->model = eval(theCall, R_GlobalEnv), rFitFunction->modelIndex);
311
312         UNPROTECT(2); // theCall, estimate
313 }
314
315 void FitContext::copyParamToModel(omxState* os)
316 {
317         copyParamToModel(os, est);
318 }
319
320 void FitContext::maybeCopyParamToModel(omxState* os)
321 {
322         if (changedEstimates) {
323                 copyParamToModel(os, est);
324                 changedEstimates = false;
325         }
326 }
327
328 void FitContext::copyParamToModel(omxState* os, double *at)
329 {
330         size_t numParam = varGroup->vars.size();
331         if(OMX_DEBUG) {
332                 mxLog("Copying %lu free parameter estimates to model %p", numParam, os);
333         }
334
335         if(numParam == 0) return;
336
337         // Confidence Intervals & Hessian Calculation probe the parameter space
338         // near the best estimate. If stale, we need to restore the best estimate
339         // before returning results to the user.
340         os->stale = at != est;
341
342         os->computeCount++;
343
344         if(OMX_VERBOSE) {
345                 std::string buf;
346                 buf += string_snprintf("Call: %d.%d (%ld) ", os->majorIteration, os->minorIteration, os->computeCount);
347                 buf += ("Estimates: [");
348                 for(size_t k = 0; k < numParam; k++) {
349                         buf += string_snprintf(" %f", at[k]);
350                 }
351                 buf += ("]\n");
352                 mxLogBig(buf);
353         }
354
355         for(size_t k = 0; k < numParam; k++) {
356                 omxFreeVar* freeVar = varGroup->vars[k];
357                 for(size_t l = 0; l < freeVar->locations.size(); l++) {
358                         omxFreeVarLocation *loc = &freeVar->locations[l];
359                         omxMatrix *matrix = os->matrixList[loc->matrix];
360                         int row = loc->row;
361                         int col = loc->col;
362                         omxSetMatrixElement(matrix, row, col, at[k]);
363                         if(OMX_DEBUG) {
364                                 mxLog("Setting location (%d, %d) of matrix %d to value %f for var %lu",
365                                         row, col, loc->matrix, at[k], k);
366                         }
367                 }
368         }
369
370         if (RFitFunction) omxRepopulateRFitFunction(RFitFunction, at, numParam);
371
372         varGroup->markDirty(os);
373
374         if (!os->childList) return;
375
376         for(int i = 0; i < Global->numChildren; i++) {
377                 copyParamToModel(os->childList[i], at);
378         }
379 }
380
381 double *FitContext::take(int want)
382 {
383         if (!(want & (wanted | FF_COMPUTE_ESTIMATE))) {
384                 error("Attempt to take %d but not available", want);
385         }
386
387         double *ret = NULL;
388         switch(want) {
389         case FF_COMPUTE_ESTIMATE:
390                 ret = est;
391                 est = NULL;
392                 break;
393         case FF_COMPUTE_HESSIAN:
394                 ret = hess;
395                 hess = NULL;
396                 break;
397         case FF_COMPUTE_IHESSIAN:
398                 ret = ihess;
399                 ihess = NULL;
400                 break;
401         default:
402                 error("Taking of %d is not implemented", want);
403         }
404         if (!ret) error("Attempt to take %d, already taken", want);
405         return ret;
406 }
407
408 void FitContext::preInfo()
409 {
410         size_t numParam = varGroup->vars.size();
411         size_t npsq = numParam * numParam;
412
413         if (!infoA) infoA = new double[npsq];
414         if (!infoB) infoB = new double[npsq];
415
416         switch (infoMethod) {
417         case INFO_METHOD_SANDWICH:
418         case INFO_METHOD_MEAT:
419                 OMXZERO(infoB, npsq);
420         case INFO_METHOD_BREAD:
421                 OMXZERO(infoA, npsq);
422                 break;
423         case INFO_METHOD_HESSIAN:
424                 OMXZERO(hess, npsq);
425                 break;
426         default:
427                 error("Unknown information matrix estimation method %d", infoMethod);
428         }
429 }
430
431 void FitContext::postInfo()
432 {
433         size_t numParam = varGroup->vars.size();
434         switch (infoMethod) {
435         case INFO_METHOD_SANDWICH:{
436                 omxBuffer<double> work(numParam * numParam);
437                 Matrix amat(infoA, numParam, numParam);
438                 InvertSymmetricIndef(amat, 'U');
439                 _fixSymmetry("InfoB", infoB, numParam, false);
440                 Matrix bmat(infoB, numParam, numParam);
441                 Matrix wmat(work.data(), numParam, numParam);
442                 Matrix hmat(ihess, numParam, numParam);
443                 SymMatrixMultiply('L', 'U', 1, 0, amat, bmat, wmat);
444                 SymMatrixMultiply('R', 'U', 1, 0, amat, wmat, hmat);
445                 wanted |= FF_COMPUTE_IHESSIAN;
446                 break;}
447         case INFO_METHOD_MEAT:
448                 // copy upper triangle only TODO
449                 for (size_t d1=0; d1 < numParam; ++d1) {
450                         for (size_t d2=0; d2 < numParam; ++d2) {
451                                 int cell = d1 * numParam + d2;
452                                 hess[cell] = infoB[cell];
453                         }
454                 }
455                 fixHessianSymmetry(FF_COMPUTE_HESSIAN);
456                 wanted |= FF_COMPUTE_HESSIAN;
457                 break;
458         case INFO_METHOD_BREAD:
459                 // copy upper triangle only TODO
460                 for (size_t d1=0; d1 < numParam; ++d1) {
461                         for (size_t d2=0; d2 < numParam; ++d2) {
462                                 int cell = d1 * numParam + d2;
463                                 hess[cell] = infoA[cell];
464                         }
465                 }
466                 fixHessianSymmetry(FF_COMPUTE_HESSIAN);
467                 wanted |= FF_COMPUTE_HESSIAN;
468                 break;
469         case INFO_METHOD_HESSIAN:
470                 if (Global->llScale > 0) {
471                         for (size_t d1=0; d1 < numParam; ++d1) {
472                                 for (size_t d2=0; d2 <= d1; ++d2) {
473                                         int cell = d1 * numParam + d2;
474                                         hess[cell] = -hess[cell];
475                                 }
476                         }
477                 }
478                 fixHessianSymmetry(FF_COMPUTE_HESSIAN);
479                 wanted |= FF_COMPUTE_HESSIAN;
480                 break;
481         default:
482                 error("Unknown information matrix estimation method %d", infoMethod);
483         }
484 }
485
486 bool FitContext::invertHessian()
487 {
488         if (wanted & FF_COMPUTE_IHESSIAN) return TRUE;
489         if (!(wanted & FF_COMPUTE_HESSIAN)) return FALSE;
490
491         int numParams = int(varGroup->vars.size());
492
493         // Populate upper triangle
494         for(int i = 0; i < numParams; i++) {
495                 for(int j = 0; j <= i; j++) {
496                         ihess[i*numParams+j] = hess[i*numParams+j];
497                 }
498         }
499
500         Matrix wmat(ihess, numParams, numParams);
501         InvertSymmetricIndef(wmat, 'U');
502         fixHessianSymmetry(FF_COMPUTE_IHESSIAN, true);
503         wanted |= FF_COMPUTE_IHESSIAN;
504         return TRUE;
505 }
506
507 FitContext::~FitContext()
508 {
509         if (est) delete [] est;
510         if (flavor) delete [] flavor;
511         if (grad) delete [] grad;
512         if (hess) delete [] hess;
513         if (ihess) delete [] ihess;
514         if (stderrs) delete [] stderrs;
515         if (infoA) delete [] infoA;
516         if (infoB) delete [] infoB;
517 }
518
519 omxFitFunction *FitContext::RFitFunction = NULL;
520
521 void FitContext::setRFitFunction(omxFitFunction *rff)
522 {
523         if (rff) {
524                 Global->numThreads = 1;
525                 if (RFitFunction) {
526                         error("You can only create 1 MxRFitFunction per independent model");
527                 }
528         }
529         RFitFunction = rff;
530 }
531
532 Ramsay1975::Ramsay1975(FitContext *fc, int flavor, double caution, int verbose,
533                        double minCaution)
534 {
535         this->fc = fc;
536         this->flavor = flavor;
537         this->verbose = verbose;
538         this->caution = caution;
539         this->minCaution = minCaution;
540         maxCaution = 0.0;
541         highWatermark = std::max(0.5, caution);  // arbitrary guess
542
543         numParam = fc->varGroup->vars.size();
544         prevAdj1.assign(numParam, 0);
545         prevAdj2.resize(numParam);
546         prevEst.resize(numParam);
547         memcpy(prevEst.data(), fc->est, sizeof(double) * numParam);
548 }
549
550 void Ramsay1975::recordEstimate(int px, double newEst)
551 {
552         omxFreeVar *fv = fc->varGroup->vars[px];
553         bool hitBound=false;
554         double param = newEst;
555         if (param < fv->lbound) {
556                 hitBound=true;
557                 param = prevEst[px] - (prevEst[px] - fv->lbound) / 2;
558         }
559         if (param > fv->ubound) {
560                 hitBound=true;
561                 param = prevEst[px] + (fv->ubound - prevEst[px]) / 2;
562         }
563         
564         prevAdj2[px] = prevAdj1[px];
565         prevAdj1[px] = param - prevEst[px];
566         
567         if (verbose >= 4) {
568                 std::string buf;
569                 buf += string_snprintf("~%d~%s: %.4f -> %.4f", px, fv->name, prevEst[px], param);
570                 if (hitBound) {
571                         buf += string_snprintf(" wanted %.4f but hit bound", newEst);
572                 }
573                 if (prevAdj1[px] * prevAdj2[px] < 0) {
574                         buf += " *OSC*";
575                 }
576                 buf += "\n";
577                 mxLogBig(buf);
578         }
579
580         fc->est[px] = param;
581         prevEst[px] = param;
582 }
583
584 void Ramsay1975::apply()
585 {
586         for (size_t px=0; px < numParam; ++px) {
587                 recordEstimate(px, (1 - caution) * fc->est[px] + caution * prevEst[px]);
588         }
589 }
590
591 void Ramsay1975::recalibrate(bool *restart)
592 {
593         double normPrevAdj2 = 0;
594         double normAdjDiff = 0;
595         std::vector<double> adjDiff(numParam);
596
597         // The choice of norm is also arbitrary. Other norms might work better.
598         for (size_t px=0; px < numParam; ++px) {
599                 if (fc->flavor[px] != flavor) continue;
600                 adjDiff[px] = prevAdj1[px] - prevAdj2[px];
601                 normPrevAdj2 += prevAdj2[px] * prevAdj2[px];
602         }
603
604         for (size_t px=0; px < numParam; ++px) {
605                 if (fc->flavor[px] != flavor) continue;
606                 normAdjDiff += adjDiff[px] * adjDiff[px];
607         }
608         if (normAdjDiff == 0) {
609                 return;
610                 //error("Ramsay: no free variables of flavor %d", flavor);
611         }
612
613         double ratio = sqrt(normPrevAdj2 / normAdjDiff);
614         //if (verbose >= 3) mxLog("Ramsay[%d]: sqrt(%.5f/%.5f) = %.5f",
615         // flavor, normPrevAdj2, normAdjDiff, ratio);
616
617         double newCaution = 1 - (1-caution) * ratio;
618         if (newCaution > .95) newCaution = .95;  // arbitrary guess
619         if (newCaution < 0) newCaution /= 2;     // don't get overconfident
620         if (newCaution < minCaution) newCaution = minCaution;
621         if (newCaution < caution) {
622                 caution = newCaution/3 + 2*caution/3;  // don't speed up too fast, arbitrary ratio
623         } else {
624                 caution = newCaution;
625         }
626         maxCaution = std::max(maxCaution, caution);
627         if (caution < highWatermark || (normPrevAdj2 < 1e-3 && normAdjDiff < 1e-3)) {
628                 if (verbose >= 3) mxLog("Ramsay[%d]: %.2f caution", flavor, caution);
629         } else {
630                 if (verbose >= 3) mxLog("Ramsay[%d]: caution %.2f > %.2f, extreme oscillation, restart recommended",
631                                         flavor, caution, highWatermark);
632                 *restart = TRUE;
633         }
634         highWatermark += .02; // arbitrary guess
635 }
636
637 void Ramsay1975::restart()
638 {
639         memcpy(prevEst.data(), fc->est, sizeof(double) * numParam);
640         prevAdj1.assign(numParam, 0);
641         prevAdj2.assign(numParam, 0);
642         highWatermark = 1 - (1 - highWatermark) * .5; // arbitrary guess
643         caution = std::max(caution, highWatermark);   // arbitrary guess
644         maxCaution = std::max(maxCaution, caution);
645         highWatermark = caution;
646         if (verbose >= 3) {
647                 mxLog("Ramsay[%d]: restart with %.2f caution %.2f highWatermark",
648                       flavor, caution, highWatermark);
649         }
650 }
651
652 omxCompute::omxCompute()
653 {
654         varGroup = NULL;
655 }
656
657 void omxCompute::collectResultsHelper(FitContext *fc, std::vector< omxCompute* > &clist,
658                                       LocalComputeResult *lcr, MxRList *out)
659 {
660         for (std::vector< omxCompute* >::iterator it = clist.begin(); it != clist.end(); ++it) {
661                 omxCompute *c1 = *it;
662                 FitContext *context = fc;
663                 if (fc->varGroup != c1->varGroup) {
664                         context = new FitContext(fc, c1->varGroup);
665                 }
666                 c1->collectResults(context, lcr, out);
667                 if (context != fc) context->updateParentAndFree();
668         }
669 }
670
671 void omxCompute::collectResults(FitContext *fc, LocalComputeResult *lcr, MxRList *out)
672 {
673         MxRList *slots = new MxRList();
674         reportResults(fc, slots, out);
675         if (slots->size()) {
676                 lcr->push_back(std::make_pair(computeId, slots));
677         } else {
678                 delete slots;
679         }
680 }
681
682 omxCompute::~omxCompute()
683 {}
684
685 void omxCompute::initFromFrontend(SEXP rObj)
686 {
687         SEXP slotValue;
688         PROTECT(slotValue = GET_SLOT(rObj, install("id")));
689         if (length(slotValue) == 1) {
690                 computeId = INTEGER(slotValue)[0];
691                 varGroup = Global->findVarGroup(computeId);
692         }
693
694         if (!varGroup) {
695                 if (!R_has_slot(rObj, install("free.set"))) {
696                         varGroup = Global->freeGroup[FREEVARGROUP_ALL];
697                 } else {
698                         PROTECT(slotValue = GET_SLOT(rObj, install("free.set")));
699                         if (length(slotValue) != 0) {
700                                 // it's a free.set with no free variables
701                                 varGroup = Global->findVarGroup(FREEVARGROUP_NONE);
702                         } else {
703                                 varGroup = Global->freeGroup[FREEVARGROUP_ALL];
704                         }
705                 }
706         }
707 }
708
709 class ComputeContainer : public omxCompute {
710         typedef omxCompute super;
711 protected:
712         std::vector< omxCompute* > clist;
713 public:
714         virtual void collectResults(FitContext *fc, LocalComputeResult *lcr, MxRList *out);
715         virtual double getOptimizerStatus();
716 };
717
718 void ComputeContainer::collectResults(FitContext *fc, LocalComputeResult *lcr, MxRList *out)
719 {
720         super::collectResults(fc, lcr, out);
721         collectResultsHelper(fc, clist, lcr, out);
722 }
723
724 double ComputeContainer::getOptimizerStatus()
725 {
726         // for backward compatibility, not indended to work generally
727         for (size_t cx=0; cx < clist.size(); ++cx) {
728                 double got = clist[cx]->getOptimizerStatus();
729                 if (got != NA_REAL) return got;
730         }
731         return NA_REAL;
732 }
733
734 class omxComputeSequence : public ComputeContainer {
735         typedef ComputeContainer super;
736
737  public:
738         virtual void initFromFrontend(SEXP rObj);
739         virtual void compute(FitContext *fc);
740         virtual ~omxComputeSequence();
741 };
742
743 class omxComputeIterate : public ComputeContainer {
744         typedef ComputeContainer super;
745         int maxIter;
746         double tolerance;
747         int verbose;
748
749  public:
750         virtual void initFromFrontend(SEXP rObj);
751         virtual void compute(FitContext *fc);
752         virtual ~omxComputeIterate();
753 };
754
755 class omxComputeOnce : public omxCompute {
756         typedef omxCompute super;
757         std::vector< omxMatrix* > algebras;
758         std::vector< omxExpectation* > expectations;
759         int verbose;
760         const char *context;
761         bool mac;
762         bool fit;
763         bool gradient;
764         bool hessian;
765         bool ihessian;
766         bool infoMat;
767         enum ComputeInfoMethod infoMethod;
768         bool hgprod;
769
770  public:
771         virtual void initFromFrontend(SEXP rObj);
772         virtual omxFitFunction *getFitFunction();
773         virtual void compute(FitContext *fc);
774         virtual void reportResults(FitContext *fc, MxRList *slots, MxRList *out);
775 };
776
777 class ComputeEM : public omxCompute {
778         typedef omxCompute super;
779         std::vector< omxExpectation* > expectations;
780         omxCompute *fit1;
781         omxCompute *fit2;
782         int maxIter;
783         int mstepIter;
784         int totalMstepIter;
785         double tolerance;
786         double semTolerance;
787         int verbose;
788         bool useRamsay;
789         bool information;
790         double *semMethod;
791         int semMethodLen;
792         bool semDebug;
793         std::vector<Ramsay1975*> ramsay;
794         double noiseTarget;
795         double noiseTolerance;
796         std::vector<double*> estHistory;
797         std::vector<double> probeOffset;
798         std::vector<double> diffWork;
799         std::vector<int> paramHistLen;
800         FitContext *recentFC;  //nice if can use std::unique_ptr
801         std::vector<double> optimum;
802         double bestFit;
803         static const double MIDDLE_START;
804         static const double MIDDLE_END;
805         size_t maxHistLen;
806         int semProbeCount;
807
808         void setExpectationContext(const char *context);
809         void probeEM(FitContext *fc, int vx, double offset, std::vector<double> *rijWork);
810         void recordDiff(int v1, std::vector<double> &rijWork, double *stdDiff, bool *mengOK);
811
812  public:
813         virtual void initFromFrontend(SEXP rObj);
814         virtual void compute(FitContext *fc);
815         virtual void collectResults(FitContext *fc, LocalComputeResult *lcr, MxRList *out);
816         virtual void reportResults(FitContext *fc, MxRList *slots, MxRList *out);
817         virtual double getOptimizerStatus();
818         virtual ~ComputeEM();
819 };
820
821 const double ComputeEM::MIDDLE_START = 0.21072103131565256273; // -log(.9)*2 constexpr
822 const double ComputeEM::MIDDLE_END = 0.0020010006671670687271; // -log(.999)*2
823
824 class ComputeStandardError : public omxCompute {
825         typedef omxCompute super;
826  public:
827         virtual void reportResults(FitContext *fc, MxRList *slots, MxRList *out);
828 };
829
830 class ComputeHessianQuality : public omxCompute {
831         typedef omxCompute super;
832  public:
833         virtual void reportResults(FitContext *fc, MxRList *slots, MxRList *out);
834 };
835
836 static class omxCompute *newComputeSequence()
837 { return new omxComputeSequence(); }
838
839 static class omxCompute *newComputeIterate()
840 { return new omxComputeIterate(); }
841
842 static class omxCompute *newComputeOnce()
843 { return new omxComputeOnce(); }
844
845 static class omxCompute *newComputeEM()
846 { return new ComputeEM(); }
847
848 static class omxCompute *newComputeStandardError()
849 { return new ComputeStandardError(); }
850
851 static class omxCompute *newComputeHessianQuality()
852 { return new ComputeHessianQuality(); }
853
854 struct omxComputeTableEntry {
855         char name[32];
856         omxCompute *(*ctor)();
857 };
858
859 static const struct omxComputeTableEntry omxComputeTable[] = {
860         {"MxComputeNumericDeriv", &newComputeNumericDeriv},
861         {"MxComputeGradientDescent", &newComputeGradientDescent},
862         {"MxComputeSequence", &newComputeSequence },
863         {"MxComputeIterate", &newComputeIterate },
864         {"MxComputeOnce", &newComputeOnce },
865         {"MxComputeNewtonRaphson", &newComputeNewtonRaphson},
866         {"MxComputeEM", &newComputeEM },
867         {"MxComputeStandardError", &newComputeStandardError},
868         {"MxComputeHessianQuality", &newComputeHessianQuality}
869 };
870
871 omxCompute *omxNewCompute(omxState* os, const char *type)
872 {
873         omxCompute *got = NULL;
874
875         for (size_t fx=0; fx < OMX_STATIC_ARRAY_SIZE(omxComputeTable); fx++) {
876                 const struct omxComputeTableEntry *entry = omxComputeTable + fx;
877                 if(strcmp(type, entry->name) == 0) {
878                         got = entry->ctor();
879                         break;
880                 }
881         }
882
883         if (!got) error("Compute %s is not implemented", type);
884
885         return got;
886 }
887
888 void omxComputeSequence::initFromFrontend(SEXP rObj)
889 {
890         super::initFromFrontend(rObj);
891
892         SEXP slotValue;
893         PROTECT(slotValue = GET_SLOT(rObj, install("steps")));
894
895         for (int cx = 0; cx < length(slotValue); cx++) {
896                 SEXP step = VECTOR_ELT(slotValue, cx);
897                 SEXP s4class;
898                 PROTECT(s4class = STRING_ELT(getAttrib(step, install("class")), 0));
899                 omxCompute *compute = omxNewCompute(globalState, CHAR(s4class));
900                 compute->initFromFrontend(step);
901                 if (isErrorRaised(globalState)) break;
902                 clist.push_back(compute);
903         }
904 }
905
906 void omxComputeSequence::compute(FitContext *fc)
907 {
908         for (size_t cx=0; cx < clist.size(); ++cx) {
909                 FitContext *context = fc;
910                 if (fc->varGroup != clist[cx]->varGroup) {
911                         context = new FitContext(fc, clist[cx]->varGroup);
912                 }
913                 clist[cx]->compute(context);
914                 if (context != fc) context->updateParentAndFree();
915                 if (isErrorRaised(globalState)) break;
916         }
917 }
918
919 omxComputeSequence::~omxComputeSequence()
920 {
921         for (size_t cx=0; cx < clist.size(); ++cx) {
922                 delete clist[cx];
923         }
924 }
925
926 void omxComputeIterate::initFromFrontend(SEXP rObj)
927 {
928         SEXP slotValue;
929
930         super::initFromFrontend(rObj);
931
932         PROTECT(slotValue = GET_SLOT(rObj, install("maxIter")));
933         maxIter = INTEGER(slotValue)[0];
934
935         PROTECT(slotValue = GET_SLOT(rObj, install("tolerance")));
936         tolerance = REAL(slotValue)[0];
937         if (tolerance <= 0) error("tolerance must be positive");
938
939         PROTECT(slotValue = GET_SLOT(rObj, install("steps")));
940
941         for (int cx = 0; cx < length(slotValue); cx++) {
942                 SEXP step = VECTOR_ELT(slotValue, cx);
943                 SEXP s4class;
944                 PROTECT(s4class = STRING_ELT(getAttrib(step, install("class")), 0));
945                 omxCompute *compute = omxNewCompute(globalState, CHAR(s4class));
946                 compute->initFromFrontend(step);
947                 if (isErrorRaised(globalState)) break;
948                 clist.push_back(compute);
949         }
950
951         PROTECT(slotValue = GET_SLOT(rObj, install("verbose")));
952         verbose = asInteger(slotValue);
953 }
954
955 void omxComputeIterate::compute(FitContext *fc)
956 {
957         int iter = 0;
958         double prevFit = 0;
959         double mac = tolerance * 10;
960         while (1) {
961                 for (size_t cx=0; cx < clist.size(); ++cx) {
962                         FitContext *context = fc;
963                         if (fc->varGroup != clist[cx]->varGroup) {
964                                 context = new FitContext(fc, clist[cx]->varGroup);
965                         }
966                         clist[cx]->compute(context);
967                         if (context != fc) context->updateParentAndFree();
968                         if (isErrorRaised(globalState)) break;
969                 }
970                 if (fc->wanted & FF_COMPUTE_MAXABSCHANGE) {
971                         if (fc->mac < 0) {
972                                 warning("MAC estimated at %.4f; something is wrong", fc->mac);
973                                 break;
974                         } else {
975                                 mac = fc->mac;
976                                 if (verbose) mxLog("ComputeIterate: mac %.9g", mac);
977                         }
978                 }
979                 if (fc->wanted & FF_COMPUTE_FIT) {
980                         if (fc->fit == 0) {
981                                 warning("Fit estimated at 0; something is wrong");
982                                 break;
983                         }
984                         if (prevFit != 0) {
985                                 double change = prevFit - fc->fit;
986                                 if (verbose) mxLog("ComputeIterate: fit %.9g change %.9g", fc->fit, change);
987                                 mac = fabs(change);
988                         } else {
989                                 if (verbose) mxLog("ComputeIterate: initial fit %.9g", fc->fit);
990                         }
991                         prevFit = fc->fit;
992                 }
993                 if (!(fc->wanted & (FF_COMPUTE_MAXABSCHANGE | FF_COMPUTE_FIT))) {
994                         omxRaiseErrorf(globalState, "ComputeIterate: neither MAC nor fit available");
995                 }
996                 if (isErrorRaised(globalState) || ++iter > maxIter || mac < tolerance) break;
997         }
998 }
999
1000 omxComputeIterate::~omxComputeIterate()
1001 {
1002         for (size_t cx=0; cx < clist.size(); ++cx) {
1003                 delete clist[cx];
1004         }
1005 }
1006
1007 void ComputeEM::initFromFrontend(SEXP rObj)
1008 {
1009         recentFC = NULL;
1010
1011         SEXP slotValue;
1012         SEXP s4class;
1013
1014         super::initFromFrontend(rObj);
1015
1016         PROTECT(slotValue = GET_SLOT(rObj, install("maxIter")));
1017         maxIter = INTEGER(slotValue)[0];
1018
1019         PROTECT(slotValue = GET_SLOT(rObj, install("information")));
1020         information = asLogical(slotValue);
1021
1022         PROTECT(slotValue = GET_SLOT(rObj, install("semMethod")));
1023         semMethod = REAL(slotValue);
1024         semMethodLen = length(slotValue);
1025
1026         PROTECT(slotValue = GET_SLOT(rObj, install("semDebug")));
1027         semDebug = asLogical(slotValue);
1028
1029         PROTECT(slotValue = GET_SLOT(rObj, install("ramsay")));
1030         useRamsay = asLogical(slotValue);
1031
1032         PROTECT(slotValue = GET_SLOT(rObj, install("tolerance")));
1033         tolerance = REAL(slotValue)[0];
1034         if (tolerance <= 0) error("tolerance must be positive");
1035
1036         PROTECT(slotValue = GET_SLOT(rObj, install("noiseTarget")));
1037         noiseTarget = REAL(slotValue)[0];
1038         if (noiseTarget <= 0) error("noiseTarget must be positive");
1039
1040         PROTECT(slotValue = GET_SLOT(rObj, install("noiseTolerance")));
1041         noiseTolerance = REAL(slotValue)[0];
1042         if (noiseTolerance <= 0) error("noiseTolerance must be positive");
1043
1044         PROTECT(slotValue = GET_SLOT(rObj, install("what")));
1045         for (int wx=0; wx < length(slotValue); ++wx) {
1046                 int objNum = INTEGER(slotValue)[wx];
1047                 omxExpectation *expectation = globalState->expectationList[objNum];
1048                 setFreeVarGroup(expectation, varGroup);
1049                 omxCompleteExpectation(expectation);
1050                 expectations.push_back(expectation);
1051         }
1052
1053         PROTECT(slotValue = GET_SLOT(rObj, install("mstep.fit")));
1054         PROTECT(s4class = STRING_ELT(getAttrib(slotValue, install("class")), 0));
1055         fit1 = omxNewCompute(globalState, CHAR(s4class));
1056         fit1->initFromFrontend(slotValue);
1057
1058         PROTECT(slotValue = GET_SLOT(rObj, install("fit")));
1059         PROTECT(s4class = STRING_ELT(getAttrib(slotValue, install("class")), 0));
1060         fit2 = omxNewCompute(globalState, CHAR(s4class));
1061         fit2->initFromFrontend(slotValue);
1062
1063         PROTECT(slotValue = GET_SLOT(rObj, install("verbose")));
1064         verbose = asInteger(slotValue);
1065
1066         semTolerance = sqrt(tolerance);  // override needed?
1067 }
1068
1069 void ComputeEM::setExpectationContext(const char *context)
1070 {
1071         for (size_t wx=0; wx < expectations.size(); ++wx) {
1072                 omxExpectation *expectation = expectations[wx];
1073                 if (verbose >= 4) mxLog("ComputeEM: expectation[%lu] %s context %s", wx, expectation->name, context);
1074                 omxExpectationCompute(expectation, context);
1075         }
1076 }
1077
1078 void ComputeEM::probeEM(FitContext *fc, int vx, double offset, std::vector<double> *rijWork)
1079 {
1080         const int freeVarsEM = (int) fit1->varGroup->vars.size();
1081         const size_t freeVars = fc->varGroup->vars.size();
1082         const int base = paramHistLen[vx] * freeVarsEM;
1083         probeOffset[vx * maxHistLen + paramHistLen[vx]] = offset;
1084         paramHistLen[vx] += 1;
1085         memcpy(fc->est, optimum.data(), sizeof(double) * freeVars);
1086         FitContext *emfc = new FitContext(fc, fit1->varGroup);
1087
1088         double popt = optimum[emfc->mapToParent[vx]];
1089         double starting = popt + offset;
1090
1091         if (verbose >= 3) mxLog("ComputeEM: probe %d of param %d offset %.6f",
1092                                 paramHistLen[vx], vx, offset);
1093
1094         emfc->est[vx] = starting;
1095         emfc->copyParamToModel(globalState);
1096         fit1->compute(emfc);
1097
1098         for (int v1=0; v1 < freeVarsEM; ++v1) {
1099                 double got = (emfc->est[v1] - optimum[emfc->mapToParent[v1]]) / offset;
1100                 (*rijWork)[base + v1] = got;
1101         }
1102         //pda(rij.data() + base, 1, freeVarsEM);
1103         delete emfc;
1104         ++semProbeCount;
1105 }
1106
1107 void ComputeEM::recordDiff(int v1, std::vector<double> &rijWork,
1108                            double *stdDiff, bool *mengOK)
1109 {
1110         const int freeVarsEM = (int) fit1->varGroup->vars.size();
1111         int h1 = paramHistLen[v1]-2;
1112         int h2 = paramHistLen[v1]-1;
1113         double *rij1 = rijWork.data() + h1 * freeVarsEM;
1114         double *rij2 = rijWork.data() + h2 * freeVarsEM;
1115         double diff = 0;
1116         *mengOK = true;
1117         for (int v2=0; v2 < freeVarsEM; ++v2) {
1118                 double diff1 = fabs(rij1[v2] - rij2[v2]);
1119                 if (diff1 >= semTolerance) *mengOK = false;
1120                 diff += diff1;
1121         }
1122         double p1 = probeOffset[v1 * maxHistLen + h1];
1123         double p2 = probeOffset[v1 * maxHistLen + h2];
1124         double dist = fabs(p1 - p2);
1125         if (dist < tolerance/4) error("SEM: invalid probe offset distance %.9f", dist);
1126         *stdDiff = diff / (freeVarsEM * dist);
1127         diffWork[v1 * maxHistLen + h1] = *stdDiff;
1128         if (verbose >= 2) mxLog("ComputeEM: (%f,%f) width %f mengOK %d diff %f stdDiff %f",
1129                                 p1, p2, dist, *mengOK, diff, *stdDiff);
1130 }
1131
1132 void ComputeEM::compute(FitContext *fc)
1133 {
1134         int totalMstepIter = 0;
1135         int iter = 0;
1136         double prevFit = 0;
1137         double mac = tolerance * 10;
1138         bool converged = false;
1139         const size_t freeVars = fc->varGroup->vars.size();
1140         const int freeVarsEM = (int) fit1->varGroup->vars.size();
1141         bool in_middle = false;
1142         maxHistLen = 0;
1143         semProbeCount = 0;
1144
1145         OMXZERO(fc->flavor, freeVars);
1146
1147         FitContext *tmp = new FitContext(fc, fit1->varGroup);
1148         for (int vx=0; vx < freeVarsEM; ++vx) {
1149                 fc->flavor[tmp->mapToParent[vx]] = 1;
1150         }
1151         tmp->updateParentAndFree();
1152
1153         ramsay.push_back(new Ramsay1975(fc, int(ramsay.size()), 0, verbose, -1.25)); // other param
1154         ramsay.push_back(new Ramsay1975(fc, int(ramsay.size()), 0, verbose, -1));    // EM param
1155
1156         if (verbose >= 1) mxLog("ComputeEM: Welcome, tolerance=%g ramsay=%d info=%d flavors=%ld",
1157                                 tolerance, useRamsay, information, ramsay.size());
1158
1159         while (1) {
1160                 setExpectationContext("EM");
1161
1162                 if (recentFC) delete recentFC;
1163                 recentFC = new FitContext(fc, fit1->varGroup);
1164                 fit1->compute(recentFC);
1165                 if (recentFC->inform == INFORM_ITERATION_LIMIT) {
1166                         fc->inform = INFORM_ITERATION_LIMIT;
1167                         omxRaiseErrorf(globalState, "ComputeEM: iteration limited reached");
1168                         break;
1169                 }
1170                 mstepIter = recentFC->iterations;
1171                 recentFC->updateParent();
1172
1173                 setExpectationContext("");
1174
1175                 {
1176                         FitContext *context = fc;
1177                         if (fc->varGroup != fit2->varGroup) {
1178                                 context = new FitContext(fc, fit2->varGroup);
1179                         }
1180
1181                         // For IFA, PREOPTIMIZE updates latent distribution parameters
1182                         omxFitFunction *ff2 = fit2->getFitFunction();
1183                         if (ff2) omxFitFunctionCompute(ff2, FF_COMPUTE_PREOPTIMIZE, context);
1184
1185                         if (!useRamsay) {
1186                                 fc->maybeCopyParamToModel(globalState);
1187                         } else {
1188                                 context->updateParent();
1189
1190                                 bool wantRestart;
1191                                 if (iter > 3 && iter % 3 == 0) {
1192                                         for (size_t rx=0; rx < ramsay.size(); ++rx) {
1193                                                 ramsay[rx]->recalibrate(&wantRestart);
1194                                         }
1195                                 }
1196                                 for (size_t rx=0; rx < ramsay.size(); ++rx) {
1197                                         ramsay[rx]->apply();
1198                                 }
1199                                 fc->copyParamToModel(globalState);
1200                         }
1201
1202                         fit2->compute(context);
1203                         if (context != fc) context->updateParentAndFree();
1204                 }
1205
1206                 totalMstepIter += mstepIter;
1207
1208                 if (!(fc->wanted & FF_COMPUTE_FIT)) {
1209                         omxRaiseErrorf(globalState, "ComputeEM: fit not available");
1210                         break;
1211                 }
1212                 if (fc->fit == 0) {
1213                         omxRaiseErrorf(globalState, "Fit estimated at 0; something is wrong");
1214                         break;
1215                 }
1216                 double change = 0;
1217                 if (prevFit != 0) {
1218                         change = prevFit - fc->fit;
1219                         if (0 < change && change < MIDDLE_START) in_middle = true;
1220                         if (verbose >= 2) mxLog("ComputeEM[%d]: msteps %d fit %.9g change %.9g",
1221                                                 iter, mstepIter, fc->fit, change);
1222                         mac = fabs(change);
1223                 } else {
1224                         if (verbose >= 2) mxLog("ComputeEM: msteps %d initial fit %.9g",
1225                                                 mstepIter, fc->fit);
1226                 }
1227
1228                 prevFit = fc->fit;
1229                 converged = mac < tolerance;
1230                 if (isErrorRaised(globalState) || ++iter > maxIter || converged) break;
1231
1232                 // && change > MIDDLE_END
1233                 if (in_middle) estHistory.push_back(recentFC->take(FF_COMPUTE_ESTIMATE));
1234         }
1235
1236         fc->wanted = FF_COMPUTE_FIT | FF_COMPUTE_ESTIMATE;
1237         bestFit = fc->fit;
1238         if (verbose >= 1) mxLog("ComputeEM: cycles %d/%d total mstep %d fit %f",
1239                                 iter, maxIter,totalMstepIter, bestFit);
1240
1241         if (!converged || !information) return;
1242
1243         if (verbose >= 1) mxLog("ComputeEM: tolerance=%f semTolerance=%f noiseTarget=%f",
1244                                 tolerance, semTolerance, noiseTarget);
1245
1246         // what about latent distribution parameters? TODO
1247
1248         recentFC->fixHessianSymmetry(FF_COMPUTE_IHESSIAN);
1249         double *ihess = recentFC->take(FF_COMPUTE_IHESSIAN);
1250
1251         optimum.resize(freeVars);
1252         memcpy(optimum.data(), fc->est, sizeof(double) * freeVars);
1253
1254         if (semMethodLen == 0 || (semMethodLen==1 && semMethod[0] == 1)) {
1255                 maxHistLen = 4;
1256         } else if (semMethodLen==1 && semMethod[0] == 0) {
1257                 maxHistLen = estHistory.size();
1258         } else {
1259                 maxHistLen = semMethodLen;
1260         }
1261
1262         probeOffset.resize(maxHistLen * freeVarsEM);
1263         diffWork.resize(maxHistLen * freeVarsEM);
1264         paramHistLen.assign(freeVarsEM, 0);
1265
1266         omxBuffer<double> rij(freeVarsEM * freeVarsEM);
1267         setExpectationContext("EM");
1268
1269         for (int v1=0; v1 < freeVarsEM; ++v1) {
1270                 std::vector<double> rijWork(freeVarsEM * maxHistLen);
1271                 int pick = 0;
1272                 if (semMethodLen == 0 || (semMethodLen==1 && semMethod[0] == 1)) {
1273                         const double stepSize = tolerance;
1274
1275                         double offset1 = tolerance * 400;
1276                         double sign = 1;
1277                         if (estHistory.size()) {
1278                                 int hpick = 0;
1279                                 double popt = optimum[recentFC->mapToParent[v1]];
1280                                 sign = (popt < estHistory[hpick][v1])? 1 : -1;
1281                                 offset1 = fabs(estHistory[hpick][v1] - popt);
1282                                 if (offset1 < 10 * tolerance) offset1 = 10 * tolerance;
1283                         }
1284
1285                         probeEM(fc, v1, sign * offset1, &rijWork);
1286                         double offset2 = offset1 + stepSize;
1287                         probeEM(fc, v1, sign * offset2, &rijWork);
1288                         double diff;
1289                         bool mengOK;
1290                         recordDiff(v1, rijWork, &diff, &mengOK);
1291                         double midOffset = (offset1 + offset2) / 2;
1292
1293                         if (!(noiseTarget/noiseTolerance < diff && diff < noiseTarget*noiseTolerance)) {
1294                                 double coef = diff * midOffset * midOffset;
1295                                 offset1 = sqrt(coef/(noiseTarget * 1.05));
1296                                 probeEM(fc, v1, sign * offset1, &rijWork);
1297                                 if (semDebug) {
1298                                         offset2 = offset1 + stepSize;
1299                                         probeEM(fc, v1, sign * offset2, &rijWork);
1300                                         recordDiff(v1, rijWork, &diff, &mengOK);
1301                                 }
1302                                 pick = 2;
1303                         }
1304                 } else if (semMethodLen==1 && semMethod[0] == 0) {
1305                         if (!estHistory.size()) {
1306                                 if (verbose >= 1) mxLog("ComputeEM: no history available;"
1307                                                         " Tian, Cai, Thissen, Xin (2013) SEM requires convergence history");
1308                                 return;
1309                         }
1310                         for (size_t hx=0; hx < estHistory.size(); ++hx) {
1311                                 if (hx && fabs(estHistory[hx-1][v1] - estHistory[hx][v1]) < tolerance) break;
1312                                 double popt = optimum[recentFC->mapToParent[v1]];
1313                                 double offset1 = estHistory[hx][v1] - popt;
1314                                 probeEM(fc, v1, offset1, &rijWork);
1315                                 if (hx == 0) continue;
1316                                 pick = hx;
1317                                 double diff;
1318                                 bool mengOK;
1319                                 recordDiff(v1, rijWork, &diff, &mengOK);
1320                                 if (mengOK) break;
1321                         }
1322                 } else {
1323                         double sign = 1;
1324                         if (estHistory.size()) {
1325                                 int hpick = 0;
1326                                 double popt = optimum[recentFC->mapToParent[v1]];
1327                                 sign = (popt < estHistory[hpick][v1])? 1 : -1;
1328                         }
1329                         for (int hx=0; hx < semMethodLen; ++hx) {
1330                                 probeEM(fc, v1, sign * semMethod[hx], &rijWork);
1331                                 if (hx == 0) continue;
1332                                 double diff;
1333                                 bool mengOK;
1334                                 recordDiff(v1, rijWork, &diff, &mengOK);
1335                         }
1336                 }
1337
1338                 memcpy(rij.data() + v1 * freeVarsEM, rijWork.data() + pick*freeVarsEM, sizeof(double) * freeVarsEM);
1339                 if (verbose >= 2) mxLog("ComputeEM: param %d converged in %d probes",
1340                                         v1, paramHistLen[v1]);
1341         }
1342
1343         memcpy(fc->est, optimum.data(), sizeof(double) * freeVars);
1344         fc->copyParamToModel(globalState);
1345
1346         //pda(rij.data(), freeVarsEM, freeVarsEM);
1347
1348         // rij = I-rij
1349         for (int v1=0; v1 < freeVarsEM; ++v1) {
1350                 for (int v2=0; v2 < freeVarsEM; ++v2) {
1351                         int cell = v1 * freeVarsEM + v2;
1352                         double entry = rij[cell];
1353                         if (v1 == v2) entry = 1 - entry;
1354                         else entry = -entry;
1355                         rij[cell] = entry;
1356                 }
1357         }
1358         // make symmetric
1359         for (int v1=1; v1 < freeVarsEM; ++v1) {
1360                 for (int v2=0; v2 < v1; ++v2) {
1361                         int c1 = v1 * freeVarsEM + v2;
1362                         int c2 = v2 * freeVarsEM + v1;
1363                         double mean = (rij[c1] + rij[c2])/2;
1364                         rij[c1] = mean;
1365                         rij[c2] = mean;
1366                 }
1367         }
1368
1369         //mxLog("symm");
1370         //pda(rij.data(), freeVarsEM, freeVarsEM);
1371
1372         //pda(ihess, freeVarsEM, freeVarsEM);
1373
1374         // ihess = ihess %*% rij^{-1}
1375         if (0) {
1376                 omxBuffer<int> ipiv(freeVarsEM);
1377                 int info;
1378                 F77_CALL(dgesv)(&freeVarsEM, &freeVarsEM, rij.data(), &freeVarsEM,
1379                                 ipiv.data(), ihess, &freeVarsEM, &info);
1380                 if (info < 0) error("dgesv %d", info);
1381                 if (info > 0) {
1382                         if (verbose >= 1) mxLog("ComputeEM: EM map is not positive definite %d", info);
1383                         return;
1384                 }
1385         } else {
1386                 char uplo = 'U';
1387                 omxBuffer<int> ipiv(freeVarsEM);
1388                 int info;
1389                 double worksize;
1390                 int lwork = -1;
1391                 F77_CALL(dsysv)(&uplo, &freeVarsEM, &freeVarsEM, rij.data(), &freeVarsEM,
1392                                 ipiv.data(), ihess, &freeVarsEM, &worksize, &lwork, &info);
1393                 lwork = worksize;
1394                 omxBuffer<double> work(lwork);
1395                 F77_CALL(dsysv)(&uplo, &freeVarsEM, &freeVarsEM, rij.data(), &freeVarsEM,
1396                                 ipiv.data(), ihess, &freeVarsEM, work.data(), &lwork, &info);
1397                 if (info < 0) error("dsysv %d", info);
1398                 if (info > 0) {
1399                         if (verbose >= 1) mxLog("ComputeEM: Hessian from EM map is exactly singular %d", info);
1400                         return;
1401                 }
1402         }
1403
1404         for (int v1=0; v1 < freeVarsEM; ++v1) {
1405                 for (int v2=0; v2 <= v1; ++v2) {
1406                         fc->ihess[recentFC->mapToParent[v1] * freeVars + recentFC->mapToParent[v2]] =
1407                                 ihess[v1 * freeVarsEM + v2];
1408                 }
1409         }
1410         if (verbose >= 1) mxLog("ComputeEM: %d probes used to estimate Hessian", semProbeCount);
1411
1412         fc->wanted |= FF_COMPUTE_IHESSIAN;
1413         //pda(ihess, freeVarsEM, freeVarsEM);
1414
1415         delete [] ihess;
1416 }
1417
1418 void ComputeEM::collectResults(FitContext *fc, LocalComputeResult *lcr, MxRList *out)
1419 {
1420         super::collectResults(fc, lcr, out);
1421
1422         std::vector< omxCompute* > clist(2);
1423         clist[0] = fit1;
1424         clist[1] = fit2;
1425
1426         collectResultsHelper(fc, clist, lcr, out);
1427 }
1428
1429 void ComputeEM::reportResults(FitContext *fc, MxRList *slots, MxRList *)
1430 {
1431         slots->push_back(std::make_pair(mkChar("semProbeCount"),
1432                                         ScalarInteger(semProbeCount)));
1433
1434         size_t numFree = fc->varGroup->vars.size();
1435         if (!numFree) return;
1436
1437         if (semDebug) {
1438                 const int freeVarsEM = (int) fit1->varGroup->vars.size();
1439
1440                 SEXP Rpo;
1441                 PROTECT(Rpo = allocMatrix(REALSXP, maxHistLen, freeVarsEM));
1442                 memcpy(REAL(Rpo), probeOffset.data(), sizeof(double) * maxHistLen * freeVarsEM);
1443                 slots->push_back(std::make_pair(mkChar("probeOffset"), Rpo));
1444
1445                 SEXP Rdiff;
1446                 PROTECT(Rdiff = allocMatrix(REALSXP, maxHistLen, freeVarsEM));
1447                 memcpy(REAL(Rdiff), diffWork.data(), sizeof(double) * maxHistLen * freeVarsEM);
1448                 slots->push_back(std::make_pair(mkChar("semDiff"), Rdiff));
1449
1450                 SEXP Rphl;
1451                 PROTECT(Rphl = allocVector(INTSXP, freeVarsEM));
1452                 memcpy(INTEGER(Rphl), paramHistLen.data(), sizeof(int) * freeVarsEM);
1453                 slots->push_back(std::make_pair(mkChar("paramHistLen"), Rphl));
1454         }
1455 }
1456
1457 double ComputeEM::getOptimizerStatus()
1458 {
1459         // for backward compatibility, not indended to work generally
1460         return NA_REAL;
1461 }
1462
1463 ComputeEM::~ComputeEM()
1464 {
1465         for (size_t rx=0; rx < ramsay.size(); ++rx) {
1466                 delete ramsay[rx];
1467         }
1468         ramsay.clear();
1469
1470         delete fit1;
1471         delete fit2;
1472
1473         for (size_t hx=0; hx < estHistory.size(); ++hx) {
1474                 delete [] estHistory[hx];
1475         }
1476         estHistory.clear();
1477         if (recentFC) delete recentFC;
1478 }
1479
1480 enum ComputeInfoMethod omxCompute::stringToInfoMethod(const char *iMethod)
1481 {
1482         enum ComputeInfoMethod infoMethod;
1483         if (strcmp(iMethod, "sandwich")==0) {
1484                 infoMethod = INFO_METHOD_SANDWICH;
1485         } else if (strcmp(iMethod, "meat")==0) {
1486                 infoMethod = INFO_METHOD_MEAT;
1487         } else if (strcmp(iMethod, "bread")==0) {
1488                 infoMethod = INFO_METHOD_BREAD;
1489         } else if (strcmp(iMethod, "hessian")==0) {
1490                 infoMethod = INFO_METHOD_HESSIAN;
1491         } else {
1492                 error("Unknown information matrix estimation method '%s'", iMethod);
1493         }
1494         return infoMethod;
1495 }
1496
1497 void omxComputeOnce::initFromFrontend(SEXP rObj)
1498 {
1499         super::initFromFrontend(rObj);
1500
1501         SEXP slotValue;
1502         PROTECT(slotValue = GET_SLOT(rObj, install("what")));
1503         for (int wx=0; wx < length(slotValue); ++wx) {
1504                 int objNum = INTEGER(slotValue)[wx];
1505                 if (objNum >= 0) {
1506                         omxMatrix *algebra = globalState->algebraList[objNum];
1507                         if (algebra->fitFunction) {
1508                                 setFreeVarGroup(algebra->fitFunction, varGroup);
1509                                 omxCompleteFitFunction(algebra);
1510                         }
1511                         algebras.push_back(algebra);
1512                 } else {
1513                         omxExpectation *expectation = globalState->expectationList[~objNum];
1514                         setFreeVarGroup(expectation, varGroup);
1515                         omxCompleteExpectation(expectation);
1516                         expectations.push_back(expectation);
1517                 }
1518         }
1519
1520         PROTECT(slotValue = GET_SLOT(rObj, install("verbose")));
1521         verbose = asInteger(slotValue);
1522
1523         context = "";
1524
1525         PROTECT(slotValue = GET_SLOT(rObj, install("context")));
1526         if (length(slotValue) == 0) {
1527                 // OK
1528         } else if (length(slotValue) == 1) {
1529                 SEXP elem;
1530                 PROTECT(elem = STRING_ELT(slotValue, 0));
1531                 context = CHAR(elem);
1532         }
1533
1534         PROTECT(slotValue = GET_SLOT(rObj, install("maxAbsChange")));
1535         mac = asLogical(slotValue);
1536
1537         PROTECT(slotValue = GET_SLOT(rObj, install("fit")));
1538         fit = asLogical(slotValue);
1539
1540         PROTECT(slotValue = GET_SLOT(rObj, install("gradient")));
1541         gradient = asLogical(slotValue);
1542
1543         PROTECT(slotValue = GET_SLOT(rObj, install("hessian")));
1544         hessian = asLogical(slotValue);
1545
1546         PROTECT(slotValue = GET_SLOT(rObj, install("information")));
1547         infoMat = asLogical(slotValue);
1548
1549         if (hessian && infoMat) error("Cannot compute the Hessian and Fisher Information matrix simultaneously");
1550
1551         if (infoMat) {
1552                 const char *iMethod = "";
1553                 PROTECT(slotValue = GET_SLOT(rObj, install("info.method")));
1554                 if (length(slotValue) == 0) {
1555                         // OK
1556                 } else if (length(slotValue) == 1) {
1557                         SEXP elem;
1558                         PROTECT(elem = STRING_ELT(slotValue, 0));
1559                         iMethod = CHAR(elem);
1560                 }
1561
1562                 infoMethod = stringToInfoMethod(iMethod);
1563         }
1564
1565         PROTECT(slotValue = GET_SLOT(rObj, install("ihessian")));
1566         ihessian = asLogical(slotValue);
1567
1568         PROTECT(slotValue = GET_SLOT(rObj, install("hgprod")));
1569         hgprod = asLogical(slotValue);
1570
1571         if (algebras.size() == 1 && algebras[0]->fitFunction) {
1572                 omxFitFunction *ff = algebras[0]->fitFunction;
1573                 if (gradient && !ff->gradientAvailable) {
1574                         error("Gradient requested but not available");
1575                 }
1576                 if ((hessian || ihessian || hgprod) && !ff->hessianAvailable) {
1577                         // add a separate flag for hgprod TODO
1578                         error("Hessian requested but not available");
1579                 }
1580                 // add check for information TODO
1581         }
1582 }
1583
1584 omxFitFunction *omxComputeOnce::getFitFunction()
1585 {
1586         if (algebras.size() == 1 && algebras[0]->fitFunction) {
1587                 return algebras[0]->fitFunction;
1588         } else {
1589                 return NULL;
1590         }
1591 }
1592
1593 void omxComputeOnce::compute(FitContext *fc)
1594 {
1595         if (algebras.size()) {
1596                 int want = 0;
1597                 size_t numParam = fc->varGroup->vars.size();
1598                 if (mac) {
1599                         want |= FF_COMPUTE_MAXABSCHANGE;
1600                         fc->mac = 0;
1601                 }
1602                 if (fit) {
1603                         want |= FF_COMPUTE_FIT;
1604                         fc->fit = 0;
1605                 }
1606                 if (gradient) {
1607                         want |= FF_COMPUTE_GRADIENT;
1608                         OMXZERO(fc->grad, numParam);
1609                 }
1610                 if (hessian) {
1611                         want |= FF_COMPUTE_HESSIAN;
1612                         OMXZERO(fc->hess, numParam * numParam);
1613                 }
1614                 if (infoMat) {
1615                         want |= FF_COMPUTE_INFO;
1616                         fc->infoMethod = infoMethod;
1617                         fc->preInfo();
1618                 }
1619                 if (ihessian) {
1620                         want |= FF_COMPUTE_IHESSIAN;
1621                         OMXZERO(fc->ihess, numParam * numParam);
1622                 }
1623                 if (hgprod) {
1624                         want |= FF_COMPUTE_HGPROD;
1625                         fc->hgProd.resize(0);
1626                 }
1627                 if (!want) return;
1628
1629                 for (size_t wx=0; wx < algebras.size(); ++wx) {
1630                         omxMatrix *algebra = algebras[wx];
1631                         if (algebra->fitFunction) {
1632                                 if (verbose) mxLog("ComputeOnce: fit %p want %d",
1633                                                    algebra->fitFunction, want);
1634
1635                                 omxFitFunctionCompute(algebra->fitFunction, FF_COMPUTE_PREOPTIMIZE, fc);
1636                                 fc->maybeCopyParamToModel(globalState);
1637
1638                                 omxFitFunctionCompute(algebra->fitFunction, want, fc);
1639                                 fc->fit = algebra->data[0];
1640                                 if (infoMat) {
1641                                         fc->postInfo();
1642                                 }
1643                                 fc->fixHessianSymmetry(want);
1644                         } else {
1645                                 if (verbose) mxLog("ComputeOnce: algebra %p", algebra);
1646                                 omxForceCompute(algebra);
1647                         }
1648                 }
1649         } else if (expectations.size()) {
1650                 for (size_t wx=0; wx < expectations.size(); ++wx) {
1651                         omxExpectation *expectation = expectations[wx];
1652                         if (verbose) mxLog("ComputeOnce: expectation[%lu] %p context %s", wx, expectation, context);
1653                         omxExpectationCompute(expectation, context);
1654                 }
1655         }
1656 }
1657
1658 void omxComputeOnce::reportResults(FitContext *fc, MxRList *slots, MxRList *out)
1659 {
1660         if (algebras.size()==0 || algebras[0]->fitFunction == NULL) return;
1661
1662         omxMatrix *algebra = algebras[0];
1663         omxPopulateFitFunction(algebra, out);
1664 }
1665
1666 void ComputeStandardError::reportResults(FitContext *fc, MxRList *slots, MxRList *)
1667 {
1668         fc->allocStderrs();  // at least report NAs
1669
1670         if (!fc->invertHessian()) return;
1671
1672         int numParams = int(fc->varGroup->vars.size());
1673
1674         const double scale = fabs(Global->llScale);
1675
1676         // This function calculates the standard errors from the Hessian matrix
1677         // sqrt(scale * diag(solve(hessian)))
1678
1679         for(int i = 0; i < numParams; i++) {
1680                 double got = fc->ihess[i * numParams + i];
1681                 if (got <= 0) continue;
1682                 fc->stderrs[i] = sqrt(scale * got);
1683         }
1684 }
1685
1686 /*
1687 Date: Fri, 3 Jan 2014 14:02:34 -0600
1688 From: Michael Hunter <mhunter@ou.edu>
1689
1690 Determining positive definiteness of matrix is typically done by
1691 trying the Cholesky decomposition.  If it fails, the matrix is not
1692 positive definite; if it passes, the matrix is.  The benefit of the
1693 Cholesky is that it's much faster and easier to compute than a set of
1694 eigenvalues.
1695
1696 The BLAS/LAPACK routine DTRCO quickly computes a good approximation to the
1697 reciprocal condition number of a triangular matrix.  Hand it the Cholesky
1698 (a triangular matrix) the rest is history.  I don't think we need the
1699 exact condition number as long as it's just for finding very
1700 ill-conditioned problems.  For the solution to a linear system of
1701 equations, if you really care about the difference in precision between
1702 1e-14 and 1e-11, then the exact condition number is needed.  Otherwise, the
1703 approximation is faster and equally useful.
1704 */
1705 void ComputeHessianQuality::reportResults(FitContext *fc, MxRList *slots, MxRList *)
1706 {
1707         // See Luenberger & Ye (2008) Second Order Test (p. 190) and Condition Number (p. 239)
1708
1709         if (!(fc->wanted & (FF_COMPUTE_HESSIAN | FF_COMPUTE_IHESSIAN))) return;
1710
1711         int numParams = int(fc->varGroup->vars.size());
1712
1713         double *mat = (fc->wanted & FF_COMPUTE_IHESSIAN)? fc->ihess : fc->hess;
1714         omxBuffer<double> hessWork(numParams * numParams);
1715         memcpy(hessWork.data(), mat, sizeof(double) * numParams * numParams);
1716
1717         char jobz = 'N';
1718         char range = 'A';
1719         char uplo = 'U';
1720         double abstol = 0;
1721         int m;
1722         omxBuffer<double> w(numParams);
1723         double optWork;
1724         int lwork = -1;
1725         omxBuffer<int> iwork(5 * numParams);
1726         int info;
1727         double realIgn = 0;
1728         int intIgn = 0;
1729         F77_CALL(dsyevx)(&jobz, &range, &uplo, &numParams, hessWork.data(),
1730                          &numParams, &realIgn, &realIgn, &intIgn, &intIgn, &abstol, &m, w.data(),
1731                          NULL, &numParams, &optWork, &lwork, iwork.data(), NULL, &info);
1732
1733         lwork = optWork;
1734         omxBuffer<double> work(lwork);
1735         F77_CALL(dsyevx)(&jobz, &range, &uplo, &numParams, hessWork.data(),
1736                          &numParams, &realIgn, &realIgn, &intIgn, &intIgn, &abstol, &m, w.data(),
1737                          NULL, &numParams, work.data(), &lwork, iwork.data(), NULL, &info);
1738         if (info != 0) error("dsyevx %d", info);
1739
1740         bool definite = true;
1741         bool neg = w[0] < 0;
1742         for (int px=1; px < numParams; ++px) {
1743                 if ((w[px] < 0) ^ neg) {
1744                         definite = false;
1745                         break;
1746                 }
1747         }
1748
1749         fc->infoDefinite = definite;
1750
1751         if (definite) {
1752                 double ev[2] = { fabs(w[0]), fabs(w[numParams-1]) };
1753                 if (ev[0] < ev[1]) std::swap(ev[0], ev[1]);
1754                 double got = ev[0] / ev[1];
1755                 if (isfinite(got)) fc->infoCondNum = got;
1756         }
1757 }