- Main Page
- Namespaces
- Classes
- Files
- Related Pages

# /Users/fpy/titan-1-5-MBARI/Mers/Estimation/Discrete/ModeEstimationPerformance/randomMEscriptGen.cpp

    00001 //=============================================================================
    00002 //
    00003 // Copyright (c) 2004           Model-based Embedded and Robotic Systems Group
    00004 // All Rights Reserved          Massachusetts Institute of Technology
    00005 //
    00006 // This software is provided as is. Model-based Embedded and Robotic Systems
    00007 // (MERS) group does not assume any responsibility.
    00008 //
    00009 // Author:                      Oliver B. Martin
    00010 // Organization:        Model-based Embedded and Robotic Systems Group
    00011 //                                      Massachusetts Institute of Technology
    00012 //
    00013 // Random ME script generator. Reads in MOF and creates a random sequence of
    00014 //  observables and saves them into an ME script file
    00015 //
    00016 //=============================================================================
    00017
    00018 #ifdef _MSC_VER
    00019 #pragma warning( disable : 4786 )
    00020 #endif
    00021
    00022 #include <iostream>
    00023 #include <iomanip>
    00024 #include <fstream>
    00025 #include <vector>
    00026 #include <ctime>
    00027 #include <stdlib.h>
    00028
    00029 #include <Mers/Utils/Logging/logger.h>
    00030 #include <Mers/Utils/Lang/String.h>
    00031 #include <Mers/Utils/Containers/IntegerArray.h>
    00032
    00033 #include <Mers/Models/CCA/CCAModel.h>
    00034 #include <Mers/Models/CCA/Io/Mof/MOFReader.h>
    00035
    00036 #include <Mers/Models/Base/BaseModel.h>
    00037 #include <Mers/Models/Base/BaseModelVariable.h>
    00038 #include <Mers/Models/Base/Io/Mof/BaseModelReader.h>
    00039
    00040 #include <Mers/Estimation/Discrete/ModeEstimationPerformance/Monitor/ModeEstimatorDebug.h>
    00041 #include <Mers/Estimation/Discrete/ApproximateBeliefState/BFBSE/BFBSE.h>
    00042
    00043 #include <Mers/Estimation/Discrete/MEdriverUtils.h>
    00044
    00045 #include <Mers/Solvers/SAT/ValidSAT/ValidSAT.h>
    00046
    00047 using namespace Mers::Estimation::Discrete::ModeEstimationPerformance;
    00048 using namespace Mers::Estimation::Discrete;
    00049 using Mers::Estimation::Discrete::ApproximateBeliefState::BFBSE::BFBSE;
    00050 using Mers::Models::Base::BaseModelVariableArray;
    00051 using Mers::Models::Base::BaseModelClause;
    00052 using Mers::Models::Base::BaseModelClauseArray;
    00053 using Mers::Models::Base::BaseModelSentence;
    00054 using Mers::Models::CCA::CCAModel;
    00055 using Mers::Models::CCA::Io::Mof::MOFReader;
    00056 using Mers::Solvers::SAT::ValidSAT::ValidSAT;
    00057 using Mers::Utils::Lang::String;
    00058 using Mers::Utils::Containers::IntegerArray;
    00059 using std::vector;
    00060 using std::ostream;
    00061 using std::istream;
    00062 using std::ofstream;
    00063 using std::ifstream;
    00064 using std::flush;
    00065 using std::cout;
    00066 using std::cerr;
    00067 using std::endl;
    00068 using std::setprecision;
    00069
    00070 //OM #define METYPE ModeEstimatorDebug
    00071 #define METYPE BFBSE
    00072
    00073
    00074 enum    GenerationType { cmdWithNomObs = 0, randStatesRandObs = 1, allCombObs = 2, randObs = 3, beliefState = 4 };
    00075
    00076 // Function Prototypes
    00077 void    printUse(void);
    00078 void    createHeader(const String modelName, unsigned int numDataSets, const CCAModel &myCCA, GenerationType genType, ostream &outStream);
    00079 void    createScript(const CCAModel * model, ModeEstimatorBase * meb, MEPerformanceMonitor * mepm, unsigned int numDataSets, GenerationType genType, ostream &outStream);
    00080 void    createAllObservationCombinations(const BaseModel * model, ostream &outStream);
    00081 void    generateAndInsertRandomObs(const BaseModel * model, ostream &outStream);
    00082 void    loadInitFile(istream &inStream, ostream &outStream);
    00083 bool    insertConsistentObservations(ModeEstimatorBase * myME, ValidSAT * myValidSAT, const IntegerArray * dataVarIDarr, const IntegerArray * dataValIDarr, ostream &outStream);
    00084 bool    insertNonFaultyCommands(ModeEstimatorBase * meb, MEPerformanceMonitor * mepm, const IntegerArray * dataVarIDarr, const IntegerArray * dataValIDarr, ostream &outStream);
    00085 bool    insertBeliefState(ModeEstimatorBase * myME, unsigned int numEst, ostream &outStream);
    00086 void    run_ME_shell(ModeEstimatorBase * meb, MEPerformanceMonitor * mepm, BaseModel &model, istream &inputStream, ostream &outStream, ostream &timeOutStream);
    00087
    00088
    00089 void
    00090 printUse(void)
    00091 {
    00092         cout << "Use:" << endl;
    00093         cout << "meScriptGen.exe" << endl;
    00094         cout << "\t-m <CCA File (.mof)>" << endl;
    00095         cout << "\t[-c][-s][-f][-k]" << endl;
    00096         cout << "\t\t c - Random commands and nominal observations" << endl;
    00097         cout << "\t\t s - Random states preceding random observations" << endl;
    00098         cout << "\t\t f - Generate all possible combinations of observations" << endl;
    00099         cout << "\t\t k - Creates a belief state of size k. No commands or observations" << endl;
    00100         cout << "\t\t default  - Random sets of observations" << endl;
    00101         cout << "\t[-i <Initialization Script File>]" << endl;
    00102         cout << "\t[-n <# of data sets to generate>]" << endl;
    00103         cout << "\t[-o <Output File>]" << endl;
    00104         cout << "\t[-help]" << endl;
    00105         cout << "OR" << endl;
    00106         cout << "\t<CCA File>" << endl;
    00107         cout << endl;
    00108         return;
    00109 }
    00110
    00111
    00112
    00113
    00114 int
    00115 main(int argc, char** argv)
    00116 {
    00117         argc--;
    00118         argv++;
    00119
    00120         // Initialize Log4cplus
    00121         log4cplus::BasicConfigurator::doConfigure();
    00122
    00123         unsigned int i;
    00124
    00125         CCAModel myCCAmodel;
    00126         METYPE * myME = NULL;
    00127         ifstream beliefIn, inFile;
    00128         ofstream outFile, nullFile;
    00129
    00130         unsigned int numDataToGen = 5;
    00131         String modelName, numStr, initFileName;
    00132
    00133         bool modelLoaded = false;
    00134         bool initLoaded = false;
    00135         bool numDataLoaded = false;
    00136         bool outFileLoaded = false;
    00137         bool generateCommands = false;
    00138         GenerationType genType = randObs;
    00139
    00140         ModeEstimatorBase * meb = NULL;
    00141         MEPerformanceMonitor * mepm = NULL;
    00142
    00143         for (i=0; ((int)i)<argc; i++)
    00144         {
    00145                 if(argv[i][0] == '-')
    00146                 {
    00147                         if (argv[i][1] == 'h' || argv[i][1] == 'H' || String(argv[i]).compare(String("-help")) == 0)
    00148                         {
    00149                                 printUse();
    00150                                 exit(0);
    00151                         // Every input argument below here should be in pairs of <option, file name>
    00152                         } else if(argv[i][1] == 'c' || argv[i][1] == 'C')
    00153                         {
    00154                                 generateCommands = true;
    00155                                 genType = cmdWithNomObs;
    00156                         } else if(argv[i][1] == 's' || argv[i][1] == 'S')
    00157                         {
    00158                                 genType = randStatesRandObs;
    00159                         } else if(argv[i][1] == 'f' || argv[i][1] == 'F')
    00160                         {
    00161                                 genType = allCombObs;
    00162                         } else if(argv[i][1] == 'k' || argv[i][1] == 'k')
    00163                         {
    00164                                 genType = beliefState;
    00165                         } else if(((int)i+1) >= argc)
    00166                         {
    00167                                 cout << "Expected file name following " << argv[i] << ". Ignoring option." << endl;
    00168                         } else if(argv[i+1][0] == '-')
    00169                         {
    00170                                 cout << "Expected file name following " << argv[i] << ". Ignoring option." << endl;
    00171                         } else if (argv[i][1] == 'm' || argv[i][1] == 'M')
    00172                         {
    00173                                 if(!modelLoaded)
    00174                                 {
    00175                                         cout << "Loading model '" << argv[i+1] << "'..." << endl;
    00176                                         if(MOFReader::loadMOF(argv[i+1], myCCAmodel))
    00177                                         {
    00178                                                 cerr << endl << "Could not load " << argv[i+1] << "." << endl;
    00179                                                 exit(-1);
    00180                                         }
    00181                                         cout << "Success!" << endl;
    00182                                         modelName = argv[i+1];
    00183                                         modelLoaded = true;
    00184                                 }
    00185                         } else if (argv[i][1] == 'i' || argv[i][1] == 'I')
    00186                         {       // This could actually be an initialization script
    00187                                 if(!initLoaded)
    00188                                 {
    00189                                         beliefIn.open(argv[i+1]);
    00190                                         if(!beliefIn)
    00191                                                 cout << "ERROR: Couldn't open ME initialization file. Ignoring." << endl;
    00192                                         else
    00193                                         {
    00194                                                 initLoaded = true;
    00195                                                 initFileName = argv[i+1];
    00196                                         }
    00197                                 }
    00198                         } else if (argv[i][1] == 'n' || argv[i][1] == 'N')
    00199                         {
    00200                                 if(!numDataLoaded)
    00201                                 {
    00202                                         numStr = argv[i+1];
    00203                                         if(!isNumeric(numStr))
    00204                                                 cout << "ERROR: Invalid Number. Ignoring." << endl;
    00205                                         else
    00206                                         {
    00207                                                 numDataToGen = atoi(numStr.c_str());
    00208                                                 numDataLoaded = true;
    00209                                         }
    00210                                 }
    00211                         } else if (argv[i][1] == 'o' || argv[i][1] == 'O')
    00212                         {
    00213                                 if(!outFileLoaded)
    00214                                 {
    00215                                         outFile.open(argv[i+1]);
    00216                                         if(!outFile)
    00217                                                 cout << "ERROR: Couldn't open output file. Ignoring." << endl;
    00218                                         else
    00219                                                 outFileLoaded = true;
    00220                                 }
    00221                         } else
    00222                         {
    00223                                 cerr << "Unrecognized option '" << argv[i] << "'." << endl;
    00224                                 printUse();
    00225                         }
    00226                 }
    00227         }
    00228
    00229         if(argc == 1)
    00230         {
    00231                 cout << "Loading model '" << argv[0] << "'..." << endl;
    00232                 if(MOFReader::loadMOF(argv[0], myCCAmodel))
    00233                 {
    00234                         cerr << endl << "Could not load " << argv[0] << "." << endl;
    00235                         exit(-1);
    00236                 }
    00237                 cout << "Success!" << endl;
    00238                 modelName = argv[0];
    00239                 modelLoaded = true;
    00240         }
    00241
    00242
    00243         if(!modelLoaded)
    00244         {
    00245                 printUse();
    00246                 exit(-1);
    00247         }
    00248
    00249         myME = new METYPE(myCCAmodel);
    00250         meb = myME;
    00251         mepm = myME;
    00252
    00253         if(outFileLoaded)
    00254                 createHeader(modelName, numDataToGen, myCCAmodel, genType, outFile);
    00255         else
    00256                 createHeader(modelName, numDataToGen, myCCAmodel, genType, cout);
    00257
    00258
    00259
    00260         if(initLoaded)
    00261         {
    00262                 if(outFileLoaded)
    00263                         loadInitFile(beliefIn, outFile);
    00264                 else
    00265                         loadInitFile(beliefIn, cout);
    00266                 beliefIn.close();
    00267                 beliefIn.open(initFileName.c_str());
    00268                 run_ME_shell(meb, mepm, myCCAmodel, beliefIn, nullFile, nullFile);
    00269                 beliefIn.close();
    00270         }
    00271         if(outFileLoaded)
    00272         {
    00273                 createScript(&myCCAmodel, meb, mepm, numDataToGen, genType, outFile);
    00274                 outFile.close();
    00275         }
    00276         else
    00277                 createScript(&myCCAmodel, meb, mepm, numDataToGen, genType, cout);
    00278
    00279         delete myME;
    00280
    00281         return 0;
    00282 }
    00283
    00284
    00285 void
    00286 createScript(const CCAModel * model, ModeEstimatorBase * meb, MEPerformanceMonitor * mepm, unsigned int numDataSets, GenerationType genType, ostream &outStream)
    00287 {
    00288         if(genType == allCombObs)
    00289         {
    00290                 createAllObservationCombinations(model, outStream);
    00291                 return;
    00292         }
    00293
    00294         IntegerArray * dataVarIDarr = new IntegerArray();
    00295         IntegerArray * dataValIDarr = new IntegerArray();
    00296         IntegerArray * varDomainSize = new IntegerArray();
    00297         IntegerArray * stateVarIDarr = new IntegerArray();
    00298         IntegerArray * stateValIDarr = new IntegerArray();
    00299         IntegerArray * stateVarDomainSize = new IntegerArray();
    00300         BaseModelVariable::VariableType myDataType;
    00301         ValidSAT * myValidSAT = NULL;
    00302         BaseModelVariableArray * decisionVariables = NULL;
    00303
    00304         unsigned int i, j;
    00305         unsigned int commandTrials = 0;
    00306
    00307         if(genType == cmdWithNomObs)
    00308         {
    00309                 myDataType = BaseModelVariable::CONTROL;
    00310                 decisionVariables = new BaseModelVariableArray(false);
    00311                 for(i=0; i<model->getVariables()->getLength(); i++)
    00312                 {
    00313                         if((*model->getVariables())[i]->getVariableType() == BaseModelVariable::OBSERVABLE &&
    00314                                 (*model->getVariables())[i]->getDomain()->getLength() > 0)
    00315                         {
    00316                                 decisionVariables->pushBack(const_cast<BaseModelVariable *>((*model->getVariables())[i]));
    00317                         }
    00318                 }
    00319                 myValidSAT = new ValidSAT(*(model->getVariables()), *decisionVariables, model->get_all_constraints(), true);
    00320         }
    00321         else
    00322                 myDataType = BaseModelVariable::OBSERVABLE;
    00323
    00324
    00325         dataVarIDarr->resize(0);
    00326         varDomainSize->resize(0);
    00327         stateVarIDarr->resize(0);
    00328         stateVarDomainSize->resize(0);
    00329
    00330         for(i=0; i<model->getVariables()->getLength(); i++)
    00331         {
    00332                 if((*model->getVariables())[i]->getVariableType() == myDataType &&
    00333                         (*model->getVariables())[i]->getDomain()->getLength() > 0)
    00334                 {
    00335                         dataVarIDarr->resize(dataVarIDarr->getLength() + 1);
    00336                         (*dataVarIDarr)[dataVarIDarr->getLength() - 1] = (*model->getVariables())[i]->getUID();
    00337                         dataValIDarr->pushBack(0);
    00338                         varDomainSize->resize(varDomainSize->getLength() +1);
    00339                         (*varDomainSize)[varDomainSize->getLength() - 1] = (*model->getVariables())[i]->getDomain()->getLength();
    00340                 }
    00341         }
    00342
    00343         if(genType == randStatesRandObs)
    00344         {
    00345                 for(i=0; i<model->getVariables()->getLength(); i++)
    00346                 {
    00347                         if((*model->getVariables())[i]->getVariableType() == BaseModelVariable::STATE &&
    00348                                 (*model->getVariables())[i]->getDomain()->getLength() > 0)
    00349                         {
    00350                                 stateVarIDarr->resize(stateVarIDarr->getLength() + 1);
    00351                                 (*stateVarIDarr)[stateVarIDarr->getLength() - 1] = (*model->getVariables())[i]->getUID();
    00352                                 stateValIDarr->pushBack(0);
    00353                                 stateVarDomainSize->resize(stateVarDomainSize->getLength() +1);
    00354                                 (*stateVarDomainSize)[stateVarDomainSize->getLength() - 1] = (*model->getVariables())[i]->getDomain()->getLength();
    00355                         }
    00356                 }
    00357         }
    00358
    00359         srand(time(0));                         // Initialize the random seed
    00360
    00361
    00362         for(j=0; j < numDataSets; j++)
    00363         {
    00364                 outStream << "// --- Test #" << j << " --- " << endl;
    00365                 outStream << "t -b" << endl;
    00366                 outStream << "r" << endl;
    00367                 if(genType == randStatesRandObs)
    00368                 {
    00369                         for(i=0; i<stateVarIDarr->getLength(); i++)
    00370                         {
    00371                                 (*stateValIDarr)[i] = rand()%(*stateVarDomainSize)[i];
    00372                                 outStream << (*stateVarIDarr)[i] << " " << (*stateValIDarr)[i] << " ";
    00373                         }
    00374                         outStream << endl;
    00375                 }
    00376
    00377                 for(i=0; i<dataVarIDarr->getLength(); i++)
    00378                         (*dataValIDarr)[i] = rand()%(*varDomainSize)[i];
    00379                 if(genType != cmdWithNomObs)
    00380                 {
    00381                         if(genType == beliefState)
    00382                         {
    00383                                 insertBeliefState(meb, j+1, outStream);
    00384                         }
    00385                         else
    00386                         {
    00387                                 for(i=0; i<dataVarIDarr->getLength(); i++)
    00388                                         outStream << (*dataVarIDarr)[i] << " " << (*dataValIDarr)[i] << " ";
    00389                                 outStream << endl;
    00390                         }
    00391                         outStream << "e" << endl;
    00392                 }
    00393                 else
    00394                 {
    00395                         while(!insertNonFaultyCommands(meb, mepm, dataVarIDarr, dataValIDarr, outStream) && commandTrials < 800)
    00396                         {
    00397                                 commandTrials++;
    00398                                 for(i=0; i<dataVarIDarr->getLength(); i++)
    00399                                         (*dataValIDarr)[i] = rand()%(*varDomainSize)[i];
    00400                         }
    00401                         commandTrials = 0;
    00402                         if(!insertNonFaultyCommands(meb, mepm, dataVarIDarr, dataValIDarr, outStream))
    00403                         {
    00404                                 cout << "ERROR: Couldn't find any command sequences that avoided fault transitions! -- Using random command sequence." << endl;
    00405                                 cout << "               Test #" << j << endl;
    00406                         }
    00407                         for(i=0; i<dataVarIDarr->getLength(); i++)
    00408                                 outStream << (*dataVarIDarr)[i] << " " << (*dataValIDarr)[i] << " ";
    00409                         outStream << endl;
    00410                         if(!insertConsistentObservations(meb, myValidSAT, dataVarIDarr, dataValIDarr, outStream))
    00411                                 cout << "ERROR: Couldn't find any consistent assigments for Test #" << j << endl;
    00412                         outStream << "e -p" << endl;
    00413                 }
    00414
    00415                 outStream << "t -e" << endl << endl;
    00416         }
    00417
    00418         // Add terminating command
    00419         outStream << "q" << endl;
    00420         delete dataVarIDarr;
    00421         delete dataValIDarr;
    00422         delete varDomainSize;
    00423         delete stateVarIDarr;
    00424         delete stateValIDarr;
    00425         delete stateVarDomainSize;
    00426         if(genType == cmdWithNomObs)
    00427         {
    00428                 delete myValidSAT;
    00429                 delete decisionVariables;
    00430         }
    00431
    00432         return;
    00433 }
    00434
    00435 bool
    00436 insertConsistentObservations(ModeEstimatorBase * myME, ValidSAT * myValidSAT, const IntegerArray * dataVarIDarr, const IntegerArray * dataValIDarr, ostream &outStream)
    00437 {
    00438         BaseModelClauseArray * allNextStates = new BaseModelClauseArray(true);
    00439         BaseModelClause * aNextState = NULL;
    00440         BaseModelAssignmentArray * res = new BaseModelAssignmentArray(false);
    00441         bool hadError = false;
    00442         unsigned int i = 0;
    00443         // Simulate ME by inserting all the commands
    00444         for(i=0; i<dataVarIDarr->getLength(); i++)
    00445                 myME->command((*dataVarIDarr)[i], (*dataValIDarr)[i]);
    00446         const Estimate * nextEst = myME->findModeEstimate();
    00447         if(nextEst == NULL) {
    00448                 delete allNextStates;
    00449                 if(res)
    00450                         delete res;
    00451                 cerr << "Warning: No next estimate." << endl;
    00452                 return false;
    00453         }
    00454         BaseModelAssignmentArray const * bmNextStates = nextEst->getComponentAssignments();
    00455         for(i=0; i<bmNextStates->getLength(); i++)
    00456         {
    00457                 aNextState = new BaseModelClause(BaseModelClause::DISJUNCTIVE, true);
    00458                 aNextState->pushBack(new BaseModelAssignment((*bmNextStates)[i]->getVariable(), (*bmNextStates)[i]->getValue(), (*bmNextStates)[i]->getOperator()));
    00459                 allNextStates->pushBack(aNextState);
    00460         }
    00461         for(i=0; i<allNextStates->getLength(); i++)
    00462         {
    00463                 myValidSAT->addTemporaryConstraint((*allNextStates)[i]);
    00464         }
    00465         ValidSAT::SolutionType soluType = myValidSAT->tryAssignment(NULL, 0, false, *res);
    00466         if(soluType == ValidSAT::Unsatisfiable)
    00467         {
    00468                 delete allNextStates;
    00469                 if(res)
    00470                         delete res;
    00471                 cerr << "ERROR: No consistent set of observations for current state" << endl;
    00472                 return false;
    00473         }
    00474         if(res)
    00475         {
    00476                 // Test to see if observations actually don't create conflicts
    00477                 for(i=0; i<res->getLength(); i++)
    00478                         myME->observe((*res)[i]->getVariable()->getUID(),(*res)[i]->getValue()->getUID());
    00479 /*              myME->findModeEstimate();
    00480                 if(myME->getNumConflictsGenerated() > 0)
    00481                 {
    00482                         cout << "ERROR: " << myME->getNumConflictsGenerated() << " conflicts generated!" << endl;
    00483                         cout << (*myME->getCurrentTrajectories()) << endl;
    00484                         hadError = true;
    00485                         soluType = myValidSAT->tryAssignment(NULL, 0, false, *res);
    00486                         if(soluType == ValidSAT::Unsatisfiable)
    00487                         {
    00488                                 delete allNextStates;
    00489                                 if(res)
    00490                                         delete res;
    00491                                 cerr << "ERROR: No consistent set of observations for current state" << endl;
    00492                                 return false;
    00493                         }
    00494                         myME->resetCommandsAndObservations();
    00495                         // Test to see if observations actually don't create conflicts
    00496                         for(i=0; i<dataVarIDarr->getLength(); i++)
    00497                                 myME->command((*dataVarIDarr)[i], (*dataValIDarr)[i]);
    00498                         for(i=0; i<res->getLength(); i++)
    00499                                 myME->observe((*res)[i]->getVariable()->getUID(),(*res)[i]->getValue()->getUID());
    00500                         myME->findModeEstimate();
    00501                         if(myME->getNumConflictsGenerated() > 0)
    00502                                 cout << "--- Couldn't fix the problem!!!!!!!!!!!!!!!!!!!!" << endl;
    00503                         else
    00504                         {
    00505                                 cout << "Wow.. fixed the problem!" << endl;
    00506                                 hadError = false;
    00507                                 for(i=0; i<res->getLength(); i++)
    00508                                         outStream << (*res)[i]->getVariable()->getUID() << " " << (*res)[i]->getValue()->getUID() << " ";
    00509                                 outStream << endl;
    00510                         }
    00511
    00512                 }
    00513                 else
    00514                 {
    00515                         for(i=0; i<res->getLength(); i++)
    00516                                 outStream << (*res)[i]->getVariable()->getUID() << " " << (*res)[i]->getValue()->getUID() << " ";
    00517                         outStream << endl;
    00518                 }
    00519                 */
    00520                 myME->progressToEstimate();
    00521                 for(i=0; i<res->getLength(); i++)
    00522                         outStream << (*res)[i]->getVariable()->getUID() << " " << (*res)[i]->getValue()->getUID() << " ";
    00523                 outStream << endl;
    00524
    00525         } else
    00526         {
    00527                 myME->progressToEstimate();
    00528                 delete allNextStates;
    00529                 cerr << "ERROR: No consistent set of observables found" << endl;
    00530                 return false;
    00531         }
    00532
    00533         // Clean up everything
    00534         myValidSAT->clearTemporaryConstraint();
    00535         delete allNextStates;
    00536         delete res;
    00537         if(hadError)
    00538                 return false;
    00539         return true;
    00540 }
    00541
    00542 bool
    00543 insertNonFaultyCommands(ModeEstimatorBase * meb, MEPerformanceMonitor * mepm, const IntegerArray * dataVarIDarr, const IntegerArray * dataValIDarr, ostream &outStream)
    00544 {       // Simulates ME but leaves it unchanged (ie resets all commands and observations and never progresses)
    00545         // Any previous assigments to commands or observations will be destroyed
    00546         unsigned int i = 0;
    00547         // Simulate ME by inserting all the commands
    00548         meb->resetCommandsAndObservations();
    00549         for(i=0; i<dataVarIDarr->getLength(); i++)
    00550                 meb->command((*dataVarIDarr)[i], (*dataValIDarr)[i]);
    00551         meb->findModeEstimate();
    00552         if(mepm->getEstimateFaultTransitions(0) > 0)
    00553         {
    00554                 return false;
    00555         }
    00556         else
    00557         {
    00558                 return true;
    00559         }
    00560 }
    00561
    00562 bool
    00563 insertBeliefState(ModeEstimatorBase * myME, unsigned int numEst, ostream &outStream)
    00564 {
    00565         myME->setNumberTrajectories(numEst);
    00566         myME->resetCommandsAndObservations();
    00567         const Trajectories * nextTrajs = myME->findNextTrajectories();
    00568         for(unsigned int i=0; i<nextTrajs->getNumberEstimates(); i++)
    00569         {
    00570                 const Estimate * thisEst = nextTrajs->getEstimate(i);
    00571                 outStream << "a -b " << setprecision(15) << thisEst->getProbability();
    00572                 for(unsigned int j=0; j<thisEst->getComponentAssignments()->getLength(); j++)
    00573                         outStream << " " << thisEst->getAssignment(j)->getVariable()->getUID() << " " << thisEst->getAssignment(j)->getValue()->getUID();
    00574                 outStream << endl;
    00575         }
    00576         outStream << "k " << nextTrajs->getNumberEstimates() << endl;
    00577         if(numEst > nextTrajs->getNumberEstimates())
    00578         {
    00579                 cout << "Warning: Could only generate " << nextTrajs->getNumberEstimates() << " initial states." << endl;
    00580                 return false;
    00581         }
    00582
    00583         return true;
    00584 }
    00585
    00586 void
    00587 loadInitFile(istream &inStream, ostream &outStream)
    00588 {
    00589         char aLine[300] = "";
    00590
    00591         cout << "Loading initialization script... " << endl;
    00592         outStream << "// --- Using user defined input ---" << endl;
    00593         while(!inStream.eof() && !(aLine[0] == 'q' && aLine[1] == '\0'))
    00594         {
    00595                 inStream.getline(aLine, 300);
    00596                 if(!(aLine[0] == 'q' && aLine[1] == '\0'))      // Remove quit command
    00597                         outStream << aLine << endl;
    00598         }
    00599         outStream << "// --- End user defined input ---" << endl << endl;
    00600         cout << "done." << endl;
    00601         return;
    00602 }
    00603
    00604 void
    00605 createHeader(const String modelName, unsigned int numDataSets, const CCAModel &myCCA, GenerationType genType, ostream &outStream)
    00606 {
    00607         unsigned int numDecisionVars = 0;
    00608         unsigned int numDecisionVals = 0;
    00609         double aveDomainSize = 0.0;
    00610         unsigned int numAllVars = 0;
    00611         unsigned int numClauses = 0;
    00612         unsigned int numClauseAss = 0;
    00613         double aveClauseLength = 0.0;
    00614         unsigned int i;
    00615
    00616         for(i=0; i<myCCA.getVariables()->getLength(); i++)
    00617         {
    00618                 if((*myCCA.getVariables())[i]->getVariableType() == BaseModelVariable::STATE)
    00619                 {
    00620                         numDecisionVars++;
    00621                         numDecisionVals += (*myCCA.getVariables())[i]->getDomain()->getLength();
    00622                 }
    00623                 numAllVars++;
    00624         }
    00625         aveDomainSize = (double)numDecisionVals/(double)numDecisionVars;
    00626
    00627         BaseModelSentence allConstraints = myCCA.get_all_constraints();
    00628         numClauses = allConstraints.getLength();
    00629         for(i=0; i<numClauses; i++)
    00630         {
    00631                 //cout << "Counting constraint clause #" << i << endl;
    00632                 numClauseAss += allConstraints[i]->getLength();
    00633         }
    00634         aveClauseLength = (double)numClauseAss/(double)numClauses;
    00635
    00636         outStream << "// ---------------------------------------------------------------" << endl;
    00637         outStream << "// Auto generated ME script using randomMEscriptGen" << endl;
    00638         if(genType == cmdWithNomObs)
    00639                 outStream << "// Type: Randomly generated commands with consistent observations." <<  endl;
    00640         else if(genType == allCombObs)
    00641                 outStream << "// Type: all possible combinations of observations." << endl;
    00642         else if(genType == randStatesRandObs)
    00643                 outStream << "// Type: Random states with randomly generated observations." << endl;
    00644         else if(genType == randObs)
    00645                 outStream << "// Type: Randomly generated observations." << endl;
    00646         else
    00647                 outStream << "// Type: Unknown" << endl;
    00648
    00649         outStream << "// Model: " << modelName << endl;
    00650         outStream << "// Total number of variables: " << numAllVars << endl;
    00651         outStream << "// Number of decision variables: " << numDecisionVars << endl;
    00652         outStream << "// Average domain size of decision variables: " << aveDomainSize << endl;
    00653         outStream << "// Total number of clauses: " << numClauses << endl;
    00654         outStream << "// Average clause length: " << aveClauseLength << endl;
    00655         outStream << "//  " << endl;
    00656         outStream << "// Number of data sets: " << numDataSets << endl;
    00657         outStream << "// ---------------------------------------------------------------" << endl;
    00658         return;
    00659 }
    00660
    00661
    00662 void
    00663 createAllObservationCombinations(const BaseModel * model, ostream &outStream)
    00664 {
    00665         // Assumes that the variable domain values IDs are in order 0..n where n = |D(var)|
    00666
    00667         IntegerArray * obsVarIDarr = new IntegerArray();
    00668         IntegerArray * currentObsValIDarr = new IntegerArray();
    00669         IntegerArray * varDomainSize = new IntegerArray();
    00670         unsigned int varIndexToModify = 0;
    00671         bool exhausted = false;
    00672
    00673         int i;
    00674         long count = 0;
    00675
    00676         obsVarIDarr->resize(0);
    00677         currentObsValIDarr->resize(0);
    00678         varDomainSize->resize(0);
    00679
    00680         for(i=0; i<(int)model->getVariables()->getLength(); i++)
    00681         {
    00682                 if((*model->getVariables())[i]->getVariableType() == BaseModelVariable::OBSERVABLE &&
    00683                         (*model->getVariables())[i]->getDomain()->getLength() > 0)
    00684                 {
    00685                         obsVarIDarr->resize(obsVarIDarr->getLength() + 1);
    00686                         (*obsVarIDarr)[obsVarIDarr->getLength() - 1] = (*model->getVariables())[i]->getUID();
    00687                         currentObsValIDarr->resize(currentObsValIDarr->getLength() + 1);
    00688                         (*currentObsValIDarr)[currentObsValIDarr->getLength() - 1] = 0;
    00689                         varDomainSize->resize(varDomainSize->getLength() +1);
    00690                         (*varDomainSize)[varDomainSize->getLength() - 1] = (*model->getVariables())[i]->getDomain()->getLength();
    00691                 }
    00692         }
    00693
    00694         while(!exhausted)
    00695         {
    00696                 outStream << "// --- Test #" << count << " --- " << endl;
    00697                 outStream << "t -b" << endl;
    00698                 outStream << "r" << endl;
    00699                 count++;
    00700                 for(i=0; i<(int)obsVarIDarr->getLength(); i++)
    00701                 {
    00702                         outStream << (*obsVarIDarr)[i] << " " << (*currentObsValIDarr)[i] << " ";
    00703                 }
    00704                 outStream << endl;
    00705                 outStream << "e" << endl;
    00706                 outStream << "t -e" << endl << endl;
    00707
    00708                 if((*currentObsValIDarr)[varIndexToModify] + 1 < (*varDomainSize)[varIndexToModify])
    00709                 {       // just increment the current variable value
    00710                         (*currentObsValIDarr)[varIndexToModify] += 1;
    00711                 }
    00712                 else
    00713                 {
    00714                         for(i=varIndexToModify-1; i>=0; i--)
    00715                                 (*currentObsValIDarr)[i] = 0;
    00716                         while((*currentObsValIDarr)[varIndexToModify] >= (*varDomainSize)[varIndexToModify] - 1)
    00717                         {
    00718                                 (*currentObsValIDarr)[varIndexToModify] = 0;
    00719                                 varIndexToModify++;
    00720                                 if(varIndexToModify >= obsVarIDarr->getLength())
    00721                                 {
    00722                                         exhausted = true;
    00723                                         break;
    00724                                 }
    00725                         }
    00726                         if(!exhausted)
    00727                                 (*currentObsValIDarr)[varIndexToModify] += 1;
    00728                         varIndexToModify = 0;
    00729                 }
    00730
    00731         }
    00732         outStream << "q" << endl;
    00733
    00734         delete obsVarIDarr;
    00735         delete currentObsValIDarr;
    00736         delete varDomainSize;
    00737
    00738         return;
    00739 }
    00740
    00741
    00742 void
    00743 run_ME_shell(ModeEstimatorBase * meb, MEPerformanceMonitor * mepm, BaseModel &model, istream &inputStream, ostream &outStream, ostream &timeOutStream)
    00744 {
    00745         String          var_string, val_string;
    00746         String          inStr;
    00747         char            char_p[300];
    00748         bool            quit_shell = false;
    00749         bool            echoInput = false;
    00750         unsigned int    beliefEntryNum = 0;
    00751         bool            wasBeliefEntry = false;
    00752
    00753         vector<String> varSvector;
    00754         vector<String> valSvector;
    00755         clock_t startTime = clock(), timeDiff;
    00756         unsigned int numEstLoop = 0;
    00757         varSvector.resize(0);
    00758         valSvector.resize(0);
    00759
    00760         timeOutStream << "%--------------------------------------------------------" << endl;
    00761         timeOutStream << "% ME 1.2 Time Requirement Data" << endl;
    00762         timeOutStream << "%" << endl;
    00763         timeOutStream << "% time(ms) | #loops | #estimates | #faults | #conflicts" << endl;
    00764         timeOutStream << "%--------------------------------------------------------" << endl << endl;
    00765         timeOutStream << "meData = [";
    00766
    00767         outStream << endl << "COMPILED ME Shell: Enter 'h' for help.";
    00768         outStream << endl << endl;
    00769
    00770         for(unsigned int entryNumber=1; !quit_shell && !inputStream.eof(); entryNumber++)
    00771         {
    00772                 outStream << entryNumber << "> " << flush;
    00773                 eat_space_or_tab(inputStream);
    00774
    00775                 inputStream >> char_p;
    00776                 if(echoInput)
    00777                         outStream << char_p << " ";
    00778                 inStr = char_p;
    00779
    00780                 if(isNumeric(inStr) || (inStr.size() > 1 && inStr.find('/') != 0))
    00781                 {                                                       // Asserting command/observation/state
    00782                         var_string = inStr;
    00783                         eat_space_or_tab(inputStream);
    00784                         if(inputStream.peek() != '\n')
    00785                         {
    00786                                 inputStream >> char_p;
    00787                                 if(echoInput)
    00788                                         outStream << char_p << " ";
    00789                                 val_string = char_p;
    00790                                 eat_space_or_tab(inputStream);
    00791                                 varSvector.resize(varSvector.size()+1);
    00792                                 valSvector.resize(valSvector.size()+1);
    00793                                 varSvector[varSvector.size()-1] = var_string;
    00794                                 valSvector[valSvector.size()-1] = val_string;
    00795                                 assign(var_string, val_string, meb, model, outStream);
    00796                                 while(get_assignment_pairs(inputStream, outStream, var_string, val_string, echoInput))
    00797                                 {
    00798                                         assign(var_string, val_string, meb, model, outStream);
    00799                                         varSvector.resize(varSvector.size()+1);
    00800                                         valSvector.resize(valSvector.size()+1);
    00801                                         varSvector[varSvector.size()-1] = var_string;
    00802                                         valSvector[valSvector.size()-1] = val_string;
    00803                                 }
    00804                         }
    00805                         if(echoInput)
    00806                                 outStream << endl;
    00807                 }
    00808                 else if(inStr.find('/') != 0)   // Input is command since there are no 1 char variable names
    00809                 {
    00810
    00811                         switch (*inStr.c_str())
    00812                         {
    00813                                 case 'a':
    00814                                 case 'A':
    00815                                         // 'a' or 'A' corresponds to assert a sequence of (var=val)
    00816                                         eat_space_or_tab(inputStream);
    00817                                         if (inputStream.peek() != '\n')
    00818                                         {
    00819                                                 if (inputStream.peek() == '-')
    00820                                                 {
    00821                                                         inputStream >> char_p;
    00822                                                         if(echoInput)
    00823                                                                 outStream << char_p << endl;
    00824                                                         if(entryNumber - beliefEntryNum == 1 && wasBeliefEntry)         // If there has only been one loop and prev entry was a belief entry
    00825                                                         {
    00826                                                                 if(get_directed_assign(char_p[1], inputStream, outStream, meb, model, true, echoInput) && (char_p[1] == 'b' || char_p[1] == 'B'))
    00827                                                                         wasBeliefEntry = true;
    00828                                                         }
    00829                                                         else
    00830                                                         {
    00831                                                                 if(get_directed_assign(char_p[1], inputStream, outStream, meb, model, false, echoInput) && (char_p[1] == 'b' || char_p[1] == 'B'))
    00832                                                                         wasBeliefEntry = true;
    00833                                                         }
    00834                                                         if(wasBeliefEntry)              // Reset entry number on last belief input
    00835                                                                 beliefEntryNum = entryNumber;
    00836                                                 }
    00837                                                 else
    00838                                                 {
    00839                                                         while(get_assignment_pairs(inputStream, outStream, var_string, val_string, echoInput))
    00840                                                         {
    00841                                                                 assign(var_string, val_string, meb, model, outStream);
    00842                                                                 varSvector.resize(varSvector.size()+1);
    00843                                                                 valSvector.resize(valSvector.size()+1);
    00844                                                                 varSvector[varSvector.size()-1] = var_string;
    00845                                                                 valSvector[valSvector.size()-1] = val_string;
    00846                                                         }
    00847                                                 }
    00848                                         }
    00849                                         if(echoInput)
    00850                                                 outStream << endl;
    00851                                         break;
    00852                                 case 'e':
    00853                                 case 'E':
    00854                                         // 'e' or 'E' corresponds to estimate. '-p' progresses into the estimate
    00855                                         eat_space_or_tab(inputStream);
    00856                                         if (inputStream.peek() == '\n')
    00857                                         {
    00858                                                 if(echoInput)
    00859                                                         outStream << endl;
    00860 //                                              outStream << "Calculating next trajectories..." << endl << endl;
    00861                                                 meb->findModeEstimate();
    00862 //                                              outStream << endl;
    00863 //                                              meb->showNextTrajectories(outStream);
    00864                                         }
    00865                                         else
    00866                                         {
    00867                                                 inputStream >> char_p;
    00868                                                 if(echoInput)
    00869                                                         outStream << char_p << endl;
    00870                                                 if (char_p[0] == '-' && char_p[1] == 'p')
    00871                                                 {
    00872 //                                                      outStream << "Calculating next trajectories..." << endl;
    00873                                                         meb->progressToEstimate();
    00874 //                                                      outStream << "Progressing into most likely estimate..." << endl << endl;
    00875 //                                                      meb->showCurrentTrajectories(outStream);
    00876                                                 }
    00877                                                 else
    00878                                                 {
    00879 //                                                      outStream << "Calculating next trajectories..." << endl;
    00880                                                         meb->findModeEstimate();
    00881 //                                                      outStream << endl;
    00882 //                                                      meb->showNextTrajectories(outStream);
    00883                                                 }
    00884                                         }
    00885                                         break;
    00886
    00887                                 case 't':
    00888                                 case 'T':
    00889                                         // 't' or 'T' corresponds to time. '-b' starts the timer and '-e' ends it and records the data.
    00890                                         // Default is -b option
    00891                                         eat_space_or_tab(inputStream);
    00892                                         if (inputStream.peek() == '\n')
    00893                                         {
    00894                                                 if(echoInput)
    00895                                                         outStream << endl;
    00896                                                 outStream << "Starting timer..." << endl << endl;
    00897                                                 varSvector.resize(0);
    00898                                                 valSvector.resize(0);
    00899                                                 startTime = clock();
    00900                                         }
    00901                                         else
    00902                                         {
    00903                                                 inputStream >> char_p;
    00904                                                 if(echoInput)
    00905                                                         outStream << char_p << endl;
    00906                                                 if (char_p[0] == '-' && char_p[1] == 'b')
    00907                                                 {
    00908                                                         outStream << "Starting timer..." << endl << endl;
    00909                                                         varSvector.resize(0);
    00910                                                         valSvector.resize(0);
    00911                                                         startTime = clock();
    00912                                                 }
    00913                                                 else if (char_p[0] == '-' && char_p[1] == 'e')
    00914                                                 {
    00915                                                         numEstLoop = 1;
    00916                                                         timeDiff = clock() - startTime;
    00917                                                         if(timeDiff < 500)                              // 200ms is innacurrate
    00918                                                         {
    00919                                                                 while(timeDiff < 500)
    00920                                                                 {
    00921                                                                         numEstLoop++;
    00922                                                                         meb->resetCommandsAndObservations();
    00923                                                                         for(unsigned int i=0; i<varSvector.size(); i++)
    00924                                                                                 assign(varSvector[i], valSvector[i], meb, model, outStream);
    00925                                                                         meb->findModeEstimate();
    00926                                                                         timeDiff = clock() - startTime;
    00927                                                                 }
    00928                                                         }
    00929                                                 }
    00930                                                 else
    00931                                                 {
    00932                                                         outStream << "Starting timer..." << endl << endl;
    00933                                                         varSvector.resize(0);
    00934                                                         valSvector.resize(0);
    00935                                                         startTime = clock();
    00936                                                 }
    00937                                         }
    00938
    00939                                         break;
    00940                                 case 'v':
    00941                                 case 'V':
    00942                                         // 'v' or 'V' corresponds to viewing data. Default is all variables
    00943                                         eat_space_or_tab(inputStream);
    00944                                         if (inputStream.peek() == '\n')
    00945                                         {
    00946                                                 if(echoInput)
    00947                                                         outStream << endl;
    00948                                                 print_estimator_variables('a', meb, outStream);
    00949                                         }
    00950                                         else
    00951                                         {
    00952                                                 inputStream >> char_p;
    00953                                                 if(echoInput)
    00954                                                         outStream << char_p << endl;
    00955                                                 if (char_p[0] == '-' && char_p[1] != '\0')
    00956                                                         print_estimator_variables(char_p[1], meb, outStream);
    00957                                                 else
    00958                                                         print_estimator_variables('a', meb, outStream);
    00959                                         }
    00960                                         break;
    00961                                 case 'd':
    00962                                 case 'D':
    00963                                         // 'd' or 'D' sets the debug level
    00964                                         eat_space_or_tab(inputStream);
    00965                                         if(inputStream.peek() != '\n')
    00966                                         {
    00967                                                 inputStream >> char_p;
    00968                                                 if(echoInput)
    00969                                                         outStream << char_p << endl;
    00970                                                 if(isNumeric(String(char_p)))
    00971                                                 {
    00972                                                         meb->setDebug(atoi(char_p));
    00973                                                         outStream << "Debug level set to " << char_p << endl;
    00974                                                 }
    00975                                                 else
    00976                                                         outStream << "Invalid debug level." << endl;
    00977                                         }
    00978                                         break;
    00979                                 case 'k':
    00980                                 case 'K':
    00981                                         // 'k' or 'K' sets the maximum number of trajectories
    00982                                         eat_space_or_tab(inputStream);
    00983                                         if(inputStream.peek() != '\n')
    00984                                         {
    00985                                                 inputStream >> char_p;
    00986                                                 if(echoInput)
    00987                                                         outStream << char_p << endl;
    00988                                                 if(isNumeric(String(char_p)))
    00989                                                 {
    00990                                                         outStream << "Setting maximum number of trajectories set to " << char_p << endl;
    00991                                                         meb->setNumberTrajectories(atoi(char_p));
    00992                                                 }
    00993                                                 else
    00994                                                         outStream << "Invalid maximum number of trajectories" << endl;
    00995                                         }
    00996                                         break;
    00997                                 case 'h':
    00998                                 case 'H':
    00999                                 case '?':
    01000                                         // 'h' or 'H' or '?' corresponds to display help.
    01001                                         eat_space_or_tab(inputStream);
    01002                                         if (inputStream.peek() == '\n')
    01003                                         {
    01004                                                 if(echoInput)
    01005                                                         outStream << endl;
    01006                                                 //print_ME_shell_help('h', outStream);
    01007                                         }
    01008                                         else
    01009                                         {
    01010                                                 inputStream >> char_p;
    01011                                                 if(echoInput)
    01012                                                         outStream << char_p << endl;
    01013                                                 if (char_p[0] == '-' && char_p[1] != '\0')
    01014                                                 {
    01015                                                         //print_ME_shell_help(char_p[1], outStream);
    01016                                                 }
    01017                                                 else
    01018                                                 {
    01019                                                         //print_ME_shell_help('h', outStream);
    01020                                                 }
    01021                                         }
    01022                                         break;
    01023                                 case 'r':
    01024                                 case 'R':
    01025                                         // 'r' or 'R' corresponds to reseting asserted variables
    01026                                         meb->resetCommandsAndObservations();
    01027                                         break;
    01028                                 case 'q':
    01029                                 case 'Q':
    01030                                         // 'q' or 'Q' corresponds to quit.
    01031                                         quit_shell = true;
    01032                                         break;
    01033                                 case '#':
    01034                                         echoInput = !echoInput;
    01035                                         outStream << "Echo input: " << (echoInput ? "ON" : "OFF") << endl << endl;
    01036                                         break;
    01037                                 case '\0':
    01038                                 case '\n':
    01039                                         // a blank line - skip.
    01040                                 case '/':
    01041                                         // assumed to be a comment
    01042                                         break;
    01043                                 default:
    01044                                         outStream << "WARNING: '" << inStr << "' is not a recognized command." << endl;
    01045                                         break;
    01046                         }
    01047                         if(*inStr.c_str() != 'a' && *inStr.c_str() != 'A') // assert functions' responsible for this
    01048                                 clear_stream(inputStream);
    01049                 }
    01050                 else
    01051                 {
    01052                         if(echoInput)
    01053                         {
    01054                                 inputStream.get(char_p, 300, '\n');
    01055                                 outStream << char_p;
    01056                         }
    01057                         clear_stream(inputStream);
    01058                 }
    01059                 if(echoInput)
    01060                         outStream << endl;
    01061         }
    01062         timeOutStream << "];" << endl;
    01063         return;
    01064 }