// -*- coding: utf-8 -*-
// Copyright (C) 2006-2014 Rosen Diankov (rosen.diankov@gmail.com)
//
// This file is part of OpenRAVE.
// OpenRAVE 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 "libopenrave.h"
#include <algorithm>
#include <boost/algorithm/string.hpp> // boost::trim
#include <boost/lexical_cast.hpp>

#include "fparsermulti.h"

namespace OpenRAVE {

KinBody::JointInfo::JointInfo() : XMLReadable("joint"), _type(JointNone), _bIsActive(true) {
    for(size_t i = 0; i < _vaxes.size(); ++i) {
        _vaxes[i] = Vector(0,0,1);
    }
    std::fill(_vresolution.begin(), _vresolution.end(), 0.02);
    std::fill(_vmaxvel.begin(), _vmaxvel.end(), 10);
    std::fill(_vhardmaxvel.begin(), _vhardmaxvel.end(), 10);
    std::fill(_vmaxaccel.begin(), _vmaxaccel.end(), 50);
    std::fill(_vmaxtorque.begin(), _vmaxtorque.end(), 0); // set max torque to 0 to notify the system that dynamics parameters might not be valid.
    std::fill(_vmaxinertia.begin(), _vmaxinertia.end(), 0);
    std::fill(_vweights.begin(), _vweights.end(), 1);
    std::fill(_voffsets.begin(), _voffsets.end(), 0);
    std::fill(_vlowerlimit.begin(), _vlowerlimit.end(), 0);
    std::fill(_vupperlimit.begin(), _vupperlimit.end(), 0);
    std::fill(_bIsCircular.begin(), _bIsCircular.end(), 0);
}

static void fparser_polyroots2(vector<dReal>& rawroots, const vector<dReal>& rawcoeffs)
{
    BOOST_ASSERT(rawcoeffs.size()==3);
    int numroots=0;
    rawroots.resize(2);
    polyroots2<dReal>(&rawcoeffs[0],&rawroots[0],numroots);
    rawroots.resize(numroots);
}

template <int D>
static void fparser_polyroots(vector<dReal>& rawroots, const vector<dReal>& rawcoeffs)
{
    BOOST_ASSERT(rawcoeffs.size()==D+1);
    int numroots=0;
    rawroots.resize(D);
    polyroots<dReal,D>(&rawcoeffs[0],&rawroots[0],numroots);
    rawroots.resize(numroots);
}

// take triangle 3 sides and compute the angle opposite the first side
static void fparser_sssa(std::vector<dReal>& res, const vector<dReal>& coeffs)
{
    dReal a = coeffs.at(0), b = coeffs.at(1), c = coeffs.at(2);
    dReal f = (a*a+b*b-c*c)/(2*b);
    res.resize(1);
    res[0] = RaveAtan2(RaveSqrt(a*a-f*f),b-f);
}

/// take triangle 2 sides and an angle and compute the missing angle
static void fparser_sasa(std::vector<dReal>& res, const vector<dReal>& coeffs)
{
    dReal a = coeffs[0], gamma = coeffs[1], b = coeffs[2];
    res.resize(1);
    res[0] = RaveAtan2(a*RaveSin(gamma),b-a*RaveCos(gamma));
}

/// take triangle 2 sides and an angle and compute the missing side
static void fparser_sass(std::vector<dReal>& res, const vector<dReal>& coeffs)
{
    dReal a = coeffs[0], gamma = coeffs[1], b = coeffs[2];
    res.resize(1);
    res[0] = RaveSqrt(a*a+b*b-2*a*b*RaveCos(gamma));
}

OpenRAVEFunctionParserRealPtr CreateJointFunctionParser()
{
    OpenRAVEFunctionParserRealPtr parser(new OpenRAVEFunctionParserReal());
#ifdef OPENRAVE_FPARSER_SETEPSILON
    parser->setEpsilon(g_fEpsilonLinear);
#endif
    // register commonly used functions
    parser->AddBoostFunction("polyroots2",fparser_polyroots2,3);
    parser->AddBoostFunction("polyroots3",fparser_polyroots<3>,4);
    parser->AddBoostFunction("polyroots4",fparser_polyroots<4>,5);
    parser->AddBoostFunction("polyroots5",fparser_polyroots<5>,6);
    parser->AddBoostFunction("polyroots6",fparser_polyroots<6>,7);
    parser->AddBoostFunction("polyroots7",fparser_polyroots<7>,8);
    parser->AddBoostFunction("polyroots8",fparser_polyroots<8>,9);
    parser->AddBoostFunction("SSSA",fparser_sssa,3);
    parser->AddBoostFunction("SASA",fparser_sasa,3);
    parser->AddBoostFunction("SASS",fparser_sass,3);
    return parser;
}

KinBody::Joint::Joint(KinBodyPtr parent, KinBody::JointType type)
{
    _parent = parent;
    FOREACH(it,_doflastsetvalues) {
        *it = 0;
    }
    for(size_t i = 0; i < _vaxes.size(); ++i) {
        _vaxes[i] = Vector(0,0,1);
    }
    jointindex=-1;
    dofindex = -1; // invalid index
    _bInitialized = false;
    _info._type = type;
}

KinBody::Joint::~Joint()
{
}

int KinBody::Joint::GetDOF() const
{
    if( _info._type & KinBody::JointSpecialBit ) {
        switch(_info._type) {
        case KinBody::JointHinge2:
        case KinBody::JointUniversal: return 2;
        case KinBody::JointSpherical: return 3;
        case KinBody::JointTrajectory: return 1;
        default:
            throw OPENRAVE_EXCEPTION_FORMAT("invalid joint type 0x%x", _info._type, ORE_Failed);
        }
    }
    return int(_info._type & 0xf);
}

bool KinBody::Joint::IsCircular(int iaxis) const
{
    return static_cast<bool>(_info._bIsCircular.at(iaxis));
}

bool KinBody::Joint::IsRevolute(int iaxis) const
{
    if( _info._type & KinBody::JointSpecialBit ) {
        return _info._type == KinBody::JointHinge2 || _info._type == KinBody::JointUniversal;
    }
    return !(_info._type&(1<<(4+iaxis)));
}

bool KinBody::Joint::IsPrismatic(int iaxis) const
{
    if( _info._type & KinBody::JointSpecialBit ) {
        return false;
    }
    return !!(_info._type&(1<<(4+iaxis)));
}

bool KinBody::Joint::IsStatic() const
{
    if( IsMimic() ) {
        bool bstatic = true;
        KinBodyConstPtr parent(_parent);
        for(int i = 0; i < GetDOF(); ++i) {
            if( !!_vmimic.at(i) ) {
                FOREACHC(it, _vmimic.at(i)->_vmimicdofs) {
                    if( !parent->GetJointFromDOFIndex(it->dofindex)->IsStatic() ) {
                        bstatic = false;
                        break;
                    }
                }
                if( !bstatic ) {
                    break;
                }
            }
        }
        if( bstatic ) {
            return true;
        }
    }
    for(int i = 0; i < GetDOF(); ++i) {
        if( IsCircular(i) ) {
            return false;
        }
        if( _info._vlowerlimit.at(i) < _info._vupperlimit.at(i) ) {
            return false;
        }
    }
    return true;
}

void KinBody::Joint::GetValues(vector<dReal>& pValues, bool bAppend) const
{
    OPENRAVE_ASSERT_FORMAT0(_bInitialized, "joint not initialized",ORE_NotInitialized);
    if( !bAppend ) {
        pValues.resize(0);
    }
    if( GetDOF() == 1 ) {
        pValues.push_back(GetValue(0));
        return;
    }
    dReal f;
    Transform tjoint = _tinvLeft * _attachedbodies[0]->GetTransform().inverse() * _attachedbodies[1]->GetTransform() * _tinvRight;
    if( _info._type & KinBody::JointSpecialBit ) {
        switch(_info._type) {
        case KinBody::JointHinge2: {
            Vector axis1cur = tjoint.rotate(_vaxes[0]), axis2cur = tjoint.rotate(_vaxes[1]);
            Vector vec1, vec2, vec3;
            vec1 = (_vaxes[1] - _vaxes[0].dot3(_vaxes[1])*_vaxes[0]).normalize();
            vec2 = (axis2cur - _vaxes[0].dot3(axis2cur)*_vaxes[0]).normalize();
            vec3 = _vaxes[0].cross(vec1);
            f = 2.0*RaveAtan2(vec3.dot3(vec2), vec1.dot3(vec2));
            pValues.push_back(GetClosestValueAlongCircle(_info._voffsets[0]+f, _doflastsetvalues[0]));
            vec1 = (_vaxes[0] - axis2cur.dot(_vaxes[0])*axis2cur).normalize();
            vec2 = (axis1cur - axis2cur.dot(axis1cur)*axis2cur).normalize();
            vec3 = axis2cur.cross(vec1);
            f = 2.0*RaveAtan2(vec3.dot(vec2), vec1.dot(vec2));
            if( f < -PI ) {
                f += 2*PI;
            }
            else if( f > PI ) {
                f -= 2*PI;
            }
            pValues.push_back(GetClosestValueAlongCircle(_info._voffsets[1]+f, _doflastsetvalues[1]));
            break;
        }
        case KinBody::JointSpherical: {
            dReal fsinang2 = tjoint.rot.y*tjoint.rot.y+tjoint.rot.z*tjoint.rot.z+tjoint.rot.w*tjoint.rot.w;
            if( fsinang2 > 1e-10f ) {
                dReal fsinang = RaveSqrt(fsinang2);
                dReal fmult = 2*RaveAtan2(fsinang,tjoint.rot.x)/fsinang;
                pValues.push_back(tjoint.rot.y*fmult);
                pValues.push_back(tjoint.rot.z*fmult);
                pValues.push_back(tjoint.rot.w*fmult);
            }
            else {
                pValues.push_back(0);
                pValues.push_back(0);
                pValues.push_back(0);
            }
            break;
        }
        default:
            throw OPENRAVE_EXCEPTION_FORMAT("unknown joint type 0x%x", _info._type, ORE_Failed);
        }
    }
    else {
        // chain of revolute and prismatic joints
        for(int i = 0; i < GetDOF(); ++i) {
            Vector vaxis = _vaxes.at(i);
            if( IsRevolute(i) ) {
                if( i+1 < GetDOF() ) {
                    std::pair<dReal, Vector > res = normalizeAxisRotation(vaxis,tjoint.rot);
                    tjoint.rot = res.second;
                    if( res.first != 0 ) {
                        // could speed up by checking if trans is ever needed after this
                        tjoint.trans = quatRotate(quatFromAxisAngle(vaxis,res.first),tjoint.trans);
                    }
                    f = -res.first;
                }
                else {
                    f = 2.0f*RaveAtan2(tjoint.rot.y*vaxis.x+tjoint.rot.z*vaxis.y+tjoint.rot.w*vaxis.z, tjoint.rot.x);
                }
                // expect values to be within -PI to PI range
                if( f < -PI ) {
                    f += 2*PI;
                }
                else if( f > PI ) {
                    f -= 2*PI;
                }
                pValues.push_back(GetClosestValueAlongCircle(_info._voffsets[i]+f, _doflastsetvalues[i]));
            }
            else { // prismatic
                f = tjoint.trans.x*vaxis.x+tjoint.trans.y*vaxis.y+tjoint.trans.z*vaxis.z;
                pValues.push_back(_info._voffsets[i]+f);
                if( i+1 < GetDOF() ) {
                    tjoint.trans -= vaxis*f;
                }
            }
        }
    }
}

dReal KinBody::Joint::GetValue(int iaxis) const
{
    OPENRAVE_ASSERT_FORMAT0(_bInitialized, "joint not initialized",ORE_NotInitialized);
    dReal f;
    Transform tjoint = _tinvLeft * _attachedbodies[0]->GetTransform().inverse() * _attachedbodies[1]->GetTransform() * _tinvRight;
    if( _info._type & KinBody::JointSpecialBit ) {
        switch(_info._type) {
        case KinBody::JointHinge2: {
            Vector axis1cur = tjoint.rotate(_vaxes[0]), axis2cur = tjoint.rotate(_vaxes[1]);
            Vector vec1, vec2, vec3;
            if( iaxis == 0 ) {
                vec1 = (_vaxes[1] - _vaxes[0].dot3(_vaxes[1])*_vaxes[0]).normalize();
                vec2 = (axis2cur - _vaxes[0].dot3(axis2cur)*_vaxes[0]).normalize();
                vec3 = _vaxes[0].cross(vec1);
                f = 2.0*RaveAtan2(vec3.dot3(vec2), vec1.dot3(vec2));
                if( f < -PI ) {
                    f += 2*PI;
                }
                else if( f > PI ) {
                    f -= 2*PI;
                }
                return GetClosestValueAlongCircle(_info._voffsets[0]+f, _doflastsetvalues[0]);
            }
            else if( iaxis == 1 ) {
                vec1 = (_vaxes[0] - axis2cur.dot(_vaxes[0])*axis2cur).normalize();
                vec2 = (axis1cur - axis2cur.dot(axis1cur)*axis2cur).normalize();
                vec3 = axis2cur.cross(vec1);
                f = 2.0*RaveAtan2(vec3.dot(vec2), vec1.dot(vec2));
                if( f < -PI ) {
                    f += 2*PI;
                }
                else if( f > PI ) {
                    f -= 2*PI;
                }
                return GetClosestValueAlongCircle(_info._voffsets[1]+f, _doflastsetvalues[1]);
            }
            break;
        }
        case KinBody::JointSpherical: {
            dReal fsinang2 = tjoint.rot.y*tjoint.rot.y+tjoint.rot.z*tjoint.rot.z+tjoint.rot.w*tjoint.rot.w;
            if( fsinang2 > 1e-10f ) {
                dReal fsinang = RaveSqrt(fsinang2);
                dReal fmult = 2*RaveAtan2(fsinang,tjoint.rot.x)/fsinang;
                if( iaxis == 0 ) {
                    return tjoint.rot.y*fmult;
                }
                else if( iaxis == 1 ) {
                    return tjoint.rot.z*fmult;
                }
                else if( iaxis == 2 ) {
                    return tjoint.rot.w*fmult;
                }
            }
            else {
                if((iaxis >= 0)&&(iaxis < 3)) {
                    return 0;
                }
            }
            break;
        }
        case KinBody::JointTrajectory: {
            //uint64_t starttime = utils::GetMicroTime();
            vector<dReal> vsampledata;
            dReal splitpercentage = 0.01;
            dReal precision(1e-6);
            dReal timemin = 0, timemax = _info._trajfollow->GetDuration();
            Transform tbest, ttest;
            int totalcalls = 0;
            while(timemin+precision < timemax) {
                dReal timestep = (timemax-timemin)*splitpercentage;
                dReal timeclosest = timemin;
                dReal bestdist = 1e30, besttime=0;
                for(; timeclosest < timemax; timeclosest += timestep ) {
                    if( timeclosest > timemax ) {
                        timeclosest = timemax;
                    }
                    totalcalls += 1;
                    _info._trajfollow->Sample(vsampledata,timeclosest);
                    if( _info._trajfollow->GetConfigurationSpecification().ExtractTransform(ttest,vsampledata.begin(),KinBodyConstPtr()) ) {
                        dReal fdist = TransformDistanceFast(ttest,tjoint,0.3);
                        if( bestdist > fdist ) {
                            besttime = timeclosest;
                            bestdist = fdist;
                            tbest = ttest;
                        }
                    }
                }
                OPENRAVE_ASSERT_OP_FORMAT(bestdist, <, 1e30, "failed to compute trajectory value for joint %s\n",GetName(),ORE_Assert);
                timemin = max(timemin,besttime-timestep);
                timemax = min(timemax, besttime+timestep);
                splitpercentage = 0.1f;
                //RAVELOG_INFO(str(boost::format("calls: %d time: %f")%totalcalls%((utils::GetMicroTime()-starttime)*1e-6)));
            }
            return 0.5*(timemin+timemax);
        }
        default:
            break;
        }
    }
    else {
        if( _info._type == KinBody::JointPrismatic ) {
            return _info._voffsets[0]+(tjoint.trans.x*_vaxes[0].x+tjoint.trans.y*_vaxes[0].y+tjoint.trans.z*_vaxes[0].z);
        }
        else if( _info._type == KinBody::JointRevolute ) {
            f = 2.0f*RaveAtan2(tjoint.rot.y*_vaxes[0].x+tjoint.rot.z*_vaxes[0].y+tjoint.rot.w*_vaxes[0].z, tjoint.rot.x);
            // expect values to be within -PI to PI range
            if( f < -PI ) {
                f += 2*PI;
            }
            else if( f > PI ) {
                f -= 2*PI;
            }
            return GetClosestValueAlongCircle(_info._voffsets[0]+f, _doflastsetvalues[0]);
        }

        // chain of revolute and prismatic joints
        for(int i = 0; i < GetDOF(); ++i) {
            Vector vaxis = _vaxes.at(i);
            if( IsRevolute(i) ) {
                if( i+1 < GetDOF() ) {
                    std::pair<dReal, Vector > res = normalizeAxisRotation(vaxis,tjoint.rot);
                    tjoint.rot = res.second;
                    if( res.first != 0 ) {
                        // could speed up by checking if trans is ever needed after this
                        tjoint.trans = quatRotate(quatFromAxisAngle(vaxis,res.first),tjoint.trans);
                    }
                    f = -res.first;
                }
                else {
                    f = 2.0f*RaveAtan2(tjoint.rot.y*vaxis.x+tjoint.rot.z*vaxis.y+tjoint.rot.w*vaxis.z, tjoint.rot.x);
                }
                // expect values to be within -PI to PI range
                if( f < -PI ) {
                    f += 2*PI;
                }
                else if( f > PI ) {
                    f -= 2*PI;
                }
                if( i == iaxis ) {
                    return GetClosestValueAlongCircle(_info._voffsets[i]+f, _doflastsetvalues[i]);
                }
            }
            else { // prismatic
                f = tjoint.trans.x*vaxis.x+tjoint.trans.y*vaxis.y+tjoint.trans.z*vaxis.z;
                if( i == iaxis ) {
                    return _info._voffsets[i]+f;
                }
                if( i+1 < GetDOF() ) {
                    tjoint.trans -= vaxis*f;
                }
            }
        }
    }
    throw OPENRAVE_EXCEPTION_FORMAT("unknown joint type 0x%x axis %d\n", _info._type%iaxis, ORE_Failed);
}

void KinBody::Joint::GetVelocities(std::vector<dReal>& pVelocities, bool bAppend) const
{
    OPENRAVE_ASSERT_FORMAT0(_bInitialized, "joint not initialized",ORE_NotInitialized);
    if( !bAppend ) {
        pVelocities.resize(0);
    }
    if( GetDOF() == 1 ) {
        pVelocities.push_back(GetVelocity(0));
        return;
    }
    _GetVelocities(pVelocities,bAppend,_attachedbodies[0]->GetVelocity(), _attachedbodies[1]->GetVelocity());
};

dReal KinBody::Joint::GetVelocity(int axis) const
{
    OPENRAVE_ASSERT_FORMAT0(_bInitialized, "joint not initialized",ORE_NotInitialized);
    return _GetVelocity(axis,_attachedbodies[0]->GetVelocity(), _attachedbodies[1]->GetVelocity());
}

void KinBody::Joint::_GetVelocities(std::vector<dReal>& pVelocities, bool bAppend, const std::pair<Vector,Vector>& linkparentvelocity, const std::pair<Vector,Vector>& linkchildvelocity) const
{
    if( !bAppend ) {
        pVelocities.resize(0);
    }
    if( GetDOF() == 1 ) {
        pVelocities.push_back(GetVelocity(0));
        return;
    }
    const Transform& linkparenttransform = _attachedbodies[0]->_info._t;
    const Transform& linkchildtransform = _attachedbodies[1]->_info._t;
    Vector quatdelta = quatMultiply(linkparenttransform.rot,_tLeft.rot);
    Vector quatdeltainv = quatInverse(quatdelta);
    if( _info._type & KinBody::JointSpecialBit ) {
        switch(_info._type) {
        case KinBody::JointSpherical: {
            Vector v = quatRotate(quatdeltainv,linkchildvelocity.second-linkparentvelocity.second);
            pVelocities.push_back(v.x);
            pVelocities.push_back(v.y);
            pVelocities.push_back(v.z);
            break;
        }
        default:
            throw OPENRAVE_EXCEPTION_FORMAT("unknown joint type 0x%x", _info._type, ORE_InvalidArguments);
        }
    }
    else {
        // chain of revolute and prismatic joints
        Vector angvelocitycovered, linvelocitycovered;
        for(int i = 0; i < GetDOF(); ++i) {
            if( IsRevolute(i) ) {
                pVelocities.push_back(_vaxes[i].dot3(quatRotate(quatdeltainv,linkchildvelocity.second-linkparentvelocity.second-angvelocitycovered)));
                angvelocitycovered += quatRotate(quatdelta,_vaxes[i]*pVelocities.back());
            }
            else { // prismatic
                pVelocities.push_back(_vaxes[i].dot3(quatRotate(quatdeltainv,linkchildvelocity.first-linkparentvelocity.first-(linkparentvelocity.second-angvelocitycovered).cross(linkchildtransform.trans-linkparenttransform.trans)-linvelocitycovered)));
                linvelocitycovered += quatRotate(quatdelta,_vaxes[i]*pVelocities.back());
            }
        }
    }
}

dReal KinBody::Joint::_GetVelocity(int axis, const std::pair<Vector,Vector>&linkparentvelocity, const std::pair<Vector,Vector>&linkchildvelocity) const
{
    const Transform& linkparenttransform = _attachedbodies[0]->_info._t;
    const Transform& linkchildtransform = _attachedbodies[1]->_info._t;
    Vector quatdelta = quatMultiply(linkparenttransform.rot,_tLeft.rot);
    Vector quatdeltainv = quatInverse(quatdelta);
    if( _info._type & KinBody::JointSpecialBit ) {
        switch(_info._type) {
        case KinBody::JointSpherical: {
            Vector v = quatRotate(quatdeltainv,linkchildvelocity.second-linkparentvelocity.second);
            return v[axis];
        }
        default:
            throw OPENRAVE_EXCEPTION_FORMAT("unknown joint type 0x%x", _info._type, ORE_InvalidArguments);
        }
    }
    else {
        if( _info._type == KinBody::JointPrismatic ) {
            return _vaxes[0].dot3(quatRotate(quatdeltainv,linkchildvelocity.first-linkparentvelocity.first-linkparentvelocity.second.cross(linkchildtransform.trans-linkparenttransform.trans)));
        }
        else if( _info._type == KinBody::JointRevolute ) {
            return _vaxes[0].dot3(quatRotate(quatdeltainv,linkchildvelocity.second-linkparentvelocity.second));
        }
        else {
            // chain of revolute and prismatic joints
            Vector angvelocitycovered, linvelocitycovered;
            for(int i = 0; i < GetDOF(); ++i) {
                if( IsRevolute(i) ) {
                    dReal fvelocity = _vaxes[i].dot3(quatRotate(quatdeltainv,linkchildvelocity.second-linkparentvelocity.second-angvelocitycovered));
                    if( i == axis ) {
                        return fvelocity;
                    }
                    angvelocitycovered += quatRotate(quatdelta,_vaxes[i]*fvelocity);
                }
                else { // prismatic
                    dReal fvelocity = _vaxes[i].dot3(quatRotate(quatdeltainv,linkchildvelocity.first-linkparentvelocity.first-(linkparentvelocity.second-angvelocitycovered).cross(linkparenttransform.trans-linkchildtransform.trans)-linvelocitycovered));
                    if( i == axis ) {
                        return fvelocity;
                    }
                    linvelocitycovered += quatRotate(quatdelta,_vaxes[i]*fvelocity);
                }
            }
        }
    }
    throw OPENRAVE_EXCEPTION_FORMAT("unsupported joint type 0x%x", _info._type, ORE_InvalidArguments);
}

Vector KinBody::Joint::GetAnchor() const
{
    return _attachedbodies[0]->GetTransform() * _tLeft.trans;
}

Vector KinBody::Joint::GetAxis(int iaxis) const
{
    return _attachedbodies[0]->GetTransform().rotate(_tLeft.rotate(_vaxes.at(iaxis)));
}

void KinBody::Joint::_ComputeInternalInformation(LinkPtr plink0, LinkPtr plink1, const Vector& vanchorraw, const std::vector<Vector>& vaxes, const std::vector<dReal>& vcurrentvalues)
{
    OPENRAVE_ASSERT_OP_FORMAT(!!plink0,&&,!!plink1, "one or more attached _attachedbodies are invalid for joint %s", GetName(),ORE_InvalidArguments);
    for(int i = 0; i < GetDOF(); ++i) {
        OPENRAVE_ASSERT_OP_FORMAT(_info._vmaxvel[i], >=, 0, "joint %s[%d] max velocity is invalid",_info._name%i, ORE_InvalidArguments);
        OPENRAVE_ASSERT_OP_FORMAT(_info._vmaxaccel[i], >=, 0, "joint %s[%d] max acceleration is invalid",_info._name%i, ORE_InvalidArguments);
        OPENRAVE_ASSERT_OP_FORMAT(_info._vmaxtorque[i], >=, 0, "joint %s[%d] max torque is invalid",_info._name%i, ORE_InvalidArguments);
        OPENRAVE_ASSERT_OP_FORMAT(_info._vmaxinertia[i], >=, 0, "joint %s[%d] max inertia is invalid",_info._name%i, ORE_InvalidArguments);
    }

    KinBodyPtr parent(_parent);
    _bInitialized = false;
    _attachedbodies[0] = plink0;
    _attachedbodies[1] = plink1;
    Transform trel, tbody0, tbody1;
    Vector vanchor=vanchorraw;
    for(size_t i = 0; i < vaxes.size(); ++i) {
        _vaxes[i] = vaxes[i];
    }
    // make sure first body is always closer to the root, unless the second body is static and the first body is not the root link
    if( _attachedbodies[1]->IsStatic() && _attachedbodies[0]->GetIndex() > 0) {
        if( !_attachedbodies[0]->IsStatic() ) {
            Transform tswap = plink1->GetTransform().inverse() * plink0->GetTransform();
            for(int i = 0; i < GetDOF(); ++i) {
                _vaxes[i] = -tswap.rotate(_vaxes[i]);
            }
            vanchor = tswap*vanchor;
            swap(_attachedbodies[0],_attachedbodies[1]);
        }
    }

    // update _info
    for(size_t i = 0; i < vaxes.size(); ++i) {
        _info._vaxes[i] = _vaxes[i];
    }
    _info._vanchor = vanchor;

    tbody0 = _attachedbodies[0]->GetTransform();
    tbody1 = _attachedbodies[1]->GetTransform();
    trel = tbody0.inverse() * tbody1;
    _tLeft = Transform();
    _tLeftNoOffset = Transform();
    _tRight = Transform();
    _tRightNoOffset = Transform();

    if( _info._type & KinBody::JointSpecialBit ) {
        switch(_info._type) {
        case KinBody::JointUniversal:
            _tLeft.trans = vanchor;
            _tRight.trans = -vanchor;
            _tRight = _tRight * trel;
            OPENRAVE_ASSERT_OP((int)vaxes.size(),==,2);
            break;
        case KinBody::JointHinge2:
            _tLeft.trans = vanchor;
            _tRight.trans = -vanchor;
            _tRight = _tRight * trel;
            OPENRAVE_ASSERT_OP((int)vaxes.size(),==,2);
            break;
        case KinBody::JointSpherical:
            _tLeft.trans = vanchor;
            _tRight.trans = -vanchor;
            _tRight = _tRight * trel;
            break;
        case KinBody::JointTrajectory:
            if( !_info._trajfollow ) {
                throw OPENRAVE_EXCEPTION_FORMAT0("trajectory joint requires Joint::_trajfollow to be initialized",ORE_InvalidState);
            }
            _tRight = _tRight * trel;
            break;
        default:
            throw OPENRAVE_EXCEPTION_FORMAT("unrecognized joint type 0x%x", _info._type, ORE_InvalidArguments);
        }
        _tLeftNoOffset = _tLeft;
        _tRightNoOffset = _tRight;
    }
    else {
        OPENRAVE_ASSERT_OP((int)vaxes.size(),==,GetDOF());
        _tLeftNoOffset.trans = vanchor;
        _tRightNoOffset.trans = -vanchor;
        _tRightNoOffset = _tRightNoOffset * trel;
        if( GetDOF() == 1 ) {
            // in the case of one axis, create a new coordinate system such that the axis rotates about (0,0,1)
            // this is necessary in order to simplify the rotation matrices (for future symbolic computation)
            // and suppress any floating-point error. The data structures are only setup for this to work in 1 DOF.
            Transform trot; trot.rot = quatRotateDirection(_vaxes[0],Vector(0,0,1));
            _tLeftNoOffset = _tLeftNoOffset * trot.inverse();
            _tRightNoOffset = trot*_tRightNoOffset;
            _vaxes[0] = Vector(0,0,1);
        }

        Transform toffset;
        if( IsRevolute(0) ) {
            toffset.rot = quatFromAxisAngle(_vaxes[0], _info._voffsets[0]);
        }
        else {
            toffset.trans = _vaxes[0]*_info._voffsets[0];
        }
        _tLeft = _tLeftNoOffset * toffset;
        _tRight = _tRightNoOffset;
        if( GetDOF() > 1 ) {
            // right multiply by the offset of the last axis, might be buggy?
            if( IsRevolute(GetDOF()-1) ) {
                _tRight = matrixFromAxisAngle(_vaxes[GetDOF()-1], _info._voffsets[GetDOF()-1]) * _tRight;
            }
            else {
                _tRight.trans += _vaxes[GetDOF()-1]*_info._voffsets[GetDOF()-1];
            }
        }
    }

    if( vcurrentvalues.size() > 0 ) {
        // see if any joints have offsets
        Transform toffset;
        if( _info._type == KinBody::JointTrajectory ) {
            vector<dReal> vsampledata;
            Transform t0, t1;
            _info._trajfollow->Sample(vsampledata,0);
            if( !_info._trajfollow->GetConfigurationSpecification().ExtractTransform(t0,vsampledata.begin(),KinBodyConstPtr()) ) {
                throw OPENRAVE_EXCEPTION_FORMAT("failed to sample trajectory for joint %s",GetName(),ORE_Assert);
            }
            _info._trajfollow->Sample(vsampledata,vcurrentvalues.at(0));
            if( !_info._trajfollow->GetConfigurationSpecification().ExtractTransform(t1,vsampledata.begin(),KinBodyConstPtr()) ) {
                throw OPENRAVE_EXCEPTION_FORMAT("failed to sample trajectory for joint %s",GetName(),ORE_Assert);
            }
            toffset = t0*t1.inverse();
        }
        else if( !(_info._type&KinBody::JointSpecialBit) || _info._type == KinBody::JointUniversal || _info._type == KinBody::JointHinge2 ) {
            if( IsRevolute(0) ) {
                toffset.rot = quatFromAxisAngle(_vaxes[0], -vcurrentvalues[0]);
            }
            else {
                toffset.trans = -_vaxes[0]*vcurrentvalues[0];
            }
        }
        _tLeftNoOffset *= toffset;
        _tLeft *= toffset;
        if( vcurrentvalues.size() > 1 ) {
            if( IsRevolute(GetDOF()-1) ) {
                toffset.rot = quatFromAxisAngle(_vaxes[GetDOF()-1], -vcurrentvalues.at(GetDOF()-1));
            }
            else {
                toffset.trans = -_vaxes[GetDOF()-1]*vcurrentvalues.at(GetDOF()-1);
            }
            _tRightNoOffset = toffset * _tRightNoOffset;
            _tRight = toffset * _tRight;
        }
    }
    _tinvRight = _tRight.inverse();
    _tinvLeft = _tLeft.inverse();

    _vcircularlowerlimit = _info._vlowerlimit;
    _vcircularupperlimit = _info._vupperlimit;
    for(int i = 0; i < GetDOF(); ++i) {
        if( IsCircular(i) ) {
            // can rotate forever, so don't limit it. Unfortunately if numbers are too big precision will start getting lost
            _info._vlowerlimit.at(i) = -1e4;
            _info._vupperlimit.at(i) = 1e4;
        }
    }

    if( !!_attachedbodies[0] ) {
        _info._linkname0 = _attachedbodies[0]->GetName();
    }
    else {
        _info._linkname0.clear();
    }
    if( !!_attachedbodies[1] ) {
        _info._linkname1 = _attachedbodies[1]->GetName();
    }
    else {
        _info._linkname1.clear();
    }
    _info._vcurrentvalues = vcurrentvalues;

    _bInitialized = true;

    if( _attachedbodies[1]->IsStatic() && !IsStatic() ) {
        RAVELOG_WARN(str(boost::format("joint %s: all attached links are static, but joint is not!\n")%GetName()));
    }
}

KinBody::LinkPtr KinBody::Joint::GetHierarchyParentLink() const
{
    return _attachedbodies[0];
}

KinBody::LinkPtr KinBody::Joint::GetHierarchyChildLink() const
{
    return _attachedbodies[1];
}

Vector KinBody::Joint::GetInternalHierarchyAxis(int iaxis) const
{
    return _vaxes.at(iaxis);
}

Transform KinBody::Joint::GetInternalHierarchyLeftTransform() const
{
    OPENRAVE_ASSERT_FORMAT0(_bInitialized, "joint not initialized",ORE_NotInitialized);
    return _tLeftNoOffset;
}

Transform KinBody::Joint::GetInternalHierarchyRightTransform() const
{
    OPENRAVE_ASSERT_FORMAT0(_bInitialized, "joint not initialized",ORE_NotInitialized);
    return _tRightNoOffset;
}

void KinBody::Joint::GetLimits(std::vector<dReal>& vLowerLimit, std::vector<dReal>& vUpperLimit, bool bAppend) const
{
    if( !bAppend ) {
        vLowerLimit.resize(0);
        vUpperLimit.resize(0);
    }
    for(int i = 0; i < GetDOF(); ++i) {
        vLowerLimit.push_back(_info._vlowerlimit[i]);
        vUpperLimit.push_back(_info._vupperlimit[i]);
    }
}

std::pair<dReal, dReal> KinBody::Joint::GetLimit(int iaxis) const
{
    return make_pair(_info._vlowerlimit.at(iaxis),_info._vupperlimit.at(iaxis));
}

void KinBody::Joint::SetLimits(const std::vector<dReal>& vLowerLimit, const std::vector<dReal>& vUpperLimit)
{
    bool bChanged = false;
    for(int i = 0; i < GetDOF(); ++i) {
        if( _info._vlowerlimit[i] != vLowerLimit.at(i) || _info._vupperlimit[i] != vUpperLimit.at(i) ) {
            bChanged = true;
            _info._vlowerlimit[i] = vLowerLimit.at(i);
            _info._vupperlimit[i] = vUpperLimit.at(i);
            if( IsRevolute(i) && !IsCircular(i) ) {
                // TODO, necessary to set wrap?
                if( _info._vlowerlimit[i] < -PI || _info._vupperlimit[i] > PI) {
                    SetWrapOffset(0.5f * (_info._vlowerlimit.at(i) + _info._vupperlimit.at(i)),i);
                }
                else {
                    SetWrapOffset(0,i);
                }
            }
        }
    }
    if( bChanged ) {
        GetParent()->_PostprocessChangedParameters(Prop_JointLimits);
    }
}

void KinBody::Joint::GetVelocityLimits(std::vector<dReal>& vlower, std::vector<dReal>& vupper, bool bAppend) const
{
    if( !bAppend ) {
        vlower.resize(0);
        vupper.resize(0);
    }
    for(int i = 0; i < GetDOF(); ++i) {
        vlower.push_back(-_info._vmaxvel[i]);
        vupper.push_back(_info._vmaxvel[i]);
    }
}

void KinBody::Joint::GetVelocityLimits(std::vector<dReal>& vmax, bool bAppend) const
{
    if( !bAppend ) {
        vmax.resize(0);
    }
    for(int i = 0; i < GetDOF(); ++i) {
        vmax.push_back(_info._vmaxvel[i]);
    }
}

std::pair<dReal, dReal> KinBody::Joint::GetVelocityLimit(int iaxis) const
{
    return make_pair(-_info._vmaxvel.at(iaxis), _info._vmaxvel.at(iaxis));
}

void KinBody::Joint::SetVelocityLimits(const std::vector<dReal>& vmaxvel)
{
    for(int i = 0; i < GetDOF(); ++i) {
        _info._vmaxvel[i] = vmaxvel.at(i);
    }
    GetParent()->_PostprocessChangedParameters(Prop_JointAccelerationVelocityTorqueLimits);
}

void KinBody::Joint::GetAccelerationLimits(std::vector<dReal>& vmax, bool bAppend) const
{
    if( !bAppend ) {
        vmax.resize(0);
    }
    for(int i = 0; i < GetDOF(); ++i) {
        vmax.push_back(_info._vmaxaccel[i]);
    }
}

dReal KinBody::Joint::GetAccelerationLimit(int iaxis) const
{
    return _info._vmaxaccel.at(iaxis);
}

void KinBody::Joint::SetAccelerationLimits(const std::vector<dReal>& vmax)
{
    for(int i = 0; i < GetDOF(); ++i) {
        _info._vmaxaccel[i] = vmax.at(i);
    }
    GetParent()->_PostprocessChangedParameters(Prop_JointAccelerationVelocityTorqueLimits);
}

void KinBody::Joint::GetTorqueLimits(std::vector<dReal>& vmax, bool bAppend) const
{
    if( !bAppend ) {
        vmax.resize(0);
    }
    for(int i = 0; i < GetDOF(); ++i) {
        vmax.push_back(_info._vmaxtorque[i]);
    }
}

void KinBody::Joint::SetTorqueLimits(const std::vector<dReal>& vmax)
{
    for(int i = 0; i < GetDOF(); ++i) {
        _info._vmaxtorque[i] = vmax.at(i);
    }
    GetParent()->_PostprocessChangedParameters(Prop_JointAccelerationVelocityTorqueLimits);
}

void KinBody::Joint::GetInertiaLimits(std::vector<dReal>& vmax, bool bAppend) const
{
    if( !bAppend ) {
        vmax.resize(0);
    }
    for(int i = 0; i < GetDOF(); ++i) {
        vmax.push_back(_info._vmaxinertia[i]);
    }
}

void KinBody::Joint::SetInertiaLimits(const std::vector<dReal>& vmax)
{
    for(int i = 0; i < GetDOF(); ++i) {
        _info._vmaxinertia[i] = vmax.at(i);
    }
    GetParent()->_PostprocessChangedParameters(Prop_JointAccelerationVelocityTorqueLimits);
}

void KinBody::Joint::SetWrapOffset(dReal newoffset, int iaxis)
{
    if( _info._voffsets.at(iaxis) != newoffset ) {
        _info._voffsets.at(iaxis) = newoffset;
        if( iaxis == 0 ) {
            Transform toffset;
            if( IsRevolute(0) ) {
                toffset.rot = quatFromAxisAngle(_vaxes[0], newoffset);
            }
            else {
                toffset.trans = _vaxes[0]*newoffset;
            }
            _tLeft = _tLeftNoOffset * toffset;
            _tinvLeft = _tLeft.inverse();
        }
        if(GetDOF() > 1 && iaxis==GetDOF()-1 ) {
            _tRight = _tRightNoOffset;
            // right multiply by the offset of the last axis, might be buggy?
            if( IsRevolute(GetDOF()-1) ) {
                _tRight = matrixFromAxisAngle(_vaxes[GetDOF()-1], newoffset) * _tRight;
            }
            else {
                _tRight.trans += _vaxes[GetDOF()-1]*newoffset;
            }
            _tinvRight = _tRight.inverse();
        }
        GetParent()->_PostprocessChangedParameters(Prop_JointOffset);
    }
}

void KinBody::Joint::GetResolutions(std::vector<dReal>& resolutions, bool bAppend) const
{
    if( !bAppend ) {
        resolutions.resize(GetDOF());
    }
    for(int i = 0; i < GetDOF(); ++i) {
        resolutions.push_back(_info._vresolution[i]);
    }
}

dReal KinBody::Joint::GetResolution(int iaxis) const
{
    return _info._vresolution.at(iaxis);
}

void KinBody::Joint::SetResolution(dReal resolution, int iaxis)
{
    _info._vresolution.at(iaxis) = resolution;
    GetParent()->_PostprocessChangedParameters(Prop_JointProperties);
}

void KinBody::Joint::GetWeights(std::vector<dReal>& weights, bool bAppend) const
{
    if( !bAppend ) {
        weights.resize(GetDOF());
    }
    for(int i = 0; i < GetDOF(); ++i) {
        weights.push_back(_info._vweights[i]);
    }
}

dReal KinBody::Joint::GetWeight(int iaxis) const
{
    return _info._vweights.at(iaxis);
}

void KinBody::Joint::SetWeights(const std::vector<dReal>& vweights)
{
    for(int i = 0; i < GetDOF(); ++i) {
        OPENRAVE_ASSERT_OP(vweights.at(i),>,0);
        _info._vweights[i] = vweights.at(i);
    }
    GetParent()->_PostprocessChangedParameters(Prop_JointProperties);
}

void KinBody::Joint::SubtractValues(std::vector<dReal>& q1, const std::vector<dReal>& q2) const
{
    for(int i = 0; i < GetDOF(); ++i) {
        if( IsCircular(i) ) {
            q1.at(i) = utils::NormalizeCircularAngle(q1.at(i)-q2.at(i),_vcircularlowerlimit.at(i),_vcircularupperlimit.at(i));
        }
        else {
            q1.at(i) -= q2.at(i);
        }
    }
}

dReal KinBody::Joint::SubtractValue(dReal value1, dReal value2, int iaxis) const
{
    if( IsCircular(iaxis) ) {
        return utils::NormalizeCircularAngle(value1-value2,_vcircularlowerlimit.at(iaxis),_vcircularupperlimit.at(iaxis));
    }
    else {
        return value1-value2;
    }
}

void KinBody::Joint::AddTorque(const std::vector<dReal>& pTorques)
{
    GetParent()->GetEnv()->GetPhysicsEngine()->AddJointTorque(shared_from_this(), pTorques);
}

int KinBody::Joint::GetMimicJointIndex() const
{
    for(int i = 0; i < GetDOF(); ++i) {
        if( !!_vmimic.at(i) &&(_vmimic.at(i)->_vmimicdofs.size() > 0)) {
            return GetParent()->GetJointFromDOFIndex(_vmimic.at(i)->_vmimicdofs.front().dofindex)->GetJointIndex();
        }
    }
    return -1;
}

const std::vector<dReal> KinBody::Joint::GetMimicCoeffs() const
{
    RAVELOG_WARN("deprecated KinBody::Joint::GetMimicCoeffs(): could not deduce coeffs\n");
    std::vector<dReal> coeffs(2); coeffs[0] = 1; coeffs[1] = 0;
    return coeffs;
}

bool KinBody::Joint::IsMimic(int iaxis) const
{
    if( iaxis >= 0 ) {
        return !!_vmimic.at(iaxis);
    }
    for(int i = 0; i < GetDOF(); ++i) {
        if( !!_vmimic.at(i) ) {
            return true;
        }
    }
    return false;
}

std::string KinBody::Joint::GetMimicEquation(int iaxis, int itype, const std::string& format) const
{
    if( !_vmimic.at(iaxis) ) {
        return "";
    }
    if((format.size() == 0)||(format == "fparser")) {
        return _vmimic.at(iaxis)->_equations.at(itype);
    }
    else if( format == "mathml" ) {
        boost::format mathfmt("<math xmlns=\"http://www.w3.org/1998/Math/MathML\">\n%s</math>\n");
        std::vector<std::string> Vars;
        std::string sout;
        KinBodyConstPtr parent(_parent);
        FOREACHC(itdofformat, _vmimic.at(iaxis)->_vdofformat) {
            JointConstPtr pjoint = itdofformat->GetJoint(parent);
            if( pjoint->GetDOF() > 1 ) {
                Vars.push_back(str(boost::format("<csymbol>%s_%d</csymbol>")%pjoint->GetName()%(int)itdofformat->axis));
            }
            else {
                Vars.push_back(str(boost::format("<csymbol>%s</csymbol>")%pjoint->GetName()));
            }
        }
        if( itype == 0 ) {
            _vmimic.at(iaxis)->_posfn->toMathML(sout,Vars);
            if((sout.size() > 9)&&(sout.substr(0,9) == "<csymbol>")) {
                // due to a bug in ROS robot_model, have to return with <apply> (remove this in 2012).
                sout = boost::str(boost::format("<apply>\n  <plus/><cn type=\"real\">0</cn>\n  %s\n  </apply>")%sout);
            }
            sout = str(mathfmt%sout);
        }
        else if( itype == 1 ) {
            std::string stemp;
            FOREACHC(itfn, _vmimic.at(iaxis)->_velfns) {
                (*itfn)->toMathML(stemp,Vars);
                sout += str(mathfmt%stemp);
            }
        }
        else if( itype == 2 ) {
            std::string stemp;
            FOREACHC(itfn, _vmimic.at(iaxis)->_accelfns) {
                (*itfn)->toMathML(stemp,Vars);
                sout += str(mathfmt%stemp);
            }
        }
        return sout;
    }
    throw OPENRAVE_EXCEPTION_FORMAT("unsupported math format %s", format, ORE_InvalidArguments);
}

void KinBody::Joint::GetMimicDOFIndices(std::vector<int>& vmimicdofs, int iaxis) const
{
    OPENRAVE_ASSERT_FORMAT(!!_vmimic.at(iaxis), "joint %s axis %d is not mimic", GetName()%iaxis,ORE_InvalidArguments);
    vmimicdofs.resize(0);
    FOREACHC(it, _vmimic.at(iaxis)->_vmimicdofs) {
        std::vector<int>::iterator itinsert = std::lower_bound(vmimicdofs.begin(),vmimicdofs.end(), it->dofindex);
        if((itinsert == vmimicdofs.end())||(*itinsert != it->dofindex)) {
            vmimicdofs.insert(itinsert,it->dofindex);
        }
    }
}

void KinBody::Joint::SetMimicEquations(int iaxis, const std::string& poseq, const std::string& veleq, const std::string& acceleq)
{
    _vmimic.at(iaxis).reset();
    if( poseq.size() == 0 ) {
        return;
    }
    KinBodyPtr parent(_parent);
    std::vector<std::string> resultVars;
    MimicPtr mimic(new Mimic());
    mimic->_equations.at(0) = poseq;
    mimic->_equations.at(1) = veleq;
    mimic->_equations.at(2) = acceleq;

    // copy equations into the info
    if( !_info._vmimic.at(iaxis) ) {
        _info._vmimic.at(iaxis).reset(new MimicInfo());
    }
    _info._vmimic.at(iaxis)->_equations = mimic->_equations;

    OpenRAVEFunctionParserRealPtr posfn = CreateJointFunctionParser();
    mimic->_posfn = posfn;
    // because openrave joint names can hold symbols like '-' and '.' can affect the equation, so first do a search and replace
    std::vector< std::pair<std::string, std::string> > jointnamepairs; jointnamepairs.reserve(parent->GetJoints().size());
    FOREACHC(itjoint,parent->GetJoints()) {
        if( (*itjoint)->GetName().size() > 0 ) {
            std::string newname = str(boost::format("joint%d")%(*itjoint)->GetJointIndex());
            jointnamepairs.push_back(make_pair((*itjoint)->GetName(),newname));
        }
    }
    size_t index = parent->GetJoints().size();
    FOREACHC(itjoint,parent->GetPassiveJoints()) {
        if( (*itjoint)->GetName().size() > 0 ) {
            std::string newname = str(boost::format("joint%d")%index);
            jointnamepairs.push_back(make_pair((*itjoint)->GetName(),newname));
        }
        ++index;
    }

    std::map<std::string,std::string> mapinvnames;
    FOREACH(itpair,jointnamepairs) {
        mapinvnames[itpair->second] = itpair->first;
    }

    std::string eq;
    int ret = posfn->ParseAndDeduceVariables(utils::SearchAndReplace(eq,mimic->_equations[0],jointnamepairs),resultVars);
    if( ret >= 0 ) {
        throw OPENRAVE_EXCEPTION_FORMAT("failed to set equation '%s' on %s:%s, at %d. Error is %s\n", mimic->_equations[0]%parent->GetName()%GetName()%ret%posfn->ErrorMsg(),ORE_InvalidArguments);
    }
    // process the variables
    FOREACH(itvar,resultVars) {
        OPENRAVE_ASSERT_FORMAT(itvar->find("joint") == 0, "equation '%s' uses unknown variable", mimic->_equations[0], ORE_InvalidArguments);
        MIMIC::DOFFormat dofformat;
        size_t axisindex = itvar->find('_');
        if( axisindex != std::string::npos ) {
            dofformat.jointindex = boost::lexical_cast<uint16_t>(itvar->substr(5,axisindex-5));
            dofformat.axis = boost::lexical_cast<uint8_t>(itvar->substr(axisindex+1));
        }
        else {
            dofformat.jointindex = boost::lexical_cast<uint16_t>(itvar->substr(5));
            dofformat.axis = 0;
        }
        dofformat.dofindex = -1;
        JointPtr pjoint = dofformat.GetJoint(parent);
        if((pjoint->GetDOFIndex() >= 0)&& !pjoint->IsMimic(dofformat.axis) ) {
            dofformat.dofindex = pjoint->GetDOFIndex()+dofformat.axis;
            MIMIC::DOFHierarchy h;
            h.dofindex = dofformat.dofindex;
            h.dofformatindex = mimic->_vdofformat.size();
            mimic->_vmimicdofs.push_back(h);
        }
        mimic->_vdofformat.push_back(dofformat);
    }

    // need to set sVars to resultVars since that's what the user will be feeding with the input
    stringstream sVars;
    if( !resultVars.empty() ) {
        sVars << resultVars.at(0);
        for(size_t i = 1; i < resultVars.size(); ++i) {
            sVars << "," << resultVars[i];
        }
    }

    for(int itype = 1; itype < 3; ++itype) {
        if((itype == 2)&&(mimic->_equations[itype].size() == 0)) {
            continue;
        }

        std::vector<OpenRAVEFunctionParserRealPtr> vfns(resultVars.size());
        // extract the equations
        utils::SearchAndReplace(eq,mimic->_equations[itype],jointnamepairs);
        size_t index = eq.find('|');
        while(index != std::string::npos) {
            size_t startindex = index+1;
            index = eq.find('|',startindex);
            string sequation;
            if( index != std::string::npos) {
                sequation = eq.substr(startindex,index-startindex);
            }
            else {
                sequation = eq.substr(startindex);
            }
            boost::trim(sequation);
            size_t nameendindex = sequation.find(' ');
            string varname;
            if( nameendindex == std::string::npos ) {
                RAVELOG_WARN(str(boost::format("invalid equation syntax '%s' for joint %s")%sequation%_info._name));
                varname = sequation;
                sequation = "0";
            }
            else {
                varname = sequation.substr(0,nameendindex);
                sequation = sequation.substr(nameendindex);
            }
            vector<string>::iterator itnameindex = find(resultVars.begin(),resultVars.end(),varname);
            OPENRAVE_ASSERT_FORMAT(itnameindex != resultVars.end(), "variable %s from velocity equation is not referenced in the position, skipping...", mapinvnames[varname],ORE_InvalidArguments);

            OpenRAVEFunctionParserRealPtr fn = CreateJointFunctionParser();
            ret = fn->Parse(sequation,sVars.str());
            if( ret >= 0 ) {
                throw OPENRAVE_EXCEPTION_FORMAT("failed to set equation '%s' on %s:%s, at %d. Error is %s", sequation%parent->GetName()%GetName()%ret%fn->ErrorMsg(),ORE_InvalidArguments);
            }
            vfns.at(itnameindex-resultVars.begin()) = fn;
        }
        // check if anything is missing
        for(size_t j = 0; j < resultVars.size(); ++j) {
            if( !vfns[j] ) {
                // print a message instead of throwing an exception since it might be common for only position equations to be specified
                RAVELOG_WARN(str(boost::format("SetMimicEquations: missing variable %s from partial derivatives of joint %s!")%mapinvnames[resultVars[j]]%_info._name));
                vfns[j] = CreateJointFunctionParser();
                vfns[j]->Parse("0","");
            }
        }

        if( itype == 1 ) {
            mimic->_velfns.swap(vfns);
        }
        else {
            mimic->_accelfns.swap(vfns);
        }
    }
    _vmimic.at(iaxis) = mimic;
    parent->_PostprocessChangedParameters(Prop_JointMimic);
}

void KinBody::Joint::_ComputePartialVelocities(std::vector<std::pair<int,dReal> >& vpartials, int iaxis, std::map< std::pair<MIMIC::DOFFormat, int>, dReal >& mapcachedpartials) const
{
    vpartials.resize(0);
    if( dofindex >= 0 ) {
        vpartials.push_back(make_pair(dofindex+iaxis,1.0));
        return;
    }
    OPENRAVE_ASSERT_FORMAT(!!_vmimic.at(iaxis), "cannot compute partial velocities of joint %s", _info._name, ORE_Failed);
    KinBodyConstPtr parent(_parent);
    MIMIC::DOFFormat thisdofformat;
    thisdofformat.dofindex = -1; // always -1 since it is mimiced
    thisdofformat.axis = iaxis;
    thisdofformat.jointindex = jointindex;
    if( jointindex < 0 ) {
        // this is a weird computation... have to figure out the passive joint index given where it is in parent->GetPassiveJoints()
        thisdofformat.jointindex = parent->GetJoints().size() + (find(parent->GetPassiveJoints().begin(),parent->GetPassiveJoints().end(),shared_from_this()) - parent->GetPassiveJoints().begin());
    }
    std::vector<std::pair<int,dReal> > vtemppartials;
    vector<dReal> vtempvalues;
    FOREACHC(itmimicdof, _vmimic[iaxis]->_vmimicdofs) {
        std::pair<MIMIC::DOFFormat, int> key = make_pair(thisdofformat,itmimicdof->dofindex);
        std::map< std::pair<MIMIC::DOFFormat, int>, dReal >::iterator it = mapcachedpartials.find(key);
        if( it == mapcachedpartials.end() ) {
            // not in the cache so compute using the chain rule
            if( vtempvalues.empty() ) {
                FOREACHC(itdofformat, _vmimic[iaxis]->_vdofformat) {
                    vtempvalues.push_back(itdofformat->GetJoint(parent)->GetValue(itdofformat->axis));
                }
            }
            dReal fvel = _vmimic[iaxis]->_velfns.at(itmimicdof->dofformatindex)->Eval(vtempvalues.empty() ? NULL : &vtempvalues[0]);
            const MIMIC::DOFFormat& dofformat = _vmimic[iaxis]->_vdofformat.at(itmimicdof->dofformatindex);
            if( dofformat.GetJoint(parent)->IsMimic(dofformat.axis) ) {
                dofformat.GetJoint(parent)->_ComputePartialVelocities(vtemppartials,dofformat.axis,mapcachedpartials);
                dReal fpartial = 0;
                FOREACHC(itpartial,vtemppartials) {
                    if( itpartial->first == itmimicdof->dofindex ) {
                        fpartial += itpartial->second;
                    }
                }
                fvel *= fpartial;
            }
            // before pushing back, check for repetition
            bool badd = true;
            FOREACH(itpartial,vpartials) {
                if( itpartial->first == itmimicdof->dofindex ) {
                    itpartial->second += fvel;
                    badd = false;
                    break;
                }
            }
            if( badd ) {
                vpartials.push_back(make_pair(itmimicdof->dofindex, fvel));
            }
        }
        else {
            bool badd = true;
            FOREACH(itpartial,vpartials) {
                if( itpartial->first == itmimicdof->dofindex ) {
                    badd = false;
                    break;
                }
            }
            if( badd ) {
                vpartials.push_back(make_pair(itmimicdof->dofindex, it->second));
            }
        }
    }
}

int KinBody::Joint::_Eval(int axis, uint32_t timederiv, const std::vector<dReal>& vdependentvalues, std::vector<dReal>& voutput)
{
    if( timederiv == 0 ) {
        _vmimic.at(axis)->_posfn->EvalMulti(voutput, vdependentvalues.empty() ? NULL : &vdependentvalues[0]);
        return _vmimic.at(axis)->_posfn->EvalError();
    }
    else if( timederiv == 1 ) {
        voutput.resize(_vmimic.at(axis)->_velfns.size());
        for(size_t i = 0; i < voutput.size(); ++i) {
            voutput[i] = _vmimic.at(axis)->_velfns.at(i)->Eval(vdependentvalues.empty() ? NULL : &vdependentvalues[0]);
            int err = _vmimic.at(axis)->_velfns.at(i)->EvalError();
            if( err ) {
                return err;
            }
        }
    }
    else if( timederiv == 2 ) {
        voutput.resize(_vmimic.at(axis)->_accelfns.size());
        for(size_t i = 0; i < voutput.size(); ++i) {
            voutput[i] = _vmimic.at(axis)->_accelfns.at(i)->Eval(vdependentvalues.empty() ? NULL : &vdependentvalues[0]);
            int err = _vmimic.at(axis)->_accelfns.at(i)->EvalError();
            if( err ) {
                return err;
            }
        }
    }
    else {
        throw OPENRAVE_EXCEPTION_FORMAT("timederiv %d not supported",timederiv,ORE_InvalidArguments);
    }
    return 0;
}

bool KinBody::Joint::MIMIC::DOFFormat::operator <(const KinBody::Joint::MIMIC::DOFFormat& r) const
{
    return jointindex < r.jointindex || (jointindex == r.jointindex && (dofindex < r.dofindex || (dofindex == r.dofindex && axis < r.axis)));
}

bool KinBody::Joint::MIMIC::DOFFormat::operator ==(const KinBody::Joint::MIMIC::DOFFormat& r) const
{
    return jointindex == r.jointindex && dofindex == r.dofindex && axis == r.axis;
}

KinBody::JointPtr KinBody::Joint::MIMIC::DOFFormat::GetJoint(KinBodyPtr parent) const
{
    int numjoints = (int)parent->GetJoints().size();
    return jointindex < numjoints ? parent->GetJoints().at(jointindex) : parent->GetPassiveJoints().at(jointindex-numjoints);
}

KinBody::JointConstPtr KinBody::Joint::MIMIC::DOFFormat::GetJoint(KinBodyConstPtr parent) const
{
    int numjoints = (int)parent->GetJoints().size();
    return jointindex < numjoints ? parent->GetJoints().at(jointindex) : parent->GetPassiveJoints().at(jointindex-numjoints);
}

void KinBody::Joint::SetFloatParameters(const std::string& key, const std::vector<dReal>& parameters)
{
    if( parameters.size() > 0 ) {
        _info._mapFloatParameters[key] = parameters;
    }
    else {
        _info._mapFloatParameters.erase(key);
    }
    GetParent()->_PostprocessChangedParameters(Prop_JointCustomParameters);
}

void KinBody::Joint::SetIntParameters(const std::string& key, const std::vector<int>& parameters)
{
    if( parameters.size() > 0 ) {
        _info._mapIntParameters[key] = parameters;
    }
    else {
        _info._mapIntParameters.erase(key);
    }
    GetParent()->_PostprocessChangedParameters(Prop_JointCustomParameters);
}

void KinBody::Joint::SetStringParameters(const std::string& key, const std::string& value)
{
    if( value.size() > 0 ) {
        _info._mapStringParameters[key] = value;
    }
    else {
        _info._mapStringParameters.erase(key);
    }
    GetParent()->_PostprocessChangedParameters(Prop_JointCustomParameters);
}

void KinBody::Joint::UpdateInfo()
{
    _info._vcurrentvalues.resize(0);
    GetValues(_info._vcurrentvalues);
}

void KinBody::Joint::serialize(std::ostream& o, int options) const
{
    if( options & SO_Kinematics ) {
        o << dofindex << " " << jointindex << " " << _info._type << " ";
        SerializeRound(o,_tRightNoOffset);
        SerializeRound(o,_tLeftNoOffset);
        for(int i = 0; i < GetDOF(); ++i) {
            SerializeRound3(o,_vaxes[i]);
            if( !!_vmimic.at(i) ) {
                FOREACHC(iteq,_vmimic.at(i)->_equations) {
                    o << *iteq << " ";
                }
            }
        }
        o << (!_attachedbodies[0] ? -1 : _attachedbodies[0]->GetIndex()) << " " << (_attachedbodies[1]->GetIndex()) << " ";
    }
    if( options & SO_Dynamics ) {
        for(int i = 0; i < GetDOF(); ++i) {
            SerializeRound(o,_info._vmaxvel[i]);
            SerializeRound(o,_info._vmaxaccel[i]);
            SerializeRound(o,_info._vmaxtorque[i]);
            SerializeRound(o,_info._vmaxinertia[i]);
            SerializeRound(o,_info._vlowerlimit[i]);
            SerializeRound(o,_info._vupperlimit[i]);
        }
    }
}

}


// Copyright (c) Jeremy Siek 2001
// Copyright (c) Douglas Gregor 2004
//
// 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)

// NOTE: this final is generated by libs/graph/doc/biconnected_components.w

#ifndef BOOST_GRAPH_BICONNECTED_COMPONENTS_HPP
#define BOOST_GRAPH_BICONNECTED_COMPONENTS_HPP

#include <stack>
#include <vector>
#include <algorithm> // for std::min and std::max
#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/property_map.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/graph/graph_utility.hpp>

namespace boost
{
  namespace detail
  {
    template<typename ComponentMap, typename DiscoverTimeMap,
             typename LowPointMap, typename PredecessorMap,
             typename OutputIterator, typename Stack>
    struct biconnected_components_visitor : public dfs_visitor<>
    {
      biconnected_components_visitor
        (ComponentMap comp, std::size_t& c, DiscoverTimeMap dtm,
         std::size_t& dfs_time, LowPointMap lowpt, PredecessorMap pred,
         OutputIterator out, Stack& S)
          : comp(comp), c(c), dtm(dtm), dfs_time(dfs_time), lowpt(lowpt),
            pred(pred), out(out), S(S) { }

      template <typename Vertex, typename Graph>
      void start_vertex(const Vertex& u, Graph&)
      {
        put(pred, u, u);
      }

      template <typename Vertex, typename Graph>
      void discover_vertex(const Vertex& u, Graph&)
      {
        put(dtm, u, ++dfs_time);
        put(lowpt, u, get(dtm, u));
      }

      template <typename Edge, typename Graph>
      void tree_edge(const Edge& e, Graph& g)
      {
        S.push(e);
        put(pred, target(e, g), source(e, g));
      }

      template <typename Edge, typename Graph>
      void back_edge(const Edge& e, Graph& g)
      {
        BOOST_USING_STD_MIN();

        if ( target(e, g) != get(pred, source(e, g)) ) {
          S.push(e);
          put(lowpt, source(e, g),
              min BOOST_PREVENT_MACRO_SUBSTITUTION(get(lowpt, source(e, g)),
                                                   get(dtm, target(e, g))));
        }
      }

      template <typename Vertex, typename Graph>
      void finish_vertex(const Vertex& u, Graph& g)
      {
        BOOST_USING_STD_MIN();
        Vertex parent = get(pred, u);
        bool is_art_point = false;
        if ( get(dtm, parent) > get(dtm, u) ) {
          parent = get(pred, parent);
          is_art_point = true;
        }

        if ( parent == u ) { // at top
          if ( get(dtm, u) + 1 == get(dtm, get(pred, u)) )
            is_art_point = false;
        } else {
          put(lowpt, parent,
              min BOOST_PREVENT_MACRO_SUBSTITUTION(get(lowpt, parent),
                                                   get(lowpt, u)));

          if (get(lowpt, u) >= get(dtm, parent)) {
            if ( get(dtm, parent) > get(dtm, get(pred, parent)) ) {
              put(pred, u, get(pred, parent));
              put(pred, parent, u);
            }

            while ( get(dtm, source(S.top(), g)) >= get(dtm, u) ) {
              put(comp, S.top(), c);
              S.pop();
            }
            put(comp, S.top(), c);
              S.pop();
            ++c;
            if ( S.empty() ) {
              put(pred, u, parent);
              put(pred, parent, u);
            }
          }
        }
        if ( is_art_point )
          *out++ = u;
      }

      ComponentMap comp;
      std::size_t& c;
      DiscoverTimeMap dtm;
      std::size_t& dfs_time;
      LowPointMap lowpt;
      PredecessorMap pred;
      OutputIterator out;
      Stack& S;
    };
  } // namespace detail

  template<typename Graph, typename ComponentMap, typename OutputIterator,
           typename DiscoverTimeMap, typename LowPointMap,
           typename PredecessorMap, typename VertexIndexMap>
  std::pair<std::size_t, OutputIterator>
  biconnected_components(const Graph & g, ComponentMap comp,
                         OutputIterator out, DiscoverTimeMap discover_time,
                         LowPointMap lowpt, PredecessorMap pred,
                         VertexIndexMap index_map)
  {
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;
    typedef typename graph_traits<Graph>::edge_descriptor edge_t;
    function_requires<VertexListGraphConcept<Graph> >();
    function_requires<IncidenceGraphConcept<Graph> >();
    function_requires<WritablePropertyMapConcept<ComponentMap, edge_t> >();
    function_requires<ReadWritePropertyMapConcept<DiscoverTimeMap,
                                                  vertex_t> >();
    function_requires<ReadWritePropertyMapConcept<LowPointMap, vertex_t > >();
    function_requires<ReadWritePropertyMapConcept<PredecessorMap,
                                                  vertex_t> >();

    std::size_t num_components = 0;
    std::size_t dfs_time = 0;
    std::stack < edge_t > S;

    detail::biconnected_components_visitor<ComponentMap, DiscoverTimeMap,
        LowPointMap, PredecessorMap, OutputIterator, std::stack<edge_t> >
      vis(comp, num_components, discover_time, dfs_time, lowpt, pred, out, S);

    depth_first_search(g, visitor(vis).vertex_index_map(index_map));

    return std::pair<std::size_t, OutputIterator>(num_components, vis.out);
  }

  template<typename Graph, typename ComponentMap, typename OutputIterator,
           typename DiscoverTimeMap, typename LowPointMap,
           typename VertexIndexMap>
  std::pair<std::size_t, OutputIterator>
  biconnected_components(const Graph & g, ComponentMap comp,
                         OutputIterator out, DiscoverTimeMap discover_time,
                         LowPointMap lowpt, VertexIndexMap index_map)
  {
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;
    std::vector<vertex_t> pred(num_vertices(g));
    vertex_t vert = graph_traits<Graph>::null_vertex();
    return biconnected_components
             (g, comp, out, discover_time, lowpt,
              make_iterator_property_map(pred.begin(), index_map, vert),
              index_map);
  }

  template<typename Graph, typename ComponentMap, typename OutputIterator,
           typename VertexIndexMap>
  std::pair<std::size_t, OutputIterator>
  biconnected_components(const Graph& g, ComponentMap comp, OutputIterator out,
                         VertexIndexMap index_map)
  {
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;
    typedef typename graph_traits<Graph>::vertices_size_type
      vertices_size_type;

    std::vector<vertices_size_type> discover_time(num_vertices(g));
    std::vector<vertices_size_type> lowpt(num_vertices(g));

    vertices_size_type vst(0);

    return biconnected_components
             (g, comp, out,
              make_iterator_property_map(discover_time.begin(), index_map, vst),
              make_iterator_property_map(lowpt.begin(), index_map, vst),
              index_map);
  }

  template < typename Graph, typename ComponentMap, typename OutputIterator>
  std::pair<std::size_t, OutputIterator>
  biconnected_components(const Graph& g, ComponentMap comp, OutputIterator out)
  {
    return biconnected_components(g, comp, out, get(vertex_index, g));
  }

  namespace graph_detail {
    struct dummy_output_iterator
    {
      typedef std::output_iterator_tag iterator_category;
      typedef void value_type;
      typedef void pointer;
      typedef void difference_type;

      struct reference {
        template<typename T>
        reference& operator=(const T&) { return *this; }
      };

      reference operator*() const { return reference(); }
      dummy_output_iterator& operator++() { return *this; }
      dummy_output_iterator operator++(int) { return *this; }
    };
  } // end namespace graph_detail

  template <typename Graph, typename ComponentMap>
  std::size_t
  biconnected_components(const Graph& g, ComponentMap comp)
  {
    return biconnected_components(g, comp,
                                  graph_detail::dummy_output_iterator()).first;
  }

  template<typename Graph, typename OutputIterator, typename VertexIndexMap>
  OutputIterator
  articulation_points(const Graph& g, OutputIterator out,
                      VertexIndexMap index_map)
  {
    return biconnected_components(g, dummy_property_map(), out,
                                  index_map).second;
  }

  template<typename Graph, typename OutputIterator>
  OutputIterator
  articulation_points(const Graph& g, OutputIterator out)
  {
    return biconnected_components(g, dummy_property_map(), out,
                                  get(vertex_index, g)).second;
  }

}                               // namespace boost

#endif  /* BOOST_GRAPH_BICONNECTED_COMPONENTS_HPP */


//  (C) Copyright John Maddock 2006.
//  (C) Copyright Johan Rade 2006.
//  (C) Copyright Paul A. Bristow 2011 (added changesign).

//  Use, modification and distribution are subject to 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)

#ifndef BOOST_MATH_TOOLS_SIGN_HPP
#define BOOST_MATH_TOOLS_SIGN_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/tools/config.hpp>
#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/special_functions/detail/fp_traits.hpp>

namespace boost{ namespace math{

namespace detail {

  // signbit

#ifdef BOOST_MATH_USE_STD_FPCLASSIFY
    template<class T>
    inline int signbit_impl(T x, native_tag const&)
    {
        return (std::signbit)(x);
    }
#endif

    template<class T>
    inline int signbit_impl(T x, generic_tag<true> const&)
    {
        return x < 0;
    }

    template<class T>
    inline int signbit_impl(T x, generic_tag<false> const&)
    {
        return x < 0;
    }

    template<class T>
    inline int signbit_impl(T x, ieee_copy_all_bits_tag const&)
    {
        typedef BOOST_DEDUCED_TYPENAME fp_traits<T>::type traits;

        BOOST_DEDUCED_TYPENAME traits::bits a;
        traits::get_bits(x,a);
        return a & traits::sign ? 1 : 0;
    }

    template<class T>
    inline int signbit_impl(T x, ieee_copy_leading_bits_tag const&)
    {
        typedef BOOST_DEDUCED_TYPENAME fp_traits<T>::type traits;

        BOOST_DEDUCED_TYPENAME traits::bits a;
        traits::get_bits(x,a);

        return a & traits::sign ? 1 : 0;
    }

    // Changesign

    template<class T>
    inline T (changesign_impl)(T x, generic_tag<true> const&)
    {
        return -x;
    }

    template<class T>
    inline T (changesign_impl)(T x, generic_tag<false> const&)
    {
        return -x;
    }

    template<class T>
    inline T changesign_impl(T x, ieee_copy_all_bits_tag const&)
    {
        typedef BOOST_DEDUCED_TYPENAME fp_traits<T>::sign_change_type traits;

        BOOST_DEDUCED_TYPENAME traits::bits a;
        traits::get_bits(x,a);
        a ^= traits::sign;
        traits::set_bits(x,a);
        return x;
    }

    template<class T>
    inline T (changesign_impl)(T x, ieee_copy_leading_bits_tag const&)
    {
        typedef BOOST_DEDUCED_TYPENAME fp_traits<T>::sign_change_type traits;

        BOOST_DEDUCED_TYPENAME traits::bits a;
        traits::get_bits(x,a);
        a ^= traits::sign;
        traits::set_bits(x,a);
        return x;
    }

}   // namespace detail

template<class T> int (signbit)(T x)
{
   typedef typename detail::fp_traits<T>::type traits;
   typedef typename traits::method method;
   // typedef typename boost::is_floating_point<T>::type fp_tag;
   typedef typename tools::promote_args_permissive<T>::type result_type;
   return detail::signbit_impl(static_cast<result_type>(x), method());
}

template <class T>
inline int sign BOOST_NO_MACRO_EXPAND(const T& z)
{
   return (z == 0) ? 0 : (boost::math::signbit)(z) ? -1 : 1;
}

template <class T> typename tools::promote_args_permissive<T>::type (changesign)(const T& x)
{ //!< \brief return unchanged binary pattern of x, except for change of sign bit.
   typedef typename detail::fp_traits<T>::sign_change_type traits;
   typedef typename traits::method method;
   // typedef typename boost::is_floating_point<T>::type fp_tag;
   typedef typename tools::promote_args_permissive<T>::type result_type;

   return detail::changesign_impl(static_cast<result_type>(x), method());
}

template <class T, class U>
inline typename tools::promote_args_permissive<T, U>::type
   copysign BOOST_NO_MACRO_EXPAND(const T& x, const U& y)
{
   BOOST_MATH_STD_USING
   typedef typename tools::promote_args_permissive<T, U>::type result_type;
   return (boost::math::signbit)(static_cast<result_type>(x)) != (boost::math::signbit)(static_cast<result_type>(y))
      ? (boost::math::changesign)(static_cast<result_type>(x)) : static_cast<result_type>(x);
}

} // namespace math
} // namespace boost

#endif // BOOST_MATH_TOOLS_SIGN_HPP


/*
 * @Descripttion:
 * @version:
 * @Author: li
 * @Date: 2021-02-28 11:33:51
 * @LastEditors: li
 * @LastEditTime: 2021-02-28 13:24:26
 */
#include "ros/ros.h"
// Eigen
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include "eigen_demo/map_server.hpp"

int main(int argc, char **argv)
{
    ros::init(argc, argv, "eigen_demo_node");
    ros::NodeHandle nh;
    map_server mp;

    ROS_INFO("eigen_demo_node started...");
    ros::Rate rate(20);

    while (ros::ok())
    {
        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}


#include <boost/heap/priority_queue.hpp>
#include <iostream>

using namespace std;
using namespace boost::heap;

int main() {
  boost::heap::priority_queue<int> pq;
  pq.push(2);
  pq.push(3);
  pq.push(1);

  for (int i: pq)
    cout << i << " ";
  cout << endl;

  boost::heap::priority_queue<int> pq2;
  pq2.push(4);
  cout << boolalpha << (pq > pq2) << endl;
}


/* FILE: common.cc
 *
 * Common code for doubledouble.cc and alt_doubledouble.cc.
 *
 * NOTICE: Please see the file ../../LICENSE
 *
 */

#include <assert.h>
#include "xpcommon.h"

#if XP_USE_FLOAT_TYPE == XP_MPFR_TYPE
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/special_functions/sign.hpp>
using boost::math::isfinite;
using boost::math::signbit;
#endif

/* End includes */

////////////////////////////////////////////////////////////////////////
/// Start value safe block (associative floating point optimization
/// disallowed).
////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////
// Extended value support (infinities, NaNs):
//
// The are a host of various isnan, isinf, isfinite, which are
// non-standard and broken in various ingenious ways on different
// compilers with different compiler options.  But mostly what is
// needed in this source file is a check on whether or not a given
// value is finite.  The following test appears pretty robust:
#if XP_USE_FLOAT_TYPE == XP_MPFR_TYPE
// MPFR type doesn't support volatile qualifier, so
// use standard C++11 library routines
OC_BOOL Xp_IsFinite(XP_DDFLOAT_TYPE tx)
{ return isfinite(tx); }
OC_BOOL Xp_IsPosInf(XP_DDFLOAT_TYPE tx)
{ return (tx == XP_INFINITY); }
OC_BOOL Xp_IsNegInf(XP_DDFLOAT_TYPE tx)
{ return (tx == -XP_INFINITY); }

#else // !XP_USE_FLOAT_TYPE == XP_MPFR_TYPE

OC_BOOL Xp_IsFinite(XP_DDFLOAT_TYPE tx)
{
  volatile XP_DDFLOAT_TYPE x = tx;
  return (-XP_DDFLOAT_MAX<=x && x<=XP_DDFLOAT_MAX);
}
OC_BOOL Xp_IsPosInf(XP_DDFLOAT_TYPE tx)
{ // Depending on compiler flags, differentiating Infs from NaNs is
  // difficult.  The following is a best try:
  volatile XP_DDFLOAT_TYPE x = tx;
  if(x == XP_INFINITY && !(x == -XP_INFINITY)) return 1;
  return 0;
  /// A more robust though probably slower alternative would be
  /// return (memcmp(&x,&XP_INFINITY,sizeof(x))==0);
}
OC_BOOL Xp_IsNegInf(XP_DDFLOAT_TYPE tx)
{ // Depending on compiler flags, differentiating Infs from NaNs is
  // difficult.  The following is a best try:
  volatile XP_DDFLOAT_TYPE x = tx;
  if(x == -XP_INFINITY && !(x == XP_INFINITY)) return 1;
  return 0;
  /// A more robust though probably slower alternative would be
  ///    XP_DDFLOAT_TYPE chk = -XP_INFINITY;
  ///    return (memcmp(&x,&chk,sizeof(x))==0);
}
#endif // XP_USE_FLOAT_TYPE == XP_MPFR_TYPE

OC_BOOL Xp_IsNaN(XP_DDFLOAT_TYPE x)
{
  if(Xp_IsFinite(x) || Xp_IsPosInf(x) || Xp_IsNegInf(x)) {
    return 0;
  }
  return 1;
}

// Some compilers (e.g. g++ 5.3.1) will take code like this
//
//    if(x==0.0) {
//       signbit = Xp_SignBit(x)
//    }
//
// and replace it with
//
//    if(x==0.0) {
//       signbit = Xp_SignBit(0.0)
//    }
//
// which is a broken optimization if one wants to distinguish between x
// being +0.0 and -0.0.  A workaround is to use a non-inlined subroutine
// call to make the magnitude check, e.g.
//
//    if(Xp_IsZero(x)) {
//       signbit = Xp_SignBit(0.0)
//    }
//
// (Here if(fabs(x)==0.0) might also work, depending on how much the
// compiler knows about fabs().)  The Xp_AreEqual routine handles
// similar issues for comparisons between two values.
//
// These routines also take care of NaN handling problems.
OC_BOOL Xp_IsZero(XP_DDFLOAT_TYPE x)
{
  return (Xp_IsFinite(x) && 0.0 == x);
}
OC_BOOL Xp_AreEqual(XP_DDFLOAT_TYPE x,XP_DDFLOAT_TYPE y)
{
  if(Xp_IsNaN(x) || Xp_IsNaN(y)) return 0;
  return (x == y);
}

// The Xp_SignBit routines return 1 if x is negative, 0 if positive
// (same as std::signbit spec).
#if XP_USE_FLOAT_TYPE == XP_MPFR_TYPE
OC_BOOL Xp_SignBit(XP_DDFLOAT_TYPE x)
{ return boost::math::signbit(x); }
#else // !XP_USE_FLOAT_TYPE == XP_MPFR_TYPE
OC_BOOL Xp_SignBit(OC_REAL4 x)
{
  union BITS {
    OC_REAL4 f;
    OC_UINT4 u;
  };
  BITS a = {1.0};
  BITS b = {-1.0};
  BITS c = { x };
  if((a.u ^ b.u) & c.u) return 1;
  return 0;
}
OC_BOOL Xp_SignBit(OC_REAL8 x)
{
  union BITS {
    OC_REAL8 f;
    OC_UINT8 u;
  };
  BITS a = {1.0};
  BITS b = {-1.0};
  BITS c = { x };
  if((a.u ^ b.u) & c.u) return 1;
  return 0;
}
OC_BOOL Xp_SignBit(long double x)
{ // This code could be sped up considerably, in particular
  // by determining a priori the one bit where 1.0 and -1.0
  // differ, and then checking just that one bit.
  assert(sizeof(long double) % sizeof(unsigned int) == 0);
  static const long double af =  1.0;
  static const long double bf = -1.0;
  unsigned int const * a = reinterpret_cast<unsigned int const *>(&af);
  unsigned int const * b = reinterpret_cast<unsigned int const *>(&bf);
  unsigned int const * c = reinterpret_cast<unsigned int const *>(&x);
  for(size_t i=0; i*sizeof(unsigned int)<sizeof(long double); ++i) {
    if((a[i] ^ b[i]) & c[i]) return 1;
  }
  return 0;
}
#endif // XP_USE_FLOAT_TYPE == XP_MPFR_TYPE

////////////////////////////////////////////////////////////////////////
/// End value safe block
////////////////////////////////////////////////////////////////////////

int HexFloatWidth()
{
  return (XP_DDFLOAT_MANTISSA_PRECISION+6)/4
    + 6 + (XP_DDFLOAT_HUGE_EXP<3000 ? 4 : 5);
}

std::string HexFloatFormat(XP_DDFLOAT_TYPE value)
{
  char buf[256+XP_DDFLOAT_MANTISSA_PRECISION/4];
  if(!Xp_IsFinite(value)) { // Use %g to decide how to print the value
    snprintf(buf,sizeof(buf),"%g",static_cast<double>(value));
    char* cptr=buf;
    while(*cptr != '\0') {
      *cptr = static_cast<char>(tolower(*cptr));
      ++cptr;
    }
    std::string str = buf;
    if(str.find("inf")== std::string::npos) {
      str = "NaN";
    } else {
      if(str[0]=='-') {
        str = "-Inf";
      } else {
        str = "Inf";
      }
    }
    return str;
  }

  char* cptr = buf;
  XP_DDFLOAT_TYPE mantissa;
  int exp;
  mantissa = XP_FREXP(value,&exp);
  if(!Xp_SignBit(value)) { // Xp_SignBit detects signed zero
    *(cptr++) = ' ';  // Leave sign space
  } else {
    *(cptr++) = '-';
    mantissa *= -1;
  }

  // Write "0x" prefix to mantissa string, and leading "1."
  *(cptr++) = '0';  *(cptr++) = 'x';
  if(mantissa == 0.0) {
    *(cptr++) = '0';
  } else {
    *(cptr++) = '1';
    mantissa = 2*mantissa - 1.0;
    exp -= 1;
  }
  *(cptr++) = '.';

  // Write mantissa as hex digits
  for(int offset=1; offset<XP_DDFLOAT_MANTISSA_PRECISION; offset+=4) {
    mantissa *= 16;
    XP_DDFLOAT_TYPE fval = floor(mantissa);
    int ival = static_cast<int>(fval); // i.e., floor(mantissa)
    /// MPFR library has some trouble taking int() on temporary.
    if(ival<10) *(cptr++) = static_cast<char>('0' + ival);
    else        *(cptr++) = static_cast<char>('A' + ival - 10);
    mantissa -= ival;
  }

  if(XP_DDFLOAT_HUGE_EXP<3000) {
    snprintf(cptr,sizeof(buf)-(cptr-buf),"p%+05d",exp);
  } else {
    snprintf(cptr,sizeof(buf)-(cptr-buf),"p%+06d",exp);
  }
  return std::string(buf);
}

XP_DDFLOAT_TYPE ReadOneHexFloat(const char* buf,const char*& remainder)
{
  const int MAXSTOP = 16384; // Maximum buf size; fallback stop
  const char* CSTOP = buf + MAXSTOP;
  // Skip to data start; data has form 0x*, Inf, or NaN
  const char* cptr = strpbrk(buf,"0iInN");
  if(cptr == 0) { // No data
    remainder = 0;
    return 0.0;
  }
  // Check sign
  XP_DDFLOAT_TYPE valsign = 1.0;
  if( cptr - buf > 0 && *(cptr-1) == '-') {
    valsign = -1.0;
  }

  // Check for special values
  if(tolower(cptr[0])=='i' && tolower(cptr[1])=='n'
     && tolower(cptr[2])=='f') { // Infinity
    remainder = cptr+3;
    return valsign*XP_INFINITY;
  }
  if(tolower(cptr[0])=='n' && tolower(cptr[1])=='a'
     && tolower(cptr[2])=='n') { // NaN
    remainder = cptr+3;
    return XP_NAN;
  }

  // Otherwise, cptr should be pointing at a standard hexfloat.
  // First, convert string up to "."  Should be at most one digit.
  // (Moreover, that digit is suppose to be "1".)
  char* endptr;
  XP_DDFLOAT_TYPE val = XP_DDFLOAT_TYPE(strtol(cptr+2,&endptr,16));
  if(*endptr != '.') {
    std::string msg = "Bad input to ReadOneHexFloat: " + std::string(buf);
    throw msg;
  }
  cptr = endptr+1;
  XP_DDFLOAT_TYPE scale = 1.0/16.0;
  while(*cptr != 'p' && *cptr != '\0' && cptr != CSTOP) {
    int ival = *cptr - '0';
    if(ival>9) {
      ival -= ('A' - '0');
      if(ival>5) ival -= ('a'-'A');
      ival += 10;
    }
    val += XP_DDFLOAT_TYPE(ival)*scale;
    scale *= 1.0/16.0;
    // One alternative is to parse from back to front using a Horner
    // type algorithm to scale and sum values.
    ++cptr;
  }
  if(*cptr == 'p' || *cptr == 'P') {
    // Read exponent
    int iexp = strtol(cptr+1,&endptr,10);
    // Many implementations of ldexp misbehave in underflow
    // situations, by either flushing to zero or not rounding
    // properly.  This is not a critical path, so just brute-force it:
    if(iexp>0) {
      for(int i=0;i<iexp;++i) val *= 2;
    } else if(iexp<0) {
      for(int i=0;i>iexp;--i) val *= 0.5;
    }
    cptr = endptr;
  }

  remainder = cptr;
  return valsign*val;
}

int HexBinaryFloatWidth()
{
  return (XP_DDFLOAT_MANTISSA_PRECISION+3)/4
    + 6 + (XP_DDFLOAT_HUGE_EXP<3000 ? 3 : 4);
}

std::string HexBinaryFloatFormat(XP_DDFLOAT_TYPE value)
{
  char buf[256+XP_DDFLOAT_MANTISSA_PRECISION/4];

  if(!Xp_IsFinite(value)) { // Use %g to decide how to print the value
    snprintf(buf,sizeof(buf),"%g",static_cast<double>(value));
    char* cptr=buf;
    while(*cptr != '\0') {
      *cptr = static_cast<char>(tolower(*cptr));
      ++cptr;
    }
    std::string str = buf;
    if(str.find("inf")== std::string::npos) {
      str = "NaN";
    } else {
      if(str[0]=='-') {
        str = "-Inf";
      } else {
        str = "Inf";
      }
    }
    return str;
  }

  char* cptr = buf;
  XP_DDFLOAT_TYPE mantissa;
  int exp;
  mantissa = XP_FREXP(value,&exp);
  if(!Xp_SignBit(value)) { // Xp_SignBit detects signed zero
    *(cptr++) = ' ';  // Leave sign space
  } else {
    *(cptr++) = '-';
    mantissa *= -1;
  }

  // Write "0x" prefix to mantissa string
  *(cptr++) = '0';
  *(cptr++) = 'x';

  // Lead shift; if precision is not divisible by 4, then write
  // "left-over" bits (which won't fill a full hex digit) first.
#if XP_DDFLOAT_MANTISSA_PRECISION%4 != 0
    mantissa *= pow(XP_DDFLOAT_TYPE(2.0),XP_DDFLOAT_MANTISSA_PRECISION%4);
    exp -= XP_DDFLOAT_MANTISSA_PRECISION%4;
#else
    mantissa *= 16;
    exp -= 4;
#endif

  // Write mantissa as hex digits
  for(int offset=0; 4*offset<XP_DDFLOAT_MANTISSA_PRECISION; ++offset) {
    XP_DDFLOAT_TYPE fval = floor(mantissa);
    int ival = static_cast<int>(fval); // i.e., floor(mantissa)
    /// MPFR library has some trouble taking int() on temporary.
    if(ival<10) *(cptr++) = static_cast<char>('0' + ival);
    else        *(cptr++) = static_cast<char>('A' + ival - 10);
    mantissa -= ival;
    mantissa *= 16;
    exp -= 4;
  }

  exp += 4;
  if(XP_DDFLOAT_HUGE_EXP<3000) {
    snprintf(cptr,sizeof(buf)-(cptr-buf),"xb%+04d",exp);
  } else {
    snprintf(cptr,sizeof(buf)-(cptr-buf),"xb%+05d",exp);
  }
  return std::string(buf);
}

XP_DDFLOAT_TYPE ScanHexBinFloat(const char* cptr)
{ // Support for HexHex-Float and HexBin-Float formats, which have form
  //
  //    smmm...mmmxseee         (HexHex-Float)
  //    smmm...mmmxbseee        (HexBin-Float)
  //
  // where s is an optional mantisa sign (+ or -), mmm...mmm is an
  // arbitrary length run of hexadecimal digits (0-9, a-f, A-F), followed
  // by a literal 'x' (or 'X') or 'xb' (or 'XB') indicating the base of
  // the exponent (16 or 2, respectively), followed by an optional
  // exponent sign, and then a run of decimal digits representing the
  // power of 16 or 2.  Examples of the HexHex-Float format:
  //
  //     5a0x3
  //
  // is the decimal number (5*16^2 + 10*16) * 16^3 = 1440 * 4096 = 5898240.
  // and
  //      120Fx-7
  //
  // is the decimal number (1*16^3 + 2*16^2 + 15) * 16^-7 = 4623/268435456
  //                                      = 0.0000172220170497894287109375
  //
  // Examples of the HexBin-format:
  //
  //     5a0xb3
  //
  // is the decimal number (5*16^2 + 10*16) * 2^3 = 1440 * 8 = 11520.
  // and
  //      120Fxb-7
  //
  // is the decimal number (1*16^3 + 2*16^2 + 15) * 2^-7 = 4623/128
  //                                                     = 36.1171875
  //
  // Note: C99 and C++11 specify a different hexfloat format, represented
  // by the format specifier %a.  See the ScanC99HexFloat routine for
  // details.

  // Safer to work with unsigned chars
  const unsigned char* ptr = (const unsigned char*)cptr;

  // Skip leading junk
  unsigned char ch;
  while((ch=*ptr) != 0x0 && ch!='+' && ch!='-' &&
        !(('0'<=ch && ch<='9')                                  \
          || ('a'<=ch && ch<='f') || ('A'<=ch && ch<='F'))) {
    ++ptr;
  }
  // Set sign on mantissa
  int sign = 1;
  if(ch=='-') {
    sign = -1;
    ch = *(++ptr);
  } else if(ch=='+') {
    ch = *(++ptr);
  }

  // Skip leading "0x", if any
  if(ch == '0') {
    ch = *(++ptr);
    if(ch == 'x' || ch == 'X') {
      ch = *(++ptr);
    }
  }

  // Read mantissa
  XP_DDFLOAT_TYPE value = 0.0;
  while(ch != 0x0) {
    unsigned int digit;
    if(ch=='-') {
      sign = -1;
    } else if(ch=='+') {
      sign = 1;
    } else {
      if('0'<=ch && ch<='9') {
        digit = static_cast<unsigned int>(ch - '0');
      } else if('a'<=ch && ch<='f') {
        digit = static_cast<unsigned int>(ch - 'a' + 10);
      } else if('A'<=ch && ch<='F') {
        digit = static_cast<unsigned int>(ch - 'A' + 10);
      } else { // Bad value
        break;
      }
      value = 16*value + digit;
    }
    ch=*(++ptr);
  }

  // Exponent
  int exponent=0;
  double base=16.;
  if(ch=='x' || ch=='X') {
    ch=*(++ptr);
    if(ch=='b' || ch=='B') {
      base=2.;
      ++ptr;
    }
    exponent = atoi((const char*)(ptr));
  }
  if(base == 16) {
    exponent *= 4;
  }

  // Put it all together.  Break exponent up as necessary to protect
  // against under/overflow.  This code also in ScanC99HexFloat().
  while(exponent > XP_DDFLOAT_HUGE_EXP-1) {
    value *= pow(XP_DDFLOAT_TYPE(2),XP_DDFLOAT_TYPE(XP_DDFLOAT_HUGE_EXP-1));
    exponent -= XP_DDFLOAT_HUGE_EXP-1;
  }
  while(exponent < XP_DDFLOAT_TINY_EXP+1) {
    value *= pow(XP_DDFLOAT_TYPE(2),XP_DDFLOAT_TYPE(XP_DDFLOAT_TINY_EXP+1));
    exponent -= XP_DDFLOAT_TINY_EXP+1;
  }
  value *= pow(XP_DDFLOAT_TYPE(2),XP_DDFLOAT_TYPE(exponent));

  return sign*value;
}

XP_DDFLOAT_TYPE ScanC99HexFloat(const char* cptr)
{ // The format for the C99/C++11 hexfloat format is
  //    sOx1.mmm...mmmpseee         (hexfloat)
  // where s is an optional mantisa sign (+ or -), 0x is a literal
  // identifier indicating a hexadecimal value, 1. is the first part of
  // the normalized hexadecimal floating point value. mmm...mmm is an
  // arbitrary length run of hexadecimal digits (0-9, a-f, A-F),
  // followed by a literal 'p' (or 'P') indicating the start of the
  // base-2 exponent, then an optional exponent sign, and then a run of
  // decimal digits representing the power of 2.  Examples of the C99
  // hexfloat format:
  //
  //     0x1.68p13
  //
  // is the decimal number (1+6/16+8/16^2) * 2^13 = (360/256) * 2^13
  // = 360 * 2^5= 11520, and
  //
  //    -0x1.20Fp+5
  //
  // is the decimal number -(1*16^3 + 2*16^2 + 15) * 2^(5-12)
  // = -4623/128 = -36.1171875
  //
  // This is very similar to the HexBin format above, the difference
  // being the location of the hex point in the mantissa---this
  // shifts the exponent and re-jiggers the binary-to-hex grouping
  // in the mantissa.

  // Safer to work with unsigned chars
  const unsigned char* ptr = (const unsigned char*)cptr;

  // Skip leading junk
  unsigned char ch;
  while((ch=*ptr) != 0x0 && ch!='+' && ch!='-' &&
        !(('0'<=ch && ch<='9')                                  \
          || ('a'<=ch && ch<='f') || ('A'<=ch && ch<='F'))) {
    ++ptr;
  }
  // Set sign on mantissa
  int sign = 1;
  if(ch=='-') {
    sign = -1;
    ch = *(++ptr);
  } else if(ch=='+') {
    ch = *(++ptr);
  }

  // Skip leading "0x", if any
  if(ch == '0') {
    ch = *(++ptr);
    if(ch == 'x' || ch == 'X') {
      ch = *(++ptr);
    }
  }

  // Next two characters should be "1." or "0.", the latter only on
  // zero input.
  if(ch == '0') {
    // Optimization bugs will sometimes drop sign on sign*0.0, so use
    // copysign instead.
#if XP_HAVE_COPYSIGN
    return copysign(static_cast<XP_DDFLOAT_TYPE>(0.0),
                    static_cast<XP_DDFLOAT_TYPE>(sign));
#else
    return sign*static_cast<XP_DDFLOAT_TYPE>(0.0);
#endif
  }
  if(ch != '1' || *(++ptr) != '.') {
    std::string errmsg
      = "Error in ScanC99HexFloat; Invalid C99 hexfloat string: ";
    errmsg += cptr;
    throw errmsg;
  }

  // Read mantissa
  XP_DDFLOAT_TYPE value = 1.0;
  int exponent = 0;
  while( (ch = *(++ptr)) != 0x0) {
    unsigned int digit;
    if('0'<=ch && ch<='9') {
      digit = static_cast<unsigned int>(ch - '0');
    } else if('a'<=ch && ch<='f') {
      digit = static_cast<unsigned int>(ch - 'a' + 10);
    } else if('A'<=ch && ch<='F') {
      digit = static_cast<unsigned int>(ch - 'A' + 10);
    } else { // Bad value
      break;
    }
    value = 16*value + digit;
    exponent -= 4;
  }

  // Exponent (base is 2)
  if(ch=='p' || ch=='P') {
    exponent += atoi((const char*)(++ptr));
  }

  // Put it all together.  Break exponent up as necessary to protect
  // against under/overflow.  This code also in ScanHexBinFloat().
  while(exponent > XP_DDFLOAT_HUGE_EXP-1) {
    value *= pow(XP_DDFLOAT_TYPE(2),XP_DDFLOAT_TYPE(XP_DDFLOAT_HUGE_EXP-1));
    exponent -= XP_DDFLOAT_HUGE_EXP-1;
  }
  while(exponent < XP_DDFLOAT_TINY_EXP+1) {
    value *= pow(XP_DDFLOAT_TYPE(2),XP_DDFLOAT_TYPE(XP_DDFLOAT_TINY_EXP+1));
    exponent -= XP_DDFLOAT_TINY_EXP+1;
  }
  value *= pow(XP_DDFLOAT_TYPE(2),XP_DDFLOAT_TYPE(exponent));

  return sign*value;
}

XP_DDFLOAT_TYPE ScanFloat(const char* cptr)
{
  // If string contains an p or P, then calls ScanC99HexFloat.
  // If string contains an x or X, then calls ScanHexBinFloat.
  // Otherwise uses strtold()
  // Note: Check for 'p' first since C99HexFloat also contains
  //       an 'x' in the '0x' prefix.
  XP_DDFLOAT_TYPE value;
  // First check for infinities and NaNs.
  const char* special = strstr(cptr,"Inf");
  if(special) {
    XP_DDFLOAT_TYPE value_sign = 1;
    if(special>cptr && *(special-1) == '-') {
      value_sign = -1;
    }
    return value_sign*XP_INFINITY;
  }
  if(strstr(cptr,"NaN")) {
    return XP_NAN;
  }

  // Else, determine string format and process accordingly.
  if(strchr(cptr,'p') || strchr(cptr,'P')) {
    // Assume C99 hex-float
    value = ScanC99HexFloat(cptr);
  } else if(strchr(cptr,'x') || strchr(cptr,'X')) {
    // Assume hexhex-float
    value = ScanHexBinFloat(cptr);
  } else {
    // Assume decimal float
    value = static_cast<XP_DDFLOAT_TYPE>(strtold(cptr,0));
  }

  return value;
}


#ifndef DQN_HPP_
#define DQN_HPP_

#include <memory>
#include <random>
#include <tuple>
#include <unordered_map>
#include <vector>
#include <ale_interface.hpp>
#include <caffe/caffe.hpp>
#include <boost/functional/hash.hpp>
#include <boost/optional.hpp>

namespace dqn {

constexpr auto kRawFrameHeight = 210;
constexpr auto kRawFrameWidth = 160;
constexpr auto kCroppedFrameSize = 84;
constexpr auto kCroppedFrameDataSize = kCroppedFrameSize * kCroppedFrameSize;
constexpr auto kInputFrameCount = 4;
constexpr auto kInputDataSize = kCroppedFrameDataSize * kInputFrameCount;
constexpr auto kMinibatchSize = 32;
constexpr auto kMinibatchDataSize = kInputDataSize * kMinibatchSize;
constexpr auto kGamma = 0.95f;
constexpr auto kOutputCount = 18;

using FrameData = std::array<uint8_t, kCroppedFrameDataSize>;
using FrameDataSp = std::shared_ptr<FrameData>;
using InputFrames = std::array<FrameDataSp, 4>;
using Transition = std::tuple<InputFrames, Action, float, boost::optional<FrameDataSp>>;

using FramesLayerInputData = std::array<float, kMinibatchDataSize>;
using TargetLayerInputData = std::array<float, kMinibatchSize * kOutputCount>;
using FilterLayerInputData = std::array<float, kMinibatchSize * kOutputCount>;

/**
 * Deep Q-Network
 */
class DQN {
public:
  DQN(
      const ActionVect& legal_actions,
      const std::string& solver_param,
      const int replay_memory_capacity,
      const double gamma) :
        legal_actions_(legal_actions),
        solver_param_(solver_param),
        replay_memory_capacity_(replay_memory_capacity),
        gamma_(gamma),
        current_iter_(0),
        random_engine(0) {}

  /**
   * Initialize DQN. Must be called before calling any other method.
   */
  void Initialize();

  /**
   * Load a trained model from a file.
   */
  void LoadTrainedModel(const std::string& model_file);

  /**
   * Select an action by epsilon-greedy.
   */
  Action SelectAction(const InputFrames& input_frames, double epsilon);

  /**
   * Add a transition to replay memory
   */
  void AddTransition(const Transition& transition);

  /**
   * Update DQN using one minibatch
   */
  void Update();

  int memory_size() const { return replay_memory_.size(); }
  int current_iteration() const { return current_iter_; }

private:
  using SolverSp = std::shared_ptr<caffe::Solver<float>>;
  using NetSp = boost::shared_ptr<caffe::Net<float>>;
  using BlobSp = boost::shared_ptr<caffe::Blob<float>>;
  using MemoryDataLayerSp = boost::shared_ptr<caffe::MemoryDataLayer<float>>;

  std::pair<Action, float> SelectActionGreedily(const InputFrames& last_frames);
  std::vector<std::pair<Action, float>> SelectActionGreedily(
      const std::vector<InputFrames>& last_frames);
  void InputDataIntoLayers(
      const FramesLayerInputData& frames_data,
      const TargetLayerInputData& target_data,
      const FilterLayerInputData& filter_data);

  const ActionVect legal_actions_;
  const std::string solver_param_;
  const int replay_memory_capacity_;
  const double gamma_;
  int current_iter_;
  std::deque<Transition> replay_memory_;
  SolverSp solver_;
  NetSp net_;
  BlobSp q_values_blob_;
  MemoryDataLayerSp frames_input_layer_;
  MemoryDataLayerSp target_input_layer_;
  MemoryDataLayerSp filter_input_layer_;
  TargetLayerInputData dummy_input_data_;
  std::mt19937 random_engine;
};

/**
 * Preprocess an ALE screen (downsampling & grayscaling)
 */
FrameDataSp PreprocessScreen(const ALEScreen& raw_screen);

/**
 * Draw a frame as a string
 */
std::string DrawFrame(const FrameData& frame);

}

#endif /* DQN_HPP_ */


#ifndef CANNON_ML_LINEAR_PROGRAMMING_H
#define CANNON_ML_LINEAR_PROGRAMMING_H

/*!
 * \file cannon/ml/linear_programming.hpp
 * \brief File containing LPOptimizer class definition.
 */

#ifdef CANNON_HAVE_GUROBI
#include <memory>

#include <gurobi_c++.h>
#include <Eigen/Dense>

#include <cannon/utils/class_forward.hpp>

using namespace Eigen;

namespace cannon {
  namespace ml {

    CANNON_CLASS_FORWARD(OptimizationResult);

    /*!
     * \brief Class representing a linear programming problem. Solving this
     * optimization problem is done using Gurobi.
     */
    class LPOptimizer {
      public:
        LPOptimizer() = delete;

        /*!
         * \brief Constructor taking variable constraints and the linear
         * objective function cooeficients for a linear programming problem.
         * The number of variables in the linear program is implicitly defined
         * by the size of these vectors.
         */
        LPOptimizer(const VectorXd& lower, const VectorXd& upper, const
            VectorXd& objective_vec) : env_(true), num_vars_(lower.size()) {
          assert(lower.size() == upper.size());
          assert(lower.size() == objective_vec.size());

          env_.start();
          model_ = std::make_shared<GRBModel>(env_);

          // Improve numeric issues
          //model_->set(GRB_IntParam_NumericFocus, 3);
          //model_->set(GRB_IntParam_Presolve, 0);

          // Since this is just an LP, we don't need crossover (I think).
          // We can only disable this by explicitly using the barrier method. Note that we
          // are fine with this because approximately optimal solutions
          // satisfying the constraints are usually all we need.
          //model_->set(GRB_IntParam_Method, 2);
          //model_->set(GRB_IntParam_BarHomogeneous, 1);
          //model_->set(GRB_DoubleParam_BarConvTol, 1e-1);
          //model_->set(GRB_IntParam_Crossover, 0);

          // Create Gurobi variables
          vars_ = model_->addVars(lower.data(), upper.data(), NULL, NULL, NULL, num_vars_);

          // Create Gurobi linear objective
          GRBLinExpr obj = 0;
          for (unsigned int i = 0; i < num_vars_; i++) {
            obj += vars_[i] * objective_vec[i];
          }
          model_->setObjective(obj, GRB_MINIMIZE);
        }

        /*!
         * \brief Destructor.
         */
        ~LPOptimizer() {
          // vars_ is allocated on the heap and has to be freed
          delete[] vars_;
        }

        /*!
         * \brief Add a constraint of the form A * x <= b to this linear
         * programming problem.
         *
         * \param lhs_mat Matrix multiplying the variables of this problem in the new constraint.
         * \param rhs Vector on the right hand side of the inequality for the new constraint.
         */
        void add_constraint(const MatrixXd& lhs_mat, const VectorXd& rhs);

        /*!
         * \brief Minimize the linear program represented by this object.
         * Throws std::runtime_error and GRBException, as well as misc other
         * exceptions.
         *
         * \returns The result of attempting to solve the linear program.
         */
        OptimizationResult optimize();

      private:
        GRBEnv env_; //!< Gurobi environment
        std::shared_ptr<GRBModel> model_; //!< Gurobi model representing this LP

        GRBVar* vars_; //!< Gurobi variables for this problem
        unsigned int num_vars_; //!< Number of variables in this problem

    };

  } // namespace ml
} // namespace cannon

#endif  // CANNON_HAVE_GUROBI

#endif /* ifndef CANNON_ML_LINEAR_PROGRAMMING_H */