z1_controller/include/common/math/mathTools.h

#ifndef MATHTOOLS_H
#define MATHTOOLS_H

#include <iostream>
// #include "common/mathTypes.h"

// template<typename T1, typename T2>
// inline T1 max(const T1 a, const T2 b){
// 	return (a > b ? a : b);
// }

// template<typename T1, typename T2>
// inline T1 min(const T1 a, const T2 b){
// 	return (a < b ? a : b);
// }

template<typename T>
T max(T value){
    return value;
}

template<typename T, typename... Args>
inline T max(const T val0, const Args... restVal){
    return val0 > (max<T>(restVal...)) ? val0 : max<T>(restVal...);
}

template<typename T>
T min(T value){
    return value;
}

template<typename T, typename... Args>
inline T min(const T val0, const Args... restVal){
    return val0 > (min<T>(restVal...)) ? val0 : min<T>(restVal...);
}

inline Mat2 skew(const double& w){
    Mat2 mat; mat.setZero();
    mat(0, 1) = -w;
    mat(1, 0) =  w;
    return mat;
}

inline Mat3 skew(const Vec3& v) {
    Mat3 m;
    m << 0, -v(2), v(1),
            v(2), 0, -v(0),
            -v(1), v(0), 0;
    return m;
}

enum class TurnDirection{
    NOMATTER,
    POSITIVE,
    NEGATIVE
};

/* first - second */
inline double angleError(double first, double second, TurnDirection direction = TurnDirection::NOMATTER){
    double firstMod = fmod(first, 2.0*M_PI);
    double secondMod = fmod(second, 2.0*M_PI);

    if(direction == TurnDirection::NOMATTER){
        if(fabs(firstMod - secondMod) > fabs(secondMod - firstMod)){
            return secondMod - firstMod;
        }else{
            return firstMod - secondMod;
        }
    }
    else if(direction == TurnDirection::POSITIVE){
        if(firstMod - secondMod < 0.0){
            return 2*M_PI + firstMod - secondMod;
        }else{
            return firstMod - secondMod;
        }
    }
    else if(direction == TurnDirection::NEGATIVE){
        if(firstMod - secondMod > 0.0){
            return -2*M_PI + firstMod - secondMod;
        }else{
            return firstMod - secondMod;
        }
    }
}

/* firstVec - secondVec */
inline VecX angleError(VecX firstVec, VecX secondVec, TurnDirection directionMatter = TurnDirection::NOMATTER){
    if(firstVec.rows() != secondVec.rows()){
        std::cout << "[ERROR] angleError, the sizes of firstVec and secondVec are different!" << std::endl;
    }

    VecX result = firstVec;
    for(int i(0); i<firstVec.rows(); ++i){
        result(i) = angleError(firstVec(i), secondVec(i), directionMatter);
    }

    return result;
}

inline bool vectorEqual(VecX v1, VecX v2, double tolerance){
    if(v1.rows() != v2.rows()){
        std::cout << "[WARNING] vectorEqual, the size of two vectors is not equal, v1 is "
            << v1.rows() << ", v2 is " << v2.rows() << std::endl;
        return false;
    }
    for(int i(0); i<v1.rows(); ++i){
        if(fabs(v1(i)-v2(i))>tolerance){
            return false;
        }
    }
    return true;
}

inline bool inInterval(double value, double limValue1, double limValue2, bool canEqual1 = false, bool canEqual2 = false){
    double lowLim, highLim;
    bool lowEqual, highEqual;
    if(limValue1 >= limValue2){
        highLim   = limValue1;
        highEqual = canEqual1;
        lowLim    = limValue2;
        lowEqual  = canEqual2;
    }else{
        lowLim    = limValue1;
        lowEqual  = canEqual1;
        highLim   = limValue2;
        highEqual = canEqual2;
    }

    if((value > highLim) || (value < lowLim)){
        return false;
    }
    if((value == highLim) && !highEqual){
        return false;
    }
    if((value == lowLim) && !lowEqual){
        return false;
    }

    return true;
}

inline double saturation(const double a, double limValue1, double limValue2){
    double lowLim, highLim;
    if(limValue1 >= limValue2){
        lowLim = limValue2;
        highLim= limValue1;
    }else{
        lowLim = limValue1;
        highLim= limValue2;
    }

    if(a < lowLim){
        return lowLim;
    }
    else if(a > highLim){
        return highLim;
    }
    else{
        return a;
    }
}

inline double saturation(const double a, Vec2 limits){
    return saturation(a, limits(0), limits(1));
}

template<typename T0, typename T1>
inline T0 killZeroOffset(T0 a, const T1 limit){
    if((a > -limit) && (a < limit)){
        a = 0;
    }
    return a;
}

template<typename T0, typename T1, typename T2>
inline T1 invNormalize(const T0 value, const T1 min, const T2 max, const double minLim = -1, const double maxLim = 1){
	return (value-minLim)*(max-min)/(maxLim-minLim) + min;
}

// x: [0, 1], windowRatio: (0, 0.5)
template<typename T>
inline T windowFunc(const T x, const T windowRatio, const T xRange=1.0, const T yRange=1.0){
    if((x < 0)||(x > xRange)){
        // printf("[ERROR] The x of function windowFunc should between [0, xRange]\n");
        std::cout << "[ERROR][windowFunc] The x=" << x << ", which should between [0, xRange]" << std::endl;
    }
    if((windowRatio <= 0)||(windowRatio >= 0.5)){
        // printf("[ERROR] The windowRatio of function windowFunc should between (0, 0.5)\n");
        std::cout << "[ERROR][windowFunc] The windowRatio=" << windowRatio << ", which should between [0, 0.5]" << std::endl;
    }

    if(x/xRange < windowRatio){
        return x * yRange / (xRange * windowRatio);
    }
    else if(x/xRange > 1 - windowRatio){
        return yRange * (xRange - x)/(xRange * windowRatio);
    }
    else{
        return yRange;
    }
}

template<typename T1, typename T2>
inline void updateAverage(T1 &exp, T2 newValue, double n){
    if(exp.rows()!=newValue.rows()){
        std::cout << "The size of updateAverage is error" << std::endl;
        exit(-1);
    }
    if(fabs(n - 1) < 0.001){
        exp = newValue;
    }else{
        exp = exp + (newValue - exp)/n;
    }
}

template<typename T1, typename T2, typename T3>
inline void updateCovariance(T1 &cov, T2 expPast, T3 newValue, double n){
    if( (cov.rows()!=cov.cols()) || (cov.rows() != expPast.rows()) || (expPast.rows()!=newValue.rows())){
        std::cout << "The size of updateCovariance is error" << std::endl;
        exit(-1);
    }
    if(fabs(n - 1) < 0.1){
        cov.setZero();
    }else{
        cov = cov*(n-1)/n + (newValue-expPast)*(newValue-expPast).transpose()*(n-1)/(n*n);
    }
}

template<typename T1, typename T2, typename T3>
inline void updateAvgCov(T1 &cov, T2 &exp, T3 newValue, double n){
    // The order matters!!! covariance first!!!
    updateCovariance(cov, exp, newValue, n);
    updateAverage(exp, newValue, n);
}

/* rotate matrix about x axis */
inline RotMat rotX(const double &theta) {
    double s = std::sin(theta);
    double c = std::cos(theta);

    RotMat R;
    R << 1, 0, 0, 0, c, -s, 0, s, c;
    return R;
}

/* rotate matrix about y axis */
inline RotMat rotY(const double &theta) {
    double s = std::sin(theta);
    double c = std::cos(theta);

    RotMat R;
    R << c, 0, s, 0, 1, 0, -s, 0, c;
    return R;
}

/* rotate matrix about z axis */
inline RotMat rotZ(const double &theta) {
    double s = std::sin(theta);
    double c = std::cos(theta);

    RotMat R;
    R << c, -s, 0, s, c, 0, 0, 0, 1;
    return R;
}

inline RotMat so3ToRotMat(const Vec3& _rot){
    RotMat R;
    double theta = _rot.norm();
    if (fabs(theta) <1e-6)
        R = RotMat::Identity();
    else{
        Vec3  u_axis(_rot/theta);
        double cos_theta = cos(theta);
        R = cos_theta*RotMat::Identity()+sin(theta)*skew(u_axis) +(1-cos_theta)*(u_axis*u_axis.transpose());
    }
    return R;
}

/* row pitch yaw to rotate matrix */
inline RotMat rpyToRotMat(const double& row, const double& pitch, const double& yaw) {
    RotMat m = rotZ(yaw) * rotY(pitch) * rotX(row);
    return m;
}

inline RotMat quatToRotMat(const Quat& q) {
    double e0 = q(0);
    double e1 = q(1);
    double e2 = q(2);
    double e3 = q(3);

    RotMat R;
    R << 1 - 2 * (e2 * e2 + e3 * e3), 2 * (e1 * e2 - e0 * e3),
            2 * (e1 * e3 + e0 * e2), 2 * (e1 * e2 + e0 * e3),
            1 - 2 * (e1 * e1 + e3 * e3), 2 * (e2 * e3 - e0 * e1),
            2 * (e1 * e3 - e0 * e2), 2 * (e2 * e3 + e0 * e1),
            1 - 2 * (e1 * e1 + e2 * e2);
    return R;
}

inline Vec3 rotMatToRPY(const Mat3& R) {
    Vec3 rpy;
    rpy(0) = atan2(R(2,1),R(2,2));//asin(R(2,1)/cos(rpy(1))); // atan2(R(2,1),R(2,2));
    rpy(1) = asin(-R(2,0));
    rpy(2) = atan2(R(1,0),R(0,0));
    return rpy;
}

/* rotate matrix to exponential coordinate(omega*theta) */
inline Vec3 rotMatToExp(const RotMat& rm){
    double cosValue = rm.trace()/2.0-1/2.0;
    if(cosValue > 1.0f){
        cosValue = 1.0f;
    }else if(cosValue < -1.0f){
        cosValue = -1.0f;
    }

    double angle = acos(cosValue);
    Vec3 exp;
    if (fabs(angle) < 1e-5){
        exp=Vec3(0,0,0);
    }
    else if (fabs(angle - M_PI) < 1e-5){
        exp = angle * Vec3(rm(0,0)+1, rm(0,1), rm(0,2)) / sqrt(2*(1+rm(0, 0)));
    }
    else{
        exp=angle/(2.0f*sin(angle))*Vec3(rm(2,1)-rm(1,2),rm(0,2)-rm(2,0),rm(1,0)-rm(0,1));
    }
    return exp;
}

/* add 1 at the end of Vec3 */
inline Vec4 homoVec(Vec3 v3, double end = 1.0){
    Vec4 v4;
    v4.block(0, 0, 3, 1) = v3;
    v4(3) = end;
    return v4;
}

/* remove 1 at the end of Vec4 */
inline Vec3 noHomoVec(Vec4 v4){
    Vec3 v3;
    v3 = v4.block(0, 0, 3, 1);
    return v3;
}

/* build Homogeneous Matrix by p and m */
inline HomoMat homoMatrix(Vec3 p, Mat3 m){
    HomoMat homoM;
    homoM.setZero();
    homoM.topLeftCorner(3, 3) = m;
    homoM.topRightCorner(3, 1) = p;
    homoM(3, 3) = 1;
    return homoM;
}

/* build Homogeneous Matrix by p and w */
inline HomoMat homoMatrix(Vec3 p, Vec3 w){
    HomoMat homoM;
    homoM.setZero();
    homoM.topLeftCorner(3, 3) = so3ToRotMat(w);
    homoM.topRightCorner(3, 1) = p;
    homoM(3, 3) = 1;
    return homoM;
}

/* build Homogeneous Matrix by x axis, y axis and p */
inline HomoMat homoMatrix(Vec3 x, Vec3 y, Vec3 p){
    HomoMat homoM;
    homoM.setZero();
    Vec3 xN = x.normalized();
    Vec3 yN = y.normalized();
    homoM.block(0, 0, 3, 1) = xN;
    homoM.block(0, 1, 3, 1) = yN;
    homoM.block(0, 2, 3, 1) = skew(xN) * yN;
    homoM.topRightCorner(3, 1) = p;
    homoM(3, 3) = 1;
    return homoM;
}

/* build Homogeneous Matrix of rotate joint */
inline HomoMat homoMatrixRotate(Vec3 q, Vec3 w){
    HomoMat homoM;
    Mat3 eye3 = Mat3::Identity();
    Mat3 rotateM = so3ToRotMat(w);
    homoM.setZero();
    homoM.topLeftCorner(3, 3) = rotateM;
    homoM.topRightCorner(3, 1) = (eye3 - rotateM) * q;
    // homoM.topRightCorner(3, 1) = q;
    homoM(3, 3) = 1;

    return homoM;
}

/* build Homogeneous Matrix by posture */
inline HomoMat homoMatrixPosture(Vec6 posture){
    HomoMat homoM;
    homoM.setZero();
    homoM.topLeftCorner(3, 3) = rpyToRotMat(posture(0),posture(1),posture(2));
    homoM.topRightCorner(3, 1) << posture(3),posture(4),posture(5);
    homoM(3, 3) = 1;
    return homoM;
}

/* inverse matrix of homogeneous matrix */
inline HomoMat homoMatrixInverse(HomoMat homoM){
    HomoMat homoInv;
    homoInv.setZero();
    homoInv.topLeftCorner(3, 3) = homoM.topLeftCorner(3, 3).transpose();
    homoInv.topRightCorner(3, 1) = -homoM.topLeftCorner(3, 3).transpose() * homoM.topRightCorner(3, 1);
    homoInv(3, 3) = 1;
    return homoInv;
}

/* get position of Homogeneous Matrix */
inline Vec3 getHomoPosition(HomoMat mat){
    return mat.block(0, 3, 3, 1);
}

/* get rotate matrix of Homogeneous Matrix */
inline RotMat getHomoRotMat(HomoMat mat){
    return mat.block(0, 0, 3, 3);
}

/* convert homogeneous matrix to posture vector */
inline Vec6 homoToPosture(HomoMat mat){
    Vec6 posture;
    posture.block(0,0,3,1) = rotMatToRPY(getHomoRotMat(mat));
    posture.block(3,0,3,1) = getHomoPosition(mat);
    return posture;
}

/* convert posture vector matrix to homogeneous */
inline HomoMat postureToHomo(Vec6 posture){
    HomoMat homoM;
    homoM.setZero();
    homoM.topLeftCorner(3, 3) = rpyToRotMat(posture(0), posture(1), posture(2));
    homoM.topRightCorner(3, 1) << posture(3), posture(4), posture(5);
    homoM(3, 3) = 1;
    return homoM;
}

// Calculate average value and covariance
class AvgCov{
public:
    AvgCov(unsigned int size, std::string name, bool avgOnly=false, unsigned int showPeriod=1000, unsigned int waitCount=5000, double zoomFactor=10000)
            :_size(size), _showPeriod(showPeriod), _waitCount(waitCount), _zoomFactor(zoomFactor), _valueName(name), _avgOnly(avgOnly) {
        _exp.resize(size);
        _cov.resize(size, size);
        _defaultWeight.resize(size, size);
        _defaultWeight.setIdentity();
        _measureCount = 0;
    }
    void measure(VecX newValue){
        ++_measureCount;

        if(_measureCount > _waitCount){
            updateAvgCov(_cov, _exp, newValue, _measureCount-_waitCount);
            if(_measureCount % _showPeriod == 0){
            // if(_measureCount < _waitCount + 5){
                std::cout << "******" << _valueName << " measured count: " << _measureCount-_waitCount << "******" << std::endl;
                // std::cout << _zoomFactor << " Times newValue of " << _valueName << std::endl << (_zoomFactor*newValue).transpose() << std::endl;
                std::cout << _zoomFactor << " Times Average of " << _valueName << std::endl << (_zoomFactor*_exp).transpose() << std::endl;
                if(!_avgOnly){
                    std::cout << _zoomFactor << " Times Covariance of " << _valueName << std::endl << _zoomFactor*_cov << std::endl;
                }
            }
        }
    }
private:
    VecX _exp;
    MatX _cov;
    MatX _defaultWeight;
    bool _avgOnly;
    unsigned int _size;
    unsigned int _measureCount;
    unsigned int _showPeriod;
    unsigned int _waitCount;
    double _zoomFactor;
    std::string _valueName;
};

#endif  // MATHTOOLS_H