Add varargs replacement for omxRaiseError
[openmx:openmx.git] / src / omxState.c
1 /*
2  *  Copyright 2007-2012 The OpenMx Project
3  *
4  *  Licensed under the Apache License, Version 2.0 (the "License");
5  *  you may not use this file except in compliance with the License.
6  *  You may obtain a copy of the License at
7  *
8  *       http://www.apache.org/licenses/LICENSE-2.0
9  *
10  *  Unless required by applicable law or agreed to in writing, software
11  *  distributed under the License is distributed on an "AS IS" BASIS,
12  *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  *  See the License for the specific language governing permissions and
14  *  limitations under the License.
15  */
16
17 /***********************************************************
18 *
19 *  omxState.cc
20 *
21 *  Created: Timothy R. Brick    Date: 2009-06-05
22 *
23 *       omxStates carry the current optimization state
24 *
25 **********************************************************/
26
27 #include "omxState.h"
28
29 /* Initialize and Destroy */
30         void omxInitState(omxState* state, omxState *parentState, int numThreads) {
31                 int i;
32                 state->numMats = 0;
33                 state->numAlgs = 0;
34                 state->numExpects = 0;
35         state->numConstraints = 0;
36                 state->numData = 0;
37                 state->numFreeParams = 0;
38                 if (numThreads > 1) {
39                         state->numChildren = numThreads;
40                         state->childList = (omxState**) Calloc(numThreads, omxState*);
41                         for(i = 0; i < numThreads; i++) {
42                                 state->childList[i] = (omxState*) R_alloc(1, sizeof(omxState));
43                                 omxInitState(state->childList[i], state, 1);
44                         }
45                 } else {
46                 state->numChildren = 0;
47                         state->childList = NULL;
48                 }
49                 state->matrixList = NULL;
50                 state->algebraList = NULL;
51                 state->expectationList = NULL;
52                 state->parentState = parentState;
53                 state->dataList = NULL;
54                 state->fitMatrix = NULL;
55                 state->hessian = NULL;
56                 state->conList = NULL;
57                 state->freeVarList = NULL;
58                 state->optimizerState = NULL;
59                 state->optimalValues = NULL;
60                 state->optimum = 9999999999;
61
62                 state->majorIteration = 0;
63                 state->minorIteration = 0;
64                 state->startTime = 0;
65                 state->endTime = 0;
66                 state->numCheckpoints = 0;
67                 state->checkpointList = NULL;
68                 state->chkptText1 = NULL;
69                 state->chkptText2 = NULL;
70
71         state->currentInterval = -1;
72
73                 state->computeCount = -1;
74                 state->currentRow = -1;
75
76                 state->statusCode = 0;
77                 strncpy(state->statusMsg, "", 1);
78         }
79
80         void omxFillState(omxState* state, /*omxOptimizer *oo,*/ omxMatrix** matrixList,
81                                                 omxMatrix** algebraList, omxData** dataList, omxMatrix* fitFunction) {
82                 error("NYI: Can't fill a state from outside yet. Besides, do you really need a single function to do this?");
83         }
84         
85         omxState* omxGetState(omxState* os, int stateNumber) {
86                 // TODO: Need to implement a smarter way to enumerate children
87                 if(stateNumber == 0) return os;
88                 if((stateNumber-1) < os->numChildren) {
89                         return(os->childList[stateNumber-1]);
90                 } else {
91                         // TODO: Account for unequal numbers of grandchild states
92                         int subState = (stateNumber - os->numChildren - 1);
93                         return omxGetState(os->childList[subState % os->numChildren], subState / os->numChildren);
94                 }
95         }
96
97         void omxSetMajorIteration(omxState *state, int value) {
98                 state->majorIteration = value;
99                 for(int i = 0; i < state->numChildren; i++) {
100                         omxSetMajorIteration(state->childList[i], value);
101                 }
102         }
103
104         void omxSetMinorIteration(omxState *state, int value) {
105                 state->minorIteration = value;
106                 for(int i = 0; i < state->numChildren; i++) {
107                         omxSetMinorIteration(state->childList[i], value);
108                 }
109         }
110         
111         void omxDuplicateState(omxState* tgt, omxState* src) {
112                 tgt->numMats                    = src->numMats;
113                 tgt->numAlgs                    = src->numAlgs;
114                 tgt->numExpects                 = src->numExpects;
115                 tgt->numData                    = src->numData;
116                 tgt->dataList                   = src->dataList;
117                 tgt->numChildren                = 0;
118                 
119                 // Duplicate matrices and algebras and build parentLists.
120                 tgt->parentState                = src;
121                 tgt->matrixList                 = (omxMatrix**) R_alloc(tgt->numMats, sizeof(omxMatrix*));
122                 tgt->expectationList    = (omxExpectation**) R_alloc(tgt->numExpects, sizeof(omxExpectation*));
123                 tgt->algebraList                = (omxMatrix**) R_alloc(tgt->numAlgs, sizeof(omxMatrix*));
124                 tgt->markMatrices               = (int*) R_alloc(tgt->numMats + tgt->numAlgs, sizeof(int));
125
126                 memcpy(tgt->markMatrices, src->markMatrices, (tgt->numMats + tgt->numAlgs) * sizeof(int));
127                                 
128                 memset(tgt->matrixList, 0, sizeof(omxMatrix*) * tgt->numMats);
129                 memset(tgt->algebraList, 0, sizeof(omxMatrix*) * tgt->numAlgs);
130                 memset(tgt->expectationList, 0, sizeof(omxExpectation*) * tgt->numExpects);
131
132                 for(int j = 0; j < tgt->numMats; j++) {
133                         // TODO: Smarter inference for which matrices to duplicate
134                         tgt->matrixList[j] = omxDuplicateMatrix(src->matrixList[j], tgt);
135                 }
136
137                 tgt->numConstraints     = src->numConstraints;
138                 tgt->conList                    = (omxConstraint*) R_alloc(tgt->numConstraints, sizeof(omxConstraint));
139                 for(int j = 0; j < tgt->numConstraints; j++) {
140                         tgt->conList[j].size   = src->conList[j].size;
141                         tgt->conList[j].opCode = src->conList[j].opCode;
142                         tgt->conList[j].lbound = src->conList[j].lbound;
143                         tgt->conList[j].ubound = src->conList[j].ubound;
144                         tgt->conList[j].result = omxDuplicateMatrix(src->conList[j].result, tgt);
145                 }
146
147                 for(int j = 0; j < tgt->numAlgs; j++) {
148                         // TODO: Smarter inference for which algebras to duplicate
149                         tgt->algebraList[j] = omxDuplicateMatrix(src->algebraList[j], tgt);
150                 }
151
152                 for(int j = 0; j < tgt->numExpects; j++) {
153                         // TODO: Smarter inference for which expectations to duplicate
154                         tgt->expectationList[j] = omxDuplicateExpectation(src->expectationList[j], tgt);
155                 }
156
157                 for(int j = 0; j < tgt->numAlgs; j++) {
158                         omxDuplicateAlgebra(tgt->algebraList[j], src->algebraList[j], tgt);
159                 }
160
161                 for(int j = 0; j < tgt->numExpects; j++) {
162                         // TODO: Smarter inference for which expectations to duplicate
163                         omxCompleteExpectation(tgt->expectationList[j]);
164                 }
165
166                 tgt->childList                  = NULL;
167
168                 tgt->fitMatrix  = omxLookupDuplicateElement(tgt, src->fitMatrix);
169                 tgt->hessian                    = src->hessian;
170
171                 tgt->numFreeParams                      = src->numFreeParams;
172                 tgt->freeVarList                = (omxFreeVar*) R_alloc(tgt->numFreeParams, sizeof(omxFreeVar));
173                 for(int j = 0; j < tgt->numFreeParams; j++) {
174                         int nLocs                                                       = src->freeVarList[j].numLocations;
175                         int numDeps                                                     = src->freeVarList[j].numDeps;
176
177                         tgt->freeVarList[j].lbound                      = src->freeVarList[j].lbound;
178                         tgt->freeVarList[j].ubound                      = src->freeVarList[j].ubound;
179                         tgt->freeVarList[j].numLocations        = nLocs;
180                         tgt->freeVarList[j].numDeps                     = numDeps;
181                         
182                         tgt->freeVarList[j].matrices            = (int*) R_alloc(nLocs, sizeof(int));
183                         tgt->freeVarList[j].row                         = (int*) R_alloc(nLocs, sizeof(int));
184                         tgt->freeVarList[j].col                         = (int*) R_alloc(nLocs, sizeof(int));
185                         tgt->freeVarList[j].deps                        = (int*) R_alloc(numDeps, sizeof(int));
186
187                         for(int k = 0; k < nLocs; k++) {
188                                 int theMat                                              = src->freeVarList[j].matrices[k];
189                                 int theRow                                              = src->freeVarList[j].row[k];
190                                 int theCol                                              = src->freeVarList[j].col[k];
191
192                                 tgt->freeVarList[j].matrices[k] = theMat;
193                                 tgt->freeVarList[j].row[k]              = theRow;
194                                 tgt->freeVarList[j].col[k]              = theCol;
195                                                                 
196                                 tgt->freeVarList[j].name                = src->freeVarList[j].name;
197                         }
198
199                         for(int k = 0; k < numDeps; k++) {
200                                 tgt->freeVarList[j].deps[k] = src->freeVarList[j].deps[k];
201                         }
202                 }
203                 
204                 if (src->optimizerState) {
205                         tgt->optimizerState                                     = (omxOptimizerState*) R_alloc(1, sizeof(omxOptimizerState));
206                         tgt->optimizerState->currentParameter   = src->optimizerState->currentParameter;
207                         tgt->optimizerState->offset                             = src->optimizerState->offset;
208                         tgt->optimizerState->alpha                              = src->optimizerState->alpha;
209                 }
210                 
211                 tgt->optimalValues              = src->optimalValues;
212                 tgt->optimum                    = 9999999999;
213                                   
214                 tgt->majorIteration     = 0;
215                 tgt->minorIteration     = 0;
216                 tgt->startTime                  = src->startTime;
217                 tgt->endTime                    = 0;
218                 
219                 // TODO: adjust checkpointing based on parallelization method
220                 tgt->numCheckpoints     = 0;
221                 tgt->checkpointList     = NULL;
222                 tgt->chkptText1                 = NULL;
223                 tgt->chkptText2                 = NULL;
224                                   
225                 tgt->computeCount               = src->computeCount;
226                 tgt->currentRow                 = src->currentRow;
227
228                 tgt->statusCode                 = 0;
229                 strncpy(tgt->statusMsg, "", 1);
230         }
231
232         omxMatrix* omxLookupDuplicateElement(omxState* os, omxMatrix* element) {
233                 if(os == NULL || element == NULL) return NULL;
234
235                 if (element->hasMatrixNumber) {
236                         int matrixNumber = element->matrixNumber;
237                         if (matrixNumber >= 0) {
238                                 return(os->algebraList[matrixNumber]);
239                         } else {
240                                 return(os->matrixList[-matrixNumber - 1]);
241                         }
242                 }
243
244                 omxConstraint* parentConList = os->parentState->conList;
245
246                 for(int i = 0; i < os->numConstraints; i++) {
247                         if(parentConList[i].result == element) {
248                                 if(os->conList[i].result != NULL) {   // Not sure of proper failure behavior here.
249                 return(os->conList[i].result);
250                                 } else {
251                     omxRaiseError(os, -2, "Initialization Copy Error: Constraint required but not yet processed.");
252             }
253                         }
254                 }
255
256                 return NULL;
257         }
258         
259         omxExpectation* omxLookupDuplicateExpectation(omxState* os, omxExpectation* ox) {
260                 if(os == NULL || ox == NULL) return NULL;
261
262                 return(os->expectationList[ox->expNum]);
263         }
264
265         int omxCountLeafNodes(omxState *state) {
266                 int children = state->numChildren;
267                 if (children == 0) {
268                         return(1);
269                 } else {
270                         int sum = 0;
271                         for(int i = 0; i < children; i++) {
272                                 sum += omxCountLeafNodes(state->childList[i]);
273                         }
274                         return(sum);
275                 }
276         }
277
278         /* Traverse to the root of the state hierarchy,
279          * and then count the number of leaf nodes */
280         int omxTotalThreadCount(omxState *state) {
281
282                 while(state->parentState != NULL) {
283                         state = state->parentState;
284                 }
285         
286                 return(omxCountLeafNodes(state));
287         }
288
289         void omxFreeState(omxState *state) {
290                 int k;
291
292                 if (state->numChildren > 0) {
293                         for(k = 0; k < state->numChildren; k++) {
294                                 omxFreeState(state->childList[k]);
295                         }
296                         Free(state->childList);
297                         state->childList = NULL;
298                         state->numChildren = 0;
299                 }
300
301                 if(OMX_DEBUG) { Rprintf("Freeing %d Algebras.\n", state->numAlgs);}
302                 for(k = 0; k < state->numAlgs; k++) {
303                         if(OMX_DEBUG) { Rprintf("Freeing Algebra %d at 0x%x.\n", k, state->algebraList[k]); }
304                         omxFreeAllMatrixData(state->algebraList[k]);
305                 }
306
307                 if(OMX_DEBUG) { Rprintf("Freeing %d Matrices.\n", state->numMats);}
308                 for(k = 0; k < state->numMats; k++) {
309                         if(OMX_DEBUG) { Rprintf("Freeing Matrix %d at 0x%x.\n", k, state->matrixList[k]); }
310                         omxFreeAllMatrixData(state->matrixList[k]);
311                 }
312                 
313                 if(OMX_DEBUG) { Rprintf("Freeing %d Model Expectations.\n", state->numExpects);}
314                 for(k = 0; k < state->numExpects; k++) {
315                         if(OMX_DEBUG) { Rprintf("Freeing Expectation %d at 0x%x.\n", k, state->expectationList[k]); }
316                         omxFreeExpectationArgs(state->expectationList[k]);
317                 }
318
319                 if(OMX_DEBUG) { Rprintf("Freeing %d Constraints.\n", state->numConstraints);}
320                 for(k = 0; k < state->numConstraints; k++) {
321                         if(OMX_DEBUG) { Rprintf("Freeing Constraint %d at 0x%x.\n", k, state->conList[k]); }
322                         omxFreeAllMatrixData(state->conList[k].result);
323                 }
324
325                 if(OMX_DEBUG) { Rprintf("Freeing %d Data Sets.\n", state->numData);}
326                 for(k = 0; k < state->numData; k++) {
327                         if(OMX_DEBUG) { Rprintf("Freeing Data Set %d at 0x%x.\n", k, state->dataList[k]); }
328                         omxFreeData(state->dataList[k]);
329                 }
330
331         if(OMX_DEBUG) {Rprintf("Freeing %d Children.\n", state->numChildren);}
332         for(k = 0; k < state->numChildren; k++) {
333                         if(OMX_DEBUG) { Rprintf("Freeing Child State %d at 0x%x.\n", k, state->childList[k]); }
334                         omxFreeState(state->childList[k]);            
335         }
336
337                 if(OMX_DEBUG) { Rprintf("Freeing %d Checkpoints.\n", state->numCheckpoints);}
338                 for(k = 0; k < state->numCheckpoints; k++) {
339                         if(OMX_DEBUG) { Rprintf("Freeing Data Set %d at 0x%x.\n", k, state->checkpointList[k]); }
340                         omxCheckpoint oC = state->checkpointList[k];
341                         switch(oC.type) {
342                                 case OMX_FILE_CHECKPOINT:
343                                         fclose(oC.file);
344                                         break;
345                                 case OMX_SOCKET_CHECKPOINT:     // NYI :::DEBUG:::
346                                         // TODO: Close socket
347                                         break;
348                                 case OMX_CONNECTION_CHECKPOINT: // NYI :::DEBUG:::
349                                         // Do nothing: this should be handled by R upon return.
350                                         break;
351                         }
352                         if(state->chkptText1 != NULL) {
353                                 Free(state->chkptText1);
354                         }
355                         if(state->chkptText2 != NULL) {
356                                 Free(state->chkptText2);
357                         }
358                         // Checkpoint list itself is freed by R.
359                 }
360
361                 if(OMX_DEBUG) { Rprintf("State Freed.\n");}
362         }
363
364         void omxSaveState(omxState *os, double* freeVals, double minimum) {
365                 if(os->optimalValues == NULL) {
366                         os->optimalValues = (double*) R_alloc(os->numFreeParams, sizeof(double));
367                 }
368
369                 for(int i = 0; i < os->numFreeParams; i++) {
370                         os->optimalValues[i] = freeVals[i];
371                 }
372                 os->optimum = minimum;
373                 os->optimumStatus = os->statusCode;
374                 strncpy(os->optimumMsg, os->statusMsg, 250);
375         }
376
377         void omxResetStatus(omxState *state) {
378                 int numChildren = state->numChildren;
379                 state->statusCode = 0;
380                 state->statusMsg[0] = '\0';
381                 for(int i = 0; i < numChildren; i++) {
382                         omxResetStatus(state->childList[i]);
383                 }
384         }
385
386 void omxRaiseErrorf(omxState *state, char* errorMsg, ...)
387 {
388         va_list ap;
389         va_start(ap, errorMsg);
390         int fit = vsnprintf(state->statusMsg, MAX_STRING_LEN, errorMsg, ap);
391         va_end(ap);
392         if(OMX_DEBUG) {
393                 if (!(fit > -1 && fit < MAX_STRING_LEN)) {
394                         Rprintf("Error exceeded maximum length: %s\n", errorMsg);
395                 } else {
396                         Rprintf("Error raised: %s\n", state->statusMsg);
397                 }
398         }
399         state->statusCode = -1;  // this provides no additional information beyond errorMsg[0]!=0 TODO
400 }
401
402         void omxRaiseError(omxState *state, int errorCode, char* errorMsg) {
403                 if(OMX_DEBUG && errorCode) { Rprintf("Error %d raised: %s\n", errorCode, errorMsg);}
404                 if(OMX_DEBUG && !errorCode) { Rprintf("Error status cleared."); }
405                 state->statusCode = errorCode;
406                 strncpy(state->statusMsg, errorMsg, 249);
407                 state->statusMsg[249] = '\0';
408                 if(state->computeCount <= 0 && errorCode < 0) {
409                         state->statusCode--;                    // Decrement status for init errors.
410                 }
411         }
412
413         void omxStateNextRow(omxState *state) {
414                 state->currentRow++;
415         };
416
417         void omxStateNextEvaluation(omxState *state) {
418                 state->currentRow = -1;
419                 state->computeCount++;
420         };
421
422         void omxWriteCheckpointHeader(omxState *os, omxCheckpoint* oC) {
423                 // FIXME: Is it faster to allocate this on the stack?
424                 os->chkptText1 = (char*) Calloc((24 + 15 * os->numFreeParams), char);
425                 os->chkptText2 = (char*) Calloc(1.0 + 15.0 * os->numFreeParams*
426                         (os->numFreeParams + 1.0) / 2.0, char);
427                 if (oC->type == OMX_FILE_CHECKPOINT) {
428                         fprintf(oC->file, "iterations\ttimestamp\tobjective\t");
429                         for(int j = 0; j < os->numFreeParams; j++) {
430                                 if(strcmp(os->freeVarList[j].name, CHAR(NA_STRING)) == 0) {
431                                         fprintf(oC->file, "%s", os->freeVarList[j].name);
432                                 } else {
433                                         fprintf(oC->file, "\"%s\"", os->freeVarList[j].name);
434                                 }
435                                 if (j != os->numFreeParams - 1) fprintf(oC->file, "\t");
436                         }
437                         fprintf(oC->file, "\n");
438                         fflush(oC->file);
439                 }
440         }
441  
442         void omxWriteCheckpointMessage(omxState *os, char *msg) {
443                 for(int i = 0; i < os->numCheckpoints; i++) {
444                         omxCheckpoint* oC = &(os->checkpointList[i]);
445                         if(os->chkptText1 == NULL) {    // First one: set up output
446                                 omxWriteCheckpointHeader(os, oC);
447                         }
448                         if (oC->type == OMX_FILE_CHECKPOINT) {
449                                 fprintf(oC->file, "%d \"%s\" NA ", os->majorIteration, msg);
450                                 for(int j = 0; j < os->numFreeParams; j++) {
451                                         fprintf(oC->file, "NA ");
452                                 }
453                                 fprintf(oC->file, "\n");
454                         }
455                 }
456         }
457
458         void omxSaveCheckpoint(omxState *os, double* x, double* f, int force) {
459                 time_t now = time(NULL);
460                 int soFar = now - os->startTime;                // Translated into minutes
461                 int n;
462                 for(int i = 0; i < os->numCheckpoints; i++) {
463                         n = 0;
464                         omxCheckpoint* oC = &(os->checkpointList[i]);
465                         // Check based on time            
466                         if((oC->time > 0 && (soFar - oC->lastCheckpoint) >= oC->time) || force) {
467                                 oC->lastCheckpoint = soFar;
468                                 n = 1;
469                         }
470                         // Or iterations
471                         if((oC->numIterations > 0 && (os->majorIteration - oC->lastCheckpoint) >= oC->numIterations) || force) {
472                                 oC->lastCheckpoint = os->majorIteration;
473                                 n = 1;
474                         }
475
476                         if(n) {         //In either case, save a checkpoint.
477                                 if(os->chkptText1 == NULL) {    // First one: set up output
478                                         omxWriteCheckpointHeader(os, oC);
479                                 }
480                                 char tempstring[25];
481                                 sprintf(tempstring, "%d", os->majorIteration);
482
483                                 if(strncmp(os->chkptText1, tempstring, strlen(tempstring))) {   // Returns zero if they're the same.
484                                         struct tm * nowTime = localtime(&now);                                          // So this only happens if the text is out of date.
485                                         strftime(tempstring, 25, "%b %d %Y %I:%M:%S %p", nowTime);
486                                         sprintf(os->chkptText1, "%d \"%s\" %9.5f", os->majorIteration, tempstring, f[0]);
487                                         for(int j = 0; j < os->numFreeParams; j++) {
488                                                 sprintf(tempstring, " %9.5f", x[j]);
489                                                 strncat(os->chkptText1, tempstring, 14);
490                                         }
491
492                                         double* hessian = os->hessian;
493                                         if(hessian != NULL) {
494                                                 for(int j = 0; j < os->numFreeParams; j++) {
495                                                         for(int k = 0; k <= j; k++) {
496                                                                 sprintf(tempstring, " %9.5f", hessian[j]);
497                                                                 strncat(os->chkptText2, tempstring, 14);
498                                                         }
499                                                 }
500                                         }
501                                 }
502
503                                 if(oC->type == OMX_FILE_CHECKPOINT) {
504                                         fprintf(oC->file, "%s", os->chkptText1);
505                                         if(oC->saveHessian)
506                                                 fprintf(oC->file, "%s", os->chkptText2);
507                                         fprintf(oC->file, "\n");
508                                         fflush(oC->file);
509                                 } else if(oC->type == OMX_SOCKET_CHECKPOINT) {
510                                         n = write(oC->socket, os->chkptText1, strlen(os->chkptText1));
511                                         if(n != strlen(os->chkptText1)) warning("Error writing checkpoint.");
512                                         if(oC->saveHessian) {
513                                                 n = write(oC->socket, os->chkptText2, strlen(os->chkptText2));
514                                                 if(n != strlen(os->chkptText1)) warning("Error writing checkpoint.");
515                                         }
516                                         n = write(oC->socket, "\n", 1);
517                                         if(n != 1) warning("Error writing checkpoint.");
518                                 } else if(oC->type == OMX_CONNECTION_CHECKPOINT) {
519                                         warning("NYI: R_connections are not yet implemented.");
520                                         oC->numIterations = 0;
521                                         oC->time = 0;
522                                 }
523                         }
524                 }
525         }