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