//
// Created by jachu on 18.07.17.
//

#define CATCH_CONFIG_MAIN
#include "catch.hpp"

#include <vector>

#include <Eigen/Eigen>

#include "Matching.hpp"
#include "Misc.hpp"

using namespace std;

void transformObjs(const std::vector<Eigen::Vector3d> &points,
                   const std::vector<Eigen::Vector4d> &planes,
                   const std::vector<Vector6d> &lines,
                   std::vector<Eigen::Vector3d> &retPoints,
                   std::vector<Eigen::Vector4d> &retPlanes,
                   std::vector<Vector6d> &retLines,
                   Eigen::Matrix3d rotMat,
                   Eigen::Vector3d trans,
                   double rotNoise = 0.0,
                   double transNoise = 0.0)
{
    for(int p = 0; p < points.size(); ++p){
        retPoints.push_back(rotMat * points[p] + trans + transNoise * Eigen::Vector3d::Random());
    }
    Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
    T.block<3, 3>(0,0) = rotMat;
    T.block<3, 1>(0, 3) = trans;
    Eigen::Matrix4d Tinvt = T.inverse().transpose();
    for(int pl = 0; pl < planes.size(); ++pl){
        Eigen::Vector4d planeTrans = Tinvt * planes[pl];
        planeTrans.head<3>() += rotNoise * Eigen::Vector3d::Random();
        planeTrans.tail<1>() += transNoise * Eigen::MatrixXd::Random(1, 1);
        double nNorm = planeTrans.head<3>().norm();
        planeTrans /= nNorm;
        retPlanes.push_back(planeTrans);
    }
    for(int l = 0; l < lines.size(); ++l){
        Eigen::Vector3d p = lines[l].head<3>();
        Eigen::Vector3d n = lines[l].tail<3>();
        Eigen::Vector3d pTrans = rotMat * p + trans + transNoise * Eigen::Vector3d::Random();
        Eigen::Vector3d nTrans = rotMat * n + rotNoise * Eigen::Vector3d::Random();
        nTrans.normalize();
        // move point to be closest to the origin
        pTrans = Misc::closestPointOnLine(Eigen::Vector3d::Zero(), pTrans, nTrans);
        Vector6d lTrans;
        lTrans.head<3>() = pTrans;
        lTrans.tail<3>() = nTrans;
        retLines.push_back(lTrans);
    }
}

void testFullConstr(const std::vector<Eigen::Vector3d> &points,
                    const std::vector<Eigen::Vector3d> &virtPoints,
                    const std::vector<Eigen::Vector3d> &dirs,
                    const std::vector<double> &dists,
                    const std::vector<Eigen::Vector3d> &distDirs,
                    const std::vector<Eigen::Vector3d> &distPts,
                    const std::vector<Eigen::Vector3d> &distPtsDirs,
                    bool &fullConstrRot,
                    bool &fullConstrTrans)
{
    {
        int rotEqNum = dirs.size();
        if (points.size() + virtPoints.size() > 0) {
            rotEqNum += points.size() + virtPoints.size() - 1;
        }
        Eigen::MatrixXd rotMat(3, rotEqNum);
        int rotEqCnt = 0;

        // treating points as directions from the first point
        Eigen::Vector3d origPoint;
        if(points.size() > 0){
            origPoint = points[0];
        }
        else if(virtPoints.size() > 0){
            origPoint = virtPoints[0];
        }
        for (int p = 1; p < points.size(); ++p) {
            rotMat.block<3, 1>(0, rotEqCnt++) = points[p] - origPoint;
        }
        // same for virtual points
        int vpStart = 0;
        if(points.size() == 0){
            vpStart = 1;
        }
        for(int vp = vpStart; vp < virtPoints.size(); ++vp){
            rotMat.block<3, 1>(0, rotEqCnt++) = virtPoints[vp] - origPoint;
        }

        for (int d = 0; d < dirs.size(); ++d) {
            rotMat.block<3, 1>(0, rotEqCnt++) = dirs[d];
        }
//        if(rotEqCnt != rotEqNum){
//            cout << "rotEqCnt != rotEqNum" << endl;
//            throw std::exception();
//        }
        Eigen::FullPivLU<Eigen::MatrixXd> rotLu(rotMat);
//            lu.setThreshold(sinValsThresh * distScale);
        int rotRank = rotLu.rank();
        cout << "rotLu.rank() = " << rotLu.rank() << endl;
        if (rotRank < 2) {
            fullConstrRot = false;
        } else {
            fullConstrRot = true;
        }
    }
    {
        int transEqNum = dists.size();
        if (fullConstrRot) {
            transEqNum += points.size() * 3 + distPts.size();
        }
        Eigen::MatrixXd transMat(3, transEqNum);
        int transEqCnt = 0;
        for(int d = 0; d < dists.size(); ++d){
            transMat.block<3, 1>(0, transEqCnt++) = distDirs[d];
        }
        if(fullConstrRot) {
            for (int p = 0; p < points.size(); ++p) {
                transMat.block<3, 1>(0, transEqCnt++) = Eigen::Vector3d::UnitX();
                transMat.block<3, 1>(0, transEqCnt++) = Eigen::Vector3d::UnitY();
                transMat.block<3, 1>(0, transEqCnt++) = Eigen::Vector3d::UnitZ();
            }
            cout << "distPts.size() = " << distPts.size() << endl;
            cout << "distPtsDirs.size() = " << distPtsDirs.size() << endl;
            for(int dp = 0; dp < distPts.size(); ++dp){
                transMat.block<3, 1>(0, transEqCnt++) = distPtsDirs[dp];
            }
        }
//        if(transEqCnt != transEqNum){
//            cout << "transEqCnt != transEqNum" << endl;
//            throw std::exception();
//        }
        Eigen::FullPivLU<Eigen::MatrixXd> transLu(transMat);
//            lu.setThreshold(sinValsThresh * distScale);
        int transRank = transLu.rank();
        cout << "transLu.rank() = " << transLu.rank() << endl;
        if (transRank < 3) {
            fullConstrTrans = false;
        } else {
            fullConstrTrans = true;
        }
    }

}

Vector7d testTransform(const std::vector<Eigen::Vector3d> &points,
                       const std::vector<Eigen::Vector4d> &planes,
                       const std::vector<Vector6d> &lines,
                       Vector7d transform,
                       double sinValsThresh)
{
    cout << endl << "testing transformation" << endl;
    cout << "point.size() = " << points.size() << endl;
    cout << "planes.size() = " << planes.size() << endl;
    cout << "lines.size() = " << lines.size() << endl;

    for(int pt = 0; pt < points.size(); ++pt){
        cout << "points[" << pt << "] = " << points[pt].transpose() << endl;
    }
    for(int pl = 0; pl < planes.size(); ++pl){
        cout << "planes[" << pl << "] = " << planes[pl].transpose() << endl;
    }
    for(int l = 0; l < lines.size(); ++l){
        cout << "lines[" << l << "] = " << lines[l].transpose() << endl;
    }

    Eigen::Vector3d trans = transform.head<3>();
    Eigen::Vector4d rot = transform.tail<4>();
    Eigen::Matrix3d rotMat = Eigen::Quaterniond(rot[3], rot[0], rot[1], rot[2]).toRotationMatrix();

    std::vector<Eigen::Vector3d> retPoints;
    std::vector<Eigen::Vector3d> retVirtPoints;
    std::vector<Eigen::Vector3d> retDirs;
    std::vector<double> retDists;
    std::vector<Eigen::Vector3d> retDistDirs;
    std::vector<Eigen::Vector3d> retDistPts;
    std::vector<Eigen::Vector3d> retDistPtsDirs;
    Matching::convertToPointsDirsDists(points,
                                       planes,
                                       lines,
                                       retPoints,
                                       retVirtPoints,
                                       retDirs,
                                       retDists,
                                       retDistDirs,
                                       retDistPts,
                                       retDistPtsDirs);

    bool fullConstrRot = true;
    bool fullConstrTrans = true;
    testFullConstr(retPoints,
                   retVirtPoints,
                   retDirs,
                   retDists,
                   retDistDirs,
                   retDistPts,
                   retDistPtsDirs,
                   fullConstrRot,
                   fullConstrTrans);

    vector<Eigen::Vector3d> transPoints;
    vector<Eigen::Vector4d> transPlanes;
    vector<Vector6d> transLines;
    transformObjs(points,
                  planes,
                  lines,
                  transPoints,
                  transPlanes,
                  transLines,
                  rotMat,
                  trans,
                  0.01,
                  0.01);

    std::vector<Eigen::Vector3d> retTransPoints;
    std::vector<Eigen::Vector3d> retTransVirtPoints;
    std::vector<Eigen::Vector3d> retTransDirs;
    std::vector<double> retTransDists;
    std::vector<Eigen::Vector3d> retTransDistDirs;
    std::vector<Eigen::Vector3d> retTransDistPts;
    std::vector<Eigen::Vector3d> retTransDistPtsDirs;
    Matching::convertToPointsDirsDists(transPoints,
                       transPlanes,
                       transLines,
                       retTransPoints,
                       retTransVirtPoints,
                       retTransDirs,
                       retTransDists,
                       retTransDistDirs,
                       retTransDistPts,
                       retTransDistPtsDirs);

    bool fullConstrRotComp = true;
    bool fullConstrTransComp = true;
    cout << "Running bestTransformPointsDirsDists" << endl;
    Vector7d transformComp = Matching::bestTransformPointsDirsDists(retTransPoints,
                                                                    retPoints,
                                                                    vector<double>(retPoints.size(), 1.0),
                                                                    retTransVirtPoints,
                                                                    retVirtPoints,
                                                                    vector<double>(retVirtPoints.size(), 1.0),
                                                                    retTransDirs,
                                                                    retDirs,
                                                                    vector<double>(retDirs.size(), 1.0),
                                                                    retTransDists,
                                                                    retDists,
                                                                    retTransDistDirs,
                                                                    vector<double>(retDists.size(), 1.0),
                                                                    retTransDistPts,
                                                                    retDistPts,
                                                                    retTransDistPtsDirs,
                                                                    vector<double>(retDistPts.size(), 1.0),
                                                                    sinValsThresh,
                                                                    fullConstrRotComp,
                                                                    fullConstrTransComp);

//    for(int dp = 0; dp < retDistPts.size(); ++dp){
//        Eigen::Matrix4d Wrt = Misc::matrixW(Eigen::Quaterniond(transform[6],
//                                               transform[3],
//                                               transform[4],
//                                               transform[5]).normalized()).transpose();
//        Eigen::Matrix4d Qr = Misc::matrixQ(Eigen::Quaterniond(transform[6],
//                                                              transform[3],
//                                                              transform[4],
//                                                              transform[5]).normalized());
//
//        cout << "retDistPts = " << retDistPts[dp].transpose() << endl;
//        cout << "retTransDistPts = " << retTransDistPts[dp].transpose() << endl;
//        cout << "retTransDistPtsDir = " << retTransDistPtsDirs[dp].transpose() << endl;
//
//        double d1 = retTransDistPtsDirs[dp].dot(retTransDistPts[dp]);
//
//        Eigen::Vector4d p2quat = Eigen::Vector4d::Zero();
//        p2quat.head<3>() = retDistPts[dp];
//        Eigen::Vector4d p2trans = Wrt * Qr * p2quat;
//        double d2 = retTransDistPtsDirs[dp].dot(p2trans.head<3>());
//
//        cout << "d1 = " << d1 << endl;
//        cout << "d2 = " << d2 << endl;
//
//        cout << "t = " << trans.transpose() << endl;
//        cout << "n * t = " << retTransDistPtsDirs[dp].dot(trans) << endl;
//        cout << "d1 - d2 = " << (d1 - d2) << endl;
//        cout << "n * t - (d1 - d2) = " << retTransDistPtsDirs[dp].dot(trans) - (d1 - d2) << endl;
//
//
//    }

    double dist = Misc::transformLogDist(transform, transformComp);

    cout << "transform = " << transform.transpose() << endl;
    cout << "transformComp = " << transformComp.transpose() << endl;
    REQUIRE(fullConstrRot == fullConstrRotComp);
    REQUIRE(fullConstrTrans == fullConstrTransComp);
    if(fullConstrRot == true &&
       fullConstrTrans == true)
    {
        REQUIRE(dist < 0.4);
    }
    if(fullConstrRot == true){
        double rotDist = Misc::rotLogDist(transform.tail<4>(), transformComp.tail<4>());
        REQUIRE(rotDist < 0.2);
    }
    if(fullConstrTrans == true){
        Eigen::Vector3d t = transform.head<3>();
        Eigen::Vector3d tComp = transformComp.head<3>();
        Eigen::Vector3d tDiff = t - tComp;
        double tDist = tDiff.transpose() * tDiff;
        REQUIRE(tDist < 0.2);
    }
}

TEST_CASE("best transformations are correct", "[transformations]"){
    static constexpr int numPts = 10;
    static constexpr int numPls = 10;
    static constexpr int numLines = 10;
    static constexpr int numTests = 10;
    static constexpr double distScale = 10.0;
    static constexpr double sinValsThresh = 0.001;

    cout << "Starting test" << endl;
    //[x, y, z]
    vector<Eigen::Vector3d> points;
    //[nx, ny, nz, -d]
    vector<Eigen::Vector4d> planes;
    //[px, py, pz, nx, ny, nz]
    vector<Vector6d> lines;

    for(int pt = 0; pt < numPts; ++pt){
        points.push_back(Eigen::Vector3d::Random());
        // from -10 do 10
        points.back() *= distScale;
    }
    for(int pl = 0; pl < numPls; ++pl){
        Eigen::Vector3d n = Eigen::Vector3d::Random();
        // avoid numerical errors due to dividing by small numbers
        while(n.norm() < 1e-6){
            n = Eigen::Vector3d::Random();
        }
        n.normalize();

        Eigen::Vector4d curPl = Eigen::Vector4d::Random();
        curPl.head<3>() = n;
        curPl[3] *= distScale;
        planes.push_back(curPl);
    }
    for(int l = 0; l < numLines; ++l){
        Eigen::Vector3d n = Eigen::Vector3d::Random();
        // avoid numerical errors due to dividing by small numbers
        while(n.norm() < 1e-6){
            n = Eigen::Vector3d::Random();
        }
        n.normalize();
        Eigen::Vector3d p = Eigen::Vector3d::Random();
        p *= distScale;
        // move point to be closest to the origin
        p = Misc::closestPointOnLine(Eigen::Vector3d::Zero(),
                                                    p,
                                                    n);

        Vector6d curLine;
        curLine.head<3>() = p;
        curLine.tail<3>() = n;
        lines.push_back(curLine);
    }

    for(int t = 0; t < numTests; ++t){
        Eigen::Vector3d trans = Eigen::Vector3d::Random();
        Eigen::Vector4d rot = Eigen::Vector4d::Random();
        rot.normalize();
//        Eigen::Matrix3d rotMat = Eigen::Quaterniond(rot[3], rot[0], rot[1], rot[2]).toRotationMatrix();
        Vector7d transform;
        transform.head<3>() = trans;
        transform.tail<4>() = rot;

        //points only
        for(int cpts = 0; cpts <= numPts; ++cpts){
            std::default_random_engine gen;
            std::uniform_int_distribution<int> distrPts(0, numPts - 1);

            vector<Eigen::Vector3d> curPts;
            for(int p = 0; p < cpts; ++p){
                int idx = distrPts(gen);
                curPts.push_back(points[idx]);
            }

            testTransform(curPts,
                          vector<Eigen::Vector4d>(),
                          vector<Vector6d>(),
                          transform,
                          sinValsThresh);
        }

        //planes only
        for(int cpls = 0; cpls <= numPls; ++cpls){
            std::default_random_engine gen;
            std::uniform_int_distribution<int> distrPls(0, numPls - 1);

            vector<Eigen::Vector4d> curPls;
            for(int pl = 0; pl < cpls; ++pl){
                int idx = distrPls(gen);
                curPls.push_back(planes[idx]);
            }

            testTransform(vector<Eigen::Vector3d>(),
                          curPls,
                          vector<Vector6d>(),
                          transform,
                          sinValsThresh);
        }

        //lines only
        for(int clines = 0; clines <= numLines; ++clines){
            std::default_random_engine gen;
            std::uniform_int_distribution<int> distrLines(0, numLines - 1);

            vector<Vector6d> curLines;
            for(int l = 0; l < clines; ++l){
                int idx = distrLines(gen);
                curLines.push_back(lines[idx]);
            }

            testTransform(vector<Eigen::Vector3d>(),
                          vector<Eigen::Vector4d>(),
                          curLines,
                          transform,
                          sinValsThresh);
        }

        //points + planes
        for(int cpts = 0; cpts <= numPts; ++cpts){
            for(int cpls = 0; cpls <= numPls; ++cpls) {
                std::default_random_engine gen;

                std::uniform_int_distribution<int> distrPts(0, numPts - 1);

                vector<Eigen::Vector3d> curPts;
                for (int p = 0; p < cpts; ++p) {
                    int idx = distrPts(gen);
                    curPts.push_back(points[idx]);
                }

                std::uniform_int_distribution<int> distrPls(0, numPls - 1);

                vector<Eigen::Vector4d> curPls;
                for (int pl = 0; pl < cpls; ++pl) {
                    int idx = distrPls(gen);
                    curPls.push_back(planes[idx]);
                }

                testTransform(curPts,
                              curPls,
                              vector<Vector6d>(),
                              transform,
                              sinValsThresh);
            }
        }

        //points + lines
        for(int cpts = 0; cpts <= numPts; ++cpts){
            for(int clines = 0; clines <= numLines; ++clines) {
                std::default_random_engine gen;
                std::uniform_int_distribution<int> distrPts(0, numPts - 1);

                vector<Eigen::Vector3d> curPts;
                for (int p = 0; p < cpts; ++p) {
                    int idx = distrPts(gen);
                    curPts.push_back(points[idx]);
                }

                std::uniform_int_distribution<int> distrLines(0, numLines - 1);

                vector<Vector6d> curLines;
                for (int l = 0; l < clines; ++l) {
                    int idx = distrLines(gen);
                    curLines.push_back(lines[idx]);
                }

                testTransform(curPts,
                              vector<Eigen::Vector4d>(),
                              curLines,
                              transform,
                              sinValsThresh);
            }
        }

        //planes + lines
        for(int cpls = 0; cpls <= numPls; ++cpls){
            for(int clines = 0; clines <= numLines; ++clines) {
                std::default_random_engine gen;
                std::uniform_int_distribution<int> distrPls(0, numPls - 1);

                vector<Eigen::Vector4d> curPls;
                for (int pl = 0; pl < cpls; ++pl) {
                    int idx = distrPls(gen);
                    curPls.push_back(planes[idx]);
                }

                std::uniform_int_distribution<int> distrLines(0, numLines - 1);

                vector<Vector6d> curLines;
                for (int l = 0; l < clines; ++l) {
                    int idx = distrLines(gen);
                    curLines.push_back(lines[idx]);
                }

                testTransform(vector<Eigen::Vector3d>(),
                              curPls,
                              curLines,
                              transform,
                              sinValsThresh);
            }
        }

        //points + planes + lines
        for(int cpts = 0; cpts <= numPts; ++cpts){
            for(int cpls = 0; cpls <= numPls; ++cpls) {
                for(int clines = 0; clines <= numLines; ++clines) {
                    std::default_random_engine gen;

                    std::uniform_int_distribution<int> distrPts(0, numPts - 1);

                    vector<Eigen::Vector3d> curPts;
                    for (int p = 0; p < cpts; ++p) {
                        int idx = distrPts(gen);
                        curPts.push_back(points[idx]);
                    }

                    std::uniform_int_distribution<int> distrPls(0, numPls - 1);

                    vector<Eigen::Vector4d> curPls;
                    for (int pl = 0; pl < cpls; ++pl) {
                        int idx = distrPls(gen);
                        curPls.push_back(planes[idx]);
                    }

                    std::uniform_int_distribution<int> distrLines(0, numLines - 1);

                    vector<Vector6d> curLines;
                    for (int l = 0; l < clines; ++l) {
                        int idx = distrLines(gen);
                        curLines.push_back(lines[idx]);
                    }

                    testTransform(curPts,
                                  curPls,
                                  curLines,
                                  transform,
                                  sinValsThresh);
                }
            }
        }
    }
}


#include <iostream>
#include <Eigen/Dense>
#include <unsupported/Eigen/LevenbergMarquardt>
#include <unsupported/Eigen/MatrixFunctions>

using namespace Eigen;
using namespace std;

void flatten(Matrix<double, 4, 4> &g, Matrix<double,1,12> &y) {
    y(0) = g(0,0);
    y(1) = g(0,1);
    y(2) = g(0,2);
    y(3) = g(1,0);
    y(4) = g(1,1);
    y(5) = g(1,2);
    y(6) = g(2,0);
    y(7) = g(2,1);
    y(8) = g(2,2);
    y(9) = g(0,3);
    y(10) = g(1,3);
    y(11) = g(2,3);
}

void unflatten(Matrix<double, 1, 12> &y, Matrix<double, 4, 4> &g) {
    g(0,0) = y(0);
    g(0,1) = y(1);
    g(0,2) = y(2);
    g(1,0) = y(3);
    g(1,1) = y(4);
    g(1,2) = y(5);
    g(2,0) = y(6);
    g(2,1) = y(7);
    g(2,2) = y(8);
    g(0,3) = y(9);
    g(1,3) = y(10);
    g(2,3) = y(11);
    g(3,0) = 0;
    g(3,1) = 0;
    g(3,2) = 0;
    g(3,3) = 1;
}

void skew(Matrix<double, 3, 1> &x, Matrix<double, 3, 3> &y) {
    y(0,1) = -x(2);
    y(0,2) = x(1);
    y(1,0) = x(2);
    y(1,2) = -x(0);
    y(2,0) = -x(1);
    y(2,1) = x(0);
    y(0,0) = 0;
    y(1,1) = 0;
    y(2,2) = 0;
}

void adjoint(Matrix<double, 6, 1> &x, Matrix<double, 6, 6> &y) {
    Vector3d w = x.head<3>();
    Vector3d v = x.tail<3>();
    Matrix3d w_skew;
    Matrix3d v_skew;

    skew(w,w_skew);
    skew(v,v_skew);

    y.block<3,3>(0,0) = w_skew;
    y.block<3,3>(3,0) = v_skew;
    y.block<3,3>(0,3) = Matrix3d::Zero();
    y.block<3,3>(3,3) = w_skew;
}

void se(Matrix<double, 6, 1> &x, Matrix<double, 4, 4> &y) {
    Vector3d w = x.head<3>();
    Vector3d v = x.tail<3>();
    Matrix3d w_skew;
    skew(w,w_skew);
    y.block<3,3>(0,0) = w_skew;
    y.block<3,1>(0,3) = v;
    y(3,0) = 0;
    y(3,1) = 0;
    y(3,2) = 0;
    y(3,3) = 0;

}

// initial implementation of rod
class Rod {
    private:
        double D;
        double L;
        double E;
        double G;
        double rho;
        double mu;
        double ds;
        int N;
        Matrix<double, 6, 6> K; //not necessarily diagonal, but for now its alright
        DiagonalMatrix<double, 6> M; //always diagonal
        DiagonalMatrix<double, 6> V; //I suppose not always diagonal
        Matrix<double, 6, 1> xi_ref;
    public:
        Matrix<double, Dynamic, 6> xi;
        Matrix<double, Dynamic, 6> eta;
        Matrix<double, Dynamic, 12> g;
        Rod(double D, double L, double E, double rho, double mu, int N);
        double energy(void);
        void step(double dt);
        VectorXd condition(Rod *prev, double dt, VectorXd xi0);
        void integrate(Rod *prev, double dt, VectorXd xi0);

};

// function object, for some reason called a functor in c++
class LM_Functor : public DenseFunctor<double> {
    double dt;
    Rod *prev;
    Rod &next;
    public:
        LM_Functor(Rod *r, Rod &c, double t) : DenseFunctor<double>(6, 6), prev(r), next(c), dt(t) {};
        int operator()(const VectorXd &x, VectorXd &fvec) const {
            // x is the input for xi0
            // fvec is the tip condition values
            fvec = next.condition(prev, dt, x);
            return 0;
        }
};

Rod::Rod(double d, double l, double e, double r, double m, int n) {
    D = d;
    L = l;
    E = e;
    G = E/3;
    rho = r;
    mu = m;
    N = n;
    ds = L/(N-1);

    double A = M_PI/4*pow(D,2);
    double I = M_PI/64*pow(D,4);
    double J = 2*I;

    //simplest initialization
    // seems like there would be a better way to initialize these
    int i;
    MatrixXd xi_temp(N,6);
    MatrixXd g_temp(N,12);
    for (i=0; i<N; i++) {
        xi_temp(i,0) = 0;
        xi_temp(i,1) = M_PI/(4*10e-2);
        xi_temp(i,2) = 0;
        xi_temp(i,3) = 0;
        xi_temp(i,4) = 0;
        xi_temp(i,5) = 1;

        g_temp(i,0) = 1;
        g_temp(i,1) = 0;
        g_temp(i,2) = 0;
        g_temp(i,3) = 0;
        g_temp(i,4) = 1;
        g_temp(i,5) = 0;
        g_temp(i,6) = 0;
        g_temp(i,7) = 0;
        g_temp(i,8) = 1;
        g_temp(i,9) = 0;
        g_temp(i,10) = 0;
        g_temp(i,11) = ds*i;
    }
    xi = xi_temp;
    g = g_temp;
    eta = MatrixXd::Constant(N,6,0);

    K = Matrix<double, 6, 6>::Zero();

    K.diagonal() << E*I, E*I, G*J, G*A, G*A, E*A;
    M.diagonal() << I, I, J, A, A, A;
    M = rho*M;
    V.diagonal() << 3*I, 3*I, J, A, A, 3*A;
    V = mu*V;

    xi_ref << 0, 0, 0, 0, 0, 1;

}

double Rod::energy(void) {
    double H = 0;
    int i;
    for (i=0; i<N; i++) {
        H += 0.5 * eta.row(i) * M * eta.row(i).transpose();
        H += 0.5 * (xi.row(i).transpose() - xi_ref).transpose() * K * (xi.row(i).transpose() - xi_ref);
    }
    return ds*H;
}

void Rod::step(double dt) {
    // solve the condition
    Rod prev = *this;
    VectorXd guess = xi.row(0);
    LM_Functor functor(&prev, *this, dt);
    DenseIndex nfev;
    int info;
    info = LevenbergMarquardt<LM_Functor>::lmdif1(functor, guess, &nfev);

//    this->integrate(&prev, dt, guess);
}

VectorXd Rod::condition(Rod *prev, double dt, VectorXd xi0) {

    this->integrate(prev, dt, xi0);
    return xi.row(N-1).transpose() - xi_ref;

}

void Rod::integrate(Rod *prev, double dt, VectorXd xi0) {

    Matrix<double, 6, 1> xi_half;
    Matrix<double, 6, 1> eta_half;
    Matrix<double, 6, 1> xi_half_next;
    Matrix<double, 6, 1> eta_half_next;
    Matrix<double, 6, 1> xi_dot;
    Matrix<double, 6, 1> eta_dot;
    Matrix<double, 6, 1> xi_der;
    Matrix<double, 6, 1> eta_der;
    Matrix<double, 6, 1> B_bar;

    Matrix<double, 6, 6> A_bar;
    Matrix<double, 6, 6> xi_ad;
    Matrix<double, 6, 6> eta_ad;

    Matrix4d eta_se;
    Matrix<double, 1, 12> g_row;

    xi.row(0) = xi0.transpose();

    Matrix4d G = Matrix4d::Identity();
    int i;
    for (i=0; i<N-1; i++) {
        xi_half = (xi.row(i) + prev->xi.row(i)).transpose()/2;
        eta_half = (eta.row(i) + prev->eta.row(i)).transpose()/2;

        xi_dot = (xi.row(i) - prev->xi.row(i)).transpose()/dt;
        eta_dot = (eta.row(i) - prev->eta.row(i)).transpose()/dt;

        A_bar = Matrix<double, 6, 6>::Zero();
        B_bar = Matrix<double, 6, 1>::Zero();

        B_bar = V * xi_dot;

        adjoint(xi_half, xi_ad);
        adjoint(eta_half, eta_ad);

        xi_der = K.ldlt().solve(((M * eta_dot) - (eta_ad.transpose() * M * eta_half) + (xi_ad.transpose() * K * (xi_half - xi_ref)) + B_bar));
        eta_der = xi_dot - xi_ad * eta_half;

        xi_half_next = xi_half + ds * xi_der;
        eta_half_next = eta_half + ds * eta_der;

        xi.row(i+1) = (2 * xi_half_next.transpose() - prev->xi.row(i+1));
        eta.row(i+1) = (2 * eta_half_next.transpose() - prev->eta.row(i+1));

    }

//    for (i=0; i<N; i++) {
//        g_row = prev->g.row(i);
//        unflatten(g_row, G);
//        eta_half = (eta.row(i)+prev->eta.row(i)).transpose()/2*ds;
//        se(eta_half, eta_se);
//        G = G * eta_se.exp();
//        flatten(G, g_row);
//        g.row(i) = g_row;
//    }

}

int main() {
    double dt = 0.05;
    Rod r1(1e-2,10e-2,1e6,1e3,0,40);
    cout << r1.energy() << endl;
    int i;
    for (i=0; i<1000; i++) {
        r1.step(dt);
        cout << r1.energy() << endl;
    }

//    Matrix<double, 6,1> xi;
//    xi << 1, 2, 3, 4, 5, 6;
//    Matrix4d xi_se;
//    Matrix<double, 6, 6> xi_ad;
//
//    cout << xi << endl;
//    se(xi,xi_se);
//    cout << xi_se << endl;
//    adjoint(xi,xi_ad);
//    cout << xi_ad << endl;

    return 0;
}


/* boost random/binomial_distribution.hpp header file
 *
 * Copyright Jens Maurer 2002
 * 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)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: binomial_distribution.hpp,v 1.9 2004/07/27 03:43:32 dgregor Exp $
 *
 */

#ifndef BOOST_RANDOM_BINOMIAL_DISTRIBUTION_HPP
#define BOOST_RANDOM_BINOMIAL_DISTRIBUTION_HPP

#include <cmath>
#include <cassert>
#include <boost/random/bernoulli_distribution.hpp>

namespace boost {

// Knuth
template<class IntType = int, class RealType = double>
class binomial_distribution
{
public:
  typedef typename bernoulli_distribution<RealType>::input_type input_type;
  typedef IntType result_type;

  explicit binomial_distribution(IntType t = 1,
                                 const RealType& p = RealType(0.5))
    : _t(t)
  {
    assert(t >= 0);
    assert(RealType(0) <= 0 && p <= RealType(1));
  }

  // compiler-generated copy ctor and assignment operator are fine

  IntType t() const { return _t; }
  RealType p() const { return _bernoulli.p(); }
  void reset() { }

  template<class Engine>
  result_type operator()(Engine& eng)
  {
    // TODO: This is O(_t), but it should be O(log(_t)) for large _t
    result_type n = 0;
    for(IntType i = 0; i < _t; ++i)
      if(_bernoulli(eng))
        ++n;
    return n;
  }

#if !defined(BOOST_NO_OPERATORS_IN_NAMESPACE) && !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS)
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const binomial_distribution& bd)
  {
    os << bd._bernoulli << " " << bd._t;
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, binomial_distribution& bd)
  {
    is >> std::ws >> bd._bernoulli >> std::ws >> bd._t;
    return is;
  }
#endif

private:
  bernoulli_distribution<RealType> _bernoulli;
  IntType _t;
};

} // namespace boost

#endif // BOOST_RANDOM_BINOMIAL_DISTRIBUTION_HPP


/*
 * Copyright 2016 C. Brett Witherspoon
 */

#include <algorithm>
#include <chrono>
#include <cmath>
#include <complex>
#include <iostream>
#include <stdexcept>
#include <random>

#include <boost/preprocessor/stringize.hpp>
#include <boost/program_options.hpp>
#include <boost/compute/core.hpp>

#include <signum/opencl/fft.hpp>

namespace compute = boost::compute;
namespace po = boost::program_options;
namespace opencl = signum::opencl;

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

    po::options_description desc("Supported options");
    desc.add_options()
        ("help,h", "print help message")
        ("length,l", po::value<size_t>(&length)->default_value(8), "set FFT length")
        ("verbose,v", "print verbose messages");
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help"))
    {
        std::cerr << desc << std::endl;
        return 1;
    }

    compute::device device = compute::system::default_device();
    compute::context context(device);
    compute::command_queue queue(context, device, compute::command_queue::enable_profiling);

    // Print some device information
    std::cout << device.platform().name() << ": " << device.name() << std::endl;
    std::cout << "Global memory size: " << device.global_memory_size() << std::endl;
    std::cout << "Local memory size: " << device.local_memory_size() << std::endl;
    std::cout << "Compute units: " << device.compute_units() << std::endl;
    std::cout << "Preferred vector width: " <<device.preferred_vector_width<float>() << std::endl;

    // Create FFT object
    opencl::fft fft(queue, length);

    // Initialize input buffer
    auto input = fft.map(compute::command_queue::map_write);
    std::default_random_engine eng;
    std::normal_distribution<> dist{0, 1};
    auto rand = std::bind(dist, eng);
    std::generate(input, input + length, rand);
    if (vm.count("verbose"))
    {
        std::cout << "Input: " << std::endl;
        for (size_t i = 0; i < length; ++i) std::cout << input[i] << std::endl;
    }
    fft.unmap().wait();

    // Enqueue kernels
    auto events = fft();

    events.wait();

    // Print profiling information
    std::chrono::nanoseconds time{0};
    for (const auto &event : events)
    {
        time += event.duration<std::chrono::nanoseconds>();
    }
    std::cout << "Execute time: " << time.count() << " ns" << std::endl;

    // Print output buffer
    auto output = fft.map(compute::command_queue::map_read);
    if (vm.count("verbose"))
    {
        std::cout << "Output: " << std::endl;
        for (size_t i = 0; i < length; ++i) std::cout << output[i] << std::endl;
    }
    fft.unmap().wait();

    return 0;
}


/*
 * Mesh.cpp
 *
 * This class contains the mesh and mesh properties to be used
 * in the nonlinear finite element method (FiniteElemNL.cpp).
 *
 * It constructs the mesh, then constructs the stiffness matrix and
 * calculates the boundary. It can be refined by calling
 * uniformrefine().
 *
 *  Created on: Aug 13, 2016
 *      Author: Ted Kwan
 */

#include "MeshMG.h"
#include "ArmaFuns.h"
#include <cmath>
#include <iostream>
#include <armadillo>

using namespace std;
using namespace arma;

MeshMG::MeshMG() {

}

/**
 * Constructor for MeshMG. It takes one input, a vector containing the
 * properties of the mesh to be created.
 *
 * @param meshprops - double vector with all of the necessary properties.
 */
MeshMG::MeshMG(vector<double> meshprops) {
	// Extract the mesh size value.
	h = meshprops[4];
	n = round(1 / h);
	// Create vectors to be used for mesh creation.
	vec xr = linspace<vec>(meshprops[0], meshprops[1], n + 1);
	vec yr = linspace<vec>(meshprops[2], meshprops[3], n + 1);
	// Create the mesh.
	makeMesh(xr, yr);
	// Assemble stiffness matrix
	vector<sp_mat> stiffmass = assembleMatrix();
	stiffness=stiffmass[0];
	mass=stiffmass[1];
	// Calculate boundary nodes.
	findBoundary();
}

/**
 * makeMeshMG creates the mesh for the 2D Cartesian grid.
 *
 * @param xr - input vector containing the x values.
 * @param yr - input vector containing the y values.
 */
void MeshMG::makeMesh(vec xr, vec yr) {
	// Findthe lengths to use.
	uword xrl = xr.n_rows;
	uword yrl = xr.n_rows;
	mat x = zeros<mat>(xrl, yrl);
	mat y = zeros<mat>(xrl, yrl);
	// Construct the meshes. This is just the same
	// as calling meshgrid() in MATLAB.
	for (uword i = 0; i < yrl; i++) {
		x.row(i) = xr.t();
	}
	for (uword i = 0; i < xrl; i++) {
		y.col(i) = yr;
	}
	// Vectorise the values to use for the nodes matrix.
	vec xv = vectorise(x);
	N = xv.n_rows;
	Ns.push_back(N);
	uword ni = x.n_rows;
	// Initialize node matrix.
	node = zeros<mat>(N, 2);
	// put all the values in the node matrix
	node.col(0) = xv;
	node.col(1) = vectorise(y);
	// Map the indices for the nodes to create elem matrix.
	uvec t2nidxMapnz = regspace<uvec>(0, N - ni - 1);
	uvec topNode = regspace<uvec>(ni - 1, ni, N - ni - 1);
	// Set all of the doubled nodes to not be included.
	t2nidxMapnz(topNode) = zeros<uvec>(topNode.n_rows);
	// Find nonzeros in the original indices map.
	uvec nnz = nonzeros(t2nidxMapnz);
	uvec k = zeros<uvec>(nnz.n_rows + 1);
	// Calculate the different values used to create nodes.
	k(span(1, nnz.n_rows)) = nnz;
	uword NE = k.n_rows;
	HBs.push_back(zeros<umat>(3,3));
	coarse2Fine.push_back(zeros<uvec>(3));
	Pro.push_back(zeros<sp_mat>(3,3));
	// Create the elements which will differ by odd and
	// even elements.
	umat elemup = zeros<umat>(NE, 3);
	umat elemdown = zeros<umat>(NE, 3);
	uvec niv = ones<uvec>(k.n_rows);
	niv = niv * ni;
	uvec onek = ones<uvec>(k.n_rows);
	// Map elements to nodes in order.
	elemup.col(0) = k + niv;
	elemup.col(1) = k + niv + onek;
	elemup.col(2) = k;
	elemdown.col(0) = k + onek;
	elemdown.col(1) = k;
	elemdown.col(2) = k + niv + onek;
	// join all columns together and stack them properly.
	elem = join_cols(elemup, elemdown);
	NT = elem.n_rows;
	refines=0;
}

/**
 * assembleMatrix assembles the stiffness matrix using the quick
 * construction method from armadillo for sparse matrices.
 *
 * @return - Stiffness matrix as a sparse matrix.
 */
vector<sp_mat> MeshMG::assembleMatrix() {
	// Initialize index maps and value vector.
	uvec ii = zeros<uvec>(9 * NT);
	uvec jj = zeros<uvec>(9 * NT);
	vec sA = zeros<vec>(9 * NT);
	// Calculate the area of each node.
	cube ve(NT, 2, 3);
	ve.slice(0) = node.rows(elem.col(2)) - node.rows(elem.col(1));
	ve.slice(1) = node.rows(elem.col(0)) - node.rows(elem.col(2));
	ve.slice(2) = node.rows(elem.col(1)) - node.rows(elem.col(0));
	// Find the area using the dot product on the second dimension.
	area = 0.5*abs((ve.slice(2).col(0) % ve.slice(1).col(1))
					-(ve.slice(2).col(1) % ve.slice(1).col(0)));
	uword index = 0;
	// Loop to map values and indices.
	for (uword i = 0; i < 3; i++) {
		for (uword j = 0; j < 3; j++) {
			// Setup indices to map in this iteration.
			uvec inds = regspace<uvec>(index, index + NT - 1);
			// Setup element maps for indices.
			ii(inds) = elem.col(i);
			jj(inds) = elem.col(j);
			// Calculate values of stiffness matrix at
			// these points.
			mat prod = ve.slice(i) % ve.slice(j);
			// Store calculated value.
			sA(inds) = sum(prod, 1) / (4 * area);
			index = index + NT;
		}
	}
	// Setup index map to be a 2x9NT matrix.
	umat inds = join_horiz(ii, jj);
	// Create the sparse matrix using the same
	// method as sparse(row indices, col indices, values, size)
	sp_mat A(true, inds.t(), sA, N, N, true, true);
	vector<sp_mat> matvec(2);
	matvec[0]=A;
	// Create mass matrix.
	vec Mv=accumArrayM(join_vert(elem.col(0), join_vert(elem.col(1),elem.col(2)))
			,join_vert(area, join_vert(area,area))/(3.0), N);
	uvec inds2=regspace<uvec>(0,Mv.n_rows-1);
	umat subs2=join_horiz(inds2,inds2);
	sp_mat M(true,subs2.t(),Mv,N,N,true,true);
	matvec[1]=M;
	masses.push_back(Mv);
	stiffs.push_back(A);
	return matvec;
}

/**
 * Find the boundary nodes and elements.
 *
 * This method calculates the boundary for the given mesh to be used
 * to set the boundary condition for the finite element method.
 *
 * Values are stored so that they can be accesssed later.
 *
 */
void MeshMG::findBoundary() {

	// Setup as two column vectors to find the edges.
	umat e1 = join_vert(join_horiz(elem.col(2), elem.col(1)),
			join_horiz(elem.col(0), elem.col(2)));
	// Calculate all of the values of the edges.
	umat totalEdge = join_vert(e1, join_horiz(elem.col(1), elem.col(0)));
	totalEdge = sort(totalEdge, "ascend", 1);
	vec onev = ones<vec>(totalEdge.n_rows);
	// Create sparse matrix containing the edges which are not being double counted
	// and are thus exterior edges.
	sp_mat fndmat(true, totalEdge.t(), onev, totalEdge.n_rows, totalEdge.n_rows,
			true, true);
	// Initialize edge matrix.
	umat bdEdge = zeros<umat>(totalEdge.n_rows, 2);
	vec s = nonzeros(fndmat);
	uvec ii = zeros<uvec>(s.n_elem);
	uvec jj = zeros<uvec>(s.n_elem);
	uword k = 0;
	// Get the indices for the edges which are exterior edges.
	// The indices map back to nodes.
	for (uword i = 0; i < fndmat.n_rows; i++) {
		for (uword j = 0; j < fndmat.n_cols; j++) {
			double spot = fndmat(i, j);
			// Only find edges where there are two boundary nodes.
			if (spot == 1.0 && k < s.n_elem) {
				ii(k) = i;
				jj(k) = j;
				k++;
			}
		}
	}
	uvec i1 = nonzeros(ii);
	uvec j1 = nonzeros(jj);
	// Setup boolean vector which has whether or not a node is a boundary
	// node.
	isbdNode = zeros<uvec>(N);
	isbdNode.rows(i1) = ones<uvec>(i1.n_rows);
	isbdNode.rows(j1) = ones<uvec>(j1.n_rows);
	isbdNode(0) = 1.0;
	// Find indices of boundary nodes.
	bdNode = find(isbdNode);
	uvec onebd = ones<uvec>(isbdNode.n_rows);
	// Find indices of interior nodes.
	freeNode = find(onebd - isbdNode);
	freeNodes.push_back(freeNode);

}

/**
 * accumArrayM implements the accumarray method from MATLAB.
 *
 * This is a direct implementation and does the exact same thing
 * as the MATLAB function.
 *
 * @param subs - vector of indices.
 * @param ar - vector of values.
 * @param N - size of the array to be made.
 * @return accumulated array (same as MATLAB would).
 */
vec MeshMG::accumArrayM(uvec subs,vec ar,uword N){

	vec S=zeros<vec>(N);

	for(uword i=0;i<N;i++){
		// Get the subscripts.
		uvec q1=find(subs ==(i));
		vec spot;
		double thesum=0.0;
		if(!q1.is_empty()){
			// Find elements at indices q1
			spot=ar.elem(q1);
			// Sum elements.
			thesum=sum(spot);
			// Set at position i.
			S(i)=thesum;
		}
		// If it is empty, add 0.
		else{
			S(i)=0.0;
		}
	}

	return S;
}

/**
 * uniformrefine refines the mesh and re-calculates the boundary
 * as well as all of the needed matrices and vectors.
 *
 * This function ensures that the mesh is refined by adding nodes at
 * the midpoints of all edges in each element.
 *
 */
void MeshMG::uniformrefine(){
	// Find all of the edges.
	umat e23=join_horiz(elem.col(1),elem.col(2));
	umat e31=join_horiz(elem.col(2),elem.col(0));
	umat e12=join_horiz(elem.col(0),elem.col(1));
	// Matrix with all edge indices (n1 -> n2),
	umat totalEdge=sort(join_vert(e23,join_vert(e31,e12)),"ascend",1);
	ArmaFuns funs;
	// Implementation of unique in MATLAB.
	umat edge=funs.uniqueu(totalEdge);
	uvec js=funs.uniqueidx;
	uword NE=edge.n_rows;
	// Map elems to edges.
	umat elem2edge=zeros<umat>(NT*3,1);
	elem2edge.col(0)=js;
	elem2edge.reshape(NT,3);
	// New nodes to create.
	mat nnode=(node.rows(edge.col(0))+node.rows(edge.col(1)))/(2.0);
	// Concatinate the new nodes with the old.
	node=join_vert(node,nnode);
	// Re-map edges including new nodes.
	uvec edge2newNode=regspace<uvec>(N,N+NE-1);
	// Make hierarchical basis.
	HB=join_horiz(edge2newNode,edge);
	uvec t=regspace<uvec>(0,NT-1);
	// Indices for the new elements.
	umat p= join_horiz(elem,
			join_horiz(edge2newNode.rows(elem2edge.col(0)),
			join_horiz(edge2newNode.rows(elem2edge.col(1)),
					edge2newNode.rows(elem2edge.col(2)))));
	// Add elements to the elem matrix.
	elem.rows(t)=join_horiz(p.col(0),join_horiz(p.col(4),p.col(5)));
	// Create new elements.
	umat nelem=zeros<umat>(4*NT,3);
	nelem.rows(span(NT,2*NT-1))=join_horiz(p.col(5),join_horiz(p.col(1),p.col(3)));
	nelem.rows(span(2*NT,3*NT-1))=join_horiz(p.col(4),join_horiz(p.col(3),p.col(2)));
	nelem.rows(span(3*NT,4*NT-1))=join_horiz(p.col(3),join_horiz(p.col(4),p.col(5)));
	nelem.rows(span(0,NT-1))=elem;
	elem=nelem;
	// Save new information.
	NT=elem.n_rows;
	N=node.n_rows;
	refines=refines+1;
	Ns.push_back(N);
	HBs.push_back(HB);
	ProHB();
	vector<sp_mat> stiffmass = assembleMatrix();
	stiffness=stiffmass[0];
	mass=stiffmass[1];
	findBoundary();
}

/**
 * ProHB creates the prolongation and restriction matrices,
 * as well as maps the fine nodes to the coarse nodes for the FAS.
 *
 * This function ensures that the multigrid method works properly and
 * interpolates the coarse grid to the fine grid, as well as restricts
 * the coarse grid and the fine grid.
 *
 */
void MeshMG::ProHB(){
	// Get the number of coarse nodes.
	uword nCoarse = Ns[Ns.size()-2];
	uword nTotal=Ns[Ns.size()-1];
	// Get the number of fine grid only nodes.
	uword nFineNode=nTotal-nCoarse;
	uvec coarseNode=regspace<uvec>(0,nCoarse-1);
	// Save data for use by FEM.
	coarse2Fine.push_back(coarseNode);
	// Setup indices for the prolongation matrix.
	uvec ii=join_vert(coarseNode,join_vert(HB.col(0),HB.col(0)));
	uvec jj=join_vert(coarseNode,join_vert(HB.col(1),HB.col(2)));
	vec ss=join_vert(ones<vec>(nCoarse),
			join_vert(0.5*ones<vec>(nFineNode),0.5*ones<vec>(nFineNode)));
	// Quick sparse construction of the prolongation matrix.
	sp_mat Procurr(true,join_horiz(ii,jj).t(),ss,nTotal,nCoarse);
	Pro.push_back(Procurr);
}

MeshMG::~MeshMG() {

}


#include <iostream>
#include <Eigen/Core>
#include <unsupported/Eigen/CXX11/Tensor>

#include "IO/readPLY.h"
#include "IO/writePLY.h"

#include "visualization/plotMesh.h"
#include "visualization/plotTwoMeshes.h"

#include "mesh/computeNormals.h"
#include "mesh/computeFacesCentroids.h"
#include "occupancyGrid.h"

int main() {
    bool visualization = true;
    int grid_resolution = 100;          // grid_resolution is used to define the grid resolution in the maximum direction
    double bounding_box_scale = 1;

    // IO: load files
    std::cout << "Progress: load data\n";
    Eigen::MatrixXd V, cubes_V, faces_V;
    Eigen::MatrixXi F, cubes_F;
    Eigen::MatrixXd N, faces_N;
    Eigen::MatrixXi RGB;

    readPLY("../data/Lucy100k.ply", V, F, N, RGB);

    std::cout << "size of V: " << V.rows() << ", " << V.cols() << "\n";

    faces_V = compute_faces_centroids(V,F);
    faces_N = compute_faces_normals(V,F);

    if (visualization)
        plot_mesh(V,F);

    OccupancyGrid occupancy_grid(faces_V, faces_N, grid_resolution, bounding_box_scale);

    Eigen::MatrixXd graph_V;
    Eigen::MatrixXi graph_E;
    occupancy_grid.generate_graph(graph_V, graph_E);
    occupancy_grid.print_to_folder("../data/occupancy_grid/");
    occupancy_grid.print_to_yaml("../data/lucy");

    occupancy_grid.generate_mesh(cubes_V, cubes_F);

    if (visualization)
        plot_two_meshes(V,F,cubes_V,cubes_F);

}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <OpenMesh/Core/IO/MeshIO.hh>
#include <OpenMesh/Core/Mesh/PolyMesh_ArrayKernelT.hh>

#include <CGAL/boost/graph/graph_traits_PolyMesh_ArrayKernelT.h>
#include <CGAL/Polygon_mesh_processing/triangulate_hole.h>

#include <CGAL/boost/graph/helpers.h>
#include <iostream>
#include <fstream>
#include <vector>
#include <boost/foreach.hpp>

typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;

typedef OpenMesh::PolyMesh_ArrayKernelT< > Mesh;

typedef boost::graph_traits<Mesh>::vertex_descriptor vertex_descriptor;
typedef boost::graph_traits<Mesh>::halfedge_descriptor halfedge_descriptor;
typedef boost::graph_traits<Mesh>::face_descriptor face_descriptor;

int main(int argc, char* argv[])
{
  const char* filename = (argc > 1) ? argv[1] : "data/mech-holes-shark.off";

  Mesh mesh;
  OpenMesh::IO::read_mesh(mesh, filename);

  // Incrementally fill the holes
  unsigned int nb_holes = 0;
  BOOST_FOREACH(halfedge_descriptor h, halfedges(mesh))
  {
    if(CGAL::is_border(h,mesh))
    {
      std::vector<face_descriptor>  patch_facets;
      std::vector<vertex_descriptor> patch_vertices;
      bool success = CGAL::cpp11::get<0>(
        CGAL::Polygon_mesh_processing::triangulate_refine_and_fair_hole(
                  mesh,
                  h,
                  std::back_inserter(patch_facets),
                  std::back_inserter(patch_vertices),
     CGAL::Polygon_mesh_processing::parameters::vertex_point_map(get(CGAL::vertex_point, mesh)).
                  geom_traits(Kernel())) );

      CGAL_assertion(CGAL::is_valid_polygon_mesh(mesh));

      std::cout << "* FILL HOLE NUMBER " << ++nb_holes << std::endl;
      std::cout << "  Number of facets in constructed patch: " << patch_facets.size() << std::endl;
      std::cout << "  Number of vertices in constructed patch: " << patch_vertices.size() << std::endl;
      std::cout << "  Is fairing successful: " << success << std::endl;
    }
  }

  CGAL_assertion(CGAL::is_valid_polygon_mesh(mesh));
  std::cout << std::endl;
  std::cout << nb_holes << " holes have been filled" << std::endl;

    OpenMesh::IO::write_mesh(mesh, "filled_OM.off");
  return 0;
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_HYPOT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_HYPOT_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing hypot capabilities

    Computes \f$(x^2 + y^2)^{1/2}\f$

    @par semantic:
    For any given value @c x,  @c y of floating type @c T:

    @code
    T r = hypot(x, y);
    @endcode

    The code is similar to:

    @code
    T r = sqrt(sqr(x)+sqr(y));
    @endcode

    @par Note

    - Provision are made to avoid overflow as possible and to compute
    @c hypot accurately.

    - If these considerations can be put aside, use the decorator fast_.

    @par Decorators

    std_,  fast_ for floating entries

  **/
  const boost::dispatch::functor<tag::hypot_> hypot = {};
} }
#endif

#include <boost/simd/function/scalar/hypot.hpp>
#include <boost/simd/function/scalar/hypot.hpp>
#include <boost/simd/function/simd/hypot.hpp>

#endif


#include <iostream>
#include <fstream>

using namespace std;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <octomap/octomap.h>    // for octomap

#include <eigen3/Eigen/Geometry>
#include <boost/format.hpp>  // for formating strings

int main(int argc, char **argv) {
    vector<cv::Mat> colorImgs, depthImgs;    // 색상 맵 및 깊이 맵
    vector<Eigen::Isometry3d> poses;         // 카메라 포즈

    ifstream fin("../dense_RGBD/data/pose.txt");
    if (!fin) {
        cerr << "cannot find pose file" << endl;
        return 1;
    }

    for (int i = 0; i < 5; i++) {
        boost::format fmt("../dense_RGBD/data/%s/%d.%s"); // 이미지 파일 형식
        colorImgs.push_back(cv::imread((fmt % "color" % (i + 1) % "png").str()));
        depthImgs.push_back(cv::imread((fmt % "depth" % (i + 1) % "png").str(), -1)); // -1을 사용하여 원본 이미지를 읽습니다.

        double data[7] = {0};
        for (int i = 0; i < 7; i++) {
            fin >> data[i];
        }
        Eigen::Quaterniond q(data[6], data[3], data[4], data[5]);
        Eigen::Isometry3d T(q);
        T.pretranslate(Eigen::Vector3d(data[0], data[1], data[2]));
        poses.push_back(T);
    }

    // 포인트 클라우드 및 스티치 계산
    // 카메라 내부 매개변수
    double cx = 319.5;
    double cy = 239.5;
    double fx = 481.2;
    double fy = -480.0;
    double depthScale = 5000.0;

    cout << "이미지 변환 Octomap ..." << endl;

    // octomap tree
    octomap::OcTree tree(0.01); // 매개변수는 해상도입니다.

    for (int i = 0; i < 5; i++) {
        cout << "이미지 변환: " << i + 1 << endl;
        cv::Mat color = colorImgs[i];
        cv::Mat depth = depthImgs[i];
        Eigen::Isometry3d T = poses[i];
        octomap::Pointcloud cloud;  // the point cloud in octomap

        for (int v = 0; v < color.rows; v++)
            for (int u = 0; u < color.cols; u++) {
                unsigned int d = depth.ptr<unsigned short>(v)[u]; // 깊이 값
                if (d == 0) continue; // 0은 측정 없음을 의미합니다.
                Eigen::Vector3d point;
                point[2] = double(d) / depthScale;
                point[0] = (u - cx) * point[2] / fx;
                point[1] = (v - cy) * point[2] / fy;
                Eigen::Vector3d pointWorld = T * point;
                // 세계 좌표계의 점을 점 구름에 넣습니다.
                cloud.push_back(pointWorld[0], pointWorld[1], pointWorld[2]);
            }

        // 투영선을 계산할 수 있도록 원점이 주어진 옥트리 맵에 포인트 클라우드를 저장합니다.
        tree.insertPointCloud(cloud, octomap::point3d(T(0, 3), T(1, 3), T(2, 3)));
    }

    // 중간 노드의 점유 정보를 업데이트하고 디스크에 기록
    tree.updateInnerOccupancy();
    cout << "saving octomap ... " << endl;
    tree.writeBinary("octomap.bt");
    return 0;
}


#include "incidencematrices.h"

#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <array>
#include <memory>

namespace IncidenceMatrices {

/** @brief Create the mesh consisting of a triangle and quadrilateral
 *         from the exercise sheet.
 * @return Shared pointer to the hybrid2d mesh.
 */
std::shared_ptr<lf::mesh::Mesh> createDemoMesh() {
  // builder for a hybrid mesh in a world of dimension 2
  std::shared_ptr<lf::mesh::hybrid2d::MeshFactory> mesh_factory_ptr =
      std::make_shared<lf::mesh::hybrid2d::MeshFactory>(2);

  // Add points
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0, 0});    // (0)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{1, 0});    // (1)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{1, 1});    // (2)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0, 1});    // (3)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0.5, 1});  // (4)

  // Add the triangle
  // First set the coordinates of its nodes:
  Eigen::MatrixXd nodesOfTria(2, 3);
  nodesOfTria << 1, 1, 0.5, 0, 1, 1;
  mesh_factory_ptr->AddEntity(
      lf::base::RefEl::kTria(),  // we want a triangle
      std::array<lf::mesh::Mesh::size_type, 3>{
          {1, 2, 4}},  // indices of the nodes
      std::make_unique<lf::geometry::TriaO1>(nodesOfTria));  // node coords

  // Add the quadrilateral
  Eigen::MatrixXd nodesOfQuad(2, 4);
  nodesOfQuad << 0, 1, 0.5, 0, 0, 0, 1, 1;
  mesh_factory_ptr->AddEntity(
      lf::base::RefEl::kQuad(),
      std::array<lf::mesh::Mesh::size_type, 4>{{0, 1, 4, 3}},
      std::make_unique<lf::geometry::QuadO1>(nodesOfQuad));

  std::shared_ptr<lf::mesh::Mesh> demoMesh_p = mesh_factory_ptr->Build();

  return demoMesh_p;
}

/** @brief Compute the edge-vertex incidence matrix G for a given mesh
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *        such as lf::mesh::hybrid2d::Mesh)
 * @return The edge-vertex incidence matrix as Eigen::SparseMatrix<int>
 */
/* SAM_LISTING_BEGIN_1 */
Eigen::SparseMatrix<int> computeEdgeVertexIncidenceMatrix(
    const lf::mesh::Mesh &mesh) {
  // Store edge-vertex incidence matrix here
  Eigen::SparseMatrix<int, Eigen::RowMajor> G;

  //====================
  // Your code goes here
  //====================

  return G;
}
/* SAM_LISTING_END_1 */

/** @brief Compute the cell-edge incidence matrix D for a given mesh
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *        such as lf::mesh::hybrid2d::Mesh)
 * @return The cell-edge incidence matrix as Eigen::SparseMatrix<int>
 */
/* SAM_LISTING_BEGIN_2 */
Eigen::SparseMatrix<int> computeCellEdgeIncidenceMatrix(
    const lf::mesh::Mesh &mesh) {
  // Store cell-edge incidence matrix here
  Eigen::SparseMatrix<int, Eigen::RowMajor> D;

  //====================
  // Your code goes here
  //====================

  return D;
}
/* SAM_LISTING_END_2 */

/** @brief For a given mesh test if the product of cell-edge and edge-vertex
 *        incidence matrix is zero: D*G == 0?
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *             such as lf::mesh::hybrid2d::Mesh)
 * @return true, if the product is zero and false otherwise
 */
/* SAM_LISTING_BEGIN_3 */
bool testZeroIncidenceMatrixProduct(const lf::mesh::Mesh &mesh) {
  bool isZero = false;

  //====================
  // Your code goes here
  //====================
  return isZero;
}
/* SAM_LISTING_END_3 */

}  // namespace IncidenceMatrices


/**
 * Copyright (c) 2022 <Daumantas Kavolis>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#pragma once

#include "config.hpp"

MSVC_WARNING_DISABLE(4619)
#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/chebyshev.hpp>
#include <boost/math/special_functions/hermite.hpp>
#include <boost/math/special_functions/laguerre.hpp>
#include <boost/math/special_functions/legendre.hpp>
#include <boost/math/special_functions/legendre_stieltjes.hpp>
MSVC_WARNING_POP()

#include "quadrature.hpp"

namespace poly {
namespace detail {

template <typename Integral>
struct BasicImpl {
  using OrderType = Integral;
  using Storage = Integral;

  constexpr static inline bool is_trivial = true;

  static auto make_storage(OrderType order = 0) noexcept -> Storage { return order; }
  static auto get_order(Storage const& item) noexcept -> OrderType { return item; }
  static void set_order(Storage& item, OrderType const& value) noexcept { item = value; }
};

}  // namespace detail

template <typename Real_, unsigned N = QuadraturePoints>
struct LegendreImpl : detail::BasicImpl<std::int16_t> {
  using Real = Real_;
  constexpr static inline bool is_orthonormal = true;

  static auto eval(Storage const& item, Real x) -> Real { return boost::math::legendre_p(item, x); }
  static auto prime(Storage const& item, Real x) -> Real {
    return boost::math::legendre_p_prime(item, x);
  }
  static auto zeros(Storage const& item) -> std::vector<Real> {
    using std::abs;
    auto z = boost::math::legendre_p_zeros<Real>(item);
    detail::reflect_in_place<detail::Reflection::Odd>(
        z, abs(z[0]) < 2 * std::numeric_limits<Real>::epsilon());
    return z;
  }
  static auto next(Storage const& item, Real x, Real Pl, Real Plm1) -> Real {
    return boost::math::legendre_next(item, x, Pl, Plm1);
  }

  template <bool check = false>
  static auto weights(Storage const& item, bounds_check<check> c = no_bounds_check)
      -> view<Real const> {
    return GaussQuadrature<Real, N>::weights(item + 1, c);
  }
  template <bool check = false>
  static auto abscissa(Storage const& item, bounds_check<check> c = no_bounds_check)
      -> view<Real const> {
    return GaussQuadrature<Real, N>::abscissa(item + 1, c);
  }
  static auto domain() -> std::pair<Real, Real> { return {-1, 1}; }
};

template <typename Real_>
struct LaguerreImpl : detail::BasicImpl<std::int16_t> {
  using Real = Real_;
  constexpr static inline bool is_orthonormal = true;

  static auto eval(Storage const& item, Real x) -> Real { return boost::math::laguerre(item, x); }
  static auto next(Storage const& item, Real x, Real Pl, Real Plm1) -> Real {
    return boost::math::laguerre_next(item, x, Pl, Plm1);
  }
  static auto domain() -> std::pair<Real, Real> { return {0, 1}; }
};

template <typename Real_>
struct HermiteImpl : detail::BasicImpl<std::uint16_t> {
  using Real = Real_;
  constexpr static inline bool is_orthonormal = true;

  static auto eval(Storage const& item, Real x) -> Real { return boost::math::hermite(item, x); }
  static auto next(Storage const& item, Real x, Real Pl, Real Plm1) -> Real {
    return boost::math::hermite_next(item, x, Pl, Plm1);
  }
  static auto domain() -> std::pair<Real, Real> {
    return {-std::numeric_limits<Real>::infinity(), std::numeric_limits<Real>::infinity()};
  }
};

template <typename, unsigned>
struct ChebyshevImpl;
namespace detail {
template <typename Real, unsigned N = QuadraturePoints>
struct ChebyshevQuadrature {
  static auto abscissa() -> view<Real const> {
    static std::vector<Real> v = ChebyshevImpl<Real, N>::zeros({N});
    return {v.data(), v.size()};
  }
  static auto weights() -> view<Real const> {
    static std::vector<Real> v(std::size_t(N), boost::math::constants::pi<Real>() / N);
    return {v.data(), v.size()};
  }
};
}  // namespace detail

template <typename Real, unsigned N = QuadraturePoints>
using ChebyshevQuadrature = detail::Quadrature<Real, detail::ChebyshevQuadrature, N, false>;

template <typename Real_, unsigned N = QuadraturePoints>
struct ChebyshevImpl : detail::BasicImpl<std::int16_t> {
  using Real = Real_;
  constexpr static inline bool is_orthonormal = true;

  static auto eval(Storage const& item, Real x) -> Real {
    return boost::math::chebyshev_t(item, x);
  }
  static auto prime(Storage const& item, Real x) -> Real {
    return boost::math::chebyshev_t_prime(item, x);
  }
  static auto next(Storage const& /* unused */, Real x, Real Pl, Real Plm1) -> Real {
    return boost::math::chebyshev_next(x, Pl, Plm1);
  }
  static auto zeros(Storage const& item) -> std::vector<Real> {
    using std::cos;
    std::vector<Real> values(item, 0);
    Real factor{boost::math::constants::pi<Real>() / 2.0 / item};
    auto half = item / 2;
    auto start = (item + 1) / 2;
    for (OrderType i = 0; i < half; ++i) {
      Real x = cos((2 * (start + i) + 1) * factor);
      values[start + i] = -x;
      values[half - i - 1] = x;
    }
    return values;
  }

  template <bool check = false>
  static auto weights(Storage const& item, bounds_check<check> c = no_bounds_check)
      -> view<Real const> {
    return ChebyshevQuadrature<Real, N>::weights(item + 1, c);
  }
  template <bool check = false>
  static auto abscissa(Storage const& item, bounds_check<check> c = no_bounds_check)
      -> view<Real const> {
    return ChebyshevQuadrature<Real, N>::abscissa(item + 1, c);
  }
  static auto domain() -> std::pair<Real, Real> { return {-1, 1}; }
};

template <typename Real_>
struct LegendreStieltjesImpl {
  using Real = Real_;
  using OrderType = std::int16_t;

  constexpr static inline bool is_orthonormal = true;

  class Storage {
   public:
    explicit Storage(OrderType m) : polynomial{m}, order{m} {};

   private:
    boost::math::legendre_stieltjes<Real> polynomial;
    OrderType order;

    template <class>
    friend struct LegendreStieltjesImpl;
  };

  static auto make_storage(OrderType order = 0) noexcept -> Storage { return Storage{order}; }
  static auto get_order(Storage const& item) noexcept -> OrderType { return item.order; }
  static void set_order(Storage& item, OrderType const& value) noexcept { item = Storage{value}; }

  static auto eval(Storage const& item, Real x) -> Real { return item.polynomial(x); }
  static auto prime(Storage const& item, Real x) -> Real { return item.polynomial.prime(x); }
  static auto zeros(Storage const& item) -> std::vector<Real> {
    auto vector = item.polynomial.zeros();
    detail::reflect_in_place<detail::Reflection::Odd>(vector, vector[0] == 0);
    return vector;
  }
  static auto domain() -> std::pair<Real, Real> { return {-1, 1}; }
};

template <typename Real_, unsigned N = QuadraturePoints>
struct GaussKronrodImpl : detail::BasicImpl<std::int16_t> {
  using Real = Real_;

  constexpr static inline bool is_orthonormal = true;

  template <bool check = false>
  static auto weights(Storage const& item, bounds_check<check> c = no_bounds_check)
      -> view<Real const> {
    return GaussKronrodQuadrature<Real, N>::weights(item, c);
  }
  template <bool check = false>
  static auto abscissa(Storage const& item, bounds_check<check> c = no_bounds_check)
      -> view<Real const> {
    return GaussKronrodQuadrature<Real, N>::abscissa(item, c);
  }
  static auto domain() -> std::pair<Real, Real> { return {-1, 1}; }
};

}  // namespace poly


#include "FiniteElement.h"

#include <Eigen/Core>
#include <iostream>

using namespace Eigen;

FiniteElement::FiniteElement(
        FEMObject* femObj,
        Cell cell,
        ElasticMaterial material)
    : mFemObj(femObj)
    , mMaterial(material)
{
    mDensity = 100.0;
    mCell = cell;
    mStiffnessMatrixDirty = false;
    mQ.setIdentity();
}

void FiniteElement::setMaterial(ElasticMaterial material)
{
    mMaterial = material;
    mStiffnessMatrixDirty = true;
}

void FiniteElement::initialize()
{
    updateDnx();

    // Initialize mQ with the correct rotation calculated with the slow singular
    // value decomposition. Use that result with the iterative approach later.
    updateRotation();
    mQ = Eigen::Quaterniond(mR);

    updateCauchyStressStrain();
    updateLinearStiffnessMatrix();
}

void FiniteElement::updateRotation()
{
    updateF();
    JacobiSVD<Matrix3d> svd(mF, ComputeFullU | ComputeFullV);
    double det = (svd.matrixU() * svd.matrixV().transpose()).determinant();
    Matrix3d C = Matrix3d::Identity();
    C(2,2) = det;
    mR = svd.matrixU() * C * svd.matrixV().transpose();
//    if (det < 0)
//        std::cout << "inversion detected! det = " << det << "\n";

}

void FiniteElement::updateRotationFast(size_t maxIterations, double tolerance)
{
    updateF();

    for (size_t i = 0; i < maxIterations; ++i)
    {
        Eigen::Matrix3d R = mQ.toRotationMatrix();
        double s = std::abs(R.col(0).dot(mF.col(0)) +
                            R.col(1).dot(mF.col(1)) +
                            R.col(2).dot(mF.col(2)));

        if (s < 1e-12)
            break;

        double sInv = 1.0 / s + tolerance;
        Eigen::Vector3d v = R.col(0).cross(mF.col(0)) +
                            R.col(1).cross(mF.col(1)) +
                            R.col(2).cross(mF.col(2));

        Eigen::Vector3d omega = sInv * v;
        double w = omega.norm();

        if (w < tolerance)
            break;

        Eigen::Quaterniond omegaQ =
                Eigen::Quaterniond(Eigen::AngleAxisd(w, 1.0 / w * omega));

        mQ = omegaQ * mQ;
    }

    mR = mQ.toRotationMatrix();
}

void FiniteElement::update(bool corotated)
{
    if (corotated)
    {
        updateCorotatedStiffnessMatrix();
        updateCorotatedForces();
    }
    else
    {
        updateLinearForces();
    }
}

void FiniteElement::updateStiffnessMatrix(bool corotated)
{
    if (corotated)
        updateCorotatedStiffnessMatrix();
    else if (mStiffnessMatrixDirty)
        updateLinearStiffnessMatrix();
}

void FiniteElement::updateLinearStiffnessMatrix()
{
    for (size_t a = 0; a < 4; ++a)
    {
        for (size_t b = 0; b < 4; ++b)
        {
            double second_part = mMaterial.getLameMu() * mDnx[a].dot(mDnx[b]);
            Matrix3d& K = mK[a][b];
            for (Index i = 0; i < 3; ++i)
            {
                for (Index k = 0; k < 3; ++k)
                {
                    //K(i,k) = ...
                    double value = 0.0;
                    if (i == k)
                        value = second_part;
                    value += mMaterial.getLameLambda() * mDnx[a](i) * mDnx[b](k)
                            + mMaterial.getLameMu() * mDnx[a](k) * mDnx[b](i);
                    K(i,k) = value * mVolume;
                }
            }
        }
    }

    updatePlasticityMatrix();
}

void FiniteElement::updateCorotatedStiffnessMatrix()
{
    for (size_t a = 0; a < 4; ++a)
    {
        for (size_t b = 0; b < 4; ++b)
        {
            Matrix3d& K = mK[a][b];
            Matrix3d& K_corot = mKCorot[a][b];
            K_corot = mR * K * mR.transpose();
        }
    }
}

void FiniteElement::updatePlasticityMatrix()
{
    Eigen::Matrix<double, 6, 6> D;
    double mu = mMaterial.getLameMu();
    double lambda = mMaterial.getLameLambda();

    double temp = lambda + 2 * mu;

    D << temp, lambda, lambda, 0, 0, 0,
            lambda, temp, lambda, 0, 0, 0,
            lambda, lambda, temp, 0, 0, 0,
            0, 0, 0, mu, 0, 0,
            0, 0, 0, 0, mu, 0,
            0, 0, 0, 0, 0, mu;

    mP = mVolume * mB.transpose() * D;
}

void FiniteElement::updateForces(bool corotated)
{
    if (corotated)
        updateCorotatedForces();
    else
        updateLinearForces();
}

void FiniteElement::updateCorotatedForces()
{
    // TODO: Optimizations
    // - Apply rotation only once
    // - reformulate equation to precalculate parts of it
    for (size_t a = 0; a < 4; ++a)
    {
        mForces[a].setZero();
        for (size_t b = 0; b < 4; ++b)
        {
            mForces[a] += mR * mK[a][b] *
                    (mR.transpose() * y(b) - x(b));
        }
    }

    updatePlasticForces(0.01);
}

void FiniteElement::updateLinearForces()
{
    for (size_t a = 0; a < 4; ++a)
    {
        mForces[a] = Vector::Zero();
        for (size_t b = 0; b < 4; ++b)
        {
            mForces[a] += mK[a][b] * u(b);

            // directly globally updates them this way
            // but this is not wanted here
            //f(a) += mK[a][b] * u(b);
        }
    }
}

void FiniteElement::updatePlasticForces(double stepSize)
{
    if (mMaterial.getPlasticMaxStrain() > 1e-12)
    {
        // Calculate total strain.
        Eigen::Matrix<double, 6, 1> strainTotal =
                Eigen::Matrix<double, 6, 1>::Zero();
        Eigen::Matrix3d RInv = mR.inverse();
        for (Eigen::Index a = 0; a < 4; ++a)
        {
            strainTotal += mB.block(0, a * 3, 6, 3) * (RInv * y(a) - x(a));
        }

        // Update plastic strain.
        Eigen::Matrix<double, 6, 1> strainElastic =
                strainTotal - mStrainPlastic;
        if (strainElastic.norm() > mMaterial.getPlasticYield())
            mStrainPlastic += stepSize * mMaterial.getPlasticCreep() * strainElastic;
        double strainPlasticNorm = mStrainPlastic.norm();
        if (strainPlasticNorm > mMaterial.getPlasticMaxStrain())
            mStrainPlastic *= mMaterial.getPlasticMaxStrain() / strainPlasticNorm;

        // Calculate plastic forces.
        for (Eigen::Index a = 0; a < 4; ++a)
        {
            mForces[a] -= mR * mP.block(a * 3, 0, 3, 6) * mStrainPlastic;
        }
    }
}

void FiniteElement::updateF()
{
//    mF = Matrix3d::Identity();
//    for (size_t i = 0; i < 4; ++i) {
//        mF += u(i) * mDnx[i].transpose();
//    }
    mF = Matrix3d::Zero();
    for (size_t i = 0; i < 4; ++i) {
        mF += y(i) * mDnx[i].transpose();
    }

    mB << mDnx[0](0), 0, 0, mDnx[1](0), 0, 0, mDnx[2](0), 0, 0, mDnx[3](0), 0, 0,
            0, mDnx[0](1), 0, 0, mDnx[1](1), 0, 0, mDnx[2](1), 0, 0, mDnx[3](1), 0,
            0, 0, mDnx[0](2), 0, 0, mDnx[1](2), 0, 0, mDnx[2](2), 0, 0, mDnx[3](2),
            mDnx[0](1), mDnx[0](0), 0, mDnx[1](1), mDnx[1](0), 0, mDnx[2](1), mDnx[2](0), 0, mDnx[3](1), mDnx[3](0), 0,
            mDnx[0](2), 0, mDnx[0](0), mDnx[1](2), 0, mDnx[1](0), mDnx[2](2), 0, mDnx[2](0), mDnx[3](2), 0, mDnx[3](0),
            0, mDnx[0](2), mDnx[0](1), 0, mDnx[1](2), mDnx[1](1), 0, mDnx[2](2), mDnx[2](1), 0, mDnx[3](2), mDnx[3](1);
}

void FiniteElement::updateCauchyStressStrain()
{
    // Cauchy strain
    mCauchyStrain = 0.5 * (mF + mF.transpose()) - Matrix3d::Identity();
    double trace = mCauchyStrain.trace();
    for (Index i = 0; i < 3; ++i)
         for (Index j = 0; j < 3; ++j)
            mCauchyStress(i, j) = (i == j) * mMaterial.getLameLambda() * trace
                    + 2 * mMaterial.getLameMu() * mCauchyStrain(i, j);
}

void FiniteElement::updateDnx()
{
    // As described in
    // http://www.iue.tuwien.ac.at/phd/nentchev/node30.html and
    // http://www.iue.tuwien.ac.at/phd/nentchev/node31.html

    Vector r1 = mFemObj->x(mCell[1]) - mFemObj->x(mCell[0]);
    Vector r2 = mFemObj->x(mCell[2]) - mFemObj->x(mCell[0]);
    Vector r3 = mFemObj->x(mCell[3]) - mFemObj->x(mCell[0]);

    Vector r4 = mFemObj->x(mCell[2]) - mFemObj->x(mCell[1]);
    Vector r5 = mFemObj->x(mCell[1]) - mFemObj->x(mCell[3]);

    mVolume = r1.cross(r2).dot(r3);

    // Sometimes tetrahedrons can be so degenerated that their volume is zero.
    if (mVolume < 1e-10)
        mVolume = 1e-5;

    mDnx[0] = r4.cross(r5) / mVolume;
    mDnx[1] = r2.cross(r3) / mVolume;
    mDnx[2] = r3.cross(r1) / mVolume;
    mDnx[3] = r1.cross(r2) / mVolume;

    mVolume /= 6.0;
}

void FiniteElement::updateDny()
{
    updateF(); // TODO: look if necessary
    Matrix3d F_inv = mF.inverse();
    for (size_t i = 0; i < 4; ++i) {
        mDny[i] = Vector(0,0,0);
        for (int a = 0; a < 3; ++a) {
            for (int k = 0; k < 3; ++k) {
                mDny[i](a) += mDnx[i](k) * F_inv(k,a);
            }
        }
    }
}


#include <boost/test/unit_test.hpp>
#include "common/equality.hpp"
#include "algorithms/data_structures/array/median_of_two_sorted_arrays.hpp"

BOOST_AUTO_TEST_SUITE(TestMedianOfTwoSortedArrays)

BOOST_AUTO_TEST_CASE(empty_arrays)
{
    std::vector<int> first, second;
    MedianOfTwoSortedArrays::Solution solution;
    double result = solution.findMedianSortedArrays(first, second);

    BOOST_CHECK(equal(result, 0.0));
}

BOOST_AUTO_TEST_CASE(empty_and_size_1_arrays)
{
    std::vector<int> first;
    std::vector<int> second = {1};

    MedianOfTwoSortedArrays::Solution solution;
    double result = solution.findMedianSortedArrays(first, second);

    BOOST_CHECK(equal(result, 1.0));
}

BOOST_AUTO_TEST_CASE(empty_and_small_arrays)
{
    std::vector<int> first;
    std::vector<int> second = {2, 3};

    MedianOfTwoSortedArrays::Solution solution;
    double result = solution.findMedianSortedArrays(first, second);

    BOOST_CHECK(equal(result, 2.5));
}

BOOST_AUTO_TEST_CASE(two_arrays)
{
    std::vector<int> first = {1, 2};
    std::vector<int> second = {3, 4};

    MedianOfTwoSortedArrays::Solution solution;
    double result = solution.findMedianSortedArrays(first, second);

    BOOST_CHECK(equal(result, 2.5));
}

BOOST_AUTO_TEST_CASE(two_small_arrays)
{
    std::vector<int> first = {1, 1};
    std::vector<int> second = {1, 2};

    MedianOfTwoSortedArrays::Solution solution;
    double result = solution.findMedianSortedArrays(first, second);

    BOOST_CHECK(equal(result, 1.0));
}

BOOST_AUTO_TEST_CASE(two_arrays_diff_size)
{
    std::vector<int> first = {1, 2, 3, 6};
    std::vector<int> second = {9, 14, 17, 25, 26};

    MedianOfTwoSortedArrays::Solution solution;
    double result = solution.findMedianSortedArrays(first, second);

    BOOST_CHECK(equal(result, 9.0));
}

BOOST_AUTO_TEST_SUITE_END()


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/include/functions/sinpi.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <complex>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/tests/basic.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/i.hpp>

NT2_TEST_CASE_TPL ( sinpi,  NT2_REAL_TYPES)
{

  using nt2::sinpi;
  using nt2::tag::sinpi_;
  typedef std::complex<T> cT;
  typedef typename nt2::meta::call<sinpi_(cT)>::type r_t;
  typedef typename nt2:: meta::as_complex<T>::type wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  NT2_TEST_ULP_EQUAL(nt2::sinpi(cT(nt2::Inf<T>())), cT(nt2::Nan<T>()), 10);
  NT2_TEST_ULP_EQUAL(nt2::sinpi(cT(nt2::Minf<T>())), cT(nt2::Nan<T>()), 10);
  NT2_TEST_ULP_EQUAL(nt2::sinpi(cT(1, 1)),std::sin(nt2::Pi<T>()*cT(1.0, 1.0)), 10);
  NT2_TEST_ULP_EQUAL(nt2::sinpi(cT(1, 0.5)),std::sin(nt2::Pi<T>()*cT(1.0, 0.5)), 10);
  NT2_TEST_ULP_EQUAL(nt2::sinpi(cT(0.5, 1)),std::sin(nt2::Pi<T>()*cT(0.5, 1.0)), 10);
  NT2_TEST_ULP_EQUAL(nt2::sinpi(cT(0.5, 0.5)),std::sin(nt2::Pi<T>()*cT(0.5, 0.5)), 10);
  NT2_TEST_ULP_EQUAL(nt2::sinpi(cT(0, 1)),std::sin(nt2::Pi<T>()*cT(0.0, 1.0)), 10);
  NT2_TEST_ULP_EQUAL(nt2::sinpi(cT(0, 0.5)),std::sin(nt2::Pi<T>()*cT(0.0, 0.5)), 10);
  NT2_TEST_ULP_EQUAL(nt2::sinpi(cT(0.5, 0)),std::sin(nt2::Pi<T>()*cT(0.5, 0.0)), 10);

  const int N = 20;
  cT inputs[N] =
    { cT(nt2::Zero<T>(),nt2::Zero<T>()),cT(nt2::Inf<T>(),nt2::Zero<T>()),cT(nt2::Minf<T>(),nt2::Zero<T>()),cT(nt2::Nan<T>(),nt2::Zero<T>()),
      cT(nt2::Zero<T>(),nt2::Inf<T>()), cT(nt2::Inf<T>(),nt2::Inf<T>()), cT(nt2::Minf<T>(),nt2::Inf<T>()), cT(nt2::Nan<T>(),nt2::Inf<T>()),
      cT(nt2::Zero<T>(),nt2::Minf<T>()),cT(nt2::Inf<T>(),nt2::Minf<T>()),cT(nt2::Minf<T>(),nt2::Minf<T>()),cT(nt2::Nan<T>(),nt2::Minf<T>()),
      cT(nt2::Zero<T>(),nt2::Nan<T>()), cT(nt2::Inf<T>(),nt2::Nan<T>()), cT(nt2::Minf<T>(),nt2::Nan<T>()), cT(nt2::Nan<T>(),nt2::Nan<T>()),
      cT(nt2::Zero<T>(),nt2::One<T>()), cT(nt2::Inf<T>(),nt2::One<T>()), cT(nt2::Minf<T>(),nt2::One<T>()), cT(nt2::Nan<T>(),nt2::One<T>()),
    };

  for(int i=0; i < N; i++)
   {
     NT2_TEST_ULP_EQUAL(nt2::sinpi(-inputs[i]), -nt2::sinpi(inputs[i]), 4);
     NT2_TEST_ULP_EQUAL(nt2::sinpi(inputs[i]), nt2::sin(nt2::multiplies(nt2::Pi<T>(), inputs[i])), 4);
   }
} // end of test for floating_


// Boost.Geometry
// Unit Test

// Copyright (c) 2017 Adam Wulkiewicz, Lodz, Poland.

// Copyright (c) 2016, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is 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_GEOMETRY_TEST_STRATEGIES_SEGMENT_INTERSECTION_GEO_HPP
#define BOOST_GEOMETRY_TEST_STRATEGIES_SEGMENT_INTERSECTION_GEO_HPP

#include "segment_intersection_sph.hpp"

#include <boost/geometry/strategies/geographic/intersection.hpp>
#include <boost/geometry/strategies/geographic/intersection_elliptic.hpp>

template <typename S, typename P>
void test_default_strategy(std::string const& s1_wkt, std::string const& s2_wkt,
                           char m, std::size_t expected_count,
                           std::string const& ip0_wkt = "", std::string const& ip1_wkt = "",
                           int opposite_id = -1)
{
    typename bg::strategy::intersection::services::default_strategy
        <
            bg::geographic_tag
        >::type strategy;

    test_strategy<S, S, P>(s1_wkt, s2_wkt, strategy, m, expected_count, ip0_wkt, ip1_wkt, opposite_id);
}

template <typename S, typename P>
void test_great_elliptic(std::string const& s1_wkt, std::string const& s2_wkt,
                         char m, std::size_t expected_count,
                         std::string const& ip0_wkt = "", std::string const& ip1_wkt = "",
                         int opposite_id = -1)
{
    bg::strategy::intersection::great_elliptic_segments<> strategy;

    test_strategy<S, S, P>(s1_wkt, s2_wkt, strategy, m, expected_count, ip0_wkt, ip1_wkt, opposite_id);
}
/*
template <typename S, typename P>
void test_experimental_elliptic(std::string const& s1_wkt, std::string const& s2_wkt,
                                char m, std::size_t expected_count,
                                std::string const& ip0_wkt = "", std::string const& ip1_wkt = "",
                                int opposite_id = -1)
{
    bg::strategy::intersection::experimental_elliptic_segments<> strategy;

    test_strategy<S, S, P>(s1_wkt, s2_wkt, strategy, m, expected_count, ip0_wkt, ip1_wkt, opposite_id);
}
*/
template <typename S, typename P>
void test_geodesic_vincenty(std::string const& s1_wkt, std::string const& s2_wkt,
                            char m, std::size_t expected_count,
                            std::string const& ip0_wkt = "", std::string const& ip1_wkt = "",
                            int opposite_id = -1)
{
    bg::strategy::intersection::geographic_segments<bg::strategy::vincenty, 4> strategy;

    test_strategy<S, S, P>(s1_wkt, s2_wkt, strategy, m, expected_count, ip0_wkt, ip1_wkt, opposite_id);
}

template <typename S, typename P>
void test_geodesic_thomas(std::string const& s1_wkt, std::string const& s2_wkt,
                          char m, std::size_t expected_count,
                          std::string const& ip0_wkt = "", std::string const& ip1_wkt = "",
                          int opposite_id = -1)
{
    bg::strategy::intersection::geographic_segments<bg::strategy::thomas, 2> strategy;

    test_strategy<S, S, P>(s1_wkt, s2_wkt, strategy, m, expected_count, ip0_wkt, ip1_wkt, opposite_id);
}

template <typename S, typename P>
void test_geodesic_andoyer(std::string const& s1_wkt, std::string const& s2_wkt,
                           char m, std::size_t expected_count,
                           std::string const& ip0_wkt = "", std::string const& ip1_wkt = "",
                           int opposite_id = -1)
{
    bg::strategy::intersection::geographic_segments<bg::strategy::andoyer, 1> strategy;

    test_strategy<S, S, P>(s1_wkt, s2_wkt, strategy, m, expected_count, ip0_wkt, ip1_wkt, opposite_id);
}

struct strategy_base
{
    strategy_base(char m_)
        : m(m_), expected_count(0), opposite(-1)
    {}
    strategy_base(char m_, std::string const& wkt1_)
        : m(m_), expected_count(1), wkt1(wkt1_), opposite(-1)
    {}
    strategy_base(char m_, std::string const& wkt1_, std::string const& wkt2_, bool opposite_)
        : m(m_), expected_count(1), wkt1(wkt1_), wkt2(wkt2_), opposite(opposite_ ? 1 : 0)
    {}

    char m;
    std::size_t expected_count;
    std::string wkt1, wkt2;
    int opposite;
};
struct strategy_default : strategy_base
{
    strategy_default(char m)
        : strategy_base(m)
    {}
    strategy_default(char m, std::string const& wkt1)
        : strategy_base(m, wkt1)
    {}
    strategy_default(char m, std::string const& wkt1, std::string const& wkt2, bool opposite)
        : strategy_base(m, wkt1, wkt2, opposite)
    {}
};
struct geodesic_vincenty : strategy_base
{
    geodesic_vincenty(char m)
        : strategy_base(m)
    {}
    geodesic_vincenty(char m, std::string const& wkt1)
        : strategy_base(m, wkt1)
    {}
    geodesic_vincenty(char m, std::string const& wkt1, std::string const& wkt2, bool opposite)
        : strategy_base(m, wkt1, wkt2, opposite)
    {}
};
struct geodesic_thomas : strategy_base
{
    geodesic_thomas(char m)
        : strategy_base(m)
    {}
    geodesic_thomas(char m, std::string const& wkt1)
        : strategy_base(m, wkt1)
    {}
    geodesic_thomas(char m, std::string const& wkt1, std::string const& wkt2, bool opposite)
        : strategy_base(m, wkt1, wkt2, opposite)
    {}
};
struct geodesic_andoyer : strategy_base
{
    geodesic_andoyer(char m)
        : strategy_base(m)
    {}
    geodesic_andoyer(char m, std::string const& wkt1)
        : strategy_base(m, wkt1)
    {}
    geodesic_andoyer(char m, std::string const& wkt1, std::string const& wkt2, bool opposite)
        : strategy_base(m, wkt1, wkt2, opposite)
    {}
};
struct great_elliptic : strategy_base
{
    great_elliptic(char m)
        : strategy_base(m)
    {}
    great_elliptic(char m, std::string const& wkt1)
        : strategy_base(m, wkt1)
    {}
    great_elliptic(char m, std::string const& wkt1, std::string const& wkt2, bool opposite)
        : strategy_base(m, wkt1, wkt2, opposite)
    {}
};

template <typename S, typename P>
void test_strategy(std::string const& s1_wkt, std::string const& s2_wkt,
                   strategy_default const& s)
{
    test_default_strategy<S, P>(s1_wkt, s2_wkt, s.m, s.expected_count, s.wkt1, s.wkt2);
}

template <typename S, typename P>
void test_strategy(std::string const& s1_wkt, std::string const& s2_wkt,
                   great_elliptic const& s)
{
    test_great_elliptic<S, P>(s1_wkt, s2_wkt, s.m, s.expected_count, s.wkt1, s.wkt2);
}

template <typename S, typename P>
void test_strategy(std::string const& s1_wkt, std::string const& s2_wkt,
                   geodesic_vincenty const& s)
{
    test_geodesic_vincenty<S, P>(s1_wkt, s2_wkt, s.m, s.expected_count, s.wkt1, s.wkt2);
}

template <typename S, typename P>
void test_strategy(std::string const& s1_wkt, std::string const& s2_wkt,
                   geodesic_thomas const& s)
{
    test_geodesic_thomas<S, P>(s1_wkt, s2_wkt, s.m, s.expected_count, s.wkt1, s.wkt2);
}

template <typename S, typename P>
void test_strategy(std::string const& s1_wkt, std::string const& s2_wkt,
                   geodesic_andoyer const& s)
{
    test_geodesic_andoyer<S, P>(s1_wkt, s2_wkt, s.m, s.expected_count, s.wkt1, s.wkt2);
}

template <typename S, typename P, typename SR1>
void test_strategies(std::string const& s1_wkt, std::string const& s2_wkt,
                     SR1 const& sr1)
{
    test_strategy<S, P>(s1_wkt, s2_wkt, sr1);
}
template <typename S, typename P, typename SR1, typename SR2>
void test_strategies(std::string const& s1_wkt, std::string const& s2_wkt,
                     SR1 const& sr1, SR2 const& sr2)
{
    test_strategy<S, P>(s1_wkt, s2_wkt, sr1);
    test_strategy<S, P>(s1_wkt, s2_wkt, sr2);
}
template <typename S, typename P, typename SR1, typename SR2, typename SR3>
void test_strategies(std::string const& s1_wkt, std::string const& s2_wkt,
                     SR1 const& sr1, SR2 const& sr2, SR3 const& sr3)
{
    test_strategy<S, P>(s1_wkt, s2_wkt, sr1);
    test_strategy<S, P>(s1_wkt, s2_wkt, sr2);
    test_strategy<S, P>(s1_wkt, s2_wkt, sr3);
}
template <typename S, typename P, typename SR1, typename SR2, typename SR3, typename SR4>
void test_strategies(std::string const& s1_wkt, std::string const& s2_wkt,
                     SR1 const& sr1, SR2 const& sr2, SR3 const& sr3, SR4 const& sr4)
{
    test_strategy<S, P>(s1_wkt, s2_wkt, sr1);
    test_strategy<S, P>(s1_wkt, s2_wkt, sr2);
    test_strategy<S, P>(s1_wkt, s2_wkt, sr3);
    test_strategy<S, P>(s1_wkt, s2_wkt, sr4);
}

template <typename S, typename P>
void test_all_strategies(std::string const& s1_wkt, std::string const& s2_wkt,
                         char m, std::string const& ip0_wkt = "")
{
    std::size_t expected_count = ip0_wkt.empty() ? 0 : 1;

    test_default_strategy<S, P>(s1_wkt, s2_wkt, m, expected_count, ip0_wkt);
    test_great_elliptic<S, P>(s1_wkt, s2_wkt, m, expected_count, ip0_wkt);
    //test_experimental_elliptic<S, P>(s1_wkt, s2_wkt, m, expected_count, ip0_wkt);
    test_geodesic_vincenty<S, P>(s1_wkt, s2_wkt, m, expected_count, ip0_wkt);
    test_geodesic_thomas<S, P>(s1_wkt, s2_wkt, m, expected_count, ip0_wkt);
    test_geodesic_andoyer<S, P>(s1_wkt, s2_wkt, m, expected_count, ip0_wkt);
}

template <typename S, typename P>
void test_all_strategies(std::string const& s1_wkt, std::string const& s2_wkt,
                         char m,
                         std::string const& ip0_wkt, std::string const& ip1_wkt,
                         bool opposite)
{
    int opposite_id = opposite ? 1 : 0;

    test_default_strategy<S, P>(s1_wkt, s2_wkt, m, 2, ip0_wkt, ip1_wkt, opposite_id);
    test_great_elliptic<S, P>(s1_wkt, s2_wkt, m, 2, ip0_wkt, ip1_wkt, opposite_id);
    //test_experimental_elliptic<S, P>(s1_wkt, s2_wkt, m, 2, ip0_wkt, ip1_wkt, opposite_id);
    test_geodesic_vincenty<S, P>(s1_wkt, s2_wkt, m, 2, ip0_wkt, ip1_wkt, opposite_id);
    test_geodesic_thomas<S, P>(s1_wkt, s2_wkt, m, 2, ip0_wkt, ip1_wkt, opposite_id);
    test_geodesic_andoyer<S, P>(s1_wkt, s2_wkt, m, 2, ip0_wkt, ip1_wkt, opposite_id);
}

#endif // BOOST_GEOMETRY_TEST_STRATEGIES_SEGMENT_INTERSECTION_GEO_HPP


#include "GlobalMap.h"

#include <Eigen/LU>

GlobalMap::GlobalMap(double areaSize, int scanStride, int maxIter, double relErrChangeThresh)
	: tree(areaSize), icp(maxIter, relErrChangeThresh,
						  std::bind(&GlobalMap::getClosest, this, std::placeholders::_1)),
	  scanStride(scanStride) {
}

points_t GlobalMap::getPoints() const {
	return tree.getAllPoints();
}

size_t GlobalMap::size() const {
	return tree.getSize();
}

void GlobalMap::addPoints(const transform_t& robotTrf, const points_t& toAdd, double overlap) {
	if (toAdd.empty()) {
		return;
	}
	transform_t trfInv = robotTrf.inverse();
	points_t transformed;
	for (size_t i = 0; i < toAdd.size(); i += scanStride) {
		const point_t& point = toAdd[i];
		if (point(2) != 0) {
			transformed.push_back(trfInv * point);
		}
	}

	if (!tree.empty()) {
		if (overlap > 0) {
			// find the transformation from the sample to the map
			transform_t adj = icp.correct(transformed, overlap);
			for (point_t& p : transformed) {
				p = adj * p;
			}
		}
	}

	// add to global map
	for (const point_t& p : transformed) {
		assert(tree.add(p));
	}
}

point_t GlobalMap::getClosest(const point_t& point) const {
	if (tree.empty()) {
		return {0, 0, 0};
	}
	point_t closest = tree.getClosest(point);
	return closest;
}

points_t GlobalMap::getPointsWithin(const point_t& point, double dist) const {
	points_t points;
	if (!tree.empty()) {
		points_t within = tree.getPointsWithin(point, dist * 2);
		for (const point_t& p1 : within) {
			if ((p1 - point).topRows<2>().norm() <= dist) {
				points.push_back(p1);
			}
		}
	}
	return points;
}

bool GlobalMap::hasPointWithin(const point_t& point, double dist) const {
	return tree.hasPointWithin(point, dist);
}


#include <iostream>
#include <math.h>
#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Core>

#include "../include/UKFilter.h"
#include "../include/Robot.h"
#include "../include/Odom.h"
#include "../include/Imu.h"
#include "../include/Gps.h"

UnscentedKalmanFilter unscentedKalmanFilter;
UnscentedKalmanFilter *punscentedKalmanFilter;

// Constructor
UnscentedKalmanFilter::UnscentedKalmanFilter()
{
    is_initialized_ = false;
    lastTimeStamp_ = 0;
    nowTimeStamp_ = 0;
    deltaTime_ = 0;
}

//Destructor
UnscentedKalmanFilter::~UnscentedKalmanFilter()
{
    delete punscentedKalmanFilter;
}

void UnscentedKalmanFilter::Initialization()
{
    lastTimeStamp_ = nowTimeStamp_ = getSysTime();

    Eigen::VectorXf x_in = Eigen::VectorXf::Zero(5,1);
    SetX(x_in);

    // state covariance matrix the prediction error
    Eigen::MatrixXf P_in(5,5);
    P_in << 0.1, 0.0, 0.0, 0.0, 0.0,
            0.0, 0.1, 0.0, 0.0, 0.0,
            0.0, 0.0, 0.1, 0.0, 0.0,
            0.0, 0.0, 0.0, 0.1, 0.0,
            0.0, 0.0, 0.0, 0.0, 0.1;
    SetP(P_in);

    //predicted sigma
    Eigen::MatrixXf PreSigmaX_in = Eigen::MatrixXf::Zero(5, 15);
    SetPreSigmaX(PreSigmaX_in);

    //measurement covariance matrix
    // R is provided by Sensor supplier
    Eigen::MatrixXf R_in(3,3);
    R_in << 0.1, 0.0, 0.0,
            0.0, 0.1, 0.0,
            0.0, 0.0, 0.1;
    SetR(R_in);

    //Observation
    Eigen::VectorXf z_in(3,1);
    z_in << 0.0,
            0.0,
            0.0;
    SetZ(z_in);

    is_initialized_ = true;

}

//state prediction
//state defination: x, y, v, yaw, w
void UnscentedKalmanFilter::StatePrediction(float d_t)
{
    Eigen::MatrixXf augmented_sigma = Eigen::MatrixXf::Zero(7, 15);
    Eigen::VectorXf augmented_x = Eigen::VectorXf::Zero(7);
    Eigen::MatrixXf augmented_P = Eigen::MatrixXf::Zero(7, 7);
    augmented_x.head(5) = x_;
    augmented_P.topLeftCorner(5, 5) = P_;
    augmented_P(5, 5) = 0.1;
    augmented_P(6, 6) = 0.1;
    const Eigen::MatrixXf L = augmented_P.llt().matrixL();
    augmented_sigma.col(0) = augmented_x;
    for(int c = 0; c < 7; c++)
    {
        const int i = c + 1;
        augmented_sigma.col(i)  = augmented_x + 1.732 * L.col(c);
        augmented_sigma.col(i + 7)  = augmented_x - 1.732 * L.col(c);
    }

    Eigen::MatrixXf predicted_sigma = Eigen::MatrixXf(5, 15);
    for(int c = 0; c < 15; ++c)
    {
        /*************************************
        * Get the current state
        *************************************/
        const float px = augmented_sigma(0, c);
        const float py = augmented_sigma(1, c);
        const float speed = augmented_sigma(2, c);
        const float yaw = augmented_sigma(3, c);
        const float yawrate = augmented_sigma(4, c);
        const float speed_noise = augmented_sigma(5, c);
        const float yawrate_noise = augmented_sigma(6, c);

        /*************************************
        * predict the next state with noise
        * USING THE CTRV MODEL
        *************************************/
        const float cos_yaw = cos(yaw);
        const float sin_yaw = sin(yaw);
        const float d_t2 = d_t * d_t;

        // predicted position noise
        const float p_noise = 0.5 * speed_noise * d_t2;
        // predicted yaw noise
        const float y_noise = 0.5 * yawrate_noise * d_t2;
        const float dyaw = yawrate * d_t; //change in yaw
        const float dspeed = speed * d_t; //change in speed

        // predicted speed = assumed constant speed + noise
        const float p_speed = speed + speed_noise * d_t;

        // predicted yaw
        const float p_yaw = yaw + dyaw + y_noise;
        // predicted yaw rate = assumed constant yawrate + noise
        const float p_yawrate = yawrate + yawrate_noise * d_t;
        // where predicted positions will be stored
        float p_px, p_py;

        if(fabs(yawrate) <= 0.001)
        {
            // moving straight
            p_px = px + dspeed * cos_yaw + p_noise * cos_yaw;
            p_py = py + dspeed * sin_yaw + p_noise * sin_yaw;

        }
        else
        {
            const float k = speed / yawrate;
            const float theta = yaw + dyaw;
            p_px = px + k * (sin(theta) - sin_yaw) + p_noise * cos_yaw;
            p_py = py + k * (cos_yaw - cos(theta)) + p_noise * sin_yaw ;
        }

        /*************************************
        * Write the prediction to the appropriate column
        *************************************/
        predicted_sigma(0, c) = p_px;
        predicted_sigma(1, c) = p_py;
        predicted_sigma(2, c) = p_speed;
        predicted_sigma(3, c) = p_yaw;
        predicted_sigma(4, c) = p_yawrate;
    }

    Eigen::VectorXf predicted_x = Eigen::VectorXf::Zero(5);
    for(int c = 0; c < 15; c++)
    {
        predicted_x += WEIGHTS[c] * predicted_sigma.col(c);
    }
    Eigen::MatrixXf predicted_P = Eigen::MatrixXf::Zero(5, 5);
    Eigen::VectorXf dx = Eigen::VectorXf(5);
    for(int c = 0; c < 15; c++)
    {
        dx = predicted_sigma.col(c) - predicted_x;
        dx(3) = (fabs(dx(3)) > M_PI) ? remainder(dx(3), 2. * M_PI) : dx(3);
        predicted_P += WEIGHTS[c] * dx * dx.transpose();
    }
    SetPreSigmaX(predicted_sigma);
    SetX(predicted_x);
    SetP(predicted_P);
}

void UnscentedKalmanFilter::MeasurementPrediction()//measurement prediction
{
    Eigen::MatrixXf sigma = Eigen::MatrixXf::Zero(3, 15);
    for(int c = 0; c < 15; c++)
    {
        const float px = PreSigmaX_(0, c);
        const float py = PreSigmaX_(1, c);
        const float v = PreSigmaX_(2, c);
        const float yaw = PreSigmaX_(3, c);

        const float vx = cos(yaw) * v;
        const float vy = sin(yaw) * v;

        const float rho = sqrt(px * px + py * py);
        const float phi = atan2(py, px);
        const float rhodot = (rho > 0.0001) ? ((px * vx + py * vy) / rho) : 0.0;

        // avoid division by zero
        sigma(0, c) = rho;
        sigma(1, c) = phi;
        sigma(2, c) = rhodot;
    }

    Eigen::VectorXf z = Eigen::VectorXf::Zero(3);
    Eigen::VectorXf dz;
    Eigen::MatrixXf S = Eigen::MatrixXf::Zero(3, 3);
    for(int c = 0; c < 15; c++){
        z += WEIGHTS[c] * sigma.col(c);
    }
    for(int c = 0; c < 15; c++)
    {
        dz = sigma.col(c) - z;
        dz(1) =  (fabs(dz(1)) > M_PI) ? remainder(dz(1), 2. * M_PI) : dz(1);
        S += WEIGHTS[c] * dz * dz.transpose();
    }
    S += R_;

    SetPreSigmaZ(sigma);
    SetZ(z);
    SetS(S);
}

void UnscentedKalmanFilter::StateUpdate(Eigen::VectorXf z_new)
{
    Eigen::VectorXf dz;
    Eigen::VectorXf dx;
    Eigen::MatrixXf Tc = Eigen::MatrixXf::Zero(5, 3);

    for(int c = 0; c < 15; c++)
    {
        dx = PreSigmaX_.col(c) - x_;
        dx(3) = (fabs(dx(3)) > M_PI) ? remainder(dx(3), 2. * M_PI) : dx(3);
        dz = PreSigmaZ_.col(c) - z_;
        dz(1) = (fabs(dz(1)) > M_PI) ? remainder(dz(1), 2. * M_PI) : dz(1);
        Tc += WEIGHTS[c] * dx * dz.transpose();
    }
    Eigen::MatrixXf Si = S_.inverse();
    Eigen::MatrixXf K = Tc * Si;
    Eigen::VectorXf dz = z_new - z_;
    dz(1) = (fabs(dz(1)) > M_PI) ? remainder(dz(1), 2. * M_PI) : dz(1);

    SetX(x_ + K * dz);
    SetP(P_ - K * S_ * K.transpose());
}

bool UnscentedKalmanFilter::GetIsInitialized()
{
    return is_initialized_;
}

void UnscentedKalmanFilter::SetX(Eigen::VectorXf x_in)
{
    x_ = x_in;
}

void UnscentedKalmanFilter::SetP(Eigen::MatrixXf P_in)
{
    P_ = P_in;
}

void UnscentedKalmanFilter::SetPreSigmaX(Eigen::MatrixXf PreSigmaX_in)
{
    PreSigmaX_ = PreSigmaX_in;
}

void UnscentedKalmanFilter::SetPreSigmaZ(Eigen::MatrixXf PreSigmaZ_in)
{
    PreSigmaZ_ = PreSigmaZ_in;
}

void UnscentedKalmanFilter::SetZ(Eigen::VectorXf z_in)
{
    z_ = z_in;
}

void UnscentedKalmanFilter::SetR(Eigen::MatrixXf S_in)
{
    S_ = S_in;
}

void UnscentedKalmanFilter::SetR(Eigen::MatrixXf R_in)
{
    R_ = R_in;
}

void UnscentedKalmanFilter::setLastTimeStamp(const long int lastTimeStamp)
{
    this->lastTimeStamp_ = lastTimeStamp;
}

void UnscentedKalmanFilter::setNowTimeStamp(const long int nowTimeStamp )
{
    this->nowTimeStamp_ = nowTimeStamp;
}
void UnscentedKalmanFilter::setDeltaTime(const long int deltaTime)
{
    this->deltaTime_ = deltaTime;
}

void UnscentedKalmanFilter::getLastTimeStamp(long int& lastTimeStamp)
{
    lastTimeStamp = this->lastTimeStamp_;
}
void UnscentedKalmanFilter::getNowTimeStamp(long int& nowTimeStamp)
{
    nowTimeStamp = this->nowTimeStamp_;
}
void UnscentedKalmanFilter::getDeltaTime(long int& deltaTime)
{
    deltaTime = this->deltaTime_;
}