text:
// -*- coding: utf-8 -*-
// Copyright (C) 2006-2012 Rosen Diankov <rosen.diankov@gmail.com>
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.
#include "plugindefs.h"
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

#ifdef Boost_IOSTREAMS_FOUND
#include <boost/iostreams/device/file_descriptor.hpp>
#include <boost/iostreams/stream.hpp>
#include <boost/version.hpp>

// used for inverse jacobian computation
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

#if BOOST_VERSION >= 104400
#define FILE_DESCRIPTOR_FLAG boost::iostreams::never_close_handle
#else
#define FILE_DESCRIPTOR_FLAG false
#endif
#endif

#include <errno.h>
#include <stdio.h>
#include <stdlib.h>

#ifdef _WIN32
#define WIN32_LEAN_AND_MEAN
#include <windows.h>
#define PLUGIN_EXT ".dll"
#define MYPOPEN _popen
#define MYPCLOSE _pclose
#else

#ifdef __APPLE_CC__
#define PLUGIN_EXT ".dylib"
#else
#define PLUGIN_EXT ".so"
#endif

#include <dlfcn.h>
#include <sys/types.h>
#include <dirent.h>
#define MYPOPEN popen
#define MYPCLOSE pclose
#endif

#ifdef HAVE_BOOST_FILESYSTEM
#include <boost/filesystem/operations.hpp>
#endif

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>

#include "next_combination.h"

#define LOAD_IKFUNCTION0(fnname) { \
        ikfunctions->_ ## fnname = (typename ikfast::IkFastFunctions<T>::fnname ## Fn)SysLoadSym(plib, # fnname); \
}

#define LOAD_IKFUNCTION(fnname) { \
        LOAD_IKFUNCTION0(fnname); \
        if( !ikfunctions->_ ## fnname ) { \
            RAVELOG_WARN(str(boost::format("failed to find " # fnname " in %s. If the library is correct, have you compiled with IKFAST_CLIBRARY define that enables extern \"C\"?")%_libraryname)); \
            return false; \
        } \
}

class IkFastModule : public ModuleBase
{
    static int determinant_sign(const boost::numeric::ublas::permutation_matrix<std::size_t>& pm)
    {
        int pm_sign=1;
        std::size_t size = pm.size();
        for (std::size_t i = 0; i < size; ++i) {
            if (i != pm(i)) {
                pm_sign *= -1.0; // swap_rows would swap a pair of rows here, so we change sign
            }
        }
        return pm_sign;
    }

    static dReal determinant( boost::numeric::ublas::matrix<dReal>& m ) {
        boost::numeric::ublas::permutation_matrix<std::size_t> pm(m.size1());
        dReal det = 1.0;
        if( boost::numeric::ublas::lu_factorize(m,pm) ) {
            det = 0.0;
        }
        else {
            for(size_t i = 0; i < m.size1(); i++) {
                det *= m(i,i);                                  // multiply by elements on diagonal
            }
            det = det * determinant_sign( pm );
        }
        return det;
    }

    class IkLibrary : public boost::enable_shared_from_this<IkLibrary>
    {
public:
        template <typename T>
        class MyFunctions : public ikfast::IkFastFunctions<T>
        {
public:
            MyFunctions() : ikfast::IkFastFunctions<T>() {
            }
            MyFunctions(const MyFunctions& r) {
                *this = r;
            }
            boost::shared_ptr<IkLibrary> _library;
        };

        IkLibrary() : plib(NULL) {
        }
        ~IkLibrary() {
#ifdef OPENRAVE_IKFAST_FLOAT32
            _ikfloat.reset();
#endif
            _ikdouble.reset();
            if( plib != NULL ) {
                SysCloseLibrary(plib);
            }
        }

        bool Init(const string& ikname, const string& libraryname)
        {
            _viknames.resize(0);
            AddIkName(ikname);
            _libraryname = libraryname;
            plib = SysLoadLibrary(_libraryname.c_str());
            if( plib == NULL ) {
                RAVELOG_WARN(str(boost::format("failed to load library %s")%_libraryname));
                return false;
            }

            // don't matter what the template is
            ikfast::IkFastFunctions<void>::GetIkRealSizeFn GetIkRealSize = (ikfast::IkFastFunctions<void>::GetIkRealSizeFn)SysLoadSym(plib, "GetIkRealSize");
            if( GetIkRealSize == NULL ) {
                RAVELOG_WARN(str(boost::format("failed to find GetIkRealSize in %s")%_libraryname));
                return false;
            }

            int realsize = GetIkRealSize();
#ifdef OPENRAVE_IKFAST_FLOAT32
            if( realsize == 4 ) {
                boost::shared_ptr<MyFunctions<float> > ikfunctions(new MyFunctions<float>());
                _InitFunctions(ikfunctions);
                _ikfloat=ikfunctions;
            }
            else
#endif
            if( realsize == 8 ) {
                boost::shared_ptr<MyFunctions<double> > ikfunctions(new MyFunctions<double>());
                _InitFunctions(ikfunctions);
                _ikdouble=ikfunctions;
            }
            else {
                RAVELOG_WARN(str(boost::format("unsupported size ikfast real size %d")%realsize));
                return false;
            }
            return true;
        }

        template <typename T>
        bool _InitFunctions(boost::shared_ptr<MyFunctions<T> > ikfunctions)
        {
            LOAD_IKFUNCTION0(ComputeIk);
            LOAD_IKFUNCTION0(ComputeIk2);
            LOAD_IKFUNCTION(ComputeFk);
            LOAD_IKFUNCTION(GetNumFreeParameters);
            LOAD_IKFUNCTION(GetFreeParameters);
            LOAD_IKFUNCTION(GetNumJoints);
            LOAD_IKFUNCTION(GetIkRealSize);
            LOAD_IKFUNCTION(GetIkFastVersion);
            LOAD_IKFUNCTION(GetIkType);
            LOAD_IKFUNCTION(GetKinematicsHash);
            return true;
        }

        IkSolverBasePtr CreateSolver(EnvironmentBasePtr penv, const vector<dReal>& vfreeinc)
        {
            std::stringstream sversion(GetIkFastVersion());
            uint32_t ikfastversion = 0;
            sversion >> std::hex >> ikfastversion;
            if( ikfastversion < 60 ) {
                throw OPENRAVE_EXCEPTION_FORMAT("ikfast version %d not supported", ikfastversion, ORE_InvalidArguments);
            }
            std::stringstream ss;
#ifdef OPENRAVE_IKFAST_FLOAT32
            if( !!_ikfloat ) {
                boost::shared_ptr<MyFunctions<float> > newfunctions(new MyFunctions<float>(*_ikfloat));
                newfunctions->_library = shared_from_this();
                return CreateIkFastSolver(penv,ss,newfunctions,vfreeinc);
            }
#endif
            if( !!_ikdouble ) {
                boost::shared_ptr<MyFunctions<double> > newfunctions(new MyFunctions<double>(*_ikdouble));
                newfunctions->_library = shared_from_this();
                return CreateIkFastSolver(penv,ss,newfunctions,vfreeinc);
            }
            throw OPENRAVE_EXCEPTION_FORMAT0("uninitialized ikfast functions",ORE_InvalidState);
        }

        const vector<string>& GetIkNames() const {
            return _viknames;
        }

        /// \brief adds ikname entry in lower case since all interface types are case insensitive
        void AddIkName(const string& ikname) {
            size_t index = _viknames.size();
            _viknames.resize(index+1);
            _viknames.back().resize(ikname.size());
            std::transform(ikname.begin(), ikname.end(), _viknames.back().begin(), ::tolower);
        }

        const string& GetLibraryName() const {
            return _libraryname;
        }

        int GetIKType() {
#ifdef OPENRAVE_IKFAST_FLOAT32
            if( !!_ikfloat ) {
                return _ikfloat->_GetIkType();
            }
#endif
            if( !!_ikdouble ) {
                return _ikdouble->_GetIkType();
            }
            throw OPENRAVE_EXCEPTION_FORMAT0("uninitialized ikfast functions",ORE_InvalidState);
        }
        std::string GetIkFastVersion() {
#ifdef OPENRAVE_IKFAST_FLOAT32
            if( !!_ikfloat ) {
                return _ikfloat->_GetIkFastVersion();
            }
#endif
            if( !!_ikdouble ) {
                return _ikdouble->_GetIkFastVersion();
            }
            throw OPENRAVE_EXCEPTION_FORMAT0("uninitialized ikfast functions",ORE_InvalidState);
        }

#ifdef OPENRAVE_IKFAST_FLOAT32
        boost::shared_ptr<MyFunctions<float> > _ikfloat;
#endif
        boost::shared_ptr<MyFunctions<double> > _ikdouble;
private:
        void* SysLoadLibrary(const char* lib)
        {
#ifdef _WIN32
            void* plib = LoadLibraryA(lib);
            if( plib == NULL ) {
                RAVELOG_WARN("Failed to load %s\n", lib);
            }
#else
            void* plib = dlopen(lib, RTLD_NOW);
            if( plib == NULL ) {
                RAVELOG_WARN("%s\n", dlerror());
            }
#endif
            return plib;
        }

        void* SysLoadSym(void* lib, const char* sym)
        {
#ifdef _WIN32
            return GetProcAddress((HINSTANCE)lib, sym);
#else
            return dlsym(lib, sym);
#endif
        }

        void SysCloseLibrary(void* lib)
        {
#ifdef _WIN32
            FreeLibrary((HINSTANCE)lib);
#else
            dlclose(lib);
#endif
        }

        void* plib;
        string _libraryname;
        vector<string> _viknames;
    };

    inline boost::shared_ptr<IkFastModule> shared_problem() {
        return boost::dynamic_pointer_cast<IkFastModule>(shared_from_this());
    }
    inline boost::shared_ptr<IkFastModule const> shared_problem_const() const {
        return boost::dynamic_pointer_cast<IkFastModule const>(shared_from_this());
    }

public:
    IkFastModule(EnvironmentBasePtr penv, std::istream& sinput) : ModuleBase(penv)
    {
        __description = ":Interface Author: Rosen Diankov\n\nAllows dynamic loading and registering of ikfast shared objects to openrave plugins.\nAlso contains several test routines for inverse kinematics.";
        RegisterCommand("AddIkLibrary",boost::bind(&IkFastModule::AddIkLibrary,this,_1,_2),
                        "Dynamically adds an ik solver to openrave by loading a shared object (based on ikfast code generation).\n"
                        "Usage::\n\n  AddIkLibrary iksolvername iklibrarypath\n\n"
                        "return the type of inverse kinematics solver (IkParamterization::Type)");
#ifdef Boost_IOSTREAMS_FOUND
        RegisterCommand("LoadIKFastSolver",boost::bind(&IkFastModule::LoadIKFastSolver,this,_1,_2),
                        "Dynamically calls the inversekinematics.py script to generate an ik solver for a robot, or to load an existing one\n"
                        "Usage::\n\n  LoadIKFastSolver robotname iktype_id [free increment]\n\n"
                        "return nothing, but does call the SetIKSolver for the robot");
#endif
        RegisterCommand("PerfTiming",boost::bind(&IkFastModule::PerfTiming,this,_1,_2),
                        "Times the ik call of a given library.\n"
                        "Usage::\n\n  PerfTiming [options] iklibrarypath\n\n"
                        "return the set of time measurements made in nano-seconds");
        RegisterCommand("IKTest",boost::bind(&IkFastModule::IKtest,this,_1,_2),
                        "Tests for an IK solution if active manipulation has an IK solver attached");
        RegisterCommand("DebugIK",boost::bind(&IkFastModule::DebugIK,this,_1,_2),
                        "Function used for debugging and testing an IK solver. Input parameters are:\n\n\
* string readfile - file containing joint values to read, starts with number of entries.\n\n\
* int numtests - if file not specified, number of random tests to perform (defualt is 1000).\n\n\
* float sampledegeneratecases - probability in [0,1] specifies the probability of sampling joint values on [-pi/2,0,pi/2] (default is 0.2).\n\n\
* int selfcollision - if true, will check IK only for non-self colliding positions of the robot (default is 0).\n\n\
* string robot - name of the robot to test. the active manipulator of the roobt is used.\n\n");
    }

    virtual ~IkFastModule() {
    }

    int main(const string& cmd)
    {
        return 0;
    }

    virtual void Destroy()
    {
    }

    bool AddIkLibrary(ostream& sout, istream& sinput)
    {
        if( sinput.eof() ) {
            return false;
        }
        string ikname, libraryname;
        sinput >> ikname;
        std::transform(ikname.begin(), ikname.end(), ikname.begin(), ::tolower);
        if (!getline(sinput, libraryname) ) {
            return false;
        }
        boost::trim(libraryname);
        if( !sinput ||( libraryname.size() == 0) ||( ikname.size() == 0) ) {
            RAVELOG_DEBUG("bad input\n");
            return false;
        }

        boost::shared_ptr<IkLibrary> lib = _AddIkLibrary(ikname,libraryname);
        if( !lib ) {
            return false;
        }
        sout << lib->GetIKType();
        return true;
    }

    boost::shared_ptr<IkLibrary> _AddIkLibrary(const string& ikname, const string& _libraryname)
    {
#ifdef HAVE_BOOST_FILESYSTEM
        string libraryname = boost::filesystem::system_complete(boost::filesystem::path(_libraryname, boost::filesystem::native)).string();
#else
        string libraryname=_libraryname;
#endif

        // before adding a new library, check for existing
        boost::mutex::scoped_lock lock(GetLibraryMutex());
        boost::shared_ptr<IkLibrary> lib;
        FOREACH(it, *GetLibraries()) {
            if( libraryname == (*it)->GetLibraryName() ) {
                lib = *it;
                lib->AddIkName(ikname);
                break;
            }
        }
        if( !lib ) {
            lib.reset(new IkLibrary());
            if( !lib->Init(ikname, libraryname) ) {
                return boost::shared_ptr<IkLibrary>();
            }
            GetLibraries()->push_back(lib);
        }
        return lib;
    }

#ifdef Boost_IOSTREAMS_FOUND
    bool LoadIKFastSolver(ostream& sout, istream& sinput)
    {
        EnvironmentMutex::scoped_lock envlock(GetEnv()->GetMutex());
        string robotname;
        string striktype;
        bool bForceIK = false;
        sinput >> robotname;
        sinput >> striktype;
        if( !sinput ) {
            return false;
        }
        sinput >> bForceIK;     // optional
        RobotBasePtr probot = GetEnv()->GetRobot(robotname);
        if( !probot || !probot->GetActiveManipulator() ) {
            return false;
        }
        RobotBase::ManipulatorPtr pmanip = probot->GetActiveManipulator();
        IkParameterizationType iktype = IKP_None;
        try {
            iktype = static_cast<IkParameterizationType>(boost::lexical_cast<int>(striktype));
            striktype = RaveGetIkParameterizationMap().find(iktype)->second;
        }
        catch(const boost::bad_lexical_cast&) {
            // striktype is already correct, so check that it exists in RaveGetIkParameterizationMap
            std::transform(striktype.begin(), striktype.end(), striktype.begin(), ::tolower);
            FOREACHC(it,RaveGetIkParameterizationMap()) {
                string mapiktype = it->second;
                std::transform(mapiktype.begin(), mapiktype.end(), mapiktype.begin(), ::tolower);
                if( mapiktype == striktype ) {
                    iktype = it->first;
                    striktype = it->second; // get the correct capitalizations
                    break;
                }
            }
            if(iktype == IKP_None) {
                throw openrave_exception(str(boost::format("could not find iktype %s")%striktype));
            }
        }

        // get ikfast version
        if( _ikfastversion.size() == 0 || _platform.size() == 0 ) {
            string output;
            FILE* pipe = MYPOPEN(OPENRAVE_PYTHON_EXECUTABLE " -c \"import openravepy.ikfast; import platform; print(openravepy.ikfast.__version__+' '+platform.machine())\"", "r");
            {
                boost::iostreams::stream_buffer<boost::iostreams::file_descriptor_source> fpstream(fileno(pipe),FILE_DESCRIPTOR_FLAG);
                std::istream in(&fpstream);
                std::getline(in, output);
            }
            int generateexit = MYPCLOSE(pipe);
            if( generateexit != 0 ) {
                usleep(100000);
                RAVELOG_VERBOSE("failed to close pipe\n"); // not sure how critical this error is
            }
            boost::trim(output);
            size_t index = output.find_first_of(' ');
            if( index == std::string::npos ) {
                RAVELOG_WARN(str(boost::format("failed to parse string: %s")%output));
                return false;
            }
            _ikfastversion = output.substr(0,index);
            _platform = output.substr(index+1);
        }

        string ikfilename;
        for(int iter = 0; iter < 2; ++iter) {
            string ikfilenamefound;
            // check if exists and is loadable, if not, regenerate the IK.
            std::string ikfilenameprefix = str(boost::format("kinematics.%s/ikfast%s.%s.%s.")%pmanip->GetInverseKinematicsStructureHash(iktype)%_ikfastversion%striktype%_platform);
            int ikdof = IkParameterization::GetDOF(iktype);
            if( ikdof > pmanip->GetArmDOF() ) {
                RAVELOG_WARN(str(boost::format("not enough joints (%d) for ik %s")%pmanip->GetArmIndices().size()%striktype));
            }
            if( ikdof < pmanip->GetArmDOF() ) {
                std::vector<int> vindices = pmanip->GetArmIndices();
                std::vector<int> vsolveindices(ikdof), vfreeindices(vindices.size()-ikdof);
                do {
                    std::copy(vindices.begin(),vindices.begin()+ikdof,vsolveindices.begin());
                    sort(vsolveindices.begin(),vsolveindices.end());
                    std::copy(vindices.begin()+ikdof,vindices.end(),vfreeindices.begin());
                    sort(vfreeindices.begin(),vfreeindices.end());
                    ikfilename=ikfilenameprefix;
                    for(size_t i = 0; i < vsolveindices.size(); ++i) {
                        ikfilename += boost::lexical_cast<std::string>(vsolveindices[i]);
                        ikfilename += '_';
                    }
                    ikfilename += "f";
                    for(size_t i = 0; i < vfreeindices.size(); ++i) {
                        if( i > 0 ) {
                            ikfilename += '_';
                        }
                        ikfilename += boost::lexical_cast<std::string>(vfreeindices[i]);
                    }
                    ikfilename += PLUGIN_EXT;
                    ikfilenamefound = RaveFindDatabaseFile(ikfilename);
                    if (ikfilenamefound.size() > 0 ) {
                        break;
                    }
                } while (next_combination(&vindices[0], &vindices[ikdof], &vindices[vindices.size()]));
            }
            else {
                ikfilename=ikfilenameprefix;
                for(size_t i = 0; i < pmanip->GetArmIndices().size(); ++i) {
                    if( i > 0 ) {
                        ikfilename += '_';
                    }
                    ikfilename += boost::lexical_cast<std::string>(pmanip->GetArmIndices().at(i));
                }
                ikfilename += PLUGIN_EXT;
                ikfilenamefound = RaveFindDatabaseFile(ikfilename);
            }

            if( ikfilenamefound.size() == 0 ) {
                if( iter > 0 ) {
                    RAVELOG_WARN(str(boost::format("failed to find ikfile: %s")%ikfilename));
                    return false;
                }

                // create a temporary file and store COLLADA kinematics representation
                AttributesList atts;
                atts.push_back(make_pair(string("skipwrite"), string("visual readable sensors physics")));
                atts.push_back(make_pair(string("target"), probot->GetName()));
                string tempfilename = RaveGetHomeDirectory() + str(boost::format("/testikfastrobot%d.dae")%(RaveRandomInt()%1000));
                // file not found, so create
                RAVELOG_INFO(str(boost::format("Generating inverse kinematics %s for manip %s:%s, hash=%s, saving intermediate data to %s, will take several minutes...\n")%striktype%probot->GetName()%pmanip->GetName()%pmanip->GetInverseKinematicsStructureHash(iktype)%tempfilename));
                GetEnv()->Save(tempfilename,EnvironmentBase::SO_Body,atts);
                string cmdgen = str(boost::format("openrave.py --database inversekinematics --usecached --robot=\"%s\" --manipname=%s --iktype=%s")%tempfilename%pmanip->GetName()%striktype);
                // use raw system call, popen causes weird crash in the inversekinematics compiler
                int generateexit = system(cmdgen.c_str());
                //FILE* pipe = MYPOPEN(cmdgen.c_str(), "r");
                //int generateexit = MYPCLOSE(pipe);
                if( generateexit != 0 ) {
                    usleep(100000);
                    RAVELOG_DEBUG("failed to close pipe\n");
                }
                continue;
            }

            // check for file
            if( !ifstream(ikfilenamefound.c_str()) ) {
                RAVELOG_INFO(str(boost::format("could not find: %s")%ikfilenamefound));
                return false;
            }

            string ikfastname = str(boost::format("ikfast.%s.%s.%s")%pmanip->GetInverseKinematicsStructureHash(iktype)%striktype%pmanip->GetName());
            boost::shared_ptr<IkLibrary> lib = _AddIkLibrary(ikfastname,ikfilenamefound);
            bool bsuccess = true;
            if( !lib ) {
                bsuccess = false;
            }
            else {
                if( lib->GetIKType() != (int)iktype ) {
                    bsuccess = false;
                }
                else {
                    IkSolverBasePtr iksolver = RaveCreateIkSolver(GetEnv(),string("ikfast ")+ikfastname);
                    if( !iksolver ) {
                        RAVELOG_WARN(str(boost::format("failed to create ik solver %s!")%ikfastname));
                        bsuccess = false;
                    }
                    else {
                        bsuccess = pmanip->SetIkSolver(iksolver);
                    }
                }
            }
            // if not forcing the ik, then return true as long as a valid ik solver is set
            if( bForceIK && !bsuccess ) {
                return false;
            }
            return !!pmanip->GetIkSolver() && pmanip->GetIkSolver()->Supports(iktype);
        }

        return false;
    }

#endif

    bool PerfTiming(ostream& sout, istream& sinput)
    {
        EnvironmentMutex::scoped_lock lock(GetEnv()->GetMutex());
        string cmd, libraryname;
        int num=1000;
        dReal maxtime = 1200;
        while(!sinput.eof()) {
            istream::streampos pos = sinput.tellg();
            sinput >> cmd;
            if( !sinput ) {
                break;
            }
            std::transform(cmd.begin(), cmd.end(), cmd.begin(), ::tolower);

            if( cmd == "num" ) {
                sinput >> num;
            }
            else if( cmd == "maxtime" ) {
                sinput >> maxtime;
            }
            else {
                sinput.clear();     // have to clear eof bit
                sinput.seekg(pos);
                getline(sinput, libraryname);
                break;
            }
        }

        boost::trim(libraryname);
        if( libraryname.size() == 0 ) {
            return false;
        }
        boost::shared_ptr<IkLibrary> lib(new IkLibrary());
        if( !lib->Init("", libraryname) ) {
            RAVELOG_WARN(str(boost::format("failed to init library %s\n")%libraryname));
            return false;
        }

#ifdef OPENRAVE_IKFAST_FLOAT32
        if( !!lib->_ikfloat ) {
            return _PerfTiming<float>(sout,lib->_ikfloat,num, maxtime);
        }
        else
#endif
        if( !!lib->_ikdouble ) {
            return _PerfTiming<double>(sout,lib->_ikdouble,num, maxtime);
        }
        else {
            throw openrave_exception("bad real size");
        }
        return true;
    }

    template<typename T> bool _PerfTiming(ostream& sout, boost::shared_ptr<ikfast::IkFastFunctions<T> > ikfunctions, int num, dReal maxtime)
    {
        OPENRAVE_ASSERT_OP(ikfunctions->_GetIkRealSize(),==,sizeof(T));
        BOOST_ASSERT((!!ikfunctions->_ComputeIk || !!ikfunctions->_ComputeIk2) && !!ikfunctions->_ComputeFk);

        vector<uint64_t> vtimes(num);
        ikfast::IkSolutionList<T> solutions;
        vector<T> vjoints(ikfunctions->_GetNumJoints()), vfree(ikfunctions->_GetNumFreeParameters());
        T eerot[9],eetrans[3];
        uint32_t runstarttimems = utils::GetMilliTime();
        uint32_t runmaxtimems = (uint32_t)(1000*maxtime);
        size_t i = 0;
        for(i = 0; i < vtimes.size(); ++i) {
            // don't want to slow down the tests too much with polling
            if(( (i%100) == 0) &&( (utils::GetMilliTime() - runstarttimems) > runmaxtimems) ) {
                break;
            }
            for(size_t j = 0; j < vjoints.size(); ++j) {
                vjoints[j] = RaveRandomDouble()*2*PI;
            }
            for(size_t j = 0; j < vfree.size(); ++j) {
                vfree[j] = vjoints[ikfunctions->_GetFreeParameters()[j]];
            }
            ikfunctions->_ComputeFk(&vjoints[0],eetrans,eerot);
            solutions.Clear();
            uint64_t numtoaverage=10;
            uint64_t starttime = utils::GetNanoPerformanceTime();
            if( !!ikfunctions->_ComputeIk2 ) {
                for(uint64_t j = 0; j < numtoaverage; ++j) {
                    ikfunctions->_ComputeIk2(eetrans,eerot,vfree.size() > 0 ? &vfree[0] : NULL, solutions, NULL);
                }
            }
            else if( !!ikfunctions->_ComputeIk ) {
                for(uint64_t j = 0; j < numtoaverage; ++j) {
                    ikfunctions->_ComputeIk(eetrans,eerot,vfree.size() > 0 ? &vfree[0] : NULL,solutions);
                }
            }
            vtimes[i] = (utils::GetNanoPerformanceTime()-starttime)/numtoaverage;
        }
        while(i-- > 0) {
            sout << vtimes[i] << " ";
        }
        return true;
    }

    bool IKtest(ostream& sout, istream& sinput)
    {
        EnvironmentMutex::scoped_lock lock(GetEnv()->GetMutex());
        RAVELOG_DEBUG("Starting IKtest...\n");
        vector<dReal> varmjointvals, values;
        bool bInitialized=false;
        int filteroptions = IKFO_CheckEnvCollisions;
        RobotBasePtr robot;
        RobotBase::ManipulatorConstPtr pmanip;
        IkParameterization ikparam;
        string cmd;
        while(!sinput.eof()) {
            sinput >> cmd;
            if( !sinput ) {
                break;
            }
            std::transform(cmd.begin(), cmd.end(), cmd.begin(), ::tolower);
            if( cmd == "matrix" ) {
                TransformMatrix handTm;
                sinput >> handTm;
                ikparam.SetTransform6D(handTm);
                bInitialized = true;
            }
            else if( cmd == "ikparam" ) {
                sinput >> ikparam;
            }
            else if( cmd == "armjoints" ) {
                varmjointvals.resize(pmanip->GetArmIndices().size());
                FOREACH(it, varmjointvals) {
                    sinput >> *it;
                }
            }
            else if( cmd == "nocol" ) {
                filteroptions = 0;
            }
            else if( cmd == "robot" ) {
                string name;
                sinput >> name;
                robot = GetEnv()->GetRobot(name);
                pmanip = robot->GetActiveManipulator();
            }
            else {
                RAVELOG_WARN(str(boost::format("unrecognized command: %s\n")%cmd));
                break;
            }

            if( !sinput ) {
                RAVELOG_ERROR(str(boost::format("failed processing command %s\n")%cmd));
                return false;
            }
        }

        if( !robot ) {
            return false;
        }
        RobotBase::RobotStateSaver saver(robot);

        if( !bInitialized ) {
            ikparam = pmanip->GetIkParameterization(IKP_Transform6D);
        }
        robot->GetDOFValues(values);

        for(size_t i = 0; i < varmjointvals.size(); i++) {
            values[pmanip->GetArmIndices()[i]] = varmjointvals[i];
        }
        robot->SetDOFValues(values);

        vector<dReal> q1;
        if( !pmanip->FindIKSolution(ikparam, q1, filteroptions) ) {
            RAVELOG_WARN("No IK solution found\n");
            return false;
        }

        stringstream s2;
        s2 << std::setprecision(std::numeric_limits<dReal>::digits10+1);     /// have to do this or otherwise precision gets lost
        s2 << "ik sol: ";
        FOREACH(it, q1) {
            s2 << *it << " ";
            sout << *it << " ";
        }
        s2 << endl;
        RAVELOG_DEBUG(s2.str());
        return true;
    }

    bool DebugIKFindSolution(RobotBase::ManipulatorPtr pmanip, const IkParameterization& twrist, std::vector<dReal>& viksolution, int filteroptions, std::vector<dReal>& parameters, int paramindex, dReal deltafree)
    {
        // ignore boundary cases since next to limits and can fail due to limit errosr
        for(dReal f = 0; f <= 1; f += deltafree) {
            parameters[paramindex] = f;
            if( paramindex > 0 ) {
                if( DebugIKFindSolution(pmanip, twrist, viksolution, filteroptions, parameters, paramindex-1,deltafree) ) {
                    return true;
                }
            }
            else {
                if( pmanip->FindIKSolution(twrist, parameters, viksolution, filteroptions) ) {
                    return true;
                }
            }
        }

        return false;
    }

    void DebugIKFindSolutions(RobotBase::ManipulatorPtr pmanip, const IkParameterization& twrist, vector< vector<dReal> >& viksolutions, int filteroptions, std::vector<dReal>& parameters, int paramindex, dReal deltafree)
    {
        // ignore boundary cases since next to limits and can fail due to limit errosr
        for(dReal f = 0; f <= 1; f += deltafree) {
            parameters.at(paramindex) = f;
            if( paramindex > 0 ) {
                DebugIKFindSolutions(pmanip, twrist, viksolutions, filteroptions, parameters, paramindex-1,deltafree);
            }
            else {
                vector< vector<dReal> > vtempsol;
                if( pmanip->FindIKSolutions(twrist, parameters, vtempsol, filteroptions) ) {
                    viksolutions.insert(viksolutions.end(), vtempsol.begin(), vtempsol.end());
                }
            }
        }
    }

    bool DebugIK(ostream& sout, istream& sinput)
    {
        using namespace boost::numeric;
        EnvironmentMutex::scoped_lock lock(GetEnv()->GetMutex());

        int num_itrs = 1000;
        stringstream s;
        s << std::setprecision(std::numeric_limits<dReal>::digits10+1);     /// have to do this or otherwise precision gets lost
        fstream fsfile;
        string readfilename;
        bool bReadFile = false, bTestSelfCollision = false;
        dReal sampledegeneratecases = 0.2f, fthreshold = 0.000001f;

        // if nonzero, checks the jacobian of the end effector and only accepts the solution if all its eigenvalues are > than jacobianthreshold
        dReal jacobianthreshold=0;

        int filteroptions = IKFO_IgnoreJointLimits|IKFO_IgnoreSelfCollisions|IKFO_IgnoreCustomFilters;
        RobotBasePtr robot;
        string cmd;
        while(!sinput.eof()) {
            sinput >> cmd;
            if( !sinput ) {
                break;
            }
            std::transform(cmd.begin(), cmd.end(), cmd.begin(), ::tolower);

            if( cmd == "readfile" ) {
                sinput >> readfilename;
                bReadFile = true;
            }
            else if( cmd == "numtests" ) {
                sinput >> num_itrs;
            }
            else if( cmd == "sampledegeneratecases" ) {
                sinput >> sampledegeneratecases;
            }
            else if( cmd == "selfcollision" ) {
                sinput >> bTestSelfCollision;
            }
            else if( cmd == "robot" ) {
                string name;
                sinput >> name;
                robot = GetEnv()->GetRobot(name);
            }
            else if( cmd == "threshold" ) {
                sinput >> fthreshold;
                fthreshold *= fthreshold;
            }
            else if( cmd == "jacobianthreshold" ) {
                sinput >> jacobianthreshold;
                RAVELOG_INFO("using jacobianthreshold %e\n", jacobianthreshold);
            }
            else {
                RAVELOG_WARN(str(boost::format("unrecognized command: %s\n")%cmd));
                break;
            }

            if( !sinput ) {
                RAVELOG_ERROR(str(boost::format("failed processing command %s\n")%cmd));
                return false;
            }
        }

        RAVELOG_DEBUG("Starting DebugIK... max iterations=%d\n", num_itrs);
        RobotBase::RobotStateSaver saver(robot);
        robot->Enable(bTestSelfCollision);
        RobotBase::ManipulatorPtr pmanip = robot->GetActiveManipulator();
        if( !pmanip->GetIkSolver() ) {
            RAVELOG_ERROR(str(boost::format("no ik solver set on manipulator %s")%pmanip->GetName()));
            return false;
        }

        // set the ik threshold to something big so wrong solutions can be returned
        stringstream soutik, sinputik; sinputik << "SetIkThreshold 1000";
        pmanip->GetIkSolver()->SendCommand(soutik,sinputik);

        vector<dReal> vrealsolution(pmanip->GetArmIndices().size(),0), vrand(pmanip->GetArmIndices().size(),0);
        vector<dReal> vlowerlimit, vupperlimit, viksolution;
        vector< vector<dReal> > viksolutions, viksolutions2;

        robot->SetActiveDOFs(pmanip->GetArmIndices());
        robot->GetActiveDOFLimits(vlowerlimit, vupperlimit);
        // shrink the limits to prevent solutions close to limits from returning errors
        for(size_t i = 0; i < vlowerlimit.size(); ++i) {
            dReal newlower = vlowerlimit[i]*0.9999+0.0001*vupperlimit[i];
            dReal newupper = vlowerlimit[i]*0.0001+0.9999*vupperlimit[i];
            vlowerlimit[i] = newlower;
            vupperlimit[i] = newupper;
        }

        if(bReadFile) {
            fsfile.open(readfilename.c_str(),ios_base::in);
            if(!fsfile.is_open()) {
                RAVELOG_ERROR("IkFastModule::DebugIK - Error: Cannot open specified file.\n");
                return false;
            }
            fsfile >> num_itrs;
        }

        RaveInitRandomGeneration(utils::GetMilliTime());     // have to seed a new number

        IkParameterization twrist, twrist_out;
        vector<dReal> vfreeparameters_real, vfreeparameters, vfreeparameters_out;
        boost::array<vector<pair<IkParameterization, vector<dReal> > >, 3> vsolutionresults;
        vector<pair<IkParameterization, vector<dReal> > >& vwrongsolutions = vsolutionresults[0];     // wrong solution is returned
        vector<pair<IkParameterization, vector<dReal> > >& vnosolutions = vsolutionresults[1];     // no solution found
        vector<pair<IkParameterization, vector<dReal> > >& vnofullsolutions = vsolutionresults[2];     // solution returned, but not all of them
        int success=0;
        int nTotalIterations = 0;
        int nNumFreeTests = 100*pmanip->GetIkSolver()->GetNumFreeParameters();

        std::vector<dReal> vjacobian;
        boost::numeric::ublas::matrix<dReal> J, Jt, JJt;
        ublas::matrix<dReal, boost::numeric::ublas::column_major > U(6, 6), Vt(6,6);
        ublas::vector<dReal> S(6);
        J.resize(6,pmanip->GetArmIndices().size());

        FOREACHC(itiktype, RaveGetIkParameterizationMap()) {
            if( !pmanip->GetIkSolver()->Supports(itiktype->first) ) {
                continue;
            }
            nTotalIterations += num_itrs;
            int i = 0;
            while(i < num_itrs) {
                if(bReadFile) {
                    FOREACH(it, vrealsolution) {
                        fsfile >> *it;
                    }
                    if( !fsfile ) {
                        break;
                    }
                }
                else {
                    if( i == 0 ) {
                        // test the all 0s case (it is so common to get this imperfect)
                        for(int j = 0; j < (int)vrealsolution.size(); j++) {
                            vrealsolution[j] = 0;
                        }
                    }
                    else {
                        if( jacobianthreshold > 0 ) {
                            // don't want any degenerate cases
                            for(int j = 0; j < (int)vrealsolution.size(); j++) {
                                int dof = pmanip->GetArmIndices().at(j);
                                if( robot->GetJointFromDOFIndex(dof)->IsCircular(dof-robot->GetJointFromDOFIndex(dof)->GetDOFIndex()) ) {
                                    vrealsolution[j] = -PI + 2*PI*RaveRandomFloat();
                                }
                                else {
                                    vrealsolution[j] = vlowerlimit[j] + (vupperlimit[j]-vlowerlimit[j])*RaveRandomFloat();
                                }
                            }
                        }
                        else {
                            for(int j = 0; j < (int)vrealsolution.size(); j++) {
                                if( RaveRandomFloat() >= sampledegeneratecases ) {
                                    int dof = pmanip->GetArmIndices().at(j);
                                    if( robot->GetJointFromDOFIndex(dof)->IsCircular(dof-robot->GetJointFromDOFIndex(dof)->GetDOFIndex()) ) {
                                        vrealsolution[j] = -PI + 2*PI*RaveRandomFloat();
                                    }
                                    else {
                                        vrealsolution[j] = vlowerlimit[j] + (vupperlimit[j]-vlowerlimit[j])*RaveRandomFloat();
                                    }
                                }
                                else {
                                    switch(RaveRandomInt()%3) {
                                    case 0: vrealsolution[j] = utils::ClampOnRange(dReal(-PI*0.5),vlowerlimit[j],vupperlimit[j]); break;
                                    case 2: vrealsolution[j] = utils::ClampOnRange(dReal(PI*0.5),vlowerlimit[j],vupperlimit[j]); break;
                                    default: vrealsolution[j] = utils::ClampOnRange(dReal(0),vlowerlimit[j],vupperlimit[j]); break;
                                    }
                                }
                            }
                        }
                    }
                }

                robot->SetActiveDOFValues(vrealsolution,false);
                if( itiktype->first == IKP_Lookat3D) {
                    twrist.SetLookat3D(Vector(RaveRandomFloat()-0.5,RaveRandomFloat()-0.5,RaveRandomFloat()-0.5)*10);
                    twrist = pmanip->GetIkParameterization(twrist);
                }
                else if( itiktype->first == IKP_TranslationLocalGlobal6D) {
                    twrist.SetTranslationLocalGlobal6D(Vector(RaveRandomFloat()-0.5,RaveRandomFloat()-0.5,RaveRandomFloat()-0.5),Vector());
                    twrist = pmanip->GetIkParameterization(twrist);
                }
                else {
                    twrist = pmanip->GetIkParameterization(itiktype->first);
                }

                if( jacobianthreshold > 0 ) {
                    // compute the jacobian and get its determinant
                    // compute jacobians, make sure to transform by the world frame
                    pmanip->CalculateAngularVelocityJacobian(vjacobian);
                    size_t armdof = pmanip->GetArmIndices().size();
                    for(size_t j = 0; j < armdof; ++j) {
                        J(0,j) = vjacobian[j];
                        J(1,j) = vjacobian[armdof+j];
                        J(2,j) = vjacobian[2*armdof+j];
                    }
                    pmanip->CalculateJacobian(vjacobian);
                    for(size_t j = 0; j < armdof; ++j) {
                        J(3+0,j) = vjacobian[j];
                        J(3+1,j) = vjacobian[armdof+j];
                        J(3+2,j) = vjacobian[2*armdof+j];
                    }
                    // pseudo inverse of jacobian
                    Jt = ublas::trans(J);
                    JJt = ublas::prod(J,Jt);
                    //boost::numeric::bindings::lapack::gesvd(JJt, S, U, Vt);
                    dReal jdeterminant = RaveFabs(determinant(JJt));
                    // because the original eigenvalues are squared the multiplier is 6*2=12
                    if( RaveLog(jdeterminant) <= 12*RaveLog(jacobianthreshold) ) {
                        RAVELOG_VERBOSE("determinant is too small\n");
                        continue;
                    }
                }

                if( !pmanip->GetIkSolver()->GetFreeParameters(vfreeparameters_real) ) {
                    RAVELOG_WARN("failed to get freeparameters");
                }
                if( bTestSelfCollision && robot->CheckSelfCollision()) {
                    RAVELOG_VERBOSE("robot in self-collision\n");
                    continue;
                }

                // have to start at a random config
                while(1) {
                    for(int j = 0; j < (int)vrand.size(); j++) {
                        vrand[j] = vlowerlimit[j] + (vupperlimit[j]-vlowerlimit[j])*RaveRandomFloat();
                    }
                    robot->SetActiveDOFValues(vrand, false);
                    if(!bTestSelfCollision || !robot->CheckSelfCollision()) {
                        break;
                    }
                }
                RAVELOG_DEBUG("iteration %d\n",i);
                if( !pmanip->GetIkSolver()->GetFreeParameters(vfreeparameters) ) {
                    RAVELOG_WARN("failed to get freeparameters");
                }
                vfreeparameters_out.resize(vfreeparameters.size());

                bool bsuccess = true;
                bool bnoiksolution = false;
                if( !pmanip->FindIKSolution(twrist, viksolution, filteroptions) ) {
                    if( !bnoiksolution ) {
                        vnosolutions.push_back(make_pair(twrist,vfreeparameters));
                        bnoiksolution = true;
                    }
                    bsuccess = false;
                    s.str("");
                    s << "FindIKSolution: No ik solution found, i = " << i << endl << "jointvalues=[";
                    FOREACH(it, vrealsolution) {
                        s << *it << ", ";
                    }
                    s << "]" << endl << "ikparameterization=\"" << twrist << "\"" << endl;
                    s << "raw ik command: ";
                    GetIKFastCommand(s, twrist, pmanip);
                    FOREACH(itfree,vfreeparameters) {
                        s << *itfree << " ";
                    }
                    s << endl << endl;
                    RAVELOG_WARN(s.str());
                }
                else {
                    robot->SetActiveDOFValues(viksolution,false);
                    twrist_out = pmanip->GetIkParameterization(twrist);
                    if( !pmanip->GetIkSolver()->GetFreeParameters(vfreeparameters_out) ) {
                        RAVELOG_WARN("failed to get freeparameters");
                    }
                    if( twrist.ComputeDistanceSqr(twrist_out) > fthreshold) {
                        vwrongsolutions.push_back(make_pair(twrist,vfreeparameters_out));
                        bsuccess = false;
                        s.str("");
                        s << "FindIKSolution: Incorrect IK, i = " << i <<" error: " << RaveSqrt(twrist.ComputeDistanceSqr(twrist_out)) << endl
                          << "originaljointvalues=[";
                        FOREACH(it, vrealsolution) {
                            s << *it << ", ";
                        }
                        s << "]" << endl << "returnedjointvalues=[";
                        FOREACH(it, viksolution) {
                            s << *it << ", ";
                        }
                        s << "]" << endl << "ikparamin=\"" << twrist << "\"" << endl;
                        s << "ikparamout=\"" << twrist_out << "\"" << endl;
                        s << "raw ik command: ";
                        GetIKFastCommand(s, twrist, pmanip);
                        FOREACH(itfree,vfreeparameters_out) {
                            s << *itfree << " ";
                        }
                        s << endl << endl;
                        RAVELOG_ERROR(s.str());
                        ++i;
                        continue;
                    }
                }

                // test all possible solutions
                robot->SetActiveDOFValues(vrand, false);
                pmanip->FindIKSolutions(twrist, viksolutions, filteroptions);
                if( vfreeparameters_real.size() > 0 ) {
                    pmanip->FindIKSolutions(twrist, vfreeparameters_real, viksolutions2, filteroptions);
                    viksolutions.insert(viksolutions.end(),viksolutions2.begin(),viksolutions2.end());
                }
                if( viksolutions.size() == 0 ) {
                    FOREACH(itfree,vfreeparameters_out) {
                        *itfree = -1;
                    }
                    if( !bnoiksolution ) {
                        vnosolutions.push_back(make_pair(twrist,vfreeparameters_out));
                        bnoiksolution = true;
                    }
                    bsuccess = false;
                    s.str("");
                    s << "FindIKSolutions: No ik solution found for, i = " << i << endl << "Joint Val: ";
                    FOREACH(it, vrealsolution) {
                        s << *it << " ";
                    }
                    s << endl << "Transform: " << twrist << endl << endl;
                    RAVELOG_WARN(s.str());
                }
                else {
                    bool bfail = false;
                    bool bfoundinput = false;
                    FOREACH(itsol, viksolutions) {
                        robot->SetActiveDOFValues(*itsol, false);
                        twrist_out = pmanip->GetIkParameterization(twrist);
                        if( !pmanip->GetIkSolver()->GetFreeParameters(vfreeparameters_out) ) {
                            RAVELOG_WARN("failed to get freeparameters");
                        }
                        if(twrist.ComputeDistanceSqr(twrist_out) > fthreshold ) {
                            vwrongsolutions.push_back(make_pair(twrist,vfreeparameters_out));
                            s.str("");
                            s << "FindIKSolutions: Incorrect IK, i = " << i << " error: " << RaveSqrt(twrist.ComputeDistanceSqr(twrist_out)) << endl
                              << "originalJointValues=[";
                            FOREACH(it, vrealsolution) {
                                s << *it << ", ";
                            }
                            s << "]" << endl << "Returned Joint Val=[";
                            FOREACH(it, *itsol) {
                                s << *it << ", ";
                            }
                            s << "]" << endl << "in: " << twrist << endl;
                            s << "out: " << twrist_out << endl;
                            s << "raw ik command: ";
                            GetIKFastCommand(s, twrist, pmanip);
                            FOREACH(itfree,vfreeparameters_out) {
                                s << *itfree << " ";
                            }
                            s << endl << endl;
                            RAVELOG_ERROR(s.str());
                            bfail = true;
                            break;
                        }
                        robot->SubtractActiveDOFValues(*itsol,vrealsolution);
                        dReal diff = 0;
                        FOREACHC(it,*itsol) {
                            diff += *it**it;
                        }
                        if( diff <= fthreshold ) {
                            bfoundinput = true;
                        }
                    }

                    if( bfail ) {
                        ++i;
                        continue;
                    }
                    if( !bfoundinput ) {
                        s.str("");
                        s << "FindIKSolutions: expected ik solution not found." << std::endl << "originalJointValues=[";
                        FOREACH(it, vrealsolution) {
                            s << *it << ", ";
                        }
                        s << "]" << std::endl;
                        RAVELOG_VERBOSE(s.str());
                        vnofullsolutions.push_back(make_pair(twrist,vfreeparameters_real));
                    }
                }

                if( pmanip->GetIkSolver()->GetNumFreeParameters() > 0 && nNumFreeTests > 0 ) {
                    dReal freerange = 1;
                    dReal deltafree = freerange/dReal(exp(log(double(nNumFreeTests))/double(pmanip->GetIkSolver()->GetNumFreeParameters())));

                    // test with the free parameters
                    robot->SetActiveDOFValues(vrand, false);
                    if( DebugIKFindSolution(pmanip, twrist, viksolution, filteroptions, vfreeparameters, vfreeparameters_out.size()-1,deltafree) ) {
                        robot->SetActiveDOFValues(viksolution, false);
                        twrist_out = pmanip->GetIkParameterization(twrist);
                        if(twrist.ComputeDistanceSqr(twrist_out) > fthreshold ) {
                            vwrongsolutions.push_back(make_pair(twrist,vfreeparameters));
                            bsuccess = false;
                            s.str("");
                            s << "FindIKSolution (freeparams): Incorrect IK, i = " << i << " error: " << RaveSqrt(twrist.ComputeDistanceSqr(twrist_out)) << endl
                              << "freeparams: ";
                            FOREACH(it, vfreeparameters) {
                                s << *it << " ";
                            }
                            s << endl << "originalJointValues=[";
                            FOREACH(it, vrealsolution) {
                                s << *it << ", ";
                            }
                            s << "]" << endl << "Returned Joint Val=[";
                            FOREACH(it, viksolution) {
                                s << *it << ", ";
                            }
                            s << "]" << endl << "in: " << twrist << endl;
                            s << "out: " << twrist_out << endl << endl;
                            RAVELOG_ERROR(s.str());
                            ++i;
                            continue;
                        }

                        // make sure they are the same
                        if( !pmanip->GetIkSolver()->GetFreeParameters(vfreeparameters_out) ) {
                            RAVELOG_WARN("failed to get freeparameters");
                        }
                        for(int j = 0; j < pmanip->GetIkSolver()->GetNumFreeParameters(); ++j) {
                            if( fabsf(vfreeparameters.at(j)-vfreeparameters_out.at(j)) > 0.0001f ) {
                                RAVELOG_WARN(str(boost::format("free params %d not equal: %f!=%f\n")%j%vfreeparameters[j]%vfreeparameters_out[j]));
                                vnofullsolutions.push_back(make_pair(twrist,vfreeparameters));
                                bsuccess = false;
                                break;
                            }
                        }
                    }
                    else {
                        // not sure if should record failure here... the free parameters increment are different than the IK one
                    }

                    // test the multiple solution function
                    robot->SetActiveDOFValues(vrand, false);
                    viksolutions.resize(0);
                    DebugIKFindSolutions(pmanip, twrist, viksolutions, filteroptions, vfreeparameters_out, vfreeparameters_out.size()-1,deltafree);
                    // not sure if should record failure if no solutions found... the free parameters increment are different than the IK one
                    bool bfail = false;
                    FOREACH(itsol, viksolutions) {
                        robot->SetActiveDOFValues(*itsol, false);
                        twrist_out = pmanip->GetIkParameterization(twrist);
                        if(twrist.ComputeDistanceSqr(twrist_out) > fthreshold ) {
                            vwrongsolutions.push_back(make_pair(twrist,vfreeparameters_out));
                            s.str("");
                            s << "FindIKSolutions (freeparams): Incorrect IK, i = " << i <<" error: " << RaveSqrt(twrist.ComputeDistanceSqr(twrist_out)) << endl
                              << "originalJointValues=[";
                            FOREACH(it, vrealsolution) {
                                s << *it << ", ";
                            }
                            s << endl << "]" << endl << "; returnedJointValues=[";
                            FOREACH(it, *itsol) {
                                s << *it << ", ";
                            }
                            s << endl << "]" << endl << "in: " << twrist << endl;
                            s << "out: " << twrist_out << endl;
                            s << "raw ik command: ";
                            GetIKFastCommand(s, twrist, pmanip);
                            FOREACH(itfree,vfreeparameters_out) {
                                s << *itfree << " ";
                            }
                            s << endl << endl;
                            RAVELOG_ERROR(s.str());
                            bfail = true;
                            break;
                        }
                    }
                    if( bfail ) {
                        ++i;
                        continue;
                    }
                }

                if( bsuccess ) {
                    if( IS_DEBUGLEVEL(Level_Verbose) ) {
                        s.str("");
                        s << "raw ik command: ";
                        GetIKFastCommand(s, twrist, pmanip);
                        FOREACH(itfree,vfreeparameters_real) {
                            s << *itfree << " ";
                        }
                        s << endl << endl;
                        RAVELOG_VERBOSE(s.str());
                    }
                    success++;
                }
                i++;
            }
        }

        dReal itotal = dReal(1.0)/nTotalIterations;
        RAVELOG_DEBUG(str(boost::format("DebugIK done, rates %f, %f, %f, %f\n")%(success*itotal)%(vwrongsolutions.size()*itotal)%(vnosolutions.size()*itotal)%(vnofullsolutions.size()*itotal)));
        sout << num_itrs << " " << success << " ";
        FOREACH(itresults,vsolutionresults) {
            sout << itresults->size() << " ";
            FOREACH(it, *itresults) {
                sout << it->first << " ";
                FOREACH(itfree,it->second) {
                    sout << *itfree << " ";
                }
            }
        }

        // restore the ik threshold
        sinputik.str(""); sinputik.clear();
        sinputik << "SetIkThreshold 1e-6";
        pmanip->GetIkSolver()->SendCommand(soutik,sinputik);

        return true;
    }

    static void GetIKFastCommand(std::ostream& o, const IkParameterization& globalparam, RobotBase::ManipulatorPtr pmanip)
    {
        IkParameterization param = pmanip->GetBase()->GetTransform().inverse()*globalparam;
        switch(param.GetType()) {
        case IKP_Transform6D: {
            TransformMatrix tm = param.GetTransform6D()*pmanip->GetLocalToolTransform().inverse(); ///< ik takes the original matrix
            o << tm.m[0] << " " << tm.m[1] << " " << tm.m[2] << " " << tm.trans[0] << " " << tm.m[4] << " " << tm.m[5] << " " << tm.m[6] << " " << tm.trans[1] << " " << tm.m[8] << " " << tm.m[9] << " " << tm.m[10] << " " << tm.trans[2] << " ";
            break;
        }
        case IKP_Rotation3D: {
            TransformMatrix tm = matrixFromQuat(param.GetRotation3D());
            o << tm.m[0] << " " << tm.m[1] << " " << tm.m[2] << " 0 " << tm.m[4] << " " << tm.m[5] << " " << tm.m[6] << " 0 " << tm.m[8] << " " << tm.m[9] << " " << tm.m[10] << " 0 ";
            break;
        }
        case IKP_Translation3D: {
            Vector v = param.GetTranslation3D();
            o << "0 0 0 " << v.x << " 0 0 0 " << v.y << " 0 0 0 " << v.z << " ";
            break;
        }
        case IKP_Direction3D: {
            Vector dir = param.GetDirection3D();
            o << dir.x << " " << dir.y << " " << dir.z << " 0 0 0 0 0 0 0 0 0 ";
            break;
        }
        case IKP_Ray4D: {
            Vector pos = param.GetRay4D().pos;
            Vector dir = param.GetRay4D().dir;
            o << dir.x << " " << dir.y << " " << dir.z << " " << pos.x << " 0 0 0 " << pos.y << " 0 0 0 " << pos.z << " ";
            break;
        }
        case IKP_Lookat3D: {
            Vector v = param.GetLookat3D();
            o << "0 0 0 " << v.x << " 0 0 0 " << v.y << " 0 0 0 " << v.z << " ";
            break;
        }
        case IKP_TranslationDirection5D: {
            Vector dir = param.GetTranslationDirection5D().dir;
            Vector pos = param.GetTranslationDirection5D().pos;
            o << dir.x << " " << dir.y << " " << dir.z << " " << pos.x << " 0 0 0 " << pos.y << " 0 0 0 " << pos.z << " ";
            break;
        }
        case IKP_TranslationXY2D: {
            Vector v = param.GetTranslationXY2D();
            o << "0 0 0 " << v.x << " 0 0 0 " << v.y << " 0 0 0 0 ";
            break;
        }
        case IKP_TranslationXYOrientation3D: {
            Vector v = param.GetTranslationXYOrientation3D();
            o << "0 0 0 " << v.x << " 0 0 0 " << v.y << " 0 0 0 " << v.z << " ";
            break;
        }
        case IKP_TranslationLocalGlobal6D: {
            std::pair<Vector,Vector> p = param.GetTranslationLocalGlobal6D();
            o << p.first.x << " 0 0 " << p.second.x << " 0 " << p.first.y << " 0 " << p.second.y << " 0 0 " << p.first.z << " " << p.second.z << " ";
            break;
        }
        case IKP_TranslationXAxisAngle4D: {
            std::pair<Vector,dReal> p = param.GetTranslationXAxisAngle4D();
            o << p.second << " 0 0 " << p.first.x << " 0 0 0 " << p.first.y << " 0 0 0 " << p.first.z << " ";
            break;
        }
        case IKP_TranslationYAxisAngle4D: {
            std::pair<Vector,dReal> p = param.GetTranslationYAxisAngle4D();
            o << p.second << " 0 0 " << p.first.x << " 0 0 0 " << p.first.y << " 0 0 0 " << p.first.z << " ";
            break;
        }
        case IKP_TranslationZAxisAngle4D: {
            std::pair<Vector,dReal> p = param.GetTranslationZAxisAngle4D();
            o << p.second << " 0 0 " << p.first.x << " 0 0 0 " << p.first.y << " 0 0 0 " << p.first.z << " ";
            break;
        }
        case IKP_TranslationXAxisAngleZNorm4D: {
            std::pair<Vector,dReal> p = param.GetTranslationXAxisAngleZNorm4D();
            o << p.second << " 0 0 " << p.first.x << " 0 0 0 " << p.first.y << " 0 0 0 " << p.first.z << " ";
            break;
        }
        case IKP_TranslationYAxisAngleXNorm4D: {
            std::pair<Vector,dReal> p = param.GetTranslationYAxisAngleXNorm4D();
            o << p.second << " 0 0 " << p.first.x << " 0 0 0 " << p.first.y << " 0 0 0 " << p.first.z << " ";
            break;
        }
        case IKP_TranslationZAxisAngleYNorm4D: {
            std::pair<Vector,dReal> p = param.GetTranslationZAxisAngleYNorm4D();
            o << p.second << " 0 0 " << p.first.x << " 0 0 0 " << p.first.y << " 0 0 0 " << p.first.z << " ";
            break;
        }
        default:
            BOOST_ASSERT(0);
        }
    }

    static list< boost::shared_ptr<IkLibrary> >*& GetLibraries()
    {
        static list< boost::shared_ptr<IkLibrary> >* s_vStaticLibraries=NULL;
        if( s_vStaticLibraries == NULL ) {
            s_vStaticLibraries = new list< boost::shared_ptr<IkLibrary> >();
        }
        return s_vStaticLibraries;
    }

    static boost::mutex& GetLibraryMutex()
    {
        static boost::mutex s_LibraryMutex;
        return s_LibraryMutex;
    }

    /// sinput holds the freeindices and other run-time configuraiton parameters
    static IkSolverBasePtr CreateIkSolver(const string& _name, const std::vector<dReal>& vfreeinc, EnvironmentBasePtr penv)
    {
        string name; name.resize(_name.size());
        std::transform(_name.begin(), _name.end(), name.begin(), ::tolower);
        /// start from the newer libraries
        boost::mutex::scoped_lock lock(GetLibraryMutex());
        for(list< boost::shared_ptr<IkLibrary> >::reverse_iterator itlib = GetLibraries()->rbegin(); itlib != GetLibraries()->rend(); ++itlib) {
            FOREACHC(itikname,(*itlib)->GetIkNames()) {
                if( name == *itikname ) {
                    return (*itlib)->CreateSolver(penv,vfreeinc);
                }
            }
        }
        return IkSolverBasePtr();
    }

    string _ikfastversion; ///< current ikfast version (assuming doesn't change during process lifetime)
    string _platform; ///<  current platform architecture. ie x86-64
};

ModuleBasePtr CreateIkFastModule(EnvironmentBasePtr penv, std::istream& sinput)
{
    return ModuleBasePtr(new IkFastModule(penv,sinput));
}

void DestroyIkFastLibraries() {
    delete IkFastModule::GetLibraries();
    IkFastModule::GetLibraries() = NULL;
}

IkSolverBasePtr CreateIkSolverFromName(const string& _name, const std::vector<dReal>& vfreeinc, EnvironmentBasePtr penv)
{
    return IkFastModule::CreateIkSolver(_name,vfreeinc,penv);
}

max_issues_repo_name: jdsika/holy

max_forks_repo_name: jdsika/holy


text:
#pragma once

#include <coma/detail/core_async.hpp>
#if defined(COMA_COROUTINES)

#include <boost/asio/awaitable.hpp>
#include <utility>

namespace coma {

template<class Function>
auto co_lift(Function f) -> boost::asio::awaitable<decltype(std::move(f)())>
{
	co_return std::move(f)();
}

} // namespace coma

#endif

ext: hpp

max_stars_repo_name: gummif/coma

max_issues_repo_name: gummif/coma

max_forks_repo_name: gummif/coma


text:
#ifndef FSTDATAINTERVALLER_H
#define FSTDATAINTERVALLER_H

/*!======================================================================
   Feature Selection Toolbox 3 source code
   ---------------------------------------

   \file    data_intervaller.hpp
   \brief   Container to hold a list of Data_Interval; implements nested interval reduction
   \author  Petr Somol (somol@utia.cas.cz) with collaborators, see Contacts at http://fst.utia.cz
   \date    October 2010
   \version 3.0.0.beta
   \note    FST3 was developed using gcc 4.3 and requires
   \note    \li Boost library (http://www.boost.org/, tested with versions 1.33.1 and 1.44),
   \note    \li (\e optionally) LibSVM (http://www.csie.ntu.edu.tw/~cjlin/libsvm/,
                tested with version 3.00)
   \note    Note that LibSVM is required for SVM related tools only,
            as demonstrated in demo12t.cpp, demo23.cpp, demo25t.cpp, demo32t.cpp, etc.

*/ /*
=========================================================================
Copyright:
  * FST3 software (with exception of any externally linked libraries)
    is copyrighted by Institute of Information Theory and Automation (UTIA),
    Academy of Sciences of the Czech Republic.
  * FST3 source codes as presented here do not contain code of third parties.
    FST3 may need linkage to external libraries to exploit its functionality
    in full. For details on obtaining and possible usage restrictions
    of external libraries follow their original sources (referenced from
    FST3 documentation wherever applicable).
  * FST3 software is available free of charge for non-commercial use.
    Please address all inquires concerning possible commercial use
    of FST3, or if in doubt, to FST3 maintainer (see http://fst.utia.cz).
  * Derivative works based on FST3 are permitted as long as they remain
    non-commercial only.
  * Re-distribution of FST3 software is not allowed without explicit
    consent of the copyright holder.
Disclaimer of Warranty:
  * FST3 software is presented "as is", without warranty of any kind,
    either expressed or implied, including, but not limited to, the implied
    warranties of merchantability and fitness for a particular purpose.
    The entire risk as to the quality and performance of the program
    is with you. Should the program prove defective, you assume the cost
    of all necessary servicing, repair or correction.
Limitation of Liability:
  * The copyright holder will in no event be liable to you for damages,
    including any general, special, incidental or consequential damages
    arising out of the use or inability to use the code (including but not
    limited to loss of data or data being rendered inaccurate or losses
    sustained by you or third parties or a failure of the program to operate
    with any other programs).
========================================================================== */

#include <boost/smart_ptr.hpp>
#include <algorithm>
#include <iostream>
#include <sstream>
#include <vector> // for blockiter - array of iterators indexed by loopdepth
#include "error.hpp"
#include "global.hpp"

/*============== Template parameter type naming conventions ==============
--------- Numeric types: -------------------------------------------------
DATATYPE - data sample values - usually real numbers (but may be integers
          in text processing etc.)
REALTYPE - must be real numbers - for representing intermediate results of
          calculations like mean, covariance etc.
IDXTYPE - index values for enumeration of data samples - (nonnegative) integers,
          extent depends on numbers of samples in data
DIMTYPE - index values for enumeration of features (dimensions), or classes (not
          class sizes) - (nonnegative) integers, usually lower extent than IDXTYPE,
          but be aware of expressions like _classes*_features*_features !
          in linearized representations of feature matrices for all classes
BINTYPE - feature selection marker type - represents ca. <10 different feature
          states (selected, deselected, sel./desel. temporarily 1st nested loop, 2nd...)
RETURNTYPE - criterion value: real value, but may be extended in future to support
          multiple values
--------- Class types: ---------------------------------------------------
SUBSET       - class of class type Subset
CLASSIFIER   - class implementing interface defined in abstract class Classifier
EVALUATOR    - class implementing interface defined in abstract class Sequential_Step
DISTANCE     - class implementing interface defined in abstract class Distance
DATAACCESSOR - class implementing interface defined in abstract class Data_Accessor
INTERVALCONTAINER - class of class type TIntervaller
CONTAINER    - STL container of class type TInterval
========================================================================== */

namespace FST {

//! Support structure to hold data interval (referring into data held by data accessor)
template<typename IDXTYPE>
struct Data_Interval {
	IDXTYPE startidx;
	IDXTYPE count;
};

template <class CONTAINER, typename IDXTYPE>
class Data_Intervaller;

template <class CONTAINER, typename IDXTYPE>
std::ostream& operator<<(std::ostream& os, const Data_Intervaller<CONTAINER,IDXTYPE>& ia);

//! Container to hold list of Data_Interval; implements nested interval reduction
template<class CONTAINER, typename IDXTYPE>
//! \invariant CONTAINER must be a container of Data_Interval supporting iterators, push_back(), clear()
class Data_Intervaller : public CONTAINER {
public:
	Data_Intervaller() {notify("Data_Intervaller constructor.");};
	Data_Intervaller(const Data_Intervaller& tin);
	virtual ~Data_Intervaller() {notify("Data_Intervaller destructor.");};
	Data_Interval<IDXTYPE>* const getFirstBlock(const unsigned int loopdepth=0);
	Data_Interval<IDXTYPE>* const getNextBlock(const unsigned int loopdepth=0);
	bool check_validity(const IDXTYPE maxidx);		//!< checks whether all intervals are non-overlapping, ordered ascending, and all fit to <0,maxidx> (maxidx==0 disables maxidx test)
	IDXTYPE sum();	//!< sums the sizes of all contained intervals
	IDXTYPE max_idx();	//!< returns maximum of all startidx+count values
	void reduce(const boost::shared_ptr<Data_Intervaller<CONTAINER,IDXTYPE> > reducer, boost::shared_ptr<Data_Intervaller<CONTAINER,IDXTYPE> > target);	//!< splits the "mass" contained here in intervals according to the "reducer" and store the reduced result in target
	friend std::ostream& operator<< <>(std::ostream& os, const Data_Intervaller<CONTAINER,IDXTYPE>& ia);
private:
	typedef typename Data_Intervaller<CONTAINER,IDXTYPE>::iterator TCI;
	std::vector<TCI> blockiter; //!< to enable multiple independent get*Block() loops
};

template<class CONTAINER, typename IDXTYPE>
Data_Intervaller<CONTAINER,IDXTYPE>::Data_Intervaller(const Data_Intervaller& tin)
{
	notify("Data_Intervaller copy constructor.");
	this->clear();
	for(typename Data_Intervaller<CONTAINER,IDXTYPE>::const_iterator iter=tin.begin();iter!=tin.end();iter++) this->push_back(*iter);
	for(typename std::vector<TCI>::const_iterator biter=tin.blockiter.begin();biter!=tin.blockiter.end();biter++) this->blockiter.push_back(*biter);
}

template<class CONTAINER, typename IDXTYPE>
std::ostream& operator<<(std::ostream& os, const Data_Intervaller<CONTAINER,IDXTYPE>& ia) {
	for(typename Data_Intervaller<CONTAINER,IDXTYPE>::const_iterator iter=ia.begin();iter!=ia.end();iter++) os << "si:" << iter->startidx << ", c:" << iter->count << std::endl;
	return os;
}

template<class CONTAINER, typename IDXTYPE>
Data_Interval<IDXTYPE>* const Data_Intervaller<CONTAINER,IDXTYPE>::getFirstBlock(const unsigned int loopdepth)
{
	if(blockiter.size()<=loopdepth) blockiter.resize(loopdepth+1);
	blockiter[loopdepth]=this->begin();
	if(blockiter[loopdepth]!=this->end()) return &(*blockiter[loopdepth]);
	else return NULL;
}

template<class CONTAINER, typename IDXTYPE>
Data_Interval<IDXTYPE>* const Data_Intervaller<CONTAINER,IDXTYPE>::getNextBlock(const unsigned int loopdepth)
{
	// NOTE: getNextBlock() must be preceeded by getNextBlock() with the same loopdepth value, othervise the behaviour is undefined
	assert(loopdepth<blockiter.size());
	++blockiter[loopdepth];
	if(blockiter[loopdepth]!=this->end()) return &(*blockiter[loopdepth]);
	else return NULL;
}

template<class CONTAINER, typename IDXTYPE>
bool Data_Intervaller<CONTAINER,IDXTYPE>::check_validity(const IDXTYPE maxidx)
{
	if(this->empty()) {{std::ostringstream sos; sos << "Data_Intervaller is empty" << std::endl; syncout::print(std::cerr,sos);} return false;}
	TCI iter=this->begin();
	IDXTYPE pivot=iter->startidx+iter->count;
	if(iter->startidx<0 || (maxidx>0 && (pivot>maxidx)) ) {{std::ostringstream sos; sos <<"first is wrong"<<std::endl; syncout::print(std::cerr,sos);} return false;} // the first interval reaches outside <0,maxidx>
	if(iter==this->end()) return true; //just one interval, and that one is OK
	iter++;
	while(iter!=this->end())
	{
		if(iter->startidx<pivot) {{std::ostringstream sos; sos <<"iter->startidx="<<iter->startidx<<", pivot="<<pivot<<std::endl; syncout::print(std::cerr,sos);} return false;}
		pivot=iter->startidx+iter->count;
		if(maxidx>0 && (pivot>maxidx)) {{std::ostringstream sos; sos <<"startidx+count>maxidx"<<std::endl; syncout::print(std::cerr,sos);} return false;}
		++iter;
	}
	return true;
}

template<class CONTAINER, typename IDXTYPE>
IDXTYPE Data_Intervaller<CONTAINER,IDXTYPE>::sum()
{
	IDXTYPE cnt=0;
	for(TCI iter=this->begin();iter!=this->end();iter++) cnt+=iter->count;
	return cnt;
}

template<class CONTAINER, typename IDXTYPE>
IDXTYPE Data_Intervaller<CONTAINER,IDXTYPE>::max_idx()
{
	IDXTYPE mx=0;
	for(TCI iter=this->begin();iter!=this->end();iter++) if(iter->startidx+iter->count-1>mx) mx=iter->startidx+iter->count-1;
	return mx;
}

template<class CONTAINER, typename IDXTYPE>
void Data_Intervaller<CONTAINER,IDXTYPE>::reduce(const boost::shared_ptr<Data_Intervaller<CONTAINER,IDXTYPE> > reducer, boost::shared_ptr<Data_Intervaller<CONTAINER,IDXTYPE> > target)
{
	try{
		assert(sum()>=reducer->max_idx());
		target->clear();
		typename Data_Intervaller<CONTAINER,IDXTYPE>::const_iterator rediter=reducer->begin();
		IDXTYPE Mbottom=0;
		for(TCI iter=this->begin();iter!=this->end();iter++) {
			bool stop=false;
			while((!stop) && (rediter!=reducer->end())) {
				IDXTYPE Rbottom=(*rediter).startidx;
				IDXTYPE Rtop=Rbottom+(*rediter).count-1;
				IDXTYPE MTRUEbottom=(*iter).startidx;
				IDXTYPE Mtop=Mbottom+(*iter).count-1;
				if(!(Rtop<Mbottom || Rbottom>Mtop)) { //write intersection to target
					IDXTYPE ISCbottom=Rbottom; if(Rbottom<Mbottom) ISCbottom=Mbottom;
					IDXTYPE ISCtop=Rtop; if(Rtop>Mtop) ISCtop=Mtop;
					Data_Interval<IDXTYPE> I={MTRUEbottom+(ISCbottom-Mbottom), ISCtop-ISCbottom+1};
					target->push_back(I);
				}
				if(Rtop>=Mtop) stop=true;
				if(Rtop<=Mtop) rediter++;
			}
			Mbottom+=(*iter).count;
		}

	} catch(...) {
		throw fst_error("Data_Intervaller::reduce() error.");
	}
}

} // namespace
#endif // FSTDATAINTERVALLER_H ///:~

max_stars_repo_name: boussaffawalid/FeatureSelection

max_issues_repo_name: boussaffawalid/FeatureSelection

max_forks_repo_name: boussaffawalid/FeatureSelection


text:
// Copyright 2004-present Facebook. All Rights Reserved.

#include <algorithm>
#include <future>
#include <random>
#include <sstream>
#include <string>
#include <vector>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/thread/shared_mutex.hpp>

#include <glog/logging.h>

#include <osquery/config.h>
#include <osquery/config/plugin.h>
#include <osquery/flags.h>
#include <osquery/status.h>

#include "osquery/core/md5.h"

using osquery::Status;

namespace pt = boost::property_tree;

namespace osquery {

DEFINE_osquery_flag(string,
                    config_retriever,
                    "filesystem",
                    "Config type (plugin).");

DEFINE_osquery_flag(int32,
                    schedule_splay_percent,
                    10,
                    "Percent to splay config times.");

boost::shared_mutex rw_lock;

std::shared_ptr<Config> Config::getInstance() {
  static std::shared_ptr<Config> config = std::shared_ptr<Config>(new Config());
  return config;
}

Config::Config() {
  boost::unique_lock<boost::shared_mutex> lock(rw_lock);
  OsqueryConfig conf;
  auto s = Config::genConfig(conf);
  if (!s.ok()) {
    LOG(ERROR) << "error retrieving config: " << s.toString();
    return;
  }

  // Override default arguments with flag options from config.
  for (const auto& option : conf.options) {
    if (Flag::isDefault(option.first)) {
      // Only override if option was NOT given as an argument.
      Flag::updateValue(option.first, option.second);
      VLOG(1) << "Setting flag option: " << option.first << "="
              << option.second;
    }
  }

  // Iterate over scheduled queryies and add a splay to each.
  for (auto& q : conf.scheduledQueries) {
    auto old_interval = q.interval;
    auto new_interval = splayValue(old_interval, FLAGS_schedule_splay_percent);
    LOG(INFO) << "Changing the interval for " << q.name << " from  "
              << old_interval << " to " << new_interval;
    q.interval = new_interval;
  }
  cfg_ = conf;
}

Status Config::genConfig(std::string& conf) {
  if (REGISTERED_CONFIG_PLUGINS.find(FLAGS_config_retriever) ==
      REGISTERED_CONFIG_PLUGINS.end()) {
    LOG(ERROR) << "Config retriever " << FLAGS_config_retriever << " not found";
    return Status(1, "Config retriever not found");
  }
  auto config_data =
      REGISTERED_CONFIG_PLUGINS.at(FLAGS_config_retriever)->genConfig();
  if (!config_data.first.ok()) {
    return config_data.first;
  }
  conf = config_data.second;
  return Status(0, "OK");
}

Status Config::genConfig(OsqueryConfig& conf) {
  std::stringstream json;
  pt::ptree tree;
  std::string config_string;
  auto s = genConfig(config_string);
  if (!s.ok()) {
    return s;
  }

  json << config_string;
  pt::read_json(json, tree);

  try {
    // Parse each scheduled query from the config.
    for (const pt::ptree::value_type& v : tree.get_child("scheduledQueries")) {
      OsqueryScheduledQuery q;
      q.name = (v.second).get<std::string>("name");
      q.query = (v.second).get<std::string>("query");
      q.interval = (v.second).get<int>("interval");
      conf.scheduledQueries.push_back(q);
    }

    // Flags may be set as 'options' within the config.
    if (tree.count("options") > 0) {
      for (const pt::ptree::value_type& v : tree.get_child("options")) {
        conf.options[v.first.data()] = v.second.data();
      }
    }
  } catch (const std::exception& e) {
    LOG(ERROR) << "Error parsing config JSON: " << e.what();
    return Status(1, e.what());
  }

  return Status(0, "OK");
}

std::vector<OsqueryScheduledQuery> Config::getScheduledQueries() {
  boost::shared_lock<boost::shared_mutex> lock(rw_lock);
  return cfg_.scheduledQueries;
}

int Config::splayValue(int original, int splayPercent) {
  if (splayPercent <= 0 || splayPercent > 100) {
    return original;
  }

  float percent_to_modify_by = (float)splayPercent / 100;
  int possible_difference = original * percent_to_modify_by;
  int max_value = original + possible_difference;
  int min_value = original - possible_difference;

  if (max_value == min_value) {
    return max_value;
  }

  std::default_random_engine generator;
  std::uniform_int_distribution<int> distribution(min_value, max_value);
  return distribution(generator);
}

Status Config::getMD5(std::string& hashString) {
  std::string config_string;
  auto s = genConfig(config_string);
  if (!s.ok()) {
    return s;
  }

  osquery::md5::MD5 digest;
  hashString = std::string(digest.digestString(config_string.c_str()));

  return Status(0, "OK");
}
}

ext: cpp

max_stars_repo_name: Anubisss/osquery

max_issues_repo_name: Anubisss/osquery

max_forks_repo_name: Anubisss/osquery


text:
//
// Copyright (c) 2021 Vinnie Falco (vinnie dot falco at gmail dot com)
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Official repository: https://github.com/CPPAlliance/url
//

#ifndef BOOST_URL_BNF_CHARSET_HPP
#define BOOST_URL_BNF_CHARSET_HPP

#include <boost/url/detail/config.hpp>
#include <boost/url/bnf/detail/charset.hpp>
#include <boost/type_traits/make_void.hpp>
#include <boost/static_assert.hpp>
#include <cstdint>
#include <type_traits>

// Credit to Peter Dimov for ideas regarding
// SIMD constexpr, and character set masks.

namespace boost {
namespace urls {
namespace bnf {

/** Alias for `std::true_type` if T satisfies CharSet.

    This metafunction determines if the
    type `T` meets these requirements of
    CharSet:

    @li An instance of `T` is invocable
    with this equivalent function signature:
    @code
    bool T::operator()( char ) const;
    @endcode

    @par Example
    Use with `enable_if` on the return value:
    @code
    template< class CharSet >
    typename std::enable_if< is_charset<T>::value >::type
    func( CharSet const& cs );
    @endcode

    @tparam T the type to check.
*/
#ifdef BOOST_URL_DOCS
template<class T>
using is_charset = __see_below__;
#else
template<class T, class = void>
struct is_charset : std::false_type {};

template<class T>
struct is_charset<T, boost::void_t<
    decltype(
    std::declval<bool&>() =
        std::declval<T const&>().operator()(
            std::declval<char>())
            ) > > : std::true_type
{
};
#endif

//------------------------------------------------

/** A character set based on a constexpr lookup table.

    Objects of this type are invocable with
    the equivalent signature:

    @code
    bool( char ch ) const noexcept;
    @endcode

    The function object returns `true` when
    `ch` is a member of the character set,
    and `false` otherwise. The type of the
    function satisfies the CharSet
    requirements.

    @see
        @ref is_charset,
        @ref find_if,
        @ref find_if_not.
*/
class lut_chars
{
    std::uint64_t mask_[4] = {};

    constexpr
    static
    std::uint64_t
    lo(char c) noexcept
    {
        return static_cast<
            unsigned char>(c) & 3;
    }

    constexpr
    static
    std::uint64_t
    hi(char c) noexcept
    {
        return 1ULL << (static_cast<
            unsigned char>(c) >> 2);
    }

    constexpr
    static
    lut_chars
    construct(
        char const* s) noexcept
    {
        return *s
            ? lut_chars(*s) +
                construct(s+1)
            : lut_chars();
    }

    constexpr
    static
    lut_chars
    construct(
        unsigned char ch,
        bool b) noexcept
    {
        return b
            ? lut_chars(ch)
            : lut_chars();
    }

    template<class Pred>
    constexpr
    static
    lut_chars
    construct(
        Pred pred,
        unsigned char ch) noexcept
    {
        return ch == 255
            ? construct(ch, pred(ch))
            : construct(ch, pred(ch)) +
                construct(pred, ch + 1);
    }

    constexpr
    lut_chars() = default;

    constexpr
    lut_chars(
        std::uint64_t m0,
        std::uint64_t m1,
        std::uint64_t m2,
        std::uint64_t m3) noexcept
        : mask_{ m0, m1, m2, m3 }
    {
    }

public:
    /** Constructor.

        This function constructs a character
        set which has as a single member,
        the character `ch`.

        @par Example
        @code
        constexpr lut_chars asterisk( '*' );
        @endcode

        @par Complexity
        Constant.

        @par Exception Safety
        Throws nothing.

        @param ch A character.
    */
    constexpr
    lut_chars(char ch) noexcept
        : mask_ {
            lo(ch) == 0 ? hi(ch) : 0,
            lo(ch) == 1 ? hi(ch) : 0,
            lo(ch) == 2 ? hi(ch) : 0,
            lo(ch) == 3 ? hi(ch) : 0 }
    {
    }

    /** Constructor.

        This function constructs a character
        set which has as members, all of the
        characters present in the null-terminated
        string `s`.

        @par Example
        @code
        constexpr lut_chars digits = "0123456789";
        @endcode

        @par Complexity
        Linear in `::strlen(s)`, or constant
        if `s` is a constant expression.

        @par Exception Safety
        Throws nothing.

        @param s A null-terminated string.
    */
    constexpr
    lut_chars(char const* s) noexcept
        : lut_chars(construct(s))
    {
    }

    /** Constructor.

        This function constructs a character
        set which has as members, every value
        of `char ch` for which the expression
        `pred(ch)` returns `true`.

        @par Example
        @code
        struct is_digit
        {
            constexpr bool
            operator()(char c ) const noexcept
            {
                return c >= '0' && c <= '9';
            }
        };

        constexpr lut_chars digits( is_digit{} );
        @endcode

        @par Complexity
        Linear in `pred`, or constant if
        `pred(ch)` is a constant expression.

        @par Exception Safety
        Throws nothing.

        @param pred The function object to
        use for determining membership in
        the character set.
    */
    template<class Pred
#ifndef BOOST_URL_DOCS
        ,class = typename std::enable_if<
            is_charset<Pred>::value &&
        ! std::is_same<Pred, lut_chars>::value>::type
#endif
    >
    constexpr
    lut_chars(Pred const& pred) noexcept
        : lut_chars(
            construct(pred, 0))
    {
    }

    /** Return true if ch is in the character set.

        This function returns true if the
        character `ch` is in the set, otherwise
        it returns false.

        @par Complexity
        Constant.

        @par Exception Safety
        Throws nothing.

        @param ch The character to test.
    */
    constexpr
    bool
    operator()(
        unsigned char ch) const noexcept
    {
        return mask_[lo(ch)] & hi(ch);
    }

    /** Return the union of two character sets.

        This function returns a new character
        set which contains all of the characters
        in `cs0` as well as all of the characters
        in `cs`.

        @par Example
        This creates a character set which
        includes all letters and numbers
        @code
        constexpr lut_chars alpha_chars(
            "ABCDEFGHIJKLMNOPQRSTUVWXYZ"
            "abcdefghijklmnopqrstuvwxyz");

        constexpr lut_chars alnum_chars = alpha_chars + "0123456789";
        @endcode

        @par Complexity
        Constant.

        @return The new character set.

        @param cs0 A character to join

        @param cs1 A character to join
    */
    friend
    constexpr
    lut_chars
    operator+(
        lut_chars const& cs0,
        lut_chars const& cs1) noexcept
    {
        return lut_chars(
            cs0.mask_[0] | cs1.mask_[0],
            cs0.mask_[1] | cs1.mask_[1],
            cs0.mask_[2] | cs1.mask_[2],
            cs0.mask_[3] | cs1.mask_[3]);
    }

    /** Return a new character set by subtracting

        This function returns a new character
        set which is formed from all of the
        characters in `cs0` which are not in `cs`.

        @par Example
        This statement declares a character set
        containing all the lowercase letters
        which are not vowels:
        @code
        constexpr lut_chars consonants = lut_chars("abcdefghijklmnopqrstuvwxyz") - "aeiou";
        @endcode

        @par Complexity
        Constant.

        @return The new character set.

        @param cs0 A character set to join.

        @param cs1 A character set to join.
    */
    friend
    constexpr
    lut_chars
    operator-(
        lut_chars const& cs0,
        lut_chars const& cs1) noexcept
    {
        return lut_chars(
            cs0.mask_[0] & ~cs1.mask_[0],
            cs0.mask_[1] & ~cs1.mask_[1],
            cs0.mask_[2] & ~cs1.mask_[2],
            cs0.mask_[3] & ~cs1.mask_[3]);
    }

    /** Return a new character set which is the complement of another character set.

        This function returns a new character
        set which contains all of the characters
        that are not in `*this`.

        @par Example
        This statement declares a character set
        containing everything but vowels:
        @code
        constexpr lut_chars not_vowels = ~lut_chars( "aAeEiIoOuU" );
        @endcode

        @par Complexity
        Constant.

        @par Exception Safety
        Throws nothing.

        @return The new character set.
    */
    constexpr
    lut_chars
    operator~() const noexcept
    {
        return lut_chars(
            ~mask_[0],
            ~mask_[1],
            ~mask_[2],
            ~mask_[3]
        );
    }

#ifndef BOOST_URL_DOCS
#ifdef BOOST_URL_USE_SSE2
    char const*
    find_if(
        char const* first,
        char const* last) const noexcept
    {
        return detail::find_if_pred(
            *this, first, last);
    }

    char const*
    find_if_not(
        char const* first,
        char const* last) const noexcept
    {
        return detail::find_if_not_pred(
            *this, first, last);
    }
#endif
#endif
};

//------------------------------------------------

/** A character set containing all characters.

    This is an object invocable with
    the equivalent signature:

    @code
    bool( char ch ) const noexcept;
    @endcode

    The function object returns `true` when
    `ch` is a member of the character set,
    and `false` otherwise. The type of the
    function satisfies the CharSet
    requirements.

    @par BNF
    @code
    ALL         = %x00-FF
                ; all ASCII and high-ASCII
    @endcode

    @par Specification
    @li B.1. Core Rules (rfc5234)

    @see
        @ref is_charset,
        @ref find_if,
        @ref find_if_not.
*/
constexpr
#ifdef BOOST_URL_DOCS
__implementation_defined__
#else
struct
{
    constexpr
    bool
    operator()(char) const noexcept
    {
        return true;
    }

    constexpr
    char const*
    find_if(
        char const* first,
        char const*) const noexcept
    {
        return first;
    }

    constexpr
    char const*
    find_if_not(
        char const*,
        char const* last) const noexcept
    {
        return last;
    }
}
#endif
all_chars{};

//------------------------------------------------

/** A character set containing all letters and digits.

    This is an object invocable with
    the equivalent signature:

    @code
    bool( char ch ) const noexcept;
    @endcode

    The function object returns `true` when
    `ch` is a member of the character set,
    and `false` otherwise. The type of the
    function satisfies the CharSet
    requirements.

    @par BNF
    @code
    ALNUM       = ALPHA / DIGIT

    ALPHA       =  %x41-5A / %x61-7A
                ; A-Z / a-z

    DIGIT       =  %x30-39
                ; 0-9
    @endcode

    @par Specification
    @li B.1. Core Rules (rfc5234)

    @see
        @ref is_charset,
        @ref find_if,
        @ref find_if_not.
*/
constexpr
#ifdef BOOST_URL_DOCS
__implementation_defined__
#else
struct
{
    constexpr
    bool
    operator()(char c) const noexcept
    {
        return
            (c >= '0' && c <= '9') ||
            (c >= 'A' && c <= 'Z') ||
            (c >= 'a' && c <= 'z');
    }

#ifdef BOOST_URL_USE_SSE2
    char const*
    find_if(
        char const* first,
        char const* last) const noexcept
    {
        return detail::find_if_pred(
            *this, first, last);
    }

    char const*
    find_if_not(
        char const* first,
        char const* last) const noexcept
    {
        return detail::find_if_not_pred(
            *this, first, last);
    }
#endif
}
#endif
alnum_chars{};

//------------------------------------------------

/** A CharSet containing the alpha characters

    This is an object invocable with
    the equivalent signature:

    @code
    bool( char ch ) const noexcept;
    @endcode

    The function object returns `true` when
    `ch` is a member of the character set,
    and `false` otherwise. The type of the
    function satisfies the CharSet
    requirements.

    @par BNF
    @code
    ALPHA       =  %x41-5A / %x61-7A
                ; A-Z / a-z
    @endcode

    @par Specification
    @li B.1. Core Rules (rfc5234)

    @see
        @ref is_charset,
        @ref find_if,
        @ref find_if_not.
*/
constexpr
#ifdef BOOST_URL_DOCS
__implementation_defined__
#else
struct
{
    constexpr
    bool
    operator()(char c) const noexcept
    {
        return
            (c >= 'A' && c <= 'Z') ||
            (c >= 'a' && c <= 'z');
    }

#ifdef BOOST_URL_USE_SSE2
    char const*
    find_if(
        char const* first,
        char const* last) const noexcept
    {
        return detail::find_if_pred(
            *this, first, last);
    }

    char const*
    find_if_not(
        char const* first,
        char const* last) const noexcept
    {
        return detail::find_if_not_pred(
            *this, first, last);
    }
#endif
}
#endif
alpha_chars{};

//------------------------------------------------

/** A character set containing the decimal digits.

    This is an object invocable with
    the equivalent signature:

    @code
    bool( char ch ) const noexcept;
    @endcode

    The function object returns `true` when
    `ch` is a member of the character set,
    and `false` otherwise. The type of the
    function satisfies the CharSet
    requirements.

    @par BNF
    @code
    DIGIT       =  %x30-39
                ; 0-9
    @endcode

    @par Specification
    @li B.1. Core Rules (rfc5234)

    @see
        @ref is_charset,
        @ref find_if,
        @ref find_if_not.
*/
constexpr
#ifdef BOOST_URL_DOCS
__implementation_defined__
#else
struct
{
    constexpr
    bool
    operator()(char c) const noexcept
    {
        return c >= '0' && c <= '9';
    }

#ifdef BOOST_URL_USE_SSE2
    char const*
    find_if(
        char const* first,
        char const* last) const noexcept
    {
        return detail::find_if_pred(
            *this, first, last);
    }

    char const*
    find_if_not(
        char const* first,
        char const* last) const noexcept
    {
        return detail::find_if_not_pred(
            *this, first, last);
    }
#endif
}
#endif
digit_chars{};

//------------------------------------------------

/** A character set containing the hexadecimal digits.

    This is an object invocable with
    the equivalent signature:

    @code
    bool( char ch ) const noexcept;
    @endcode

    The function object returns `true` when
    `ch` is a member of the character set,
    and `false` otherwise. The type of the
    function satisfies the CharSet
    requirements.

    @par BNF
    @code
    HEXDIG      = DIGIT
                / "A" / "B" / "C" / "D" / "E" / "F"
                / "a" / "b" / "c" / "d" / "e" / "f"
    @endcode

    @note The RFCs are inconsistent on the case
    sensitivity of hexadecimal digits. Existing
    uses suggest case-insensitivity is a de-facto
    standard.

    @par Specification
    @li B.1. Core Rules (rfc5234)
    @li 1.2. Syntax Notation (rfc7230)
    @li 2.3. Uppercase or Lowercase (rfc5952)
    @li 4.3. Lowercase (rfc5952)

    @see
        @ref is_charset,
        @ref find_if,
        @ref find_if_not.
*/
constexpr
#ifdef BOOST_URL_DOCS
__implementation_defined__
#else
struct
{
    /** Return true if c is in the character set.
    */
    constexpr
    bool
    operator()(char c) const noexcept
    {
        return
            (c >= '0' && c <= '9') ||
            (c >= 'A' && c <= 'F') ||
            (c >= 'a' && c <= 'f');
    }

#ifdef BOOST_URL_USE_SSE2
    char const*
    find_if(
        char const* first,
        char const* last) const noexcept
    {
        return detail::find_if_pred(
            *this, first, last);
    }

    char const*
    find_if_not(
        char const* first,
        char const* last) const noexcept
    {
        return detail::find_if_not_pred(
            *this, first, last);
    }
#endif
}
#endif
hexdig_chars{};

// VFALCO We can declare
// these later if needed
//
//struct hexdig_upper_chars;
//struct hexdig_lower_chars;

/** Return the numeric value of a HEXDIG, or -1 if invalid

    This function returns the numeric value
    of a single hexadecimal digit, or -1 if
    `ch` is not a hexadecimal digit.
*/
constexpr
char
hexdig_value(char c) noexcept
{
    return
        (c >= '0' && c <= '9') ?
        c - '0' : (
            (c >= 'A' && c <= 'F') ?
            10 + c - 'A' : (
                (c >= 'a' && c <= 'f') ?
                10 + c - 'a' : - 1) );
}

//------------------------------------------------

/** Find the first character in the string that is in the set.

    @par Exception Safety
    Throws nothing.

    @return A pointer to the character if found,
    otherwise a pointer to `last`.

    @param first A pointer to the first character
    in the string to search.

    @param last A pointer to one past the last
    character in the string to search.

    @param cs The character set to use.

    @see
        @ref find_if_not.
*/
template<class CharSet>
char const*
find_if(
    char const* const first,
    char const* const last,
    CharSet const& cs) noexcept
{
    BOOST_STATIC_ASSERT(
        is_charset<CharSet>::value);
    return detail::find_if(first, last, cs,
        detail::has_find_if<CharSet>{});
}

/** Find the first character in the string that is not in CharSet

    @par Exception Safety
    Throws nothing.

    @return A pointer to the character if found,
    otherwise a pointer to `last`.

    @param first A pointer to the first character
    in the string to search.

    @param last A pointer to one past the last
    character in the string to search.

    @param cs The character set to use.

    @see
        @ref find_if_not.
*/
template<class CharSet>
char const*
find_if_not(
    char const* const first,
    char const* const last,
    CharSet const& cs) noexcept
{
    BOOST_STATIC_ASSERT(
        is_charset<CharSet>::value);
    return detail::find_if_not(first, last, cs,
        detail::has_find_if_not<CharSet>{});
}

} // bnf
} // urls
} // boost

#endif

max_stars_repo_name: sandmanhome/liuguang

max_issues_repo_name: sandmanhome/liuguang

max_forks_repo_name: sandmanhome/liuguang