<commit_before>#include <iostream>
#include <fstream>
#include <sstream>
#include <boost/algorithm/string.hpp>
#include "gr1context.hpp"
#undef fail

/**
 * @brief Constructor that reads the problem instance from file and prepares the BFManager, the BFVarCubes, and the BFVarVectors
 * @param inFile the input filename
 */
GR1Context::GR1Context(std::string inFileName) {

    std::ifstream inFile(inFileName.c_str());
    if (inFile.fail()) throw "Error: Cannot open input file";

    // Prepare safety and initialization constraints
    initEnv = mgr.constantTrue();
    initSys = mgr.constantTrue();
    safetyEnv = mgr.constantTrue();
    safetySys = mgr.constantTrue();

    // The readmode variable stores in which chapter of the input file we are
    int readMode = -1;
    std::string currentLine;
    lineNumberCurrentlyRead = 0;
    while (std::getline(inFile,currentLine)) {
        lineNumberCurrentlyRead++;
        boost::trim(currentLine);
        if ((currentLine.length()>0) && (currentLine[0]!='#')) {
            if (currentLine[0]=='[') {
                if (currentLine=="[INPUT]") {
                    readMode = 0;
                } else if (currentLine=="[OUTPUT]") {
                    readMode = 1;
                } else if (currentLine=="[ENV_INIT]") {
                    readMode = 2;
                } else if (currentLine=="[SYS_INIT]") {
                    readMode = 3;
                } else if (currentLine=="[ENV_TRANS]") {
                    readMode = 4;
                } else if (currentLine=="[SYS_TRANS]") {
                    readMode = 5;
                } else if (currentLine=="[ENV_LIVENESS]") {
                    readMode = 6;
                } else if (currentLine=="[SYS_LIVENESS]") {
                    readMode = 7;
                } else {
                    std::cerr << "Sorry. Didn't recognize category " << currentLine << "\n";
                    throw "Aborted.";
                }
            } else {
                if (readMode==0) {
                    variables.push_back(mgr.newVariable());
                    variableNames.push_back(currentLine);
                    variableTypes.push_back(PreInput);
                    variables.push_back(mgr.newVariable());
                    variableNames.push_back(currentLine+"'");
                    variableTypes.push_back(PostInput);
                } else if (readMode==1) {
                    variables.push_back(mgr.newVariable());
                    variableNames.push_back(currentLine);
                    variableTypes.push_back(PreOutput);
                    variables.push_back(mgr.newVariable());
                    variableNames.push_back(currentLine+"'");
                    variableTypes.push_back(PostOutput);
                } else if (readMode==2) {
                    std::set<VariableType> allowedTypes;
                    allowedTypes.insert(PreInput);
                    allowedTypes.insert(PreOutput);
                    initEnv &= parseBooleanFormula(currentLine,allowedTypes);
                } else if (readMode==3) {
                    std::set<VariableType> allowedTypes;
                    allowedTypes.insert(PreInput);
                    allowedTypes.insert(PreOutput);
                    initSys &= parseBooleanFormula(currentLine,allowedTypes);
                } else if (readMode==4) {
                    std::set<VariableType> allowedTypes;
                    allowedTypes.insert(PreInput);
                    allowedTypes.insert(PreOutput);
                    allowedTypes.insert(PostInput);
                    safetyEnv &= parseBooleanFormula(currentLine,allowedTypes);
                } else if (readMode==5) {
                    std::set<VariableType> allowedTypes;
                    allowedTypes.insert(PreInput);
                    allowedTypes.insert(PreOutput);
                    allowedTypes.insert(PostInput);
                    allowedTypes.insert(PostOutput);
                    safetySys &= parseBooleanFormula(currentLine,allowedTypes);
                } else if (readMode==6) {
                    std::set<VariableType> allowedTypes;
                    allowedTypes.insert(PreInput);
                    allowedTypes.insert(PreOutput);
                    allowedTypes.insert(PostInput);
                    livenessAssumptions.push_back(parseBooleanFormula(currentLine,allowedTypes));
                } else if (readMode==7) {
                    std::set<VariableType> allowedTypes;
                    allowedTypes.insert(PreInput);
                    allowedTypes.insert(PreOutput);
                    allowedTypes.insert(PostInput);
                    allowedTypes.insert(PostOutput);
                    livenessGuarantees.push_back(parseBooleanFormula(currentLine,allowedTypes));
                } else {
                    std::cerr << "Error with line " << lineNumberCurrentlyRead << "!";
                    throw "Found a line in the specification file that has no proper categorial context.";
                }
            }
        }
    }

    // Compute VarVectors and VarCubes
    std::vector<BF> postOutputVars;
    std::vector<BF> postInputVars;
    std::vector<BF> preOutputVars;
    std::vector<BF> preInputVars;
    for (unsigned int i=0;i<variables.size();i++) {
        switch (variableTypes[i]) {
        case PreInput:
            preVars.push_back(variables[i]);
            preInputVars.push_back(variables[i]);
            break;
        case PreOutput:
            preVars.push_back(variables[i]);
            preOutputVars.push_back(variables[i]);
            break;
        case PostInput:
            postVars.push_back(variables[i]);
            postInputVars.push_back(variables[i]);
            break;
        case PostOutput:
            postVars.push_back(variables[i]);
            postOutputVars.push_back(variables[i]);
            break;
        default:
            throw "Error: Found a variable of unknown type";
        }
    }
    varVectorPre = mgr.computeVarVector(preVars);
    varVectorPost = mgr.computeVarVector(postVars);
    varCubePostInput = mgr.computeCube(postInputVars);
    varCubePostOutput = mgr.computeCube(postOutputVars);
    varCubePreInput = mgr.computeCube(preInputVars);
    varCubePreOutput = mgr.computeCube(preOutputVars);
    varCubePre = mgr.computeCube(preVars);

    // Make sure that there is at least one liveness assumption and one liveness guarantee
    // The synthesis algorithm might be unsound otherwise
    if (livenessAssumptions.size()==0) livenessAssumptions.push_back(mgr.constantTrue());
    if (livenessGuarantees.size()==0) livenessGuarantees.push_back(mgr.constantTrue());
}

/**
 * @brief Recurse internal function to parse a Boolean formula from a line in the input file
 * @param is the input stream from which the tokens in the line are read
 * @param allowedTypes a list of allowed variable types - this allows to check that assumptions do not refer to 'next' output values.
 * @return a BF that represents the transition constraint read from the line
 */
BF GR1Context::parseBooleanFormulaRecurse(std::istringstream &is,std::set<VariableType> &allowedTypes) {
    std::string operation = "";
    is >> operation;
    if (operation=="") {
        std::cerr << "Error reading line " << lineNumberCurrentlyRead << " from the input file. Premature end of line.\n";
        throw "Fatal Error";
    }
    if (operation=="|") return parseBooleanFormulaRecurse(is,allowedTypes) | parseBooleanFormulaRecurse(is,allowedTypes);
    if (operation=="&") return parseBooleanFormulaRecurse(is,allowedTypes) & parseBooleanFormulaRecurse(is,allowedTypes);
    if (operation=="!") return !parseBooleanFormulaRecurse(is,allowedTypes);
    if (operation=="1") return mgr.constantTrue();
    if (operation=="0") return mgr.constantFalse();

    // Has to be a variable!
    for (unsigned int i=0;i<variableNames.size();i++) {
        if (variableNames[i]==operation) {
            if (allowedTypes.count(variableTypes[i])==0) {
                std::cerr << "Error reading line " << lineNumberCurrentlyRead << " from the input file. The variable " << operation << " is not allowed for this type of expression.\n";
                throw "Fatal Error";
            }
            return variables[i];
        }
    }
    std::cerr << "Error reading line " << lineNumberCurrentlyRead << " from the input file. The variable " << operation << " has not been found.\n";
    throw "Fatal Error";
}

/**
 * @brief Internal function for parsing a Boolean formula from a line in the input file - calls the recursive function to do all the work.
 * @param currentLine the line to parse
 * @param allowedTypes a list of allowed variable types - this allows to check that assumptions do not refer to 'next' output values.
 * @return a BF that represents the transition constraint read from the line
 */
BF GR1Context::parseBooleanFormula(std::string currentLine,std::set<VariableType> &allowedTypes) {

    std::istringstream is(currentLine);

    BF result = parseBooleanFormulaRecurse(is,allowedTypes);
    std::string nextPart = "";
    is >> nextPart;
    if (nextPart=="") return result;

    std::cerr << "Error reading line " << lineNumberCurrentlyRead << " from the input file. There are stray characters: '" << nextPart << "'" << std::endl;
    throw "Fatal Error";
}

/**
 * @brief Prints the help to stderr that the user sees when running "slugs --help" or when supplying
 *        incorrect parameters
 */
void printToolUsageHelp() {
    std::cerr << "Usage of slugs:\n";
    std::cerr << "slugs [--onlyRealizability] [--sysInitRoboticsSemantics] <InFile> [OutFile]\n\n";
    std::cerr << "The input file is supposed to be in 'slugs' format. If no output file name is\n";
    std::cerr << "given, then in case of realizability, the synthesized controller is printed\n";
    std::cerr << "to the standard output. The other options are:\n\n";
    std::cerr << " --onlyRealizability  Use this parameter if no synthesized system should be\n";
    std::cerr << "                      computed, but only the realizability/unrealizability\n";
    std::cerr << "                      result is to be computed.\n\n";
    std::cerr << " --sysInitRoboticsSemantics  In standard GR(1) synthesis, a specification is\n";
    std::cerr << "                             called realizable if for every initial input\n";
    std::cerr << "                             proposition valuation that is allowed by the\n";
    std::cerr << "                             initialization contraints, there is some\n";
    std::cerr << "                             suitable output proposition valuation. In the\n";
    std::cerr << "                             modified semantics for robotics applications, the\n";
    std::cerr << "                             controller has to be fine with any admissible\n";
    std::cerr << "                             starting position.\n\n";
}

/**
 * @brief The main function. Parses arguments from the command line and instantiates a synthesizer object accordingly.
 * @return the error code: >0 means that some error has occured. In case of realizability or unrealizability, a value of 0 is returned.
 */
int main(int argc, const char **args) {
    std::cerr << "SLUGS: SmaLl bUt complete Gr(1) Synthesis tool (see the documentation for an author list).\n";

    std::string filenameInputFile = "";
    std::string filenameOutputFile = "";
    bool onlyCheckRealizability = false;
    bool initSpecialRoboticsSemantics = false;

    for (int i=1;i<argc;i++) {
        std::string arg = args[i];
        if (arg[0]=='-') {
            if (arg=="--onlyRealizability") {
                onlyCheckRealizability = true;
            } else if (arg=="--sysInitRoboticsSemantics") {
                initSpecialRoboticsSemantics = true;
            } else if (arg=="--help") {
                printToolUsageHelp();
                return 0;
            } else {
                std::cerr << "Error: Did not understand parameter " << arg << std::endl;
                printToolUsageHelp();
                return 1;
            }
        } else {
            if (filenameInputFile=="") {
                filenameInputFile = arg;
            } else if (filenameOutputFile=="") {
                filenameOutputFile = arg;
            } else {
                std::cerr << "Error: More than two file names given.\n";
                return 1;
            }
        }
    }

    if (filenameInputFile == "") {
        std::cerr << "Error: You did not give an input filename.\n";
        printToolUsageHelp();
        return 1;
    }

    // The output filename might be undefined. In that case, "stdout" is used.

    try {
        GR1Context context(filenameInputFile);
        bool realizable = context.checkRealizability(initSpecialRoboticsSemantics);
        if (realizable) {
            std::cerr << "RESULT: Specification is realizable.\n";

            // Print winning strategy
            if (!onlyCheckRealizability) {
                if (filenameOutputFile=="") {
                    context.computeAndPrintExplicitStateStrategy(std::cout);
                } else {
                    std::ofstream of(filenameOutputFile.c_str());
                    if (of.fail()) {
                        std::cerr << "Error: Could not open output file'" << filenameOutputFile << "\n";
                        return 1;
                    }
                    context.computeAndPrintExplicitStateStrategy(of);
                    if (of.fail()) {
                        std::cerr << "Error: Writing to output file'" << filenameOutputFile << "failed. \n";
                        return 1;
                    }
                    of.close();
                }

            }
        } else {
            std::cerr << "RESULT: Specification is unrealizable.\n";
        }

    } catch (const char *error) {
        std::cerr << "Error: " << error << std::endl;
        return 1;
    }

    return 0;
}
<commit_msg>Make sure that output bits cannot be used in the initialization assumptions<commit_after>#include <iostream>
#include <fstream>
#include <sstream>
#include <boost/algorithm/string.hpp>
#include "gr1context.hpp"
#undef fail

/**
 * @brief Constructor that reads the problem instance from file and prepares the BFManager, the BFVarCubes, and the BFVarVectors
 * @param inFile the input filename
 */
GR1Context::GR1Context(std::string inFileName) {

    std::ifstream inFile(inFileName.c_str());
    if (inFile.fail()) throw "Error: Cannot open input file";

    // Prepare safety and initialization constraints
    initEnv = mgr.constantTrue();
    initSys = mgr.constantTrue();
    safetyEnv = mgr.constantTrue();
    safetySys = mgr.constantTrue();

    // The readmode variable stores in which chapter of the input file we are
    int readMode = -1;
    std::string currentLine;
    lineNumberCurrentlyRead = 0;
    while (std::getline(inFile,currentLine)) {
        lineNumberCurrentlyRead++;
        boost::trim(currentLine);
        if ((currentLine.length()>0) && (currentLine[0]!='#')) {
            if (currentLine[0]=='[') {
                if (currentLine=="[INPUT]") {
                    readMode = 0;
                } else if (currentLine=="[OUTPUT]") {
                    readMode = 1;
                } else if (currentLine=="[ENV_INIT]") {
                    readMode = 2;
                } else if (currentLine=="[SYS_INIT]") {
                    readMode = 3;
                } else if (currentLine=="[ENV_TRANS]") {
                    readMode = 4;
                } else if (currentLine=="[SYS_TRANS]") {
                    readMode = 5;
                } else if (currentLine=="[ENV_LIVENESS]") {
                    readMode = 6;
                } else if (currentLine=="[SYS_LIVENESS]") {
                    readMode = 7;
                } else {
                    std::cerr << "Sorry. Didn't recognize category " << currentLine << "\n";
                    throw "Aborted.";
                }
            } else {
                if (readMode==0) {
                    variables.push_back(mgr.newVariable());
                    variableNames.push_back(currentLine);
                    variableTypes.push_back(PreInput);
                    variables.push_back(mgr.newVariable());
                    variableNames.push_back(currentLine+"'");
                    variableTypes.push_back(PostInput);
                } else if (readMode==1) {
                    variables.push_back(mgr.newVariable());
                    variableNames.push_back(currentLine);
                    variableTypes.push_back(PreOutput);
                    variables.push_back(mgr.newVariable());
                    variableNames.push_back(currentLine+"'");
                    variableTypes.push_back(PostOutput);
                } else if (readMode==2) {
                    std::set<VariableType> allowedTypes;
                    allowedTypes.insert(PreInput);
                    initEnv &= parseBooleanFormula(currentLine,allowedTypes);
                } else if (readMode==3) {
                    std::set<VariableType> allowedTypes;
                    allowedTypes.insert(PreInput);
                    allowedTypes.insert(PreOutput);
                    initSys &= parseBooleanFormula(currentLine,allowedTypes);
                } else if (readMode==4) {
                    std::set<VariableType> allowedTypes;
                    allowedTypes.insert(PreInput);
                    allowedTypes.insert(PreOutput);
                    allowedTypes.insert(PostInput);
                    safetyEnv &= parseBooleanFormula(currentLine,allowedTypes);
                } else if (readMode==5) {
                    std::set<VariableType> allowedTypes;
                    allowedTypes.insert(PreInput);
                    allowedTypes.insert(PreOutput);
                    allowedTypes.insert(PostInput);
                    allowedTypes.insert(PostOutput);
                    safetySys &= parseBooleanFormula(currentLine,allowedTypes);
                } else if (readMode==6) {
                    std::set<VariableType> allowedTypes;
                    allowedTypes.insert(PreInput);
                    allowedTypes.insert(PreOutput);
                    allowedTypes.insert(PostInput);
                    livenessAssumptions.push_back(parseBooleanFormula(currentLine,allowedTypes));
                } else if (readMode==7) {
                    std::set<VariableType> allowedTypes;
                    allowedTypes.insert(PreInput);
                    allowedTypes.insert(PreOutput);
                    allowedTypes.insert(PostInput);
                    allowedTypes.insert(PostOutput);
                    livenessGuarantees.push_back(parseBooleanFormula(currentLine,allowedTypes));
                } else {
                    std::cerr << "Error with line " << lineNumberCurrentlyRead << "!";
                    throw "Found a line in the specification file that has no proper categorial context.";
                }
            }
        }
    }

    // Compute VarVectors and VarCubes
    std::vector<BF> postOutputVars;
    std::vector<BF> postInputVars;
    std::vector<BF> preOutputVars;
    std::vector<BF> preInputVars;
    for (unsigned int i=0;i<variables.size();i++) {
        switch (variableTypes[i]) {
        case PreInput:
            preVars.push_back(variables[i]);
            preInputVars.push_back(variables[i]);
            break;
        case PreOutput:
            preVars.push_back(variables[i]);
            preOutputVars.push_back(variables[i]);
            break;
        case PostInput:
            postVars.push_back(variables[i]);
            postInputVars.push_back(variables[i]);
            break;
        case PostOutput:
            postVars.push_back(variables[i]);
            postOutputVars.push_back(variables[i]);
            break;
        default:
            throw "Error: Found a variable of unknown type";
        }
    }
    varVectorPre = mgr.computeVarVector(preVars);
    varVectorPost = mgr.computeVarVector(postVars);
    varCubePostInput = mgr.computeCube(postInputVars);
    varCubePostOutput = mgr.computeCube(postOutputVars);
    varCubePreInput = mgr.computeCube(preInputVars);
    varCubePreOutput = mgr.computeCube(preOutputVars);
    varCubePre = mgr.computeCube(preVars);

    // Make sure that there is at least one liveness assumption and one liveness guarantee
    // The synthesis algorithm might be unsound otherwise
    if (livenessAssumptions.size()==0) livenessAssumptions.push_back(mgr.constantTrue());
    if (livenessGuarantees.size()==0) livenessGuarantees.push_back(mgr.constantTrue());
}

/**
 * @brief Recurse internal function to parse a Boolean formula from a line in the input file
 * @param is the input stream from which the tokens in the line are read
 * @param allowedTypes a list of allowed variable types - this allows to check that assumptions do not refer to 'next' output values.
 * @return a BF that represents the transition constraint read from the line
 */
BF GR1Context::parseBooleanFormulaRecurse(std::istringstream &is,std::set<VariableType> &allowedTypes) {
    std::string operation = "";
    is >> operation;
    if (operation=="") {
        std::cerr << "Error reading line " << lineNumberCurrentlyRead << " from the input file. Premature end of line.\n";
        throw "Fatal Error";
    }
    if (operation=="|") return parseBooleanFormulaRecurse(is,allowedTypes) | parseBooleanFormulaRecurse(is,allowedTypes);
    if (operation=="&") return parseBooleanFormulaRecurse(is,allowedTypes) & parseBooleanFormulaRecurse(is,allowedTypes);
    if (operation=="!") return !parseBooleanFormulaRecurse(is,allowedTypes);
    if (operation=="1") return mgr.constantTrue();
    if (operation=="0") return mgr.constantFalse();

    // Has to be a variable!
    for (unsigned int i=0;i<variableNames.size();i++) {
        if (variableNames[i]==operation) {
            if (allowedTypes.count(variableTypes[i])==0) {
                std::cerr << "Error reading line " << lineNumberCurrentlyRead << " from the input file. The variable " << operation << " is not allowed for this type of expression.\n";
                throw "Fatal Error";
            }
            return variables[i];
        }
    }
    std::cerr << "Error reading line " << lineNumberCurrentlyRead << " from the input file. The variable " << operation << " has not been found.\n";
    throw "Fatal Error";
}

/**
 * @brief Internal function for parsing a Boolean formula from a line in the input file - calls the recursive function to do all the work.
 * @param currentLine the line to parse
 * @param allowedTypes a list of allowed variable types - this allows to check that assumptions do not refer to 'next' output values.
 * @return a BF that represents the transition constraint read from the line
 */
BF GR1Context::parseBooleanFormula(std::string currentLine,std::set<VariableType> &allowedTypes) {

    std::istringstream is(currentLine);

    BF result = parseBooleanFormulaRecurse(is,allowedTypes);
    std::string nextPart = "";
    is >> nextPart;
    if (nextPart=="") return result;

    std::cerr << "Error reading line " << lineNumberCurrentlyRead << " from the input file. There are stray characters: '" << nextPart << "'" << std::endl;
    throw "Fatal Error";
}

/**
 * @brief Prints the help to stderr that the user sees when running "slugs --help" or when supplying
 *        incorrect parameters
 */
void printToolUsageHelp() {
    std::cerr << "Usage of slugs:\n";
    std::cerr << "slugs [--onlyRealizability] [--sysInitRoboticsSemantics] <InFile> [OutFile]\n\n";
    std::cerr << "The input file is supposed to be in 'slugs' format. If no output file name is\n";
    std::cerr << "given, then in case of realizability, the synthesized controller is printed\n";
    std::cerr << "to the standard output. The other options are:\n\n";
    std::cerr << " --onlyRealizability  Use this parameter if no synthesized system should be\n";
    std::cerr << "                      computed, but only the realizability/unrealizability\n";
    std::cerr << "                      result is to be computed.\n\n";
    std::cerr << " --sysInitRoboticsSemantics  In standard GR(1) synthesis, a specification is\n";
    std::cerr << "                             called realizable if for every initial input\n";
    std::cerr << "                             proposition valuation that is allowed by the\n";
    std::cerr << "                             initialization contraints, there is some\n";
    std::cerr << "                             suitable output proposition valuation. In the\n";
    std::cerr << "                             modified semantics for robotics applications, the\n";
    std::cerr << "                             controller has to be fine with any admissible\n";
    std::cerr << "                             starting position.\n\n";
}

/**
 * @brief The main function. Parses arguments from the command line and instantiates a synthesizer object accordingly.
 * @return the error code: >0 means that some error has occured. In case of realizability or unrealizability, a value of 0 is returned.
 */
int main(int argc, const char **args) {
    std::cerr << "SLUGS: SmaLl bUt complete Gr(1) Synthesis tool (see the documentation for an author list).\n";

    std::string filenameInputFile = "";
    std::string filenameOutputFile = "";
    bool onlyCheckRealizability = false;
    bool initSpecialRoboticsSemantics = false;

    for (int i=1;i<argc;i++) {
        std::string arg = args[i];
        if (arg[0]=='-') {
            if (arg=="--onlyRealizability") {
                onlyCheckRealizability = true;
            } else if (arg=="--sysInitRoboticsSemantics") {
                initSpecialRoboticsSemantics = true;
            } else if (arg=="--help") {
                printToolUsageHelp();
                return 0;
            } else {
                std::cerr << "Error: Did not understand parameter " << arg << std::endl;
                printToolUsageHelp();
                return 1;
            }
        } else {
            if (filenameInputFile=="") {
                filenameInputFile = arg;
            } else if (filenameOutputFile=="") {
                filenameOutputFile = arg;
            } else {
                std::cerr << "Error: More than two file names given.\n";
                return 1;
            }
        }
    }

    if (filenameInputFile == "") {
        std::cerr << "Error: You did not give an input filename.\n";
        printToolUsageHelp();
        return 1;
    }

    // The output filename might be undefined. In that case, "stdout" is used.

    try {
        GR1Context context(filenameInputFile);
        bool realizable = context.checkRealizability(initSpecialRoboticsSemantics);
        if (realizable) {
            std::cerr << "RESULT: Specification is realizable.\n";

            // Print winning strategy
            if (!onlyCheckRealizability) {
                if (filenameOutputFile=="") {
                    context.computeAndPrintExplicitStateStrategy(std::cout);
                } else {
                    std::ofstream of(filenameOutputFile.c_str());
                    if (of.fail()) {
                        std::cerr << "Error: Could not open output file'" << filenameOutputFile << "\n";
                        return 1;
                    }
                    context.computeAndPrintExplicitStateStrategy(of);
                    if (of.fail()) {
                        std::cerr << "Error: Writing to output file'" << filenameOutputFile << "failed. \n";
                        return 1;
                    }
                    of.close();
                }

            }
        } else {
            std::cerr << "RESULT: Specification is unrealizable.\n";
        }

    } catch (const char *error) {
        std::cerr << "Error: " << error << std::endl;
        return 1;
    }

    return 0;
}
<|endoftext|>


<commit_before>#include <stdio.h>
#include <fcntl.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <assert.h>
#include <unistd.h>
#include <fstream>
#include <iostream>
#include <time.h>
#include <unistd.h>
#include <sys/shm.h>

#include "bcm_host.h"

#include "GLES2/gl2.h"
#include "EGL/egl.h"
#include "EGL/eglext.h"

#include "shader.h"
#include "inotify-cxx.h"

typedef struct {
    uint32_t screen_width;
    uint32_t screen_height;
    // OpenGL|ES objects
    EGLDisplay display;
    EGLSurface surface;
    EGLContext context;

    GLuint buf;

    Shader shader;

} CUBE_STATE_T;

bool* fragHasChanged;
std::string fragFile;
std::string vertSource =
"attribute vec4 a_position;"
"varying vec2 v_texcoord;"
"void main(void) {"
"    gl_Position = a_position;"
"    v_texcoord = a_position.xy*0.5+0.5;"
"}";

static CUBE_STATE_T _state, *state=&_state;

static void init_ogl(CUBE_STATE_T *state){
    int32_t success = 0;
    EGLBoolean result;
    EGLint num_config;

    static EGL_DISPMANX_WINDOW_T nativewindow;

    DISPMANX_ELEMENT_HANDLE_T dispman_element;
    DISPMANX_DISPLAY_HANDLE_T dispman_display;
    DISPMANX_UPDATE_HANDLE_T dispman_update;
    VC_RECT_T dst_rect;
    VC_RECT_T src_rect;

    static const EGLint attribute_list[] = {
        EGL_RED_SIZE, 8,
        EGL_GREEN_SIZE, 8,
        EGL_BLUE_SIZE, 8,
        EGL_ALPHA_SIZE, 8,
        EGL_SURFACE_TYPE, EGL_WINDOW_BIT,
        EGL_NONE
    };

    static const EGLint context_attributes[] = {
        EGL_CONTEXT_CLIENT_VERSION, 2,
        EGL_NONE
    };
    EGLConfig config;

    // get an EGL display connection
    state->display = eglGetDisplay(EGL_DEFAULT_DISPLAY);
    assert(state->display!=EGL_NO_DISPLAY);

    // initialize the EGL display connection
    result = eglInitialize(state->display, NULL, NULL);
    assert(EGL_FALSE != result);

    // get an appropriate EGL frame buffer configuration
    result = eglChooseConfig(state->display, attribute_list, &config, 1, &num_config);
    assert(EGL_FALSE != result);

    // get an appropriate EGL frame buffer configuration
    result = eglBindAPI(EGL_OPENGL_ES_API);
    assert(EGL_FALSE != result);

    // create an EGL rendering context
    state->context = eglCreateContext(state->display, config, EGL_NO_CONTEXT, context_attributes);
    assert(state->context!=EGL_NO_CONTEXT);

    // create an EGL window surface
    success = graphics_get_display_size(0 /* LCD */, &state->screen_width, &state->screen_height);
    assert( success >= 0 );

    dst_rect.x = 0;
    dst_rect.y = 0;
    dst_rect.width = state->screen_width;
    dst_rect.height = state->screen_height;

    src_rect.x = 0;
    src_rect.y = 0;
    src_rect.width = state->screen_width << 16;
    src_rect.height = state->screen_height << 16;

    dispman_display = vc_dispmanx_display_open( 0 /* LCD */);
    dispman_update = vc_dispmanx_update_start( 0 );
    dispman_element = vc_dispmanx_element_add ( dispman_update, dispman_display,
                                               0/*layer*/, &dst_rect, 0/*src*/,
                                               &src_rect, DISPMANX_PROTECTION_NONE, 0 /*alpha*/, 0/*clamp*/, 0/*transform*/);

    nativewindow.element = dispman_element;
    nativewindow.width = state->screen_width;
    nativewindow.height = state->screen_height;
    vc_dispmanx_update_submit_sync( dispman_update );

    state->surface = eglCreateWindowSurface( state->display, config, &nativewindow, NULL );
    assert(state->surface != EGL_NO_SURFACE);

    // connect the context to the surface
    result = eglMakeCurrent(state->display, state->surface, state->surface, state->context);
    assert(EGL_FALSE != result);

    // Set background color and clear buffers
    glClearColor(0.15f, 0.25f, 0.35f, 1.0f);
    glClear( GL_COLOR_BUFFER_BIT );

}

static bool loadFromPath(const std::string& path, std::string* into) {
    std::ifstream file;
    std::string buffer;

    file.open(path.c_str());
    if(!file.is_open()) return false;
    while(!file.eof()) {
        getline(file, buffer);
        (*into) += buffer + "\n";
    }

    file.close();
    return true;
}

static void draw(CUBE_STATE_T *state, GLfloat cx, GLfloat cy){

    if(*fragHasChanged) {
        std::string fragSource;
        if(loadFromPath(fragFile, &fragSource)){
            state->shader.detach(GL_FRAGMENT_SHADER | GL_VERTEX_SHADER);
            state->shader.build(fragSource,vertSource);
            *fragHasChanged = false;
            std::cout << "Parent: reloading shader" << std::endl;
        }
    }

    // Now render to the main frame buffer
    glBindFramebuffer(GL_FRAMEBUFFER,0);
    // Clear the background (not really necessary I suppose)
    glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);

    glBindBuffer(GL_ARRAY_BUFFER, state->buf);
    state->shader.use();

    state->shader.sendUniform("u_time",((float)clock())/CLOCKS_PER_SEC);
    state->shader.sendUniform("u_mouse",cx, cy);
    state->shader.sendUniform("u_resolution",state->screen_width, state->screen_height);

    glDrawArrays(GL_TRIANGLE_FAN, 0, 4);

    glBindBuffer(GL_ARRAY_BUFFER, 0);

    glFlush();
    glFinish();

    eglSwapBuffers(state->display, state->surface);
}

static int get_mouse(CUBE_STATE_T *state, int *outx, int *outy){
    static int fd = -1;
    const int width=state->screen_width, height=state->screen_height;
    static int x=800, y=400;
    const int XSIGN = 1<<4, YSIGN = 1<<5;
    if (fd<0) {
       fd = open("/dev/input/mouse0",O_RDONLY|O_NONBLOCK);
    }
    if (fd>=0) {
        struct {char buttons, dx, dy; } m;
        while (1) {
           int bytes = read(fd, &m, sizeof m);
           if (bytes < (int)sizeof m) goto _exit;
           if (m.buttons&8) {
              break; // This bit should always be set
           }
           read(fd, &m, 1); // Try to sync up again
        }
        if (m.buttons&3)
           return m.buttons&3;
        x+=m.dx;
        y+=m.dy;
        if (m.buttons&XSIGN)
           x-=256;
        if (m.buttons&YSIGN)
           y-=256;
        if (x<0) x=0;
        if (y<0) y=0;
        if (x>width) x=width;
        if (y>height) y=height;
   }
_exit:
   if (outx) *outx = x;
   if (outy) *outy = y;
   return 0;
}

void drawThread() {
    while (1) {
        int x, y, b;
        b = get_mouse(state, &x, &y);
        if (b) break;
        draw(state, x, y);
    }
}

void watchThread(const std::string& file) {
    Inotify notify;

    InotifyWatch watch(file, IN_ALL_EVENTS);
    notify.Add(watch);

    for (;;) {

        if(!(*fragHasChanged)){
           std::cout << "Child: Watching again" << std::endl;
        notify.WaitForEvents();

        size_t count = notify.GetEventCount();
        while(count-- > 0) {
            InotifyEvent event;
            bool got_event = notify.GetEvent(&event);

            if(got_event){
                std::string mask_str;
                event.DumpTypes(mask_str);
                *fragHasChanged = true;
                std::cout << "Child: Something have change " << mask_str << std::endl;
            }
        }
        }

    }
}

void init(const std::string& fragFile) {
    GLfloat cx, cy;
    bcm_host_init();

    // Clear application state
    memset( state, 0, sizeof( *state ) );

    // Start OGLES
    init_ogl(state);

    //  Build shader;
    //
    std::string fragSource;
    if(!loadFromPath(fragFile, &fragSource)) {
        return;
    }
    state->shader.build(fragSource,vertSource);

    //  Make Quad
    //
    static const GLfloat vertex_data[] = {
        -1.0,-1.0,1.0,1.0,
        1.0,-1.0,1.0,1.0,
        1.0,1.0,1.0,1.0,
        -1.0,1.0,1.0,1.0
    };
    GLint posAttribut = state->shader.getAttribLocation("a_position");

    glClearColor ( 0.0, 1.0, 1.0, 1.0 );

    glGenBuffers(1, &state->buf);

    // Prepare viewport
    glViewport ( 0, 0, state->screen_width, state->screen_height );

    // Upload vertex data to a buffer
    glBindBuffer(GL_ARRAY_BUFFER, state->buf);
    glBufferData(GL_ARRAY_BUFFER, sizeof(vertex_data),
                 vertex_data, GL_STATIC_DRAW);
    glVertexAttribPointer(posAttribut, 4, GL_FLOAT, 0, 16, 0);
    glEnableVertexAttribArray(posAttribut);

}

//==============================================================================

int main(int argc, char **argv){

    fragFile = std::string(argv[1]);

    int shmId = shmget(IPC_PRIVATE, sizeof(bool), 0666);

    pid_t pid = fork();

    fragHasChanged = (bool *) shmat(shmId, NULL, 0);

    switch(pid) {
        case -1: //error
        break;

        case 0: // child
        {
            *fragHasChanged = false;
            watchThread(fragFile);
        }
        break;

        default:
        {
            init(fragFile);
            drawThread();

            kill(pid, SIGKILL);
        }
        break;
    }

    shmctl(shmId, IPC_RMID, NULL);

    return 0;
}

<commit_msg>no loop<commit_after>#include <stdio.h>
#include <fcntl.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <assert.h>
#include <unistd.h>
#include <fstream>
#include <iostream>
#include <time.h>
#include <unistd.h>
#include <sys/shm.h>

#include "bcm_host.h"

#include "GLES2/gl2.h"
#include "EGL/egl.h"
#include "EGL/eglext.h"

#include "shader.h"

#define INOTIFY_THREAD_SAFE
#include "inotify-cxx.h"

typedef struct {
    uint32_t screen_width;
    uint32_t screen_height;
    // OpenGL|ES objects
    EGLDisplay display;
    EGLSurface surface;
    EGLContext context;

    GLuint buf;

    Shader shader;

} CUBE_STATE_T;

bool* fragHasChanged;
std::string fragFile;
std::string vertSource =
"attribute vec4 a_position;"
"varying vec2 v_texcoord;"
"void main(void) {"
"    gl_Position = a_position;"
"    v_texcoord = a_position.xy*0.5+0.5;"
"}";

static CUBE_STATE_T _state, *state=&_state;

static void init_ogl(CUBE_STATE_T *state){
    int32_t success = 0;
    EGLBoolean result;
    EGLint num_config;

    static EGL_DISPMANX_WINDOW_T nativewindow;

    DISPMANX_ELEMENT_HANDLE_T dispman_element;
    DISPMANX_DISPLAY_HANDLE_T dispman_display;
    DISPMANX_UPDATE_HANDLE_T dispman_update;
    VC_RECT_T dst_rect;
    VC_RECT_T src_rect;

    static const EGLint attribute_list[] = {
        EGL_RED_SIZE, 8,
        EGL_GREEN_SIZE, 8,
        EGL_BLUE_SIZE, 8,
        EGL_ALPHA_SIZE, 8,
        EGL_SURFACE_TYPE, EGL_WINDOW_BIT,
        EGL_NONE
    };

    static const EGLint context_attributes[] = {
        EGL_CONTEXT_CLIENT_VERSION, 2,
        EGL_NONE
    };
    EGLConfig config;

    // get an EGL display connection
    state->display = eglGetDisplay(EGL_DEFAULT_DISPLAY);
    assert(state->display!=EGL_NO_DISPLAY);

    // initialize the EGL display connection
    result = eglInitialize(state->display, NULL, NULL);
    assert(EGL_FALSE != result);

    // get an appropriate EGL frame buffer configuration
    result = eglChooseConfig(state->display, attribute_list, &config, 1, &num_config);
    assert(EGL_FALSE != result);

    // get an appropriate EGL frame buffer configuration
    result = eglBindAPI(EGL_OPENGL_ES_API);
    assert(EGL_FALSE != result);

    // create an EGL rendering context
    state->context = eglCreateContext(state->display, config, EGL_NO_CONTEXT, context_attributes);
    assert(state->context!=EGL_NO_CONTEXT);

    // create an EGL window surface
    success = graphics_get_display_size(0 /* LCD */, &state->screen_width, &state->screen_height);
    assert( success >= 0 );

    dst_rect.x = 0;
    dst_rect.y = 0;
    dst_rect.width = state->screen_width;
    dst_rect.height = state->screen_height;

    src_rect.x = 0;
    src_rect.y = 0;
    src_rect.width = state->screen_width << 16;
    src_rect.height = state->screen_height << 16;

    dispman_display = vc_dispmanx_display_open( 0 /* LCD */);
    dispman_update = vc_dispmanx_update_start( 0 );
    dispman_element = vc_dispmanx_element_add ( dispman_update, dispman_display,
                                               0/*layer*/, &dst_rect, 0/*src*/,
                                               &src_rect, DISPMANX_PROTECTION_NONE, 0 /*alpha*/, 0/*clamp*/, 0/*transform*/);

    nativewindow.element = dispman_element;
    nativewindow.width = state->screen_width;
    nativewindow.height = state->screen_height;
    vc_dispmanx_update_submit_sync( dispman_update );

    state->surface = eglCreateWindowSurface( state->display, config, &nativewindow, NULL );
    assert(state->surface != EGL_NO_SURFACE);

    // connect the context to the surface
    result = eglMakeCurrent(state->display, state->surface, state->surface, state->context);
    assert(EGL_FALSE != result);

    // Set background color and clear buffers
    glClearColor(0.15f, 0.25f, 0.35f, 1.0f);
    glClear( GL_COLOR_BUFFER_BIT );

}

static bool loadFromPath(const std::string& path, std::string* into) {
    std::ifstream file;
    std::string buffer;

    file.open(path.c_str());
    if(!file.is_open()) return false;
    while(!file.eof()) {
        getline(file, buffer);
        (*into) += buffer + "\n";
    }

    file.close();
    return true;
}

static void draw(CUBE_STATE_T *state, GLfloat cx, GLfloat cy){

    if(*fragHasChanged) {
        std::string fragSource;
        if(loadFromPath(fragFile, &fragSource)){
            state->shader.detach(GL_FRAGMENT_SHADER | GL_VERTEX_SHADER);
            state->shader.build(fragSource,vertSource);
            *fragHasChanged = false;
            std::cout << "Parent: reloading shader" << std::endl;
        }
    }

    // Now render to the main frame buffer
    glBindFramebuffer(GL_FRAMEBUFFER,0);
    // Clear the background (not really necessary I suppose)
    glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);

    glBindBuffer(GL_ARRAY_BUFFER, state->buf);
    state->shader.use();

    state->shader.sendUniform("u_time",((float)clock())/CLOCKS_PER_SEC);
    state->shader.sendUniform("u_mouse",cx, cy);
    state->shader.sendUniform("u_resolution",state->screen_width, state->screen_height);

    glDrawArrays(GL_TRIANGLE_FAN, 0, 4);

    glBindBuffer(GL_ARRAY_BUFFER, 0);

    glFlush();
    glFinish();

    eglSwapBuffers(state->display, state->surface);
}

static int get_mouse(CUBE_STATE_T *state, int *outx, int *outy){
    static int fd = -1;
    const int width=state->screen_width, height=state->screen_height;
    static int x=800, y=400;
    const int XSIGN = 1<<4, YSIGN = 1<<5;
    if (fd<0) {
       fd = open("/dev/input/mouse0",O_RDONLY|O_NONBLOCK);
    }
    if (fd>=0) {
        struct {char buttons, dx, dy; } m;
        while (1) {
           int bytes = read(fd, &m, sizeof m);
           if (bytes < (int)sizeof m) goto _exit;
           if (m.buttons&8) {
              break; // This bit should always be set
           }
           read(fd, &m, 1); // Try to sync up again
        }
        if (m.buttons&3)
           return m.buttons&3;
        x+=m.dx;
        y+=m.dy;
        if (m.buttons&XSIGN)
           x-=256;
        if (m.buttons&YSIGN)
           y-=256;
        if (x<0) x=0;
        if (y<0) y=0;
        if (x>width) x=width;
        if (y>height) y=height;
   }
_exit:
   if (outx) *outx = x;
   if (outy) *outy = y;
   return 0;
}

void drawThread() {
    while (1) {
        int x, y, b;
        b = get_mouse(state, &x, &y);
        if (b) break;
        draw(state, x, y);
    }
}

void watchThread(const std::string& file) {
    Inotify notify;

    InotifyWatch watch(file, IN_MODIFY);//IN_ALL_EVENTS);
    notify.Add(watch);

    for (;;) {

        if(!(*fragHasChanged)){
            std::cout << "Child: Watching again" << std::endl;
            notify.WaitForEvents();

            size_t count = notify.GetEventCount();
            while(count-- > 0) {
                InotifyEvent event;
                bool got_event = notify.GetEvent(&event);

                if(got_event){
                    std::string mask_str;
                    event.DumpTypes(mask_str);
                    *fragHasChanged = true;
                    std::cout << "Child: Something have change " << mask_str << std::endl;
                }
            }
        }

    }
}

void init(const std::string& fragFile) {
    GLfloat cx, cy;
    bcm_host_init();

    // Clear application state
    memset( state, 0, sizeof( *state ) );

    // Start OGLES
    init_ogl(state);

    //  Build shader;
    //
    std::string fragSource;
    if(!loadFromPath(fragFile, &fragSource)) {
        return;
    }
    state->shader.build(fragSource,vertSource);

    //  Make Quad
    //
    static const GLfloat vertex_data[] = {
        -1.0,-1.0,1.0,1.0,
        1.0,-1.0,1.0,1.0,
        1.0,1.0,1.0,1.0,
        -1.0,1.0,1.0,1.0
    };
    GLint posAttribut = state->shader.getAttribLocation("a_position");

    glClearColor ( 0.0, 1.0, 1.0, 1.0 );

    glGenBuffers(1, &state->buf);

    // Prepare viewport
    glViewport ( 0, 0, state->screen_width, state->screen_height );

    // Upload vertex data to a buffer
    glBindBuffer(GL_ARRAY_BUFFER, state->buf);
    glBufferData(GL_ARRAY_BUFFER, sizeof(vertex_data),
                 vertex_data, GL_STATIC_DRAW);
    glVertexAttribPointer(posAttribut, 4, GL_FLOAT, 0, 16, 0);
    glEnableVertexAttribArray(posAttribut);

}

//==============================================================================

int main(int argc, char **argv){

    fragFile = std::string(argv[1]);

    int shmId = shmget(IPC_PRIVATE, sizeof(bool), 0666);

    pid_t pid = fork();

    fragHasChanged = (bool *) shmat(shmId, NULL, 0);

    switch(pid) {
        case -1: //error
        break;

        case 0: // child
        {
            *fragHasChanged = false;
            watchThread(fragFile);
        }
        break;

        default:
        {
            init(fragFile);
            drawThread();

            kill(pid, SIGKILL);
        }
        break;
    }

    shmctl(shmId, IPC_RMID, NULL);

    return 0;
}

<|endoftext|>


<commit_before>//
// Created by gregorydepuille@sglk.local on 27/02/17.
//

#include "RPLidarHelper.h"

RPLidarHelper::RPLidarHelper(string comFile, unsigned int baudrate) {
    this->comFile = comFile;
    this->baudrate = baudrate;
}

void RPLidarHelper::init() {
    // create the driver instance
    this->driver = RPlidarDriver::CreateDriver(RPlidarDriver::DRIVER_TYPE_SERIALPORT);
    if (!this->driver) {
        cerr << "Insufficent memory, exit" << endl;
        exit(2);
    }

    // try to connect
    if (IS_FAIL(this->driver->connect(this->comFile.c_str(), this->baudrate))) {
        cerr << "Error, cannot bind to the specified serial port " << this->comFile << endl;
        exit(3);
    }
}

JsonResult RPLidarHelper::getDeviceInfo() {
    rplidar_response_device_info_t deviceInfo;
    if (IS_FAIL(this->driver->getDeviceInfo(deviceInfo))) {
        JsonResult fail;
        fail.status = RESPONSE_ERROR;
        fail.action = DEVICE_INFO;
        fail.errorMessage = "Impossible de récupérer les infos";
        return fail;
    }

    // Récupération du numéro de série
    ostringstream serialStream;
    for (int i = 0 ; i < 16 ; i ++) {
        _u8 v = deviceInfo.serialnum[i];
        char buff[3];
        snprintf(buff, sizeof(buff), "%02X", v);
        string tmp = buff;
        serialStream << tmp;
    }

    // Récupération du firmware
    ostringstream firmStream;
    firmStream << (deviceInfo.firmware_version >> 8) << "." << (deviceInfo.firmware_version & 0xFF);
    if ((deviceInfo.firmware_version & 0xFF) <= 10) {
        firmStream << "0";
    }

    json datas;
    datas["serialNumber"] = serialStream.str();
    datas["hardwareVersion"] = deviceInfo.hardware_version;
    datas["firmwareVersion"] = firmStream.str();

    JsonResult r;
    r.status = RESPONSE_OK;
    r.action = DEVICE_INFO;
    r.datas = datas;
    return r;
}

JsonResult RPLidarHelper::getHealth() {
    rplidar_response_device_health_t healthinfo;
    if (IS_FAIL(this->driver->getHealth(healthinfo))) {
        JsonResult fail;
        fail.status = RESPONSE_ERROR;
        fail.action = HEALTH_INFO;
        fail.errorMessage = "Impossible de récupérer les infos de santé";
        return fail;
    }

    string libelle;
    if (healthinfo.status == RPLIDAR_STATUS_ERROR) {
        libelle = "ERROR";
    } else if (healthinfo.status == RPLIDAR_STATUS_WARNING) {
        libelle = "WARNING";
    } else {
        libelle = "OK";
    }

    json datas;
    datas["value"] = healthinfo.status;
    datas["libelle"] = libelle;
    datas["errorCode"] = (unsigned int) healthinfo.error_code;

    JsonResult r;
    r.status = RESPONSE_OK;
    r.action = DEVICE_INFO;
    r.datas = datas;
    return r;
}

JsonResult RPLidarHelper::startScan(JsonQuery q) {
    if (IS_FAIL(this->driver->startMotor())) {
        JsonResult r;
        r.action = q.action;
        r.status = RESPONSE_ERROR;
        r.errorMessage = "Impossible de démarrer le moteur";
        return r;
    };
    if (IS_FAIL(this->driver->startScan())) {
        JsonResult r;
        r.action = q.action;
        r.status = RESPONSE_ERROR;
        r.errorMessage = "Impossible de démarrer le scan";
        return r;
    }
    return this->setMotorSpeed(q);
}

JsonResult RPLidarHelper::stopScan() {
    JsonResult r;
    r.action = STOP_SCAN;
    r.status = RESPONSE_OK;

    if (IS_FAIL(this->driver->stop())) {
        r.status = RESPONSE_ERROR;
        r.errorMessage = "Impossible d'arreter le scan";
    }
    if (IS_FAIL(this->driver->stopMotor())) {
        r.status = RESPONSE_ERROR;
        r.errorMessage = "Impossible d'arreter le moteur";
    }

    return r;
}

JsonResult RPLidarHelper::setMotorSpeed(JsonQuery q) {
    JsonResult r;
    r.action = q.action;
    r.datas = q.datas;
    u_result result = RESULT_OK;
    if (!q.datas["speed"].is_null()) {
        result = this->setMotorSpeed((_u16) q.datas["speed"]);
    }
    if (IS_FAIL(result)) {
        r.status = RESPONSE_ERROR;
    } else {
        r.status = RESPONSE_OK;
    }
    return r;
}

u_result RPLidarHelper::setMotorSpeed(_u16 speed) {
    if (speed > MAX_MOTOR_PWM) {
        speed = MAX_MOTOR_PWM;
    } else if (speed < 0) {
        speed = 0;
    }
    return this->driver->setMotorPWM(speed);
}

JsonResult RPLidarHelper::grabScanData() {
    JsonResult r;
    r.action = GRAB_DATA;

    rplidar_response_measurement_node_t nodes[360*2];
    size_t   count = _countof(nodes);

    u_result res = this->driver->grabScanData(nodes, count);
    if (IS_OK(res)) {
        this->driver->ascendScanData(nodes, count);

        r.status = RESPONSE_OK;

        int ignored = 0;

        json scanDatas = json::array();
        for (int pos = 0; pos < (int) count ; ++pos) {
            float distanceMm = nodes[pos].distance_q2 / 4.0f;
            if ((distanceMm < 150) || (distanceMm > 3600)) {
                ignored++;
                continue;
            }

            float angleDeg = (nodes[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f;
            bool syncBit = nodes[pos].sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT == 1;
            int quality = nodes[pos].sync_quality >> RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT;

            // Transposition des angles dans le repère robot
            if (angleDeg > 0 && angleDeg < 180) {
                angleDeg *= -1;
            } else {
                angleDeg -= 360;
            }

            json v;
            v["angleDeg"] = angleDeg;
            v["distanceMm"] = distanceMm;
            v["syncBit"] = syncBit;
            v["quality"] = quality;
            scanDatas.push_back(v);
        }

        r.datas["ignored"] = ignored;
        r.datas["scan"] = scanDatas;
    } else {
        r.status = RESPONSE_ERROR;
        r.errorMessage = "Erreur lors de la récupération des données du SCAN";
    }

    return r;
}

void RPLidarHelper::end() {
    this->stopScan();
    this->driver->disconnect();

    RPlidarDriver::DisposeDriver(this->driver);
}<commit_msg>Renomage du champ pour indiqué l'état du lidar.<commit_after>//
// Created by gregorydepuille@sglk.local on 27/02/17.
//

#include "RPLidarHelper.h"

RPLidarHelper::RPLidarHelper(string comFile, unsigned int baudrate) {
    this->comFile = comFile;
    this->baudrate = baudrate;
}

void RPLidarHelper::init() {
    // create the driver instance
    this->driver = RPlidarDriver::CreateDriver(RPlidarDriver::DRIVER_TYPE_SERIALPORT);
    if (!this->driver) {
        cerr << "Insufficent memory, exit" << endl;
        exit(2);
    }

    // try to connect
    if (IS_FAIL(this->driver->connect(this->comFile.c_str(), this->baudrate))) {
        cerr << "Error, cannot bind to the specified serial port " << this->comFile << endl;
        exit(3);
    }
}

JsonResult RPLidarHelper::getDeviceInfo() {
    rplidar_response_device_info_t deviceInfo;
    if (IS_FAIL(this->driver->getDeviceInfo(deviceInfo))) {
        JsonResult fail;
        fail.status = RESPONSE_ERROR;
        fail.action = DEVICE_INFO;
        fail.errorMessage = "Impossible de récupérer les infos";
        return fail;
    }

    // Récupération du numéro de série
    ostringstream serialStream;
    for (int i = 0 ; i < 16 ; i ++) {
        _u8 v = deviceInfo.serialnum[i];
        char buff[3];
        snprintf(buff, sizeof(buff), "%02X", v);
        string tmp = buff;
        serialStream << tmp;
    }

    // Récupération du firmware
    ostringstream firmStream;
    firmStream << (deviceInfo.firmware_version >> 8) << "." << (deviceInfo.firmware_version & 0xFF);
    if ((deviceInfo.firmware_version & 0xFF) <= 10) {
        firmStream << "0";
    }

    json datas;
    datas["serialNumber"] = serialStream.str();
    datas["hardwareVersion"] = deviceInfo.hardware_version;
    datas["firmwareVersion"] = firmStream.str();

    JsonResult r;
    r.status = RESPONSE_OK;
    r.action = DEVICE_INFO;
    r.datas = datas;
    return r;
}

JsonResult RPLidarHelper::getHealth() {
    rplidar_response_device_health_t healthinfo;
    if (IS_FAIL(this->driver->getHealth(healthinfo))) {
        JsonResult fail;
        fail.status = RESPONSE_ERROR;
        fail.action = HEALTH_INFO;
        fail.errorMessage = "Impossible de récupérer les infos de santé";
        return fail;
    }

    string state;
    if (healthinfo.status == RPLIDAR_STATUS_ERROR) {
        state = "ERROR";
    } else if (healthinfo.status == RPLIDAR_STATUS_WARNING) {
        state = "WARNING";
    } else {
        state = "OK";
    }

    json datas;
    datas["value"] = healthinfo.status;
    datas["state"] = state;
    datas["errorCode"] = (unsigned int) healthinfo.error_code;

    JsonResult r;
    r.status = RESPONSE_OK;
    r.action = DEVICE_INFO;
    r.datas = datas;
    return r;
}

JsonResult RPLidarHelper::startScan(JsonQuery q) {
    if (IS_FAIL(this->driver->startMotor())) {
        JsonResult r;
        r.action = q.action;
        r.status = RESPONSE_ERROR;
        r.errorMessage = "Impossible de démarrer le moteur";
        return r;
    };
    if (IS_FAIL(this->driver->startScan())) {
        JsonResult r;
        r.action = q.action;
        r.status = RESPONSE_ERROR;
        r.errorMessage = "Impossible de démarrer le scan";
        return r;
    }
    return this->setMotorSpeed(q);
}

JsonResult RPLidarHelper::stopScan() {
    JsonResult r;
    r.action = STOP_SCAN;
    r.status = RESPONSE_OK;

    if (IS_FAIL(this->driver->stop())) {
        r.status = RESPONSE_ERROR;
        r.errorMessage = "Impossible d'arreter le scan";
    }
    if (IS_FAIL(this->driver->stopMotor())) {
        r.status = RESPONSE_ERROR;
        r.errorMessage = "Impossible d'arreter le moteur";
    }

    return r;
}

JsonResult RPLidarHelper::setMotorSpeed(JsonQuery q) {
    JsonResult r;
    r.action = q.action;
    r.datas = q.datas;
    u_result result = RESULT_OK;
    if (!q.datas["speed"].is_null()) {
        result = this->setMotorSpeed((_u16) q.datas["speed"]);
    }
    if (IS_FAIL(result)) {
        r.status = RESPONSE_ERROR;
    } else {
        r.status = RESPONSE_OK;
    }
    return r;
}

u_result RPLidarHelper::setMotorSpeed(_u16 speed) {
    if (speed > MAX_MOTOR_PWM) {
        speed = MAX_MOTOR_PWM;
    } else if (speed < 0) {
        speed = 0;
    }
    return this->driver->setMotorPWM(speed);
}

JsonResult RPLidarHelper::grabScanData() {
    JsonResult r;
    r.action = GRAB_DATA;

    rplidar_response_measurement_node_t nodes[360*2];
    size_t   count = _countof(nodes);

    u_result res = this->driver->grabScanData(nodes, count);
    if (IS_OK(res)) {
        this->driver->ascendScanData(nodes, count);

        r.status = RESPONSE_OK;

        int ignored = 0;

        json scanDatas = json::array();
        for (int pos = 0; pos < (int) count ; ++pos) {
            float distanceMm = nodes[pos].distance_q2 / 4.0f;
            if ((distanceMm < 150) || (distanceMm > 3600)) {
                ignored++;
                continue;
            }

            float angleDeg = (nodes[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f;
            bool syncBit = nodes[pos].sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT == 1;
            int quality = nodes[pos].sync_quality >> RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT;

            // Transposition des angles dans le repère robot
            if (angleDeg > 0 && angleDeg < 180) {
                angleDeg *= -1;
            } else {
                angleDeg -= 360;
            }

            json v;
            v["angleDeg"] = angleDeg;
            v["distanceMm"] = distanceMm;
            v["syncBit"] = syncBit;
            v["quality"] = quality;
            scanDatas.push_back(v);
        }

        r.datas["ignored"] = ignored;
        r.datas["scan"] = scanDatas;
    } else {
        r.status = RESPONSE_ERROR;
        r.errorMessage = "Erreur lors de la récupération des données du SCAN";
    }

    return r;
}

void RPLidarHelper::end() {
    this->stopScan();
    this->driver->disconnect();

    RPlidarDriver::DisposeDriver(this->driver);
}<|endoftext|>


<commit_before>/*
This file is part of Groho, a simulator for inter-planetary travel and warfare.
Copyright (c) 2017-2018 by Kaushik Ghose. Some rights reserved, see LICENSE

Entry point function for command line program
*/

#include <functional>
#include <iostream>
#include <signal.h>
#include <stdlib.h>
#include <thread>

#include "app.hpp"
#include "buffer.hpp"
#include "scenario.hpp"
#include "simulator.hpp"

#define LOGURU_IMPLEMENTATION 1
#define LOGURU_WITH_STREAMS 1
#include "loguru.hpp"

volatile sig_atomic_t keep_running = true;

void ctrl_c_pressed(int) { keep_running = false; }

void print_license()
{
    std::cout <<
        R"(  Groho 0.3.0
  Copyright (c) 2018 Kaushik Ghose. All rights reserved.
  Released under the MIT License)"
              << std::endl;
}

void print_usage()
{
    std::cout << "\nUsage:\n"
              << "groho <scenario file> <result file>" << std::endl;
}

int main(int argc, char* argv[])
{
    print_license();

    if (argc < 3) {
        print_usage();
        exit(0);
    }

    loguru::init(argc, argv);

    signal(SIGINT, ctrl_c_pressed);

    std::string scn_file(argv[1]), result_file(argv[2]);

    sim::Simulator simulator;

    unsigned int interval_ms = 500;

    auto simulator_thread = std::thread([&simulator, scn_file, interval_ms]() {
        sim::Scenario scenario(scn_file);
        simulator.restart_with(scenario);

        time_t last_mod_time = scenario.latest_modification();
        while (keep_running) {
            if (last_mod_time < scenario.latest_modification()) {

                LOG_S(INFO) << "Reloading scenario files";

                sim::Scenario new_scenario(scenario.fname);
                // if (new_scenario != scenario) {
                if (true) {
                    LOG_S(INFO) << "Scenario file changed";
                    scenario = new_scenario;
                    simulator.restart_with(scenario);
                    last_mod_time = scenario.latest_modification();
                }
            }

            std::this_thread::sleep_for(std::chrono::milliseconds(interval_ms));
        }
        simulator.stop();
    });

    sim::GrohoApp app({ argc, argv }, simulator);

    int ret_val = app.exec();

    keep_running = false;

    if (simulator_thread.joinable()) {
        simulator_thread.join();
    }

    return ret_val;
}<commit_msg>docs: Improve message shown at startup<commit_after>/*
This file is part of Groho, a simulator for inter-planetary travel and warfare.
Copyright (c) 2017-2018 by Kaushik Ghose. Some rights reserved, see LICENSE

Entry point function for command line program
*/

#include <functional>
#include <iostream>
#include <signal.h>
#include <stdlib.h>
#include <thread>

#include "app.hpp"
#include "buffer.hpp"
#include "scenario.hpp"
#include "simulator.hpp"

#define LOGURU_IMPLEMENTATION 1
#define LOGURU_WITH_STREAMS 1
#include "loguru.hpp"

volatile sig_atomic_t keep_running = true;

void ctrl_c_pressed(int) { keep_running = false; }

void print_license()
{
    std::cout <<
        R"(  Groho 0.4.0: A simulator for inter-planetary travel and warfare
  Copyright (c) 2017-2018 by Kaushik Ghose.
  Released under the MIT License. Some rights reserved)"
              << std::endl;
}

void print_usage()
{
    std::cout << "\nUsage:\n"
              << "groho <scenario file> <result file>" << std::endl;
}

int main(int argc, char* argv[])
{
    print_license();

    if (argc < 3) {
        print_usage();
        exit(0);
    }

    loguru::init(argc, argv);

    signal(SIGINT, ctrl_c_pressed);

    std::string scn_file(argv[1]), result_file(argv[2]);

    sim::Simulator simulator;

    unsigned int interval_ms = 500;

    auto simulator_thread = std::thread([&simulator, scn_file, interval_ms]() {
        sim::Scenario scenario(scn_file);
        simulator.restart_with(scenario);

        time_t last_mod_time = scenario.latest_modification();
        while (keep_running) {
            if (last_mod_time < scenario.latest_modification()) {

                LOG_S(INFO) << "Reloading scenario files";

                sim::Scenario new_scenario(scenario.fname);
                // if (new_scenario != scenario) {
                if (true) {
                    LOG_S(INFO) << "Scenario file changed";
                    scenario = new_scenario;
                    simulator.restart_with(scenario);
                    last_mod_time = scenario.latest_modification();
                }
            }

            std::this_thread::sleep_for(std::chrono::milliseconds(interval_ms));
        }
        simulator.stop();
    });

    sim::GrohoApp app({ argc, argv }, simulator);

    int ret_val = app.exec();

    keep_running = false;

    if (simulator_thread.joinable()) {
        simulator_thread.join();
    }

    return ret_val;
}<|endoftext|>


<commit_before>#include <common.h>
#include <Parser.h>
#include <Tokenizer.h>
#include <Configuration.h>
#include <ErrorProcessor.h>
#include <PatternsFileProcessor.h>

using namespace Lspl::Parser;
using namespace Lspl::Pattern;
using namespace Lspl::Configuration;

int main( int argc, const char* argv[] )
{
	try {
		if( argc != 4 ) {
			cerr << "Usage: lspl2 CONFIGURATION PATTERNS TEXT" << endl;
			return 1;
		}

		CConfigurationPtr conf = LoadConfigurationFromFile( argv[1], cerr, cout );

		if( !static_cast<bool>( conf ) ) {
			return 1;
		}

		CErrorProcessor errorProcessor;
		CPatternsBuilder patternsBuilder( conf, errorProcessor );
		patternsBuilder.Read( argv[2] );
		patternsBuilder.Check();

		if( errorProcessor.HasAnyErrors() ) {
			errorProcessor.PrintErrors( cerr, argv[2] );
			return 1;
		}

		const CPatterns patterns = patternsBuilder.Save();
		patterns.Print( cout );
	} catch( exception& e ) {
		cerr << e.what() << endl;
		return 1;
	} catch( ... ) {
		cerr << "unknown error!";
		return 1;
	}
	return 0;
}
<commit_msg>test main version<commit_after>#include <common.h>
#include <Parser.h>
#include <Tokenizer.h>
#include <TextLoader.h>
#include <PatternMatch.h>
#include <Configuration.h>
#include <ErrorProcessor.h>
#include <PatternsFileProcessor.h>

using namespace Lspl;
using namespace Lspl::Text;
using namespace Lspl::Parser;
using namespace Lspl::Pattern;
using namespace Lspl::Configuration;

int main( int argc, const char* argv[] )
{
	try {
		if( argc != 4 ) {
			cerr << "Usage: lspl2 CONFIGURATION PATTERNS TEXT" << endl;
			return 1;
		}

		CConfigurationPtr conf = LoadConfigurationFromFile( argv[1], cerr, cout );

		if( !static_cast<bool>( conf ) ) {
			return 1;
		}

		CErrorProcessor errorProcessor;
		CPatternsBuilder patternsBuilder( conf, errorProcessor );
		patternsBuilder.Read( argv[2] );
		patternsBuilder.Check();

		if( errorProcessor.HasAnyErrors() ) {
			errorProcessor.PrintErrors( cerr, argv[2] );
			return 1;
		}

		const CPatterns patterns = patternsBuilder.Save();
		patterns.Print( cout );

		CWords words;
		LoadText( patterns, argv[3], words );
		CText text( move( words ) );

		for( TReference ref = 0; ref < patterns.Size(); ref++ ) {
			const CPattern& pattern = patterns.Pattern( ref );
			cout << pattern.Name() << endl;
			CStates states;
			{
				CPatternBuildContext buildContext( patterns );
				CPatternVariants variants;
				pattern.Build( buildContext, variants, 12 );
				states = move( variants.Build( patterns ) );
			}
			{
				CMatchContext matchContext( text, states );
				for( TWordIndex wi = 0; wi < text.Length(); wi++ ) {
					matchContext.Match( wi );
				}
			}
			cout << endl;
		}
	} catch( exception& e ) {
		cerr << e.what() << endl;
		return 1;
	} catch( ... ) {
		cerr << "unknown error!";
		return 1;
	}
	return 0;
}
<|endoftext|>


<commit_before>/****************************************************************************
 *
 * Copyright (c) 2015, 2016 Gus Grubba. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file main.cpp
 * ESP8266 Wifi AP, MavLink UART/UDP Bridge
 *
 * @author Gus Grubba <mavlink@grubba.com>
 */

#include "mavesp8266.h"
#include "mavesp8266_parameters.h"
#include "mavesp8266_gcs.h"
#include "mavesp8266_vehicle.h"
#include "mavesp8266_httpd.h"

#define GPIO02  2

#ifndef ENABLE_DEBUG
uint8_t                 reset_state;
#endif

//---------------------------------------------------------------------------------
//-- HTTP Update Status
class MavESP8266UpdateImp : public MavESP8266Update {
public:
    MavESP8266UpdateImp ()
        : _isUpdating(false)
    {

    }
    void updateStarted  ()
    {
        _isUpdating = true;
    }
    void updateCompleted()
    {
        //-- TODO
    }
    void updateError    ()
    {
        //-- TODO
    }
    bool isUpdating     () { return _isUpdating; }
private:
    bool _isUpdating;
};

//-- Singletons
IPAddress               localIP;
MavESP8266Parameters    Parameters;
MavESP8266GCS           GCS;
MavESP8266Vehicle       Vehicle;
MavESP8266Httpd         updateServer;
MavESP8266UpdateImp     updateStatus;
MavESP8266Log           Logger;

//---------------------------------------------------------------------------------
//-- Accessors
class MavESP8266WorldImp : public MavESP8266World {
public:
    MavESP8266Parameters*   getParameters   () { return &Parameters;    }
    MavESP8266Vehicle*      getVehicle      () { return &Vehicle;       }
    MavESP8266GCS*          getGCS          () { return &GCS;           }
    MavESP8266Log*          getLogger       () { return &Logger;        }
};

MavESP8266WorldImp      World;

MavESP8266World* getWorld()
{
    return &World;
}

//---------------------------------------------------------------------------------
//-- Wait for a DHCPD client
void wait_for_client() {
    DEBUG_LOG("Waiting for a client...\n");
    int wcount = 0;
    uint8 client_count = wifi_softap_get_station_num();
    while (!client_count) {
        #ifdef ENABLE_DEBUG
        Serial1.print(".");
        if(++wcount > 80) {
            wcount = 0;
            Serial1.println();
        }
        #endif
        delay(1000);
        client_count = wifi_softap_get_station_num();
    }
    DEBUG_LOG("Got %d client(s)\n", client_count);
}

//---------------------------------------------------------------------------------
//-- Check for reset pin
void check_reset() {
#ifndef ENABLE_DEBUG
    //-- Test for "Reset To Factory"
    /* Needs testing
    int reset = digitalRead(GPIO02);
    if(reset != reset_state) {
        resetToDefaults();
        saveAllToEeprom();
        wifi_reboot();
    }
    */
#endif
}

//---------------------------------------------------------------------------------
//-- Set things up
void setup() {
    delay(1000);
    Logger.begin(2048);
#ifndef ENABLE_DEBUG
    //-- Initialized GPIO02 (Used for "Reset To Factory")
    //   We only use it for non debug because GPIO02 is used as a serial
    //   pin (TX) when debugging.
    pinMode(GPIO02, INPUT_PULLUP);
    reset_state = digitalRead(GPIO02);
#endif
    DEBUG_LOG("\nConfiguring access point...\n");
    DEBUG_LOG("Free Sketch Space: %u\n", ESP.getFreeSketchSpace());
    Parameters.begin();

    if(Parameters.getWifiMode() == WIFI_MODE_STA){
      //-- Connect to an existing network
      WiFi.mode(WIFI_STA);
      WiFi.begin(Parameters.getWifiSsid(), Parameters.getWifiPassword());

      while (WiFi.status() != WL_CONNECTED) {
          #ifdef ENABLE_DEBUG
          Serial.print(".");
          #endif
          delay(500);
      }
      localIP = WiFi.localIP();
    }

    if(Parameters.getWifiMode() == WIFI_MODE_AP){
      //-- Start AP
      WiFi.mode(WIFI_AP);
      WiFi.encryptionType(AUTH_WPA2_PSK);
      WiFi.softAP(Parameters.getWifiSsid(), Parameters.getWifiPassword(), Parameters.getWifiChannel());
      localIP = WiFi.softAPIP();
      //-- I'm getting bogus IP from the DHCP server. Broadcasting for now.
      DEBUG_LOG("Waiting for DHCPD...\n");
      dhcp_status dstat = wifi_station_dhcpc_status();
      while (dstat != DHCP_STARTED) {
          #ifdef ENABLE_DEBUG
          Serial1.print(".");
          #endif
          delay(500);
          dstat = wifi_station_dhcpc_status();
      }
      wait_for_client();
    }

    DEBUG_LOG("Start WiFi Bridge\n");
    //-- Initialize Comm Links
    IPAddress gcs_ip(localIP);
    gcs_ip[3] = 255;
    GCS.begin((MavESP8266Bridge*)&Vehicle, gcs_ip);
    Vehicle.begin((MavESP8266Bridge*)&GCS);
    //-- Initialize Update Server
    updateServer.begin(&updateStatus);
}

//---------------------------------------------------------------------------------
//-- Main Loop
void loop() {
    if(!updateStatus.isUpdating()) {
        GCS.readMessage();
        delay(0);
        Vehicle.readMessage();
        delay(0);
        //check_reset();
        //delay(0);
    }
    updateServer.checkUpdates();
}
<commit_msg>Fallback to AP mode if no connection is established in STA mode<commit_after>/****************************************************************************
 *
 * Copyright (c) 2015, 2016 Gus Grubba. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file main.cpp
 * ESP8266 Wifi AP, MavLink UART/UDP Bridge
 *
 * @author Gus Grubba <mavlink@grubba.com>
 */

#include "mavesp8266.h"
#include "mavesp8266_parameters.h"
#include "mavesp8266_gcs.h"
#include "mavesp8266_vehicle.h"
#include "mavesp8266_httpd.h"

#define GPIO02  2

#ifndef ENABLE_DEBUG
uint8_t                 reset_state;
#endif

//---------------------------------------------------------------------------------
//-- HTTP Update Status
class MavESP8266UpdateImp : public MavESP8266Update {
public:
    MavESP8266UpdateImp ()
        : _isUpdating(false)
    {

    }
    void updateStarted  ()
    {
        _isUpdating = true;
    }
    void updateCompleted()
    {
        //-- TODO
    }
    void updateError    ()
    {
        //-- TODO
    }
    bool isUpdating     () { return _isUpdating; }
private:
    bool _isUpdating;
};

//-- Singletons
IPAddress               localIP;
MavESP8266Parameters    Parameters;
MavESP8266GCS           GCS;
MavESP8266Vehicle       Vehicle;
MavESP8266Httpd         updateServer;
MavESP8266UpdateImp     updateStatus;
MavESP8266Log           Logger;

//---------------------------------------------------------------------------------
//-- Accessors
class MavESP8266WorldImp : public MavESP8266World {
public:
    MavESP8266Parameters*   getParameters   () { return &Parameters;    }
    MavESP8266Vehicle*      getVehicle      () { return &Vehicle;       }
    MavESP8266GCS*          getGCS          () { return &GCS;           }
    MavESP8266Log*          getLogger       () { return &Logger;        }
};

MavESP8266WorldImp      World;

MavESP8266World* getWorld()
{
    return &World;
}

//---------------------------------------------------------------------------------
//-- Wait for a DHCPD client
void wait_for_client() {
    DEBUG_LOG("Waiting for a client...\n");
    int wcount = 0;
    uint8 client_count = wifi_softap_get_station_num();
    while (!client_count) {
        #ifdef ENABLE_DEBUG
        Serial1.print(".");
        if(++wcount > 80) {
            wcount = 0;
            Serial1.println();
        }
        #endif
        delay(1000);
        client_count = wifi_softap_get_station_num();
    }
    DEBUG_LOG("Got %d client(s)\n", client_count);
}

//---------------------------------------------------------------------------------
//-- Check for reset pin
void check_reset() {
#ifndef ENABLE_DEBUG
    //-- Test for "Reset To Factory"
    /* Needs testing
    int reset = digitalRead(GPIO02);
    if(reset != reset_state) {
        resetToDefaults();
        saveAllToEeprom();
        wifi_reboot();
    }
    */
#endif
}

//---------------------------------------------------------------------------------
//-- Set things up
void setup() {
    delay(1000);
    Logger.begin(2048);
#ifndef ENABLE_DEBUG
    //-- Initialized GPIO02 (Used for "Reset To Factory")
    //   We only use it for non debug because GPIO02 is used as a serial
    //   pin (TX) when debugging.
    pinMode(GPIO02, INPUT_PULLUP);
    reset_state = digitalRead(GPIO02);
#endif
    DEBUG_LOG("\nConfiguring access point...\n");
    DEBUG_LOG("Free Sketch Space: %u\n", ESP.getFreeSketchSpace());
    Parameters.begin();

    if(Parameters.getWifiMode() == WIFI_MODE_STA){
      //-- Connect to an existing network
      WiFi.mode(WIFI_STA);
      WiFi.begin(Parameters.getWifiSsid(), Parameters.getWifiPassword());

      //-- Wait a minute to connect
      for(int i = 0; i < 120 && WiFi.status() != WL_CONNECTED; i++){
          #ifdef ENABLE_DEBUG
          Serial.print(".");
          #endif
          delay(500);
      }

      if(WiFi.status() == WL_CONNECTED){
          localIP = WiFi.localIP();
      } else {
          //-- Fall back to AP mode if no connection could be established
          WiFi.disconnect(true);
          Parameters.setWifiMode(WIFI_MODE_AP);
      }
    }

    if(Parameters.getWifiMode() == WIFI_MODE_AP){
      //-- Start AP
      WiFi.mode(WIFI_AP);
      WiFi.encryptionType(AUTH_WPA2_PSK);
      WiFi.softAP(Parameters.getWifiSsid(), Parameters.getWifiPassword(), Parameters.getWifiChannel());
      localIP = WiFi.softAPIP();
      //-- I'm getting bogus IP from the DHCP server. Broadcasting for now.
      DEBUG_LOG("Waiting for DHCPD...\n");
      dhcp_status dstat = wifi_station_dhcpc_status();
      while (dstat != DHCP_STARTED) {
          #ifdef ENABLE_DEBUG
          Serial1.print(".");
          #endif
          delay(500);
          dstat = wifi_station_dhcpc_status();
      }
      wait_for_client();
    }

    DEBUG_LOG("Start WiFi Bridge\n");
    //-- Initialize Comm Links
    IPAddress gcs_ip(localIP);
    gcs_ip[3] = 255;
    GCS.begin((MavESP8266Bridge*)&Vehicle, gcs_ip);
    Vehicle.begin((MavESP8266Bridge*)&GCS);
    //-- Initialize Update Server
    updateServer.begin(&updateStatus);
}

//---------------------------------------------------------------------------------
//-- Main Loop
void loop() {
    if(!updateStatus.isUpdating()) {
        GCS.readMessage();
        delay(0);
        Vehicle.readMessage();
        delay(0);
        //check_reset();
        //delay(0);
    }
    updateServer.checkUpdates();
}
<|endoftext|>


<commit_before>#include <iostream>
#include <vector>
#include <string>
#include <cstring>
#include <string.h>
#include <stdio.h>
#include <sstream>
#include <unistd.h>
#include <stdlib.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/wait.h>
#include <stack>
#include <algorithm>
using namespace std;

//TODO:
//  Write testcases for this program (scripts)
//  Merge the branches and add the "hw3" tag
//  Create submission file

class Shell{
    public:
        Shell* first;
        Shell* second;
        Shell(){};
        virtual bool execute() = 0;
};

class Connector : public Shell{
    public:
        // Shell* first;
        // Shell* second;
        Connector() : Shell(){};
        Connector(Shell* f, Shell* s){
            first = f;
            second = s;
        };
        virtual bool execute() = 0; // for compiler
};

class Bars : public Connector{
    public:
        Bars() : Connector (){};
        Bars(Shell* f, Shell* s) : Connector(f, s){};
        bool execute(){
            if(!first->execute()){
                return second->execute();
            }
            return true;
        }
};

class Semi : public Connector{
    public:
        Semi() : Connector (){};
        Semi(Shell* f, Shell* s) : Connector(f, s){};
        bool execute(){
            first->execute();
            return second->execute();
        }
};

class Amp : public Connector{
    public:
        Amp() : Connector (){};
        Amp(Shell* f, Shell* s) : Connector(f, s){};
        bool execute(){
            if(first->execute()){
                return second->execute();
            }
            return false; // else
        }
};

class Par : public Connector{
    public:
        Par() : Connector (){};
        bool execute(){
            return first->execute();
        }
};

class Command : public Shell{
    public:
        string cmd; // is it becuase you're trying to change a const?
        vector<string> args;
        Command() : Shell(){};
        Command(string c, vector<string> a) : Shell(), cmd(c), args(a) {};
        bool execute(){
            if(cmd == "exit"){
                exit(0);
            }
            string flag;
            string directory;
            if(cmd.at(0) == '[' || cmd == "test"){
                //do the test command stuff
                if(args.size() == 1){
                    flag = "-e";
                    directory = args.at(0);
                }
                else{
                    flag = args.at(0);
                    directory = args.at(1);
                }
                struct stat sb;
                //const char* dir = new char[directory.size() + 1];
                char temp[1024];
                strcpy(temp, directory.c_str());
                const char* dir = temp;
                if(stat(dir, &sb) == -1){
                    cout << "(False)" << endl;
                    return false;
                }
                if(flag == "-f"){
                    if(S_ISREG(sb.st_mode)){
                        cout << "(True)" << endl;
                        return true;
                    }
                    cout << "(False)" << endl;
                    return false;
                }
                else if(flag == "-d"){
                    if(S_ISDIR(sb.st_mode)){
                        cout << "(True)" << endl;
                        return true;
                    }
                    cout << "(False)" << endl;
                    return false;
                }
                return true;
            }
            //size of argv
            int size = args.size() + 2;
            //
            char **argv = new char*[args.size() + 2];
            char *tempcmd = new char[cmd.size()];
            strcpy(tempcmd, cmd.c_str());
            argv[0] = tempcmd;
            for(unsigned i = 0; i < args.size(); ++i){
                char *temparg = new char[args.at(i).size()];
                strcpy(temparg, args.at(i).c_str());
                argv[i + 1] = temparg;
            }
            argv[size - 1] = NULL;

            pid_t pid;
            //int status;
            //forks the process
            if((pid = fork()) < 0){ //if fork() failed
                perror("Fork failed");
                return false;
            }
            else if(pid == 0){ //if this is the child process
                //do stuff
                if(execvp(argv[0], argv) < 0){   //executes the command: if it fails, returns -1, else continue
                    perror("( ͡° ͜ʖ ͡°)  INVALID COMMAND");
                    exit(EXIT_FAILURE);
                }

                //exit(0);
            }
            else{ //if it is the parent process
                int status;
                waitpid(pid, &status, WUNTRACED | WCONTINUED);
                if(WIFEXITED(status)){
                    if(WEXITSTATUS(status) == 0){
                        return true;
                    }
                    else{
                        return false;
                    }
                }
                // while(wait(&status) != pid)
                // ;
            }
            return false;
    }
};

Shell* stringToCommand(string commandLine){
    stringstream ss;
    ss << commandLine;
    // //sets up everything to use stringstream
    //builds new command
    Command* temp = new Command;
    string tempString;
    ss >> tempString;
    temp->cmd = tempString;
    //takes in any potential arguments
    while(ss >> tempString){
        temp->args.push_back(tempString);
    }
    return temp;
}

Shell* parse(string commandLine){
    //deletes comments
    if(commandLine.find("#") != string::npos){
        int location = commandLine.find("#");
        commandLine.erase(location, 999);
    }
    //top of the tree to be executed
    Shell* top = NULL;

    for(unsigned i = 0; i < commandLine.size(); ++i){
        string temp;
        //stack
        //bool closed = true;
        // checks for parenthesis & adjusts stack

        if(commandLine.at(i) == ';' /* && closed == true */){
            //make substr
            temp = commandLine.substr(0, i); // didn't include actual character
            //delete what we took
            commandLine.erase(0, i + 1);
            //reset i
            i = 0;
            //issue where it never enteres the first if statement
            if(top == NULL){
                Shell* connect = new Semi;
                top = connect;
                //Shell*
                connect->first = stringToCommand(temp);
            }
            else{
                Shell* connect = new Semi;
                connect->first = top;
                top->second = stringToCommand(temp);
                top = connect;
            }
        }
        else if(commandLine.at(i) == '|'){
            if(commandLine.at(i + 1) == '|'){
                //make substr
                temp = commandLine.substr(0, i);
                //delete what we took
                commandLine.erase(0, i + 2);    //see below for deleting both connectors
                //reset i
                i = 0;
                if(top == NULL){
                    Shell* connect = new Bars;
                    top = connect;
                    connect->first = stringToCommand(temp);
                }
                else{
                    Shell* connect = new Bars;
                    connect->first = top;
                    top->second = stringToCommand(temp);
                    top = connect;
                }
            }
        }
        else if(commandLine.at(i) == '&'){
            if(commandLine.at(i + 1) == '&'){
                //make substr
                temp = commandLine.substr(0, i);
                //delete what we took
                commandLine.erase(0, i + 2);    //need to change it so that it deletes both connectors
                //reset i
                i = 0;
                if(top == NULL){
                    Shell* connect = new Amp;
                    top = connect;
                    connect->first = stringToCommand(temp);
                }
                else{
                    Shell* connect = new Amp;
                    connect->first = top;
                    top->second = stringToCommand(temp);
                    top = connect;
                }
            }
        }
        else if(commandLine.at(i) == '('){
            if(commandLine.find('(') == string::npos){
                cout << "INVALID COMMAND: No matching )" << endl;
                exit(0);
            }
            stack<int> parenthesis;
            int endLocation = 0;
            //goes through commandLine character by character
            parenthesis.push(i);
            for(unsigned j = i + 1; !parenthesis.empty(); ++j){
                if(commandLine.at(j) == '('){
                    parenthesis.push(j);
                }
                else if(commandLine.at(j) == ')'){
                    parenthesis.pop();
                }
                endLocation = j;
            }
            temp = commandLine.substr(i + 1, endLocation - i - 1);
            commandLine.erase(i, endLocation - i + 1);
            //everything from above only deals with parenthesis
            //below is the same logic from every other connector
            if(top == NULL){
                Shell* connect = new Par;
                top = connect;
                connect->first = parse(temp);
            }
            else{
                Shell* connect = new Par;
                connect->first = parse(temp);
                top->second = connect;
            }
        }
    }
    if(commandLine.size() == 1){
        return top;
    }
    string temp = commandLine;
    if(top == NULL){
        top = stringToCommand(temp);
    }
    else{
        top->second = stringToCommand(temp);
    }
    //top->execute();
    return top;
}

//extra credit prompt
void getUser(string &prompt){
    char* user = getlogin();
    if(user == NULL){
        cout << "Can't get login" << endl;
        return;
    }
    char host[1024];
    gethostname(host, 1023);
    prompt = prompt + user + '@' + host;
}

int main() {
    string commandLine;
    string prompt;
    getUser(prompt);
    prompt = prompt + '$';
    while(getline(cin, commandLine) && commandLine != "exit"){
        cout << prompt;
        Shell* tree = parse(commandLine);
        tree->execute();
    }
    return 0;
}
<commit_msg>fixed a bug<commit_after>#include <iostream>
#include <vector>
#include <string>
#include <cstring>
#include <string.h>
#include <stdio.h>
#include <sstream>
#include <unistd.h>
#include <stdlib.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/wait.h>
#include <stack>
#include <algorithm>
using namespace std;

//TODO:
//  Write testcases for this program (scripts)
//  Merge the branches and add the "hw3" tag
//  Create submission file

class Shell{
    public:
        Shell* first;
        Shell* second;
        Shell(){};
        virtual bool execute() = 0;
};

class Connector : public Shell{
    public:
        // Shell* first;
        // Shell* second;
        Connector() : Shell(){};
        Connector(Shell* f, Shell* s){
            first = f;
            second = s;
        };
        virtual bool execute() = 0; // for compiler
};

class Bars : public Connector{
    public:
        Bars() : Connector (){};
        Bars(Shell* f, Shell* s) : Connector(f, s){};
        bool execute(){
            if(!first->execute()){
                return second->execute();
            }
            return true;
        }
};

class Semi : public Connector{
    public:
        Semi() : Connector (){};
        Semi(Shell* f, Shell* s) : Connector(f, s){};
        bool execute(){
            first->execute();
            return second->execute();
        }
};

class Amp : public Connector{
    public:
        Amp() : Connector (){};
        Amp(Shell* f, Shell* s) : Connector(f, s){};
        bool execute(){
            if(first->execute()){
                return second->execute();
            }
            return false; // else
        }
};

class Par : public Connector{
    public:
        Par() : Connector (){};
        bool execute(){
            return first->execute();
        }
};

class Command : public Shell{
    public:
        string cmd; // is it becuase you're trying to change a const?
        vector<string> args;
        Command() : Shell(){};
        Command(string c, vector<string> a) : Shell(), cmd(c), args(a) {};
        bool execute(){
            if(cmd == "exit"){
                exit(0);
            }
            string flag;
            string directory;
            if(cmd.at(0) == '[' || cmd == "test"){
                //do the test command stuff
                if(args.size() == 1){
                    flag = "-e";
                    directory = args.at(0);
                }
                else{
                    flag = args.at(0);
                    directory = args.at(1);
                }
                struct stat sb;
                //const char* dir = new char[directory.size() + 1];
                char temp[1024];
                strcpy(temp, directory.c_str());
                const char* dir = temp;
                if(stat(dir, &sb) == -1){
                    cout << "(False)" << endl;
                    return false;
                }
                if(flag == "-f"){
                    if(S_ISREG(sb.st_mode)){
                        cout << "(True)" << endl;
                        return true;
                    }
                    cout << "(False)" << endl;
                    return false;
                }
                else if(flag == "-d"){
                    if(S_ISDIR(sb.st_mode)){
                        cout << "(True)" << endl;
                        return true;
                    }
                    cout << "(False)" << endl;
                    return false;
                }
                cout << "(True)" << endl;
                return true;
            }
            //size of argv
            int size = args.size() + 2;
            //
            char **argv = new char*[args.size() + 2];
            char *tempcmd = new char[cmd.size()];
            strcpy(tempcmd, cmd.c_str());
            argv[0] = tempcmd;
            for(unsigned i = 0; i < args.size(); ++i){
                char *temparg = new char[args.at(i).size()];
                strcpy(temparg, args.at(i).c_str());
                argv[i + 1] = temparg;
            }
            argv[size - 1] = NULL;

            pid_t pid;
            //int status;
            //forks the process
            if((pid = fork()) < 0){ //if fork() failed
                perror("Fork failed");
                return false;
            }
            else if(pid == 0){ //if this is the child process
                //do stuff
                if(execvp(argv[0], argv) < 0){   //executes the command: if it fails, returns -1, else continue
                    perror("( ͡° ͜ʖ ͡°)  INVALID COMMAND");
                    exit(EXIT_FAILURE);
                }

                //exit(0);
            }
            else{ //if it is the parent process
                int status;
                waitpid(pid, &status, WUNTRACED | WCONTINUED);
                if(WIFEXITED(status)){
                    if(WEXITSTATUS(status) == 0){
                        return true;
                    }
                    else{
                        return false;
                    }
                }
                // while(wait(&status) != pid)
                // ;
            }
            return false;
    }
};

Shell* stringToCommand(string commandLine){
    stringstream ss;
    ss << commandLine;
    // //sets up everything to use stringstream
    //builds new command
    Command* temp = new Command;
    string tempString;
    ss >> tempString;
    temp->cmd = tempString;
    //takes in any potential arguments
    while(ss >> tempString){
        temp->args.push_back(tempString);
    }
    return temp;
}

Shell* parse(string commandLine){
    //deletes comments
    if(commandLine.find("#") != string::npos){
        int location = commandLine.find("#");
        commandLine.erase(location, 999);
    }
    //top of the tree to be executed
    Shell* top = NULL;

    for(unsigned i = 0; i < commandLine.size(); ++i){
        string temp;
        //stack
        //bool closed = true;
        // checks for parenthesis & adjusts stack

        if(commandLine.at(i) == ';' /* && closed == true */){
            //make substr
            temp = commandLine.substr(0, i); // didn't include actual character
            //delete what we took
            commandLine.erase(0, i + 1);
            //reset i
            i = 0;
            //issue where it never enteres the first if statement
            if(top == NULL){
                Shell* connect = new Semi;
                top = connect;
                //Shell*
                connect->first = stringToCommand(temp);
            }
            else{
                Shell* connect = new Semi;
                connect->first = top;
                top->second = stringToCommand(temp);
                top = connect;
            }
        }
        else if(commandLine.at(i) == '|'){
            if(commandLine.at(i + 1) == '|'){
                //make substr
                temp = commandLine.substr(0, i);
                //delete what we took
                commandLine.erase(0, i + 2);    //see below for deleting both connectors
                //reset i
                i = 0;
                if(top == NULL){
                    Shell* connect = new Bars;
                    top = connect;
                    connect->first = stringToCommand(temp);
                }
                else{
                    Shell* connect = new Bars;
                    connect->first = top;
                    top->second = stringToCommand(temp);
                    top = connect;
                }
            }
        }
        else if(commandLine.at(i) == '&'){
            if(commandLine.at(i + 1) == '&'){
                //make substr
                temp = commandLine.substr(0, i);
                //delete what we took
                commandLine.erase(0, i + 2);    //need to change it so that it deletes both connectors
                //reset i
                i = 0;
                if(top == NULL){
                    Shell* connect = new Amp;
                    top = connect;
                    connect->first = stringToCommand(temp);
                }
                else{
                    Shell* connect = new Amp;
                    connect->first = top;
                    top->second = stringToCommand(temp);
                    top = connect;
                }
            }
        }
        else if(commandLine.at(i) == '('){
            if(commandLine.find('(') == string::npos){
                cout << "INVALID COMMAND: No matching )" << endl;
                exit(0);
            }
            stack<int> parenthesis;
            int endLocation = 0;
            //goes through commandLine character by character
            parenthesis.push(i);
            for(unsigned j = i + 1; !parenthesis.empty(); ++j){
                if(commandLine.at(j) == '('){
                    parenthesis.push(j);
                }
                else if(commandLine.at(j) == ')'){
                    parenthesis.pop();
                }
                endLocation = j;
            }
            temp = commandLine.substr(i + 1, endLocation - i - 1);
            commandLine.erase(i, endLocation - i + 1);
            //everything from above only deals with parenthesis
            //below is the same logic from every other connector
            if(top == NULL){
                Shell* connect = new Par;
                top = connect;
                connect->first = parse(temp);
            }
            else{
                Shell* connect = new Par;
                connect->first = parse(temp);
                top->second = connect;
            }
        }
    }
    if(commandLine.size() == 1){
        return top;
    }
    string temp = commandLine;
    if(top == NULL){
        top = stringToCommand(temp);
    }
    else{
        top->second = stringToCommand(temp);
    }
    //top->execute();
    return top;
}

//extra credit prompt
void getUser(string &prompt){
    char* user = getlogin();
    if(user == NULL){
        cout << "Can't get login" << endl;
        return;
    }
    char host[1024];
    gethostname(host, 1023);
    prompt = prompt + user + '@' + host;
}

int main() {
    string commandLine;
    string prompt;
    getUser(prompt);
    prompt = prompt + '$';
    while(getline(cin, commandLine) && commandLine != "exit"){
        cout << prompt;
        Shell* tree = parse(commandLine);
        tree->execute();
    }
    return 0;
}
<|endoftext|>