/*
    This file is part of Mitsuba, a physically based rendering system.

    Copyright (c) 2007-2014 by Wenzel Jakob and others.

    Mitsuba is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License Version 3
    as published by the Free Software Foundation.

    Mitsuba is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include <mitsuba/core/shvector.h>
#include <mitsuba/core/transform.h>
#include <boost/math/special_functions/factorials.hpp>

MTS_NAMESPACE_BEGIN

Float *SHVector::m_normalization = NULL;

SHVector::SHVector(Stream *stream) {
    m_bands = stream->readInt();
    unsigned int size = m_bands*m_bands;
    m_coeffs.resize(size);
    for (size_t i=0; i<size; ++i)
        m_coeffs[i] = stream->readFloat();
}

void SHVector::serialize(Stream *stream) const {
    stream->writeInt(m_bands);
    for (size_t i=0; i<(size_t) m_coeffs.size(); ++i)
        stream->writeFloat(m_coeffs[i]);
}

bool SHVector::isAzimuthallyInvariant() const {
    for (int l=0; l<m_bands; ++l) {
        for (int m=1; m<=l; ++m) {
            if (std::abs(operator()(l, -m)) > Epsilon
             || std::abs(operator()(l, m)) > Epsilon)
                return false;
        }
    }
    return true;
}

Float SHVector::eval(Float theta, Float phi) const {
    Float result = 0;
    Float cosTheta = std::cos(theta);
    Float *sinPhi = (Float *) alloca(sizeof(Float)*m_bands),
          *cosPhi = (Float *) alloca(sizeof(Float)*m_bands);

    for (int m=0; m<m_bands; ++m) {
        sinPhi[m] = std::sin((m+1) * phi);
        cosPhi[m] = std::cos((m+1) * phi);
    }

    for (int l=0; l<m_bands; ++l) {
        for (int m=1; m<=l; ++m) {
            Float L = legendreP(l, m, cosTheta) * normalization(l, m);
            result += operator()(l, -m) * SQRT_TWO * sinPhi[m-1] * L;
            result += operator()(l, m)  * SQRT_TWO * cosPhi[m-1] * L;
        }

        result += operator()(l, 0) * legendreP(l, 0, cosTheta) * normalization(l, 0);
    }
    return result;
}

Float SHVector::findMinimum(int res = 32) const {
    Float hExt = (Float) M_PI / res, hInt = (2 * (Float) M_PI)/(res*2);
    Float minimum = std::numeric_limits<Float>::infinity();

    for (int i=0; i<=res; ++i) {
        Float theta = hExt*i;
        for (int j=0; j<=res*2; ++j) {
            Float phi = hInt*j;
            minimum = std::min(minimum, eval(theta, phi));
        }
    }

    return minimum;
}

void SHVector::addOffset(Float value) {
    operator()(0, 0) += 2 * value * (Float) std::sqrt(M_PI);
}

Float SHVector::eval(const Vector &v) const {
    Float result = 0;
    Float cosTheta = v.z, phi = std::atan2(v.y, v.x);
    if (phi < 0) phi += 2*M_PI;
    Float *sinPhi = (Float *) alloca(sizeof(Float)*m_bands),
          *cosPhi = (Float *) alloca(sizeof(Float)*m_bands);

    for (int m=0; m<m_bands; ++m) {
        sinPhi[m] = std::sin((m+1) * phi);
        cosPhi[m] = std::cos((m+1) * phi);
    }

    for (int l=0; l<m_bands; ++l) {
        for (int m=1; m<=l; ++m) {
            Float L = legendreP(l, m, cosTheta) * normalization(l, m);
            result += operator()(l, -m) * SQRT_TWO * sinPhi[m-1] * L;
            result += operator()(l, m)  * SQRT_TWO * cosPhi[m-1] * L;
        }

        result += operator()(l, 0) * legendreP(l, 0, cosTheta) * normalization(l, 0);
    }
    return result;
}

Float SHVector::evalAzimuthallyInvariant(Float theta, Float phi) const {
    Float result = 0, cosTheta = std::cos(theta);
    for (int l=0; l<m_bands; ++l)
        result += operator()(l, 0) * legendreP(l, 0, cosTheta) * normalization(l, 0);
    return result;
}

Float SHVector::evalAzimuthallyInvariant(const Vector &v) const {
    Float result = 0, cosTheta = v.z;
    for (int l=0; l<m_bands; ++l)
        result += operator()(l, 0) * legendreP(l, 0, cosTheta) * normalization(l, 0);
    return result;
}

void SHVector::normalize() {
    Float correction = 1/(2 * (Float) std::sqrt(M_PI)*operator()(0,0));

    for (size_t i=0; i<(size_t) m_coeffs.size(); ++i)
        m_coeffs[i] *= correction;
}

void SHVector::convolve(const SHVector &kernel) {
    SAssert(kernel.getBands() == m_bands);

    for (int l=0; l<m_bands; ++l) {
        Float alpha = std::sqrt(4 * (Float) M_PI / (2*l + 1));
        for (int m=-l; m<=l; ++m)
            operator()(l, m) *= alpha * kernel(l, 0);
    }
}

Matrix3x3 SHVector::mu2() const {
    const Float sqrt5o3 = std::sqrt((Float) 5/ (Float) 3);
    const Float sqrto3 = std::sqrt((Float) 1/ (Float) 3);
    Matrix3x3 result;
    result.setZero();

    SAssert(m_bands > 0);
    result(0, 0) = result(1, 1) =
        result(2, 2) = sqrt5o3*operator()(0,0);

    if (m_bands >= 3) {
        result(0, 0) += -operator()(2,0)*sqrto3 + operator()(2,2);
        result(0, 1) = operator()(2,-2);
        result(0, 2) = -operator()(2, 1);
        result(1, 0) = operator()(2,-2);
        result(1, 1) += -operator()(2,0)*sqrto3 - operator()(2,2);
        result(1, 2) = -operator()(2,-1);
        result(2, 0) = -operator()(2, 1);
        result(2, 1) = -operator()(2,-1);
        result(2, 2) += 2*sqrto3*operator()(2,0);
    }

    return result * (2*std::sqrt((Float) M_PI / 15));
}

std::string SHVector::toString() const {
    std::ostringstream oss;
    oss << "SHVector[bands=" << m_bands << ", {";
    int pos = 0;
    for (int i=0; i<m_bands; ++i) {
        oss << "{";
        for (int j=0, total=i*2+1; j<total; ++j) {
            oss << m_coeffs[pos++];
            if (j+1 < total)
                oss << ", ";
        }
        oss << "}";
        if (i+1 < m_bands)
            oss << ", ";
    }
    oss << "}]";
    return oss.str();
}

Float SHVector::computeNormalization(int l, int m) {
    SAssert(m>=0);
    return std::sqrt(
            ((2*l+1) * boost::math::factorial<Float>(l-m))
        /    (4 * (Float) M_PI * boost::math::factorial<Float>(l+m)));
}

void SHVector::staticInitialization() {
    m_normalization = new Float[SH_NORMTBL_SIZE*(SH_NORMTBL_SIZE+1)/2];
    for (int l=0; l<SH_NORMTBL_SIZE; ++l)
        for (int m=0; m<=l; ++m)
            m_normalization[l*(l+1)/2 + m] = computeNormalization(l, m);
}

void SHVector::staticShutdown() {
    delete[] m_normalization;
    m_normalization = NULL;
}

struct RotationBlockHelper {
    const SHRotation::Matrix &M1, &Mp;
    SHRotation::Matrix &Mn;
    int prevLevel, level;

    inline RotationBlockHelper(
        const SHRotation::Matrix &M1,
        const SHRotation::Matrix &Mp,
        SHRotation::Matrix &Mn)
        : M1(M1), Mp(Mp), Mn(Mn), prevLevel((int) Mp.rows()/2),
        level((int) Mp.rows()/2+1) { }

    inline Float delta(int i, int j) const {
        return (i == j) ? (Float) 1 : (Float) 0;
    }

    inline Float U(int l, int m, int n) const {
        return P(l, m, n, 0);
    }

    inline Float V(int l, int m, int n) const {
        if (m == 0) {
            return P(l, 1, n, 1) + P(l, -1, n, -1);
        } else if (m > 0) {
            if (m == 1)
                return SQRT_TWO * P(l, 0, n, 1);
            else
                return P(l, m-1, n, 1) - P(l, -m+1, n, -1);
        } else {
            if (m == -1)
                return SQRT_TWO * P(l, 0, n, -1);
            else
                return P(l, -m-1, n, -1) + P(l, m+1, n, 1);
        }
    }

    inline Float W(int l, int m, int n) const {
        if (m > 0) {
            return P(l, m+1, n, 1) + P(l, -m-1, n, -1);
        } else {
            return P(l, m-1, n, 1) - P(l, -m+1, n, -1);
        }
    }

    inline Float u(int l, int m, int n) const {
        int denom = (std::abs(n) == l) ? (2*l*(2*l-1)) : ((l+n)*(l-n));
        return std::sqrt((Float) ((l+m)*(l-m)) / (Float) denom);
    }

    inline Float v(int l, int m, int n) const {
        int denom = (std::abs(n) == l) ? (2*l*(2*l-1)) : ((l+n)*(l-n)), absM = std::abs(m);
        return .5f * (1-2*delta(m, 0)) * std::sqrt(
            (Float) ((1+delta(m, 0)) * (l+absM-1)*(l+absM)) / (Float) denom
        );
    }

    inline Float w(int l, int m, int n) const {
        if (m == 0)
            return 0.0f;
        int absM = std::abs(m);
        int denom = (std::abs(n) < l) ? ((l+n)*(l-n)) : (2*l*(2*l-1));

        return -.5f * std::sqrt((Float) ((l-absM-1)*(l-absM)) / (Float) denom);
    }

    inline Float P(int l, int m, int n, int i) const {
        if (std::abs(n) < l)
            return R(i, 0) * M(m, n);
        else if (n == l)
            return R(i, 1) * M(m, l-1) - R(i, -1) * M(m, -l+1);
        else if (n == -l)
            return R(i, 1) * M(m, -l+1) + R(i, -1) * M(m, l-1);
        else {
            SLog(EError, "Internal error!");
            return 0.0f;
        }
    }

    inline Float R(int m, int n) const {
        return M1(m+1, n+1);
    }

    inline Float M(int m, int n) const {
        return Mp(m+prevLevel, n+prevLevel);
    }

    void compute() {
        for (int m=-level; m<=level; ++m) {
            for (int n=-level; n<=level; ++n) {
                Float uVal = u(level, m, n), vVal = v(level, m, n), wVal = w(level, m, n);
                Mn(m+level, n+level) =
                      (uVal != 0 ? (uVal * U(level, m, n)) : (Float) 0)
                    + (vVal != 0 ? (vVal * V(level, m, n)) : (Float) 0)
                    + (wVal != 0 ? (wVal * W(level, m, n)) : (Float) 0);
            }
        }
    }
};

void SHVector::rotationBlock(
        const SHRotation::Matrix &M1,
        const SHRotation::Matrix &Mp,
        SHRotation::Matrix &Mn) {
    RotationBlockHelper rbh(M1, Mp, Mn);
    rbh.compute();
}

void SHVector::rotation(const Transform &t, SHRotation &rot) {
    rot.blocks[0](0, 0) = 1;
    if (rot.blocks.size() <= 1)
        return;

    const Matrix4x4 &trafo = t.getMatrix();
    rot.blocks[1](0, 0) =  trafo.m[1][1];
    rot.blocks[1](0, 1) = -trafo.m[2][1];
    rot.blocks[1](0, 2) =  trafo.m[0][1];
    rot.blocks[1](1, 0) = -trafo.m[1][2];
    rot.blocks[1](1, 1) =  trafo.m[2][2];
    rot.blocks[1](1, 2) = -trafo.m[0][2];
    rot.blocks[1](2, 0) =  trafo.m[1][0];
    rot.blocks[1](2, 1) = -trafo.m[2][0];
    rot.blocks[1](2, 2) =  trafo.m[0][0];

    if (rot.blocks.size() <= 2)
        return;

    for (size_t i=2; i<rot.blocks.size(); ++i)
        rotationBlock(rot.blocks[1], rot.blocks[i-1], rot.blocks[i]);
}

void SHRotation::operator()(const SHVector &source, SHVector &target) const {
    SAssert(source.getBands() == target.getBands());
    for (int l=0; l<source.getBands(); ++l) {
        const SHRotation::Matrix &M = blocks[l];
        for (int m1=-l; m1<=l; ++m1) {
            Float result = 0;
            for (int m2=-l; m2<=l; ++m2)
                result += M(m1+l, m2+l)*source(l, m2);
            target(l, m1) = result;
        }
    }
}

SHSampler::SHSampler(int bands, int depth) : m_bands(bands), m_depth(depth) {
    m_phiMap = new Float**[depth+1];
    m_legendreMap = new Float**[depth+1];
    m_normalization = new Float[m_bands*(m_bands+1)/2];
    m_dataSize = m_bands*(m_bands+1)/2;
    Assert(depth >= 1);

    for (int i=0; i<=depth; ++i) {
        int res = 1 << i;
        Float zStep  = -2 / (Float) res;
        Float phiStep = 2 * (Float) M_PI / (Float) res;
        m_phiMap[i] = new Float*[res];
        m_legendreMap[i] = new Float*[res];

        for (int j=0; j<res; ++j) {
            m_phiMap[i][j] = phiIntegrals(phiStep*j, phiStep*(j+1));
            m_legendreMap[i][j] = legendreIntegrals(1+zStep*j, 1+zStep*(j+1));
        }
    }

    for (int l=0; l<m_bands; ++l) {
        for (int m=0; m<=l; ++m) {
            Float normFactor = boost::math::tgamma_delta_ratio(
                (Float) (l - m + 1), (Float) (2 * m), boost::math::policies::policy<>());
            normFactor = std::sqrt(normFactor * (2 * l + 1) / (4 * (Float) M_PI));
            if (m != 0)
                normFactor *= SQRT_TWO;
            m_normalization[I(l, m)] = normFactor;
        }
    }
}

std::string SHSampler::toString() const {
    std::ostringstream oss;
    oss << "SHSampler[bands=" << m_bands << ", depth=" << m_depth
        << ", size=" << (m_dataSize*sizeof(double))/1024 << " KiB]";
    return oss.str();
}

Float SHSampler::warp(const SHVector &f, Point2 &sample) const {
    int i = 0, j = 0;
    Float integral = 0, integralRoot = integrate(0, 0, 0, f);

    for (int depth = 1; depth <= m_depth; ++depth) {
        /* Do not sample negative areas */
        Float q00 = std::max(integrate(depth, i, j, f), (Float) 0);
        Float q10 = std::max(integrate(depth, i, j+1, f), (Float) 0);
        Float q01 = std::max(integrate(depth, i+1, j, f), (Float) 0);
        Float q11 = std::max(integrate(depth, i+1, j+1, f), (Float) 0);

        Float z1 = q00 + q10, z2 = q01 + q11, phi1, phi2;
        Float zNorm = (Float) 1 / (z1+z2);
        z1 *= zNorm; z2 *= zNorm;

        if (sample.x < z1) {
            sample.x /= z1;
            phi1 = q00; phi2 = q10;
            i <<= 1;
        } else {
            sample.x = (sample.x - z1) / z2;
            phi1 = q01; phi2 = q11;
            i = (i+1) << 1;
        }

        Float phiNorm = (Float) 1 / (phi1+phi2);
        Float phi1Norm = phi1*phiNorm, phi2Norm = phi2*phiNorm;

        if (sample.y <= phi1Norm) {
            sample.y /= phi1Norm;
            j <<= 1;
            integral = phi1;
        } else {
            sample.y = (sample.y - phi1Norm) / phi2Norm;
            j = (j+1) << 1;
            integral = phi2;
        }
    }

    Float zStep = -2 / (Float) (1 << m_depth);
    Float phiStep = 2 * (Float) M_PI / (Float) (1 << m_depth);
    i >>= 1; j >>= 1;

    Float z = 1 + zStep * i + zStep * sample.x;
    sample.x = std::acos(z);
    sample.y = phiStep * j + phiStep * sample.y;

    /* PDF of sampling the mip-map bin */
    Float pdfBin = integral/integralRoot;

    /* Density within the bin */
    Float density = -1/(zStep*phiStep);

    return density*pdfBin;
}

SHSampler::~SHSampler() {
    for (int i=0; i<=m_depth; ++i) {
        int res = 1 << i;
        for (int j=0; j<res; ++j) {
            delete[] m_phiMap[i][j];
            delete[] m_legendreMap[i][j];
        }
        delete[] m_phiMap[i];
        delete[] m_legendreMap[i];
    }
    delete[] m_phiMap;
    delete[] m_legendreMap;
    delete[] m_normalization;
}

Float SHSampler::integrate(int depth, int zBlock, int phiBlock, const SHVector &f) const {
    Float result = 0;

    for (int l=0; l<m_bands; ++l) {
        for (int m=-l; m<=l; ++m) {
            Float basisIntegral = m_normalization[I(l, std::abs(m))]
                * lookupIntegral(depth, zBlock, phiBlock, l, m);
            result += basisIntegral * f(l, m);
        }
    }
    return result;
}

Float *SHSampler::phiIntegrals(Float a, Float b) {
    Float *sinPhiA = new Float[m_bands+1];
    Float *sinPhiB = new Float[m_bands+1];
    Float *cosPhiA = new Float[m_bands+1];
    Float *cosPhiB = new Float[m_bands+1];
    Float *result = new Float[2*m_bands+1];
    m_dataSize += 2*m_bands+1;

    cosPhiA[0] = 1; sinPhiA[0] = 0;
    cosPhiB[0] = 1; sinPhiB[0] = 0;
    cosPhiA[1] = std::cos(a);
    sinPhiA[1] = std::sin(a);
    cosPhiB[1] = std::cos(b);
    sinPhiB[1] = std::sin(b);

    for (int m=2; m<=m_bands; ++m) {
        sinPhiA[m] = 2*sinPhiA[m-1]*cosPhiA[1] - sinPhiA[m-2];
        sinPhiB[m] = 2*sinPhiB[m-1]*cosPhiB[1] - sinPhiB[m-2];

        cosPhiA[m] = 2*cosPhiA[m-1]*cosPhiA[1] - cosPhiA[m-2];
        cosPhiB[m] = 2*cosPhiB[m-1]*cosPhiB[1] - cosPhiB[m-2];
    }

    for (int m=-m_bands; m<=m_bands; ++m) {
        if (m == 0)
            result[P(m)] = b-a;
        else if (m > 0)
            result[P(m)] = (sinPhiB[m]-sinPhiA[m])/m;
        else
            result[P(m)] = (cosPhiB[-m]-cosPhiA[-m])/m;
    }

    delete[] sinPhiA;
    delete[] sinPhiB;
    delete[] cosPhiA;
    delete[] cosPhiB;
    return result;
}

Float *SHSampler::legendreIntegrals(Float a, Float b) {
    Float *P = new Float[m_bands*(m_bands+1)/2];
    m_dataSize += m_bands*(m_bands+1)/2;

    P[I(0, 0)] = b-a;

    if (m_bands == 1)
        return P;

    Float *Pa = new Float[m_bands*(m_bands+1)/2];
    Float *Pb = new Float[m_bands*(m_bands+1)/2];

    for (int l=0; l<m_bands; ++l) {
        for (int m=0; m<=l; ++m) {
            Pa[I(l,m)] = legendreP(l, m, a);
            Pb[I(l,m)] = legendreP(l, m, b);
        }
    }

    P[I(1,0)] = (b*b - a*a)/2;
    P[I(1,1)] = .5f * (-b*std::sqrt(1-b*b) - std::asin(b) + a*std::sqrt(1-a*a) + std::asin(a));

    for (int l=2; l<m_bands; ++l) {
        for (int m=0; m<=l-2; ++m) {
            Float ga = (2*l-1)*(1-a*a) * Pa[I(l-1,m)];
            Float gb = (2*l-1)*(1-b*b) * Pb[I(l-1,m)];
            P[I(l, m)] = ((l-2)*(l-1+m)*P[I(l-2, m)]-gb+ga)/((l+1)*(l-m));
        }

        P[I(l, l-1)] = (2*l-1)/(Float)(l+1) * ((1-a*a)*Pa[I(l-1, l-1)] - (1-b*b)*Pb[I(l-1, l-1)]);
        P[I(l, l)] = 1/(Float)(l+1) * (l*(2*l-3)*(2*l-1) * P[I(l-2, l-2)] + b*Pb[I(l,l)] - a*Pa[I(l, l)]);
    }

    delete[] Pa;
    delete[] Pb;

    return P;
}

MTS_IMPLEMENT_CLASS(SHSampler, false, Object)
MTS_NAMESPACE_END


#include <Eigen/Core>
#include <Eigen/Dense>
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
#include <boost/filesystem.hpp>
#include <Open3D/Open3D.h>
#include <glog/logging.h>

#include "FileSystemTools.h"
#include "YamlFileIO.h"

// fitness threshold
double min_fitness_thresh = 0.05;

struct RegistrationResult{
    // transform matrix
    Eigen::Matrix4d T;
    // fitness score
    double fitness;
    // inlier rms
    double inlier_rms;
};

// create init extrinsic file
void createLidarCamExtFile(const std::string& filepath, int lidar_id)
{
    std::string folder, filename;
    common::splitPathAndFilename(filepath, &folder, &filename);

    std::string init_T_l0_c0_filepath = common::concatenateFolderAndFileName(folder, "init_lidar0_to_camera0.yml");
    // new backpack structural value
    Eigen::Matrix4d T_l0_c0_gt = Eigen::Matrix4d::Identity();
    Eigen::AngleAxisd l0_c0_vec1(-30 * M_PI / 180.0, Eigen::Vector3d(0, 0, 1));
    Eigen::AngleAxisd l0_c0_vec2(90 * M_PI / 180.0, Eigen::Vector3d(1, 0, 0));
    Eigen::AngleAxisd l0_c0_vec3( M_PI, Eigen::Vector3d(0, 0, 1));
    Eigen::Matrix3d l0_c0_vec = l0_c0_vec1.matrix() * l0_c0_vec2.matrix() * l0_c0_vec3.matrix();
    Eigen::Vector3d t_l0_c0(0.0, -0.05657, -0.05931);
    T_l0_c0_gt.block<3, 3>(0, 0) = l0_c0_vec;
    T_l0_c0_gt.block<3, 1>(0, 3) = l0_c0_vec1.matrix() * t_l0_c0;
    // std::cout << "T_l0_c0_gt:\n" << T_l0_c0_gt << "\n";
    common::saveExtFileOpencv(init_T_l0_c0_filepath, T_l0_c0_gt);

    Eigen::Matrix4d T_imu_l0_gt = Eigen::Matrix4d::Identity();
    Eigen::AngleAxisd imu_l0_vec1(150 * M_PI / 180.0, Eigen::Vector3d(0, 0, 1));
    Eigen::AngleAxisd imu_l0_vec2(30 * M_PI / 180.0, Eigen::Vector3d(0, 0, 1));
    Eigen::Matrix3d imu_l0_vec = imu_l0_vec1.matrix() * imu_l0_vec2.matrix();
    Eigen::Vector3d t_imu_l0(0.00044, 0.03407, 0.10148);
    T_imu_l0_gt.block<3, 3>(0, 0) = imu_l0_vec;
    T_imu_l0_gt.block<3, 1>(0, 3) = imu_l0_vec1.matrix() * t_imu_l0;
    std::cout << "T_imu_l0_gt:\n" << T_imu_l0_gt << "\n";

    Eigen::Matrix4d T_l0_l1_gt = Eigen::Matrix4d::Identity();
    Eigen::AngleAxisd l0_l1_vec1(-30 * M_PI / 180.0, Eigen::Vector3d(0, 0, 1));
    Eigen::AngleAxisd l0_l1_vec2(-73.5 * M_PI / 180.0, Eigen::Vector3d(0, 1, 0));
    Eigen::Matrix3d l0_l1_vec = l0_l1_vec1.matrix() * l0_l1_vec2.matrix();
    Eigen::Vector3d t_l0_l1(-0.31405, 0, -0.39803);
    T_l0_l1_gt.block<3, 3>(0, 0) = l0_l1_vec;
    T_l0_l1_gt.block<3, 1>(0, 3) = l0_l1_vec1.matrix() * t_l0_l1;
    // calc T_l1_c0
    Eigen::Matrix4d T_l1_c0_gt = T_l0_l1_gt.inverse() * T_l0_c0_gt;
    common::saveExtFileOpencv(filepath, T_l1_c0_gt);
}

bool alignTwoPointClouds(const std::string &src_pcl_file, const std::string &target_pcl_file, const Eigen::Matrix4d &T_init,
                RegistrationResult &result){
    std::string path, file_name;
    common::splitPathAndFilename(src_pcl_file, &path, &file_name);

    auto src_pcd_ptr = open3d::io::CreatePointCloudFromFile(src_pcl_file);
    auto target_pcd_ptr = open3d::io::CreatePointCloudFromFile(target_pcl_file);

    if( src_pcd_ptr == nullptr || src_pcd_ptr == nullptr){
        LOG(ERROR) << "Fail to load src and target pointcloud file!";
        return false;
    }

    auto src_pcd_down_ptr = src_pcd_ptr->VoxelDownSample(0.05);
    src_pcd_ptr->EstimateNormals(open3d::geometry::KDTreeSearchParamHybrid(0.1, 30));

    auto target_pcd_down_ptr = target_pcd_ptr->VoxelDownSample(0.05);
    target_pcd_ptr->EstimateNormals(open3d::geometry::KDTreeSearchParamHybrid(0.1, 30));

    src_pcd_ptr->Transform(T_init);
    src_pcd_down_ptr->Transform(T_init);

    open3d::registration::RegistrationResult icp_result;

    const double max_corresp_dis = 0.03; // meter
    open3d::registration::ICPConvergenceCriteria icp_criteria(1e-6, 1e-6, 100);
    icp_result = open3d::registration::RegistrationICP(*src_pcd_ptr, *target_pcd_ptr, max_corresp_dis, Eigen::Matrix4d::Identity(),
                                                            open3d::registration::TransformationEstimationPointToPlane(), icp_criteria);
    // icp_result = open3d::registration::RegistrationICP(*src_pcd_ptr, *target_pcd_ptr, max_corresp_dis, Eigen::Matrix4d::Identity(),
    //                                                 open3d::registration::TransformationEstimationPointToPoint(false), icp_criteria);

    // transform source pointcloud with icp_transformation
    Eigen::Matrix4d T_icp = icp_result.transformation_;

    LOG(INFO) << "#############" << path << " ################";
    result.fitness = icp_result.fitness_;
    LOG(INFO) << "icp_result.fitness: " << icp_result.fitness_;
    result.inlier_rms = icp_result.inlier_rmse_;
    LOG(INFO) << "icp_result.inlier_rmse_ : " << icp_result.inlier_rmse_;

    if(result.fitness < min_fitness_thresh){
        LOG(ERROR) << "fitness score < " << min_fitness_thresh;
        return false;
    }

    // final transformation between source pointcloud and target pointcloud
    result.T = T_icp * T_init;
    src_pcd_down_ptr->Transform(T_icp);

    std::string save_file_path;

    save_file_path = common::concatenateFolderAndFileName(path, "transformed_source_pointcloud.ply");
    open3d::io::WritePointCloudToPLY(save_file_path, *src_pcd_down_ptr, true);
    std::shared_ptr<open3d::geometry::PointCloud> merge_points(new open3d::geometry::PointCloud);
    *merge_points = *src_pcd_down_ptr + *target_pcd_down_ptr;

    save_file_path = common::concatenateFolderAndFileName(path, "merged_pointcloud.ply");
    open3d::io::WritePointCloudToPLY(save_file_path, *merge_points, true);

    return true;
}

void evaluateExtrinsics(const std::vector<RegistrationResult> & v_results, const Eigen::Matrix4d &T_baseline){
    assert(v_results.size() > 1);

    const Eigen::Matrix4d T_0 = v_results[0].T;
    std::vector<double> v_inlier_rms = {v_results[0].inlier_rms};
    // rotation sigma
    std::vector<double> v_sigma_r;
    // translation sigma
    std::vector<double> v_sigma_t;

    int T_num = v_results.size();
    for(int i = 0; i < T_num; ++i){
        Eigen::Matrix4d T = v_results[i].T;
        Eigen::Matrix4d T_inv = T.inverse();

        Eigen::Matrix4d T_delt = T_inv * T_baseline;
        Eigen::AngleAxisd rot_vec(T_delt.block<3, 3>(0, 0));
        double delt_r = rot_vec.angle() * 180.0 / M_PI;
        Eigen::Vector3d t_delt = T_delt.block<3, 1>(0, 3);
        double delt_t = t_delt.norm() * 100;
        LOG(INFO) << "T_baseline and  T_" << i << " Rotation delta: "
                <<  delt_r << " °\n";
        LOG(INFO) << "T_baseline and  T_" << i << " Translation delta: " << delt_t
                << " cm\n";

        v_sigma_r.push_back(delt_r * delt_r);
        v_sigma_t.push_back(delt_t * delt_t);
        v_inlier_rms.push_back(v_results[i].inlier_rms);
    }

    double sigma_r = std::accumulate(v_sigma_r.begin(), v_sigma_r.end(), 0.0) / T_num;
    double sigma_t = std::accumulate(v_sigma_t.begin(), v_sigma_t.end(), 0.0) / T_num;
    LOG(INFO) << "Avg rotation sigma: " << sigma_r << ", Avg trans sigma: " << sigma_t;
    LOG(INFO) << "Final sigma_r" << std::sqrt(sigma_r) << ", sigma_t: " << std::sqrt(sigma_t);
    // LOG(INFO) << "Avg inlier rms: " << std::accumulate(v_inlier_rms.begin(), v_inlier_rms.end(), 0.0) / v_inlier_rms.size();

}

Eigen::Matrix4d readCameraPoseTxt(const std::string &file)
{
    Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
    std::ifstream fs(file);
    if (!fs.is_open())
    {
        std::cerr << "Fail to open camera pose file " << file << std::endl;
        return T;
    }

    std::string line;
    int i = 0;
    while (!fs.eof())
    {
        line.clear();
        getline(fs, line);
        if (line.empty())
            continue;

        std::stringstream ss(line);
        std::string dummy_str;
        if (i < 4)
        {
            ss >> T(i, 0) >> dummy_str >> T(i, 1) >> dummy_str >> T(i, 2) >> dummy_str >> T(i, 3);
            i++;
        }
        else
        {
            std::cerr << "Invalid camera pose format!\n";
            break;
        }
    }
    fs.close();

    return T;
}

int main(int argc, char **argv){
    if (argc < 6){
        LOG(FATAL) << "Usage: test_lidar2cam_calibration [init_T_l1_cam0.yaml] [dataset_folder] [cam_pose_file] [target_ply_file] [output_folder] [lidar_id]";
        return -1;
    }

    std::string init_ext_filepath(argv[1]);
    // dataset folder e.g. /path/data0/lidar_cam
    std::string dataset_folder(argv[2]);
    // teche0 pose w.r.t calibration reference
    std::string teche0_pose_filepath(argv[3]);
    // target pointcloud used in lidar localization
    std::string target_pcl_file_path(argv[4]);
    std::string output_folder(argv[5]);
    int lidar_id = 1;
    // specificly camera0!!!
    // int cam_id = 0;
    if (argc >= 7 ){
        lidar_id = std::atoi(argv[6]);
    }

    if(!common::fileExists(init_ext_filepath)){
        createLidarCamExtFile(init_ext_filepath, lidar_id);
    }
    if(!common::pathExists(dataset_folder)){
        LOG(FATAL) << "Input folder doesnot exist!";
        return -1;
    }
    if(!common::pathExists(output_folder)){
        if(!common::createPath(output_folder)){
            LOG(FATAL) << "Fail to create " << output_folder;
            return -1;
        }
    }

    // Eigen::Matrix4d T_base_l0 = horizontal_lidar_ptr->extrinsics();
    // Eigen::Matrix4d T_base_l1 = vertical_lidar_ptr->extrinsics();
    // Eigen::Matrix4d T_base_teche0 = teche0_ptr->extrinsics();
    // initial extrinsic between two lidar
    // Eigen::Matrix4d T_teche0_l1 = T_base_teche0.inverse() * T_base_l1;
    Eigen::Matrix4d T_l1_teche0 = Eigen::Matrix4d::Identity();
    if (!common::loadExtFileOpencv(init_ext_filepath, T_l1_teche0)){
        LOG(FATAL) << "Fail to load " << init_ext_filepath;
        return -1;
    }
    Eigen::Matrix4d T_teche0_l1 = T_l1_teche0.inverse();
    Eigen::Matrix4d T_w_teche0 = readCameraPoseTxt(teche0_pose_filepath);
    Eigen::Matrix4d T_w_l1_init = T_w_teche0 * T_teche0_l1;

    std::vector<RegistrationResult> v_extrinsics;
    // for (const auto & entry : boost::filesystem::directory_iterator(dataset_folder)){
    //     if(!boost::filesystem::is_directory(entry))
    //         continue;

    //     std::string data_folder = entry.path().string() + "/lidar_cam";
        std::string scan_folder_path = common::concatenateFolderAndFileName(dataset_folder, "lidar"+std::to_string(lidar_id));
        std::vector<std::string> v_pcl_paths;
        std::vector<std::string> paths = {scan_folder_path};
        common::getFileLists(paths, true, "ply", &v_pcl_paths);
        std::string src_pcl_file_path;
        for(size_t i = 0; i < v_pcl_paths.size(); ++i){
            std::string file_path = v_pcl_paths[i];
            std::string path, file_name;
            common::splitPathAndFilename(file_path, &path, &file_name);
            if(std::atoi(&file_name[0]) == lidar_id)
                src_pcl_file_path = file_path;
        }

        RegistrationResult regist_result;
        bool sts = alignTwoPointClouds(src_pcl_file_path, target_pcl_file_path, T_w_l1_init, regist_result);
        if(!sts){
            return -1;
        }

        Eigen::Matrix4d T_w_l1 = regist_result.T;
        // T_l1_teche0 after icp refine
        regist_result.T = T_w_l1.inverse() * T_w_teche0;
        std::cout << "Final T_l1_teche0: \n" << regist_result.T << "\n";
        std::string save_file_path = common::concatenateFolderAndFileName(output_folder, "lidar"+std::to_string(lidar_id)+"_to_camera0.yml");
        common::saveExtFileOpencv(save_file_path, regist_result.T);
        v_extrinsics.emplace_back(regist_result);
    // }

    // sort result by fitness
    // std::sort(v_extrinsics.begin(), v_extrinsics.end(), [&](RegistrationResult &res_a, RegistrationResult &res_b){
    //     return res_a.fitness > res_b.fitness;
    // });

    // evaluateExtrinsics(v_extrinsics, v_extrinsics[0].T);

    return 0;
}


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <chrono>

using namespace std;
using namespace cv;

void find_feature_matches (
    const Mat& img_1, const Mat& img_2,
    std::vector<KeyPoint>& keypoints_1,
    std::vector<KeyPoint>& keypoints_2,
    std::vector< DMatch >& matches );

// 像素坐标转相机归一化坐标
Point2d pixel2cam ( const Point2d& p, const Mat& K );

void bundleAdjustment (
    const vector<Point3f> points_3d,
    const vector<Point2f> points_2d,
    const Mat& K,
    Mat& R, Mat& t
);

int main ( int argc, char** argv )
{
    if ( argc != 5 )
    {
        cout<<"usage: pose_estimation_3d2d img1 img2 depth1 depth2"<<endl;
        return 1;
    }
    //-- 读取图像
    Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
    Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );

    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );
    cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;

    // 建立3D点
    Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );       // 深度图为16位无符号数，单通道图像
    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    vector<Point3f> pts_3d;
    vector<Point2f> pts_2d;
    for ( DMatch m:matches )
    {
        ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
        if ( d == 0 )   // bad depth
            continue;
        float dd = d/1000.0;
        Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
        pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );
        pts_2d.push_back ( keypoints_2[m.trainIdx].pt );
    }

    cout<<"3d-2d pairs: "<<pts_3d.size() <<endl;

    Mat r, t;
    solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
    Mat R;
    cv::Rodrigues ( r, R ); // r为旋转向量形式，用Rodrigues公式转换为矩阵

    cout<<"R="<<endl<<R<<endl;
    cout<<"t="<<endl<<t<<endl;

    cout<<"calling bundle adjustment"<<endl;

    bundleAdjustment ( pts_3d, pts_2d, K, R, t );
}

void find_feature_matches ( const Mat& img_1, const Mat& img_2,
                            std::vector<KeyPoint>& keypoints_1,
                            std::vector<KeyPoint>& keypoints_2,
                            std::vector< DMatch >& matches )
{
    //-- 初始化
    Mat descriptors_1, descriptors_2;
    // used in OpenCV3
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    // use this if you are in OpenCV2
    // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
    // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
    Ptr<DescriptorMatcher> matcher  = DescriptorMatcher::create ( "BruteForce-Hamming" );
    //-- 第一步:检测 Oriented FAST 角点位置
    detector->detect ( img_1,keypoints_1 );
    detector->detect ( img_2,keypoints_2 );

    //-- 第二步:根据角点位置计算 BRIEF 描述子
    descriptor->compute ( img_1, keypoints_1, descriptors_1 );
    descriptor->compute ( img_2, keypoints_2, descriptors_2 );

    //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    vector<DMatch> match;
    // BFMatcher matcher ( NORM_HAMMING );
    matcher->match ( descriptors_1, descriptors_2, match );

    //-- 第四步:匹配点对筛选
    double min_dist=10000, max_dist=0;

    //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        double dist = match[i].distance;
        if ( dist < min_dist ) min_dist = dist;
        if ( dist > max_dist ) max_dist = dist;
    }

    printf ( "-- Max dist : %f \n", max_dist );
    printf ( "-- Min dist : %f \n", min_dist );

    //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        if ( match[i].distance <= max ( 2*min_dist, 30.0 ) )
        {
            matches.push_back ( match[i] );
        }
    }
}

Point2d pixel2cam ( const Point2d& p, const Mat& K )
{
    return Point2d
           (
               ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),
               ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )
           );
}

void bundleAdjustment (
    const vector< Point3f > points_3d,
    const vector< Point2f > points_2d,
    const Mat& K,
    Mat& R, Mat& t )
{
    // 初始化g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose 维度为 6, landmark 维度为 3
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 线性方程求解器
    Block* solver_ptr = new Block ( linearSolver );     // 矩阵块求解器
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );

    // vertex
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
    Eigen::Matrix3d R_mat;
    R_mat <<
          R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ),
               R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ),
               R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 );
    pose->setId ( 0 );
    pose->setEstimate ( g2o::SE3Quat (
                            R_mat,
                            Eigen::Vector3d ( t.at<double> ( 0,0 ), t.at<double> ( 1,0 ), t.at<double> ( 2,0 ) )
                        ) );
    optimizer.addVertex ( pose );

    int index = 1;
    for ( const Point3f p:points_3d )   // landmarks
    {
        g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
        point->setId ( index++ );
        point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
        point->setMarginalized ( true ); // g2o 中必须设置 marg 参见第十讲内容
        optimizer.addVertex ( point );
    }

    // parameter: camera intrinsics
    g2o::CameraParameters* camera = new g2o::CameraParameters (
        K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0
    );
    camera->setId ( 0 );
    optimizer.addParameter ( camera );

    // edges
    index = 1;
    for ( const Point2f p:points_2d )
    {
        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        edge->setId ( index );
        edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
        edge->setVertex ( 1, pose );
        edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );
        edge->setParameterId ( 0,0 );
        edge->setInformation ( Eigen::Matrix2d::Identity() );
        optimizer.addEdge ( edge );
        index++;
    }

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.setVerbose ( true );
    optimizer.initializeOptimization();
    optimizer.optimize ( 100 );
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
    cout<<"optimization costs time: "<<time_used.count() <<" seconds."<<endl;

    cout<<endl<<"after optimization:"<<endl;
    cout<<"T="<<endl<<Eigen::Isometry3d ( pose->estimate() ).matrix() <<endl;
}


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          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
//==============================================================================

*/

#ifndef __OPTIMISATION2_ALGO_LM_MANY_CLASSES_SCHUR_COMPLEMENT_HPP__
#define __OPTIMISATION2_ALGO_LM_MANY_CLASSES_SCHUR_COMPLEMENT_HPP__

#include <libv/lma/ttt/traits/wrap.hpp>
#include <libv/lma/global.hpp>
#include <libv/lma/lm/ba/computing.hpp>
#include <libv/lma/lm/ba/make_type.hpp>
#include <libv/lma/lm/ba/create_hessian.hpp>
#include <libv/lma/lm/omp/omp.hpp>
#include <boost/mpl/advance.hpp>
#include <boost/mpl/remove_if.hpp>
#include <boost/mpl/contains.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/type_traits.hpp>
#include <boost/fusion/include/as_map.hpp>
#include <libv/core/tag.hpp>

namespace lma
{

  template<class VTYPE, class Ap, class H, class P, class Y> struct ProdApPH
  {
    Ap& ap;
    const H& h;
    const P& p;
    const Y& y;
    ProdApPH(Ap& ap_, const H& h_, const P& p_, const Y& y_):ap(ap_),h(h_),p(p_),y(y_){}

    template<class T> void operator()(T){}

    template<class Key1,class Key2, class Value, template<class,class> class Pair> void operator()(ttt::wrap<Pair<Pair<Key1,Key2>,Value>>,
      typename boost::disable_if<
          mpl::or_<
                    mpl::not_<br::has_key<H,Pair<Key2,VTYPE>>>,
                    mpl::not_<br::has_key<Y,Pair<Key1,VTYPE>>>
                  >
            >::type * = 0)
    {
      {
        auto& a1 = bf::at_key<Key1>(ap);
        const auto& pr2 = bf::at_key<Key2>(p);
        const auto& y1= bf::at_key<Pair<Key1,VTYPE>>(y);
        const auto& w2 = bf::at_key<Pair<Key2,VTYPE>>(h);
        const auto& v = bf::at_key<Pair<VTYPE,VTYPE>>(h);

        typedef typename boost::remove_reference<decltype(v)>::type V;

        VectorColumn< VTYPE, typename V::MatrixTag > v0;

// 	std::cout << " [1] : " << a1.name() << " = " << w2.name() << " " << pr2.name() << " " << y1.name() << std::endl;
        // v0 = Wt * pr
        prod(v0,w2,pr2);
        // a = - Y * v0 , (Y = W * V)
        prod_minus(a1,y1,v0);

        auto& a2 = bf::at_key<Key2>(ap);
        const auto& pr1 = bf::at_key<Key1>(p);
        const auto& y2= bf::at_key<Pair<Key2,VTYPE>>(y);
        const auto& w1 = bf::at_key<Pair<Key1,VTYPE>>(h);

        typedef typename boost::remove_reference<decltype(v)>::type V;

        VectorColumn< VTYPE, typename V::MatrixTag > v1;

// 	std::cout << " [2] : " << a2.name() << " = " << w1.name() << " " << pr1.name() << " " << y2.name() << std::endl;
        // v1 = Wt * pr
        prod(v1,w1,pr1);
        // a = - Y * v1, (Y = W * V)
        prod_minus(a2,y2,v1);
      }
    }

    //! cas où on est sur la diagonale
    template<class Key1, class Value, template<class,class> class Pair> void operator()(ttt::wrap<Pair<Pair<Key1,Key1>,Value>>,
      typename boost::disable_if<boost::mpl::not_<br::has_key<H,Pair<Key1,VTYPE>>>>::type * = 0)
    {
      const auto& pr = bf::at_key<Key1>(p);
      const auto& u = bf::at_key<Pair<Key1,Key1>>(h);

      auto& a = bf::at_key<Key1>(ap);
      auto& w = bf::at_key<Pair<Key1,VTYPE>>(h);
      auto& y0 = bf::at_key<Pair<Key1,VTYPE>>(y);

      auto& v = bf::at_key<Pair<VTYPE,VTYPE>>(h);

      if (a.size()==0) a.resize(pr.size());
      typedef typename boost::remove_reference<decltype(v)>::type V;
      VectorColumn< VTYPE, typename V::MatrixTag > v0;
      VectorColumn< Key1, typename V::MatrixTag > v1;
      v0.resize(v.size());
      v1.resize(u.size());

//       std::cout << " [0] : " << a.name() << " = " << w.name() << " " << pr.name() << " " << y0.name() << std::endl;
      //v0 = Wt * pr
      prod(v0,w,pr);

      //v1 = Y * v0 , ( Y = W * V )
      prod(v1,y0,v0);

      // a -= u1
//       #pragma omp parallel for if(use_omp())
      for(auto i = w.first() ; i < w.size() ; ++i)
        a(i) -= v1(i);
    }
  };

  template<class VTYPE, class AP, class H, class P, class Y> ProdApPH<VTYPE,AP,H,P,Y> prod_ap_ph(AP& ap, const H& h, const P& p, const Y& y){ return ProdApPH<VTYPE,AP,H,P,Y>(ap,h,p,y); }

  template<class Float, class I, class List, class J, class Result> struct UnrollW2 :
    mpl::push_back<
		    Result,
		    typename MakeTupleTable<
					    typename mpl::at<List,I>::type,
					    typename mpl::at<List,J>::type,
					    Float
					  >::type
		  >::type {};

  template<class Float, class K, class List, class Int, class Result> struct UnrollW1_ :
    For<K::value,mpl::size<List>::value,List,UnrollW2<Float,Int,mpl::_1,mpl::_2,mpl::_3>,Result> {};

  template<class Float, class A> using UnrollW1 = UnrollW1_<Float,A,mpl::_1,mpl::_2,mpl::_3>;

  template<class L, class Int, class Result> struct PushBack_ :
    mpl::push_back<Result,typename mpl::at<L,Int>::type> {};

  typedef PushBack_<mpl::_1,mpl::_2,mpl::_3> PushBack;

  template<class BA, class NumericTag> struct ImplicitSchurContainer
  {
    typedef typename BA::Keys Keys;
    typedef typename BA::MatrixTag MatrixTag;

    // K_ est le nombre de famille à mettre dans Vs
    // K est la position de Vs
    static const size_t K_ = Size<NumericTag>::value;
    static const size_t N = mpl::size<Keys>::value;
    static const size_t K = N - K_;

    static_assert( (N > K) , "Nombre de paramètres dans Schur >= Nombre de paramètre dans le problème");

    typedef typename For<K,N,Keys,PushBack>::type KeyVs;
    typedef typename For<0,K,Keys,PushBack>::type KeyUs;

    typedef typename For<0,K,Keys,UnrollW1<MatrixTag,mpl::int_<K>>>::type TypeWs_;
    typedef typename
                    mpl::remove_if<
                                    TypeWs_,
                                    mpl::not_<
                                              mpl::contains<typename BA::ListeHessien,mpl::_1>
                                            >
                                  >::type TypeWs;
    typedef typename br::as_map<TypeWs>::type TupleWs;

    typedef typename mpl::transform< KeyUs, VectorToPairStruct<mpl::_1,MatrixTag> >::type ListResidu;
    typedef typename mpl::transform< KeyVs, VectorToPairStruct<mpl::_1,MatrixTag> >::type ListResiduVs;
    typedef typename br::as_map<ListResidu>::type TupleResiduUs;
    typedef typename br::as_map<ListResiduVs>::type TupleResiduVs;

    typedef typename mpl::transform<KeyVs,ToTable<Single<mpl::_1>,MatrixTag>>::type TypeVs;
    typedef typename br::as_map<TypeVs>::type TupleVs;

    typedef typename mpl::transform< KeyUs, pair_>::type DiagUs;
    typedef typename mpl::transform< KeyVs, pair_>::type DiagVs;

    ImplicitSchurContainer()
    {
//        std::cout << "Parametres : " << ttt::name<ListeParametre>() << std::endl;
//        std::cout << "Us         : " << ttt::name<Us>() << std::endl;
//        std::cout << "Vs         : " << ttt::name<Vs>() << std::endl;
       //std::cout << "Ws         : " << ttt::name<TypeWs>() << std::endl;
//        std::cout << "ResiduUs   : " << ttt::name<TupleResiduUs>() << std::endl;
//        std::cout << "TypeVs     : " << ttt::name<TypeVs>() << std::endl;
//        std::cout << "TupleVs    : " << ttt::name<TupleVs>() << std::endl;
    }

    TupleWs ys;
    TupleResiduUs bs;
    TupleVs save_vs;
  };
}// eon

#endif


/**
 * @file tests/svdplusplus_test.cpp
 * @author Siddharth Agrawal
 * @author Wenhao Huang
 *
 * Test SVD++.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/methods/svdplusplus/svdplusplus.hpp>

#include <ensmallen.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::svd;

BOOST_AUTO_TEST_SUITE(SVDPlusPlusTest);

BOOST_AUTO_TEST_CASE(SVDPlusPlusEvaluate)
{
  // Define useful constants.
  const size_t numUsers = 100;
  const size_t numItems = 100;
  const size_t numRatings = 1000;
  const size_t maxRating = 5;
  const size_t rank = 5;
  const size_t numTrials = 10;

  // Make a random rating dataset.
  arma::mat data = arma::randu(3, numRatings);
  data.row(0) = floor(data.row(0) * numUsers);
  data.row(1) = floor(data.row(1) * numItems);
  data.row(2) = floor(data.row(2) * maxRating + 0.5);

  // Manually set last row to maximum user and maximum item.
  data(0, numRatings - 1) = numUsers - 1;
  data(1, numRatings - 1) = numItems - 1;

  // Make a random implicit dataset.
  arma::sp_mat implicitData = arma::sprandu(numItems, numUsers, 0.1);

  // Make a SVDPlusPlusFunction with zero regularization.
  SVDPlusPlusFunction<arma::mat> svdPPFunc(data, implicitData, rank, 0);

  for (size_t i = 0; i < numTrials; ++i)
  {
    arma::mat parameters = arma::randu(rank + 1, numUsers + 2 * numItems);

    // Calculate cost by summing up cost of each example.
    double cost = 0;
    for (size_t j = 0; j < data.n_cols; ++j)
    {
      const size_t user = data(0, j);
      const size_t item = data(1, j) + numUsers;
      const size_t implicitStart = numUsers + numItems;

      // Calculate the squared error in the prediction.
      const double rating = data(2, j);
      const double userBias = parameters(rank, user);
      const double itemBias = parameters(rank, item);

      // Iterate through each item which the user interacted with to calculate
      // user vector.
      arma::vec userVec(rank, arma::fill::zeros);
      arma::sp_mat::const_iterator it = implicitData.begin_col(user);
      arma::sp_mat::const_iterator it_end = implicitData.end_col(user);
      size_t implicitCount = 0;
      for (; it != it_end; ++it)
      {
        userVec += parameters.col(implicitStart + it.row()).subvec(0, rank - 1);
        implicitCount += 1;
      }
      if (implicitCount != 0)
        userVec /= std::sqrt(implicitCount);
      userVec += parameters.col(user).subvec(0, rank - 1);

      double ratingError = rating - userBias - itemBias -
          arma::dot(userVec, parameters.col(item).subvec(0, rank - 1));
      double ratingErrorSquared = ratingError * ratingError;

      cost += ratingErrorSquared;
    }

    // Compare calculated cost and value obtained using Evaluate().
    BOOST_REQUIRE_CLOSE(cost, svdPPFunc.Evaluate(parameters), 1e-5);
  }
}

BOOST_AUTO_TEST_CASE(SVDPlusPlusFunctionRegularizationEvaluate)
{
  // Define useful constants.
  const size_t numUsers = 100;
  const size_t numItems = 100;
  const size_t numRatings = 1000;
  const size_t maxRating = 5;
  const size_t rank = 5;
  const size_t numTrials = 10;

  // Make a random rating dataset.
  arma::mat data = arma::randu(3, numRatings);
  data.row(0) = floor(data.row(0) * numUsers);
  data.row(1) = floor(data.row(1) * numItems);
  data.row(2) = floor(data.row(2) * maxRating + 0.5);

  // Manually set last row to maximum user and maximum item.
  data(0, numRatings - 1) = numUsers - 1;
  data(1, numRatings - 1) = numItems - 1;

  // Make a random implicit dataset.
  arma::sp_mat implicitData = arma::sprandu(numItems, numUsers, 0.1);

  // Make three SVDPlusPlusFunction objects with different amounts of
  // regularization.
  SVDPlusPlusFunction<arma::mat> svdPPFuncNoReg(data, implicitData, rank, 0);
  SVDPlusPlusFunction<arma::mat> svdPPFuncSmallReg(data, implicitData, rank,
      0.5);
  SVDPlusPlusFunction<arma::mat> svdPPFuncBigReg(data, implicitData, rank, 20);

  for (size_t i = 0; i < numTrials; ++i)
  {
    arma::mat parameters = arma::randu(rank + 1, numUsers + 2 * numItems);

    // The norm square of implicit item vectors is cached to avoid repeated
    // calculation.
    arma::vec implicitVecsNormSquare(numItems);
    implicitVecsNormSquare.fill(-1);

    // Calculate the regularization contributions of parameters corresponding to
    // each rating and sum them up.
    double smallRegTerm = 0;
    double bigRegTerm = 0;
    for (size_t j = 0; j < data.n_cols; ++j)
    {
      const size_t user = data(0, j);
      const size_t item = data(1, j) + numUsers;
      const size_t implicitStart = numUsers + numItems;

      // Iterate through each item which the user interacted with.
      arma::sp_mat::const_iterator it = implicitData.begin_col(user);
      arma::sp_mat::const_iterator it_end = implicitData.end_col(user);
      double regularizationError = 0;
      size_t implicitCount = 0;
      for (; it != it_end; ++it)
      {
        if (implicitVecsNormSquare(it.row()) < 0)
        {
          implicitVecsNormSquare(it.row()) = arma::dot(
            parameters.col(implicitStart + it.row()).subvec(0, rank - 1),
            parameters.col(implicitStart + it.row()).subvec(0, rank - 1));
        }
        regularizationError += implicitVecsNormSquare(it.row());
        implicitCount += 1;
      }
      if (implicitCount != 0)
        regularizationError /= implicitCount;

      // Calculate the regularization penalty corresponding to the parameters.
      double userVecNorm = arma::norm(parameters.col(user), 2);
      double itemVecNorm = arma::norm(parameters.col(item), 2);
      regularizationError +=
          userVecNorm * userVecNorm + itemVecNorm * itemVecNorm;

      smallRegTerm += 0.5 * regularizationError;
      bigRegTerm += 20 * regularizationError;
    }

    // Cost with regularization should be close to the sum of cost without
    // regularization and the regularization terms.
    BOOST_REQUIRE_CLOSE(svdPPFuncNoReg.Evaluate(parameters) + smallRegTerm,
        svdPPFuncSmallReg.Evaluate(parameters), 1e-5);
    BOOST_REQUIRE_CLOSE(svdPPFuncNoReg.Evaluate(parameters) + bigRegTerm,
        svdPPFuncBigReg.Evaluate(parameters), 1e-5);
  }
}

BOOST_AUTO_TEST_CASE(SVDPlusPlusFunctionGradient)
{
  // Define useful constants.
  const size_t numUsers = 100;
  const size_t numItems = 100;
  const size_t numRatings = 1000;
  const size_t maxRating = 5;
  const size_t rank = 5;

  // Make a random rating dataset.
  arma::mat data = arma::randu(3, numRatings);
  data.row(0) = floor(data.row(0) * numUsers);
  data.row(1) = floor(data.row(1) * numItems);
  data.row(2) = floor(data.row(2) * maxRating + 0.5);

  // Manually set last row to maximum user and maximum item.
  data(0, numRatings - 1) = numUsers - 1;
  data(1, numRatings - 1) = numItems - 1;

  // Make a random implicit dataset.
  arma::sp_mat implicitData = arma::sprandu(numItems, numUsers, 0.1);

  arma::mat parameters = arma::randu(rank + 1, numUsers + 2 * numItems);

  // Make two SVDPlusPlusFunction objects, one with regularization and one
  // without.
  SVDPlusPlusFunction<arma::mat> svdPPFunc1(data, implicitData, rank, 0);
  SVDPlusPlusFunction<arma::mat> svdPPFunc2(data, implicitData, rank, 0.5);

  // Calculate gradients for both the objects.
  arma::mat gradient1, gradient2;
  svdPPFunc1.Gradient(parameters, gradient1);
  svdPPFunc2.Gradient(parameters, gradient2);

  // Perturbation constant.
  const double epsilon = 0.0001;
  double costPlus1, costMinus1, numGradient1;
  double costPlus2, costMinus2, numGradient2;

  for (size_t i = 0; i < rank; ++i)
  {
    for (size_t j = 0; j < numUsers + numItems; ++j)
    {
      // Perturb parameter with a positive constant and get costs.
      parameters(i, j) += epsilon;
      costPlus1 = svdPPFunc1.Evaluate(parameters);
      costPlus2 = svdPPFunc2.Evaluate(parameters);

      // Perturb parameter with a negative constant and get costs.
      parameters(i, j) -= 2 * epsilon;
      costMinus1 = svdPPFunc1.Evaluate(parameters);
      costMinus2 = svdPPFunc2.Evaluate(parameters);

      // Compute numerical gradients using the costs calculated above.
      numGradient1 = (costPlus1 - costMinus1) / (2 * epsilon);
      numGradient2 = (costPlus2 - costMinus2) / (2 * epsilon);

      // Restore the parameter value.
      parameters(i, j) += epsilon;

      // Compare numerical and backpropagation gradient values.
      if (std::abs(gradient1(i, j)) <= 1e-6)
        BOOST_REQUIRE_SMALL(numGradient1, 1e-5);
      else
        BOOST_REQUIRE_CLOSE(numGradient1, gradient1(i, j), 0.02);

      if (std::abs(gradient2(i, j)) <= 1e-6)
        BOOST_REQUIRE_SMALL(numGradient2, 1e-5);
      else
        BOOST_REQUIRE_CLOSE(numGradient2, gradient2(i, j), 0.02);
    }
  }
}

BOOST_AUTO_TEST_CASE(SVDplusPlusOutputSizeTest)
{
  // Load small GroupLens dataset.
  arma::mat data;
  data::Load("GroupLensSmall.csv", data);

  // Define useful constants.
  const size_t numUsers = max(data.row(0)) + 1;
  const size_t numItems = max(data.row(1)) + 1;
  const size_t rank = 10;
  const size_t iterations = 10;

  // Resulting user/item matrices/bias, and item implicit matrix.
  arma::mat userLatent, itemLatent;
  arma::vec userBias, itemBias;
  arma::mat itemImplicit;

  // Apply SVD++.
  SVDPlusPlus<> svdPP(iterations);
  svdPP.Apply(data, rank, itemLatent, userLatent, itemBias, userBias,
      itemImplicit);

  // Check the size of outputs.
  BOOST_REQUIRE_EQUAL(itemLatent.n_rows, numItems);
  BOOST_REQUIRE_EQUAL(itemLatent.n_cols, rank);
  BOOST_REQUIRE_EQUAL(userLatent.n_rows, rank);
  BOOST_REQUIRE_EQUAL(userLatent.n_cols, numUsers);
  BOOST_REQUIRE_EQUAL(itemBias.n_elem, numItems);
  BOOST_REQUIRE_EQUAL(userBias.n_elem, numUsers);
  BOOST_REQUIRE_EQUAL(itemImplicit.n_rows, rank);
  BOOST_REQUIRE_EQUAL(itemImplicit.n_cols, numItems);
}

BOOST_AUTO_TEST_CASE(SVDPlusPlusCleanDataTest)
{
  // Load small GroupLens dataset.
  arma::mat data;
  data::Load("GroupLensSmall.csv", data);

  // Define useful constants.
  const size_t numUsers = max(data.row(0)) + 1;
  const size_t numItems = max(data.row(1)) + 1;

  // Make an implicit dataset with the explicit rating dataset.
  arma::mat implicitData = data.submat(0, 0, 1, data.n_cols - 1);

  // We also want to test whether CleanData() can give matrix
  // of right size when maximum user/item is not in implicitData.
  for (size_t i = 0; i < implicitData.n_cols;)
  {
    if (implicitData(0, i) == numUsers - 1 ||
        implicitData(1, i) == numItems - 1)
    {
      implicitData.shed_col(i);
    }
    else
    {
      ++i;
    }
  }

  // Converts implicit data from coordinate list to sparse matrix.
  arma::sp_mat cleanedData;
  SVDPlusPlus<>::CleanData(implicitData, cleanedData, data);

  // Make sure cleanedData has correct size.
  BOOST_REQUIRE_EQUAL(cleanedData.n_rows, numItems);
  BOOST_REQUIRE_EQUAL(cleanedData.n_cols, numUsers);

  // Make sure cleanedData has correct number of implicit data.
  BOOST_REQUIRE_EQUAL(cleanedData.n_nonzero, implicitData.n_cols);

  // Make sure all implicitData are in cleanedData.
  for (size_t i = 0; i < implicitData.n_cols; ++i)
  {
    double value = cleanedData(implicitData(1, i), implicitData(0, i));
    BOOST_REQUIRE_GT(std::fabs(value), 0);
  }
}

BOOST_AUTO_TEST_CASE(SVDPlusPlusFunctionOptimize)
{
  // Define useful constants.
  const size_t numUsers = 100;
  const size_t numItems = 100;
  const size_t numRatings = 1000;
  const size_t iterations = 30;
  const size_t rank = 5;
  const double alpha = 0.01;
  const double lambda = 0;

  // Initiate random parameters.
  arma::mat parameters = arma::randu(rank + 1, numUsers + 2 * numItems);

  // Make a random rating dataset.
  arma::mat data = arma::randu(3, numRatings);
  data.row(0) = floor(data.row(0) * numUsers);
  data.row(1) = floor(data.row(1) * numItems);

  // Manually set last row to maximum user and maximum item.
  data(0, numRatings - 1) = numUsers - 1;
  data(1, numRatings - 1) = numItems - 1;

  // Make a random implicit dataset.
  arma::sp_mat implicitData = arma::sprandu(numItems, numUsers, 0.05);

  // Make rating entries based on the parameters.
  for (size_t i = 0; i < numRatings; ++i)
  {
    const size_t user = data(0, i);
    const size_t item = data(1, i) + numUsers;
    const size_t implicitStart = numUsers + numItems;

    const double userBias = parameters(rank, user);
    const double itemBias = parameters(rank, item);

    // Iterate through each item which the user interacted with to calculate
    // user vector.
    arma::vec userVec(rank, arma::fill::zeros);
    arma::sp_mat::const_iterator it = implicitData.begin_col(user);
    arma::sp_mat::const_iterator it_end = implicitData.end_col(user);
    size_t implicitCount = 0;
    for (; it != it_end; ++it)
    {
      userVec += parameters.col(implicitStart + it.row()).subvec(0, rank - 1);
      implicitCount += 1;
    }
    if (implicitCount != 0)
      userVec /= std::sqrt(implicitCount);
    userVec += parameters.col(user).subvec(0, rank - 1);

    data(2, i) = userBias + itemBias +
        arma::dot(userVec, parameters.col(item).subvec(0, rank - 1));
  }

  // Make the SVD++ function and the optimizer.
  SVDPlusPlusFunction<arma::mat> svdPPFunc(data, implicitData, rank, lambda);
  ens::StandardSGD optimizer(alpha, iterations * numRatings);

  // Obtain optimized parameters after training.
  arma::mat optParameters = arma::randu(rank + 1, numUsers + 2 * numItems);
  optimizer.Optimize(svdPPFunc, optParameters);

  // Get predicted ratings from optimized parameters.
  arma::mat predictedData(1, numRatings);
  for (size_t i = 0; i < numRatings; ++i)
  {
    const size_t user = data(0, i);
    const size_t item = data(1, i) + numUsers;
    const size_t implicitStart = numUsers + numItems;

    const double userBias = optParameters(rank, user);
    const double itemBias = optParameters(rank, item);

    // Iterate through each item which the user interacted with to calculate
    // user vector.
    arma::vec userVec(rank, arma::fill::zeros);
    arma::sp_mat::const_iterator it = implicitData.begin_col(user);
    arma::sp_mat::const_iterator it_end = implicitData.end_col(user);
    size_t implicitCount = 0;
    for (; it != it_end; ++it)
    {
      userVec +=
          optParameters.col(implicitStart + it.row()).subvec(0, rank - 1);
      implicitCount += 1;
    }
    if (implicitCount != 0)
      userVec /= std::sqrt(implicitCount);
    userVec += optParameters.col(user).subvec(0, rank - 1);

    predictedData(0, i) = userBias + itemBias +
        arma::dot(userVec, optParameters.col(item).subvec(0, rank - 1));
  }

  // Calculate relative error.
  const double relativeError = arma::norm(data.row(2) - predictedData, "frob") /
                               arma::norm(data, "frob");

  // Relative error should be small.
  BOOST_REQUIRE_SMALL(relativeError, 1e-2);
}

// The test is only compiled if the user has specified OpenMP to be
// used.
#ifdef HAS_OPENMP

// Test SVDPlusPlus with parallel SGD.
BOOST_AUTO_TEST_CASE(SVDPlusPlusFunctionParallelOptimize)
{
  // Define useful constants.
  const size_t numUsers = 100;
  const size_t numItems = 100;
  const size_t numRatings = 1000;
  const size_t iterations = 30;
  const size_t rank = 5;
  const double alpha = 0.01;
  const double lambda = 0;

  // Initiate random parameters.
  arma::mat parameters = arma::randu(rank + 1, numUsers + 2 * numItems);

  // Make a random rating dataset.
  arma::mat data = arma::randu(3, numRatings);
  data.row(0) = floor(data.row(0) * numUsers);
  data.row(1) = floor(data.row(1) * numItems);

  // Manually set last row to maximum user and maximum item.
  data(0, numRatings - 1) = numUsers - 1;
  data(1, numRatings - 1) = numItems - 1;

  // Make a random implicit dataset.
  arma::sp_mat implicitData = arma::sprandu(numItems, numUsers, 0.05);

  // Make rating entries based on the parameters.
  for (size_t i = 0; i < numRatings; ++i)
  {
    const size_t user = data(0, i);
    const size_t item = data(1, i) + numUsers;
    const size_t implicitStart = numUsers + numItems;

    const double userBias = parameters(rank, user);
    const double itemBias = parameters(rank, item);

    // Iterate through each item which the user interacted with to calculate
    // user vector.
    arma::vec userVec(rank, arma::fill::zeros);
    arma::sp_mat::const_iterator it = implicitData.begin_col(user);
    arma::sp_mat::const_iterator it_end = implicitData.end_col(user);
    size_t implicitCount = 0;
    for (; it != it_end; ++it)
    {
      userVec += parameters.col(implicitStart + it.row()).subvec(0, rank - 1);
      implicitCount += 1;
    }
    if (implicitCount != 0)
      userVec /= std::sqrt(implicitCount);
    userVec += parameters.col(user).subvec(0, rank - 1);

    data(2, i) = userBias + itemBias +
        arma::dot(userVec, parameters.col(item).subvec(0, rank - 1));
  }

  // Make the SVD++ function and the optimizer.
  SVDPlusPlusFunction<arma::mat> svdPPFunc(data, implicitData, rank, lambda);

  ens::ConstantStep decayPolicy(alpha);

  // Iterate till convergence.
  // The threadShareSize is chosen such that each function gets optimized.
  ens::ParallelSGD<ens::ConstantStep> optimizer(iterations,
      std::ceil((float) svdPPFunc.NumFunctions() / omp_get_max_threads()), 1e-5,
      true, decayPolicy);

  // Obtain optimized parameters after training.
  arma::mat optParameters = arma::randu(rank + 1, numUsers + 2 * numItems);
  optimizer.Optimize(svdPPFunc, optParameters);

  // Get predicted ratings from optimized parameters.
  arma::mat predictedData(1, numRatings);
  for (size_t i = 0; i < numRatings; ++i)
  {
    const size_t user = data(0, i);
    const size_t item = data(1, i) + numUsers;
    const size_t implicitStart = numUsers + numItems;

    const double userBias = optParameters(rank, user);
    const double itemBias = optParameters(rank, item);

    // Iterate through each item which the user interacted with to calculate
    // user vector.
    arma::vec userVec(rank, arma::fill::zeros);
    arma::sp_mat::const_iterator it = implicitData.begin_col(user);
    arma::sp_mat::const_iterator it_end = implicitData.end_col(user);
    size_t implicitCount = 0;
    for (; it != it_end; ++it)
    {
      userVec +=
          optParameters.col(implicitStart + it.row()).subvec(0, rank - 1);
      implicitCount += 1;
    }
    if (implicitCount != 0)
      userVec /= std::sqrt(implicitCount);
    userVec += optParameters.col(user).subvec(0, rank - 1);

    predictedData(0, i) = userBias + itemBias +
        arma::dot(userVec, optParameters.col(item).subvec(0, rank - 1));
  }

  // Calculate relative error.
  const double relativeError = arma::norm(data.row(2) - predictedData, "frob") /
                               arma::norm(data, "frob");

  // Relative error should be small.
  BOOST_REQUIRE_SMALL(relativeError, 1e-2);
}

#endif

BOOST_AUTO_TEST_SUITE_END();


#define BOOST_TEST_MODULE
#include <boost/test/unit_test.hpp>
#include <util/test_macros.hpp>
#include <stdlib.h>
#include <vector>
#include <string>
#include <functional>
#include <random>
#include <cfenv>

#include <ml_data/ml_data.hpp>
#include <optimization/optimization_interface.hpp>
#include <optimization/utils.hpp>
#include <toolkits/supervised_learning/supervised_learning.hpp>
#include <toolkits/supervised_learning/linear_svm.hpp>
#include <toolkits/supervised_learning/linear_svm_opt_interface.hpp>
#include <sframe/testing_utils.hpp>

using namespace turi;
using namespace turi::supervised;

void run_linear_svm_test(std::map<std::string, flexible_type> opts) {

  size_t examples = opts.at("examples");
  size_t features = opts.at("features");
  std::string target_column_name = "target";

  // Answers
  // -----------------------------------------------------------------------
  DenseVector coefs(features+1);
  coefs.randn();

  // Feature names
  std::vector<std::string> feature_names;
  std::vector<flex_type_enum> feature_types;
  for(size_t i=0; i < features; i++){
    feature_names.push_back(std::to_string(i));
    feature_types.push_back(flex_type_enum::FLOAT);
  }

  // Data
  std::vector<std::vector<flexible_type>> y_data;
  std::vector<std::vector<flexible_type>> X_data;
  for(size_t i=0; i < examples; i++){
    DenseVector x(features);
    x.randn();
    std::vector<flexible_type> x_tmp;
    for(size_t k=0; k < features; k++){
      x_tmp.push_back(x(k));
    }

    // Compute the prediction for this
    double t = dot(x, coefs.subvec(0, features-1)) + coefs(features);
    t = 1.0/(1.0+exp(-1.0*t));
    int c = turi::random::bernoulli(t);
    if (i == 0) c = 0; // Make sure category 0 is category 0 (for testing)
    std::vector<flexible_type> y_tmp;
    y_tmp.push_back(c);

    X_data.push_back(x_tmp);
    y_data.push_back(y_tmp);
  }

  // Options
  std::map<std::string, flexible_type> options = {
    {"convergence_threshold", 1e-2},
    {"max_iterations", 10},
    {"solver", "lbfgs"},
  };

  // Make the data
  sframe X = make_testing_sframe(feature_names, feature_types, X_data);
  sframe y = make_testing_sframe({"target"}, {flex_type_enum::STRING}, y_data);
  std::shared_ptr<linear_svm> model;
  model.reset(new linear_svm);
  model->init(X,y);
  model->init_options(options);
  model->train();

  // Construct the ml_data
  ml_data data = model->construct_ml_data_using_current_metadata(X, y);
  ml_data valid_data;

  // Check coefficients & options
  // ----------------------------------------------------------------------
  DenseVector _coefs(features+1);
  model->get_coefficients(_coefs);
  TS_ASSERT(_coefs.size() == features + 1);

  std::map<std::string, flexible_type> _options;
  _options = model->get_current_options();
  for (auto& kvp: options){
    TS_ASSERT(_options[kvp.first] == kvp.second);
  }
  TS_ASSERT(model->is_trained() == true);

  // Check predictions
  // ----------------------------------------------------------------------
  std::shared_ptr<sarray<flexible_type>> _pred_class;
  std::vector<flexible_type> pred_class;
  _pred_class = model->predict(data, "class");

  // Save predictions made by the model
  size_t rows;
  auto reader = _pred_class->get_reader();
  rows = reader->read_rows(0, examples, pred_class);

  // Check that the predictions made by the model are right!
  for(size_t i=0; i < examples; i++){
    DenseVector x(features + 1);
    for(size_t k=0; k < features; k++){
      x(k) = X_data[i][k];
    }
    x(features) = 1;
    double t = arma::dot(x, _coefs);
    int c = t > 0.0;
    TS_ASSERT_EQUALS(pred_class[i], std::to_string(c));
  }

  // Check save and load
  // ----------------------------------------------------------------------
  dir_archive archive_write;
  archive_write.open_directory_for_write("linear_svm_tests");
  turi::oarchive oarc(archive_write);
  oarc << *model;
  archive_write.close();

  // Load it
  dir_archive archive_read;
  archive_read.open_directory_for_read("linear_svm_tests");
  turi::iarchive iarc(archive_read);
  iarc >> *model;

  // Check predictions after saving and loading.
  // ----------------------------------------------------------------------
  DenseVector _coefs_after_load(features+1);
  model->get_coefficients(_coefs_after_load);
  TS_ASSERT(_coefs_after_load.size() == features + 1);
  TS_ASSERT(arma::approx_equal(_coefs_after_load, _coefs,"absdiff", 1e-5));
  _options = model->get_current_options();
  for (auto& kvp: options){
    TS_ASSERT(_options[kvp.first] == kvp.second);
  }
  TS_ASSERT(model->is_trained() == true);

  // Check coefficients after saving and loading.
  // ----------------------------------------------------------------------
  _pred_class = model->predict(data, "class");
  reader = _pred_class->get_reader();
  rows = reader->read_rows(0, examples, pred_class);

  // Check that the predictions made by the model are right!
  for(size_t i=0; i < examples; i++){
    DenseVector x(features + 1);
    for(size_t k=0; k < features; k++){
      x(k) = X_data[i][k];
    }
    x(features) = 1;
    double t = arma::dot(x, _coefs);
    int c = t > 0.0;
    TS_ASSERT_EQUALS(pred_class[i], std::to_string(c));
  }

  model->get_coefficients(_coefs);
  TS_ASSERT(_coefs.size() == features + 1);
  model.reset();
}

/**
 *  Check linear svm
*/
struct linear_svm_test  {

  public:

  void test_linear_svm_basic_2d() {
    std::map<std::string, flexible_type> opts = {
      {"examples", 100},
      {"features", 1}};
    run_linear_svm_test(opts);
  }

  void test_linear_svm_small() {
    std::map<std::string, flexible_type> opts = {
      {"examples", 1000},
      {"features", 10}};
    run_linear_svm_test(opts);
  }

};

void run_linear_svm_scaled_logistic_opt_interface_test(std::map<std::string,
    flexible_type> opts) {

  size_t examples = opts.at("examples");
  size_t features = opts.at("features");
  std::string target_column_name = "target";
  std::vector<std::string> column_names = {"user", "item"};

  // Answers
  // -----------------------------------------------------------------------
  DenseVector coefs(features+1);
  coefs.randn();

  // Feature names
  std::vector<std::string> feature_names;
  std::vector<flex_type_enum> feature_types;
  for(size_t i=0; i < features; i++){
    feature_names.push_back(std::to_string(i));
    feature_types.push_back(flex_type_enum::FLOAT);
  }

  // Data
  std::vector<std::vector<flexible_type>> y_data;
  std::vector<std::vector<flexible_type>> X_data;
  for(size_t i=0; i < examples; i++){
    DenseVector x(features);
    x.randn();
    std::vector<flexible_type> x_tmp;
    for(size_t k=0; k < features; k++){
      x_tmp.push_back(x(k));
    }

    // Compute the prediction for this
    double t = dot(x, coefs.subvec(0, features-1)) + coefs(features);
    t = 1.0/(1.0+exp(-1.0*t));
    int c = turi::random::bernoulli(t);
    std::vector<flexible_type> y_tmp;
    y_tmp.push_back(c);

    X_data.push_back(x_tmp);
    y_data.push_back(y_tmp);
  }

  // Construct the ml_data
  // Make the data
  sframe X = make_testing_sframe(feature_names, feature_types, X_data);
  sframe y = make_testing_sframe({"target"}, {flex_type_enum::STRING}, y_data);
  std::shared_ptr<linear_svm> model;
  model.reset(new linear_svm);
  model->init(X,y);

  // Construct the ml_data
  ml_data data = model->construct_ml_data_using_current_metadata(X, y);
  ml_data valid_data;

  std::shared_ptr<linear_svm_scaled_logistic_opt_interface> svm_interface;
  svm_interface.reset(new linear_svm_scaled_logistic_opt_interface(data, valid_data, *model));

  // Check examples & variables.
  TS_ASSERT(svm_interface->num_variables() == features+1);
  TS_ASSERT(svm_interface->num_examples() == examples);

  size_t variables = svm_interface->num_variables();
  for(size_t i=0; i < 10; i++){

    DenseVector point(variables);
    point.randn();

    // Check gradients, functions and hessians.
    DenseVector gradient(variables);
    double func_value;

    func_value = svm_interface->compute_function_value(point);
    svm_interface->compute_gradient(point, gradient);
  }

  model.reset();
  svm_interface.reset();
}

/**
 *  Check opt interface
*/
struct linear_svm_scaled_logistic_opt_interface_test  {

  public:

  void test_linear_svm_scaled_logistic_opt_interface_basic_2d() {
    std::map<std::string, flexible_type> opts = {
      {"examples", 100},
      {"features", 1}};
    run_linear_svm_scaled_logistic_opt_interface_test(opts);
  }

  void test_linear_svm_scaled_logistic_opt_interface_small() {
    std::map<std::string, flexible_type> opts = {
      {"examples", 1000},
      {"features", 10}};
    run_linear_svm_scaled_logistic_opt_interface_test(opts);
  }

};

BOOST_FIXTURE_TEST_SUITE(_linear_svm_test, linear_svm_test)
BOOST_AUTO_TEST_CASE(test_linear_svm_basic_2d) {
  linear_svm_test::test_linear_svm_basic_2d();
}
BOOST_AUTO_TEST_CASE(test_linear_svm_small) {
  linear_svm_test::test_linear_svm_small();
}
BOOST_AUTO_TEST_SUITE_END()
BOOST_FIXTURE_TEST_SUITE(_linear_svm_scaled_logistic_opt_interface_test, linear_svm_scaled_logistic_opt_interface_test)
BOOST_AUTO_TEST_CASE(test_linear_svm_scaled_logistic_opt_interface_basic_2d) {
  linear_svm_scaled_logistic_opt_interface_test::test_linear_svm_scaled_logistic_opt_interface_basic_2d();
}
BOOST_AUTO_TEST_CASE(test_linear_svm_scaled_logistic_opt_interface_small) {
  linear_svm_scaled_logistic_opt_interface_test::test_linear_svm_scaled_logistic_opt_interface_small();
}
BOOST_AUTO_TEST_SUITE_END()


#include <geometry.h>
#include <tiny.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

template<typename V, size_t N>
void test_direction_table(geometry::DirectionTable<V,N> const & DT )
{
  BOOST_CHECK_EQUAL(DT.size(), N);

  for(size_t i=0u; i < N; ++i)
  {
    V const di = DT(i);

    BOOST_CHECK_CLOSE( inner_prod(di,di), 1.0f, 0.01f);

    for(size_t j=i+1u; j < N; ++j)
    {
      V const dj = DT(j);

      V diff = di-dj;

      BOOST_CHECK_GT( inner_prod(diff,diff) , 0.01f );

    }
  }
}

BOOST_AUTO_TEST_SUITE(geometry);

BOOST_AUTO_TEST_CASE(make_direction_tables)
{
  typedef tiny::MathTypes<float> MT;
  typedef MT::vector3_type       V;

  {
    geometry::DirectionTable<V,3> A = geometry::make3<V>();
    test_direction_table(A);
  }
  {
    geometry::DirectionTable<V,4> A = geometry::make4<V>();
    test_direction_table(A);
  }
  {
    geometry::DirectionTable<V,6> A = geometry::make6<V>();
    test_direction_table(A);
  }
  {
    geometry::DirectionTable<V,7> A = geometry::make7<V>();
    test_direction_table(A);
  }
  {
    geometry::DirectionTable<V,9> A = geometry::make9<V>();
    test_direction_table(A);
  }
  {
    geometry::DirectionTable<V,10> A = geometry::make10<V>();
    test_direction_table(A);
  }
  {
    geometry::DirectionTable<V,13> A = geometry::make13<V>();
    test_direction_table(A);
  }
}

BOOST_AUTO_TEST_SUITE_END();


// Copyright (c) by respective owners including Yahoo!, Microsoft, and
// individual contributors. All rights reserved. Released under a BSD (revised)
// license as described in the file LICENSE.

#include "loss_functions.h"

#include <boost/test/test_tools.hpp>
#include <boost/test/unit_test.hpp>

#include "named_labels.h"
#include "test_common.h"

BOOST_AUTO_TEST_CASE(squared_loss_test)
{
  auto& vw = *VW::initialize("--quiet");
  const std::string loss_type("squared");

  auto loss = get_loss_function(vw, loss_type);
  shared_data sd;
  sd.min_label = 0.0f;
  sd.max_label = 1.0f;
  constexpr float eta = 0.1f;     // learning rate
  constexpr float weight = 1.0f;  // example weight

  constexpr float label = 0.5f;
  constexpr float prediction = 0.4f;
  constexpr float update_scale = eta * weight;
  constexpr float pred_per_update = 1.0f;  // Use dummy value here, see gd.cc for details.

  BOOST_CHECK_EQUAL(loss_type, loss->get_type());
  BOOST_CHECK_CLOSE(0.0f, loss->get_parameter(), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.01f, loss->get_loss(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.01812692f, loss->get_update(prediction, label, update_scale, pred_per_update), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.02f, loss->get_unsafe_update(prediction, label, update_scale), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.04f, loss->get_square_grad(prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(-0.2f, loss->first_derivative(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(2.0f, loss->second_derivative(&sd, prediction, label), FLOAT_TOL);

  VW::finish(vw);
}

BOOST_AUTO_TEST_CASE(expectile_loss_label_is_greater_than_prediction_test)
{
  auto& vw = *VW::initialize("--quiet");
  const std::string loss_type("expectile");
  constexpr float parameter(0.4f);

  auto loss = get_loss_function(vw, loss_type, parameter);
  shared_data sd;
  sd.min_label = 0.0f;
  sd.max_label = 1.0f;
  constexpr float eta = 0.1f;     // learning rate
  constexpr float weight = 1.0f;  // example weight

  constexpr float label = 0.5f;
  constexpr float prediction = 0.4f;
  constexpr float update_scale = eta * weight;
  constexpr float pred_per_update = 1.0f;  // Use dummy value here, see gd.cc for details.

  BOOST_CHECK_EQUAL(loss_type, loss->get_type());
  BOOST_CHECK_CLOSE(parameter, loss->get_parameter(), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.004f, loss->get_loss(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.007688365f, loss->get_update(prediction, label, update_scale, pred_per_update), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.008f, loss->get_unsafe_update(prediction, label, update_scale), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.0064f, loss->get_square_grad(prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(-0.08f, loss->first_derivative(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.8f, loss->second_derivative(&sd, prediction, label), FLOAT_TOL);

  VW::finish(vw);
}

BOOST_AUTO_TEST_CASE(expectile_loss_prediction_is_greater_than_label_test)
{
  auto& vw = *VW::initialize("--quiet");
  const std::string loss_type("expectile");
  constexpr float parameter(0.4f);

  auto loss = get_loss_function(vw, loss_type, parameter);
  shared_data sd;
  sd.min_label = 0.0f;
  sd.max_label = 1.0f;
  constexpr float eta = 0.1f;     // learning rate
  constexpr float weight = 1.0f;  // example weight

  constexpr float label = 0.4f;
  constexpr float prediction = 0.5f;
  constexpr float update_scale = eta * weight;
  constexpr float pred_per_update = 1.0f;  // Use dummy value here, see gd.cc for details.

  BOOST_CHECK_EQUAL(loss_type, loss->get_type());
  BOOST_CHECK_CLOSE(parameter, loss->get_parameter(), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.006f, loss->get_loss(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(-0.011307956f, loss->get_update(prediction, label, update_scale, pred_per_update), FLOAT_TOL);
  BOOST_CHECK_CLOSE(-0.012f, loss->get_unsafe_update(prediction, label, update_scale), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.0144f, loss->get_square_grad(prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.12f, loss->first_derivative(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(1.2f, loss->second_derivative(&sd, prediction, label), FLOAT_TOL);

  VW::finish(vw);
}

BOOST_AUTO_TEST_CASE(expectile_loss_parameter_equals_zero_test)
{
  auto& vw = *VW::initialize("--quiet");
  const std::string loss_type("expectile");
  constexpr float parameter(0.0f);

  auto loss = get_loss_function(vw, loss_type, parameter);
  shared_data sd;
  sd.min_label = 0.0f;
  sd.max_label = 1.0f;
  constexpr float eta = 0.1f;     // learning rate
  constexpr float weight = 1.0f;  // example weight

  constexpr float label = 0.5f;
  constexpr float prediction = 0.4f;
  constexpr float update_scale = eta * weight;
  constexpr float pred_per_update = 1.0f;  // Use dummy value here, see gd.cc for details.

  BOOST_CHECK_EQUAL(loss_type, loss->get_type());
  BOOST_CHECK_CLOSE(parameter, loss->get_parameter(), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.0f, loss->get_loss(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.0f, loss->get_update(prediction, label, update_scale, pred_per_update), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.0f, loss->get_unsafe_update(prediction, label, update_scale), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.0f, loss->get_square_grad(prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.0f, loss->first_derivative(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.0f, loss->second_derivative(&sd, prediction, label), FLOAT_TOL);

  VW::finish(vw);
}

BOOST_AUTO_TEST_CASE(expectile_loss_parameter_equals_one_test)
{
  auto& vw = *VW::initialize("--quiet");
  const std::string loss_type("expectile");
  constexpr float parameter(1.0f);

  auto loss = get_loss_function(vw, loss_type, parameter);
  shared_data sd;
  sd.min_label = 0.0f;
  sd.max_label = 1.0f;
  constexpr float eta = 0.1f;     // learning rate
  constexpr float weight = 1.0f;  // example weight

  constexpr float label = 0.5f;
  constexpr float prediction = 0.4f;
  constexpr float update_scale = eta * weight;
  constexpr float pred_per_update = 1.0f;  // Use dummy value here, see gd.cc for details.

  BOOST_CHECK_EQUAL(loss_type, loss->get_type());
  BOOST_CHECK_CLOSE(parameter, loss->get_parameter(), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.01f, loss->get_loss(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.01812692f, loss->get_update(prediction, label, update_scale, pred_per_update), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.02f, loss->get_unsafe_update(prediction, label, update_scale), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.04f, loss->get_square_grad(prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(-0.2f, loss->first_derivative(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(2.0f, loss->second_derivative(&sd, prediction, label), FLOAT_TOL);

  VW::finish(vw);
}

BOOST_AUTO_TEST_CASE(compare_expectile_loss_with_squared_loss_test)
{
  auto& vw = *VW::initialize("--quiet");
  const std::string loss_type_expectile("expectile");
  const std::string loss_type_squared("squared");
  constexpr float parameter(0.3f);

  auto loss_expectile = get_loss_function(vw, loss_type_expectile, parameter);
  auto loss_squared = get_loss_function(vw, loss_type_squared);
  shared_data sd;
  sd.min_label = 0.0f;
  sd.max_label = 1.0f;
  constexpr float eta = 0.1f;     // learning rate
  constexpr float weight = 1.0f;  // example weight

  constexpr float label = 0.5f;
  constexpr float prediction = 0.4f;
  constexpr float update_scale = eta * weight;
  constexpr float pred_per_update = 1.0f;  // Use dummy value here, see gd.cc for details.

  BOOST_CHECK_CLOSE(loss_expectile->get_loss(&sd, prediction, label),
      loss_squared->get_loss(&sd, prediction, label) * parameter, FLOAT_TOL);
  BOOST_CHECK_CLOSE(loss_expectile->get_update(prediction, label, update_scale, pred_per_update),
      loss_squared->get_update(prediction, label, update_scale * parameter, pred_per_update), FLOAT_TOL);
  BOOST_CHECK_CLOSE(loss_expectile->get_unsafe_update(prediction, label, update_scale),
      loss_squared->get_unsafe_update(prediction, label, update_scale * parameter), FLOAT_TOL);

  BOOST_CHECK_CLOSE(loss_expectile->get_square_grad(prediction, label),
      loss_squared->get_square_grad(prediction, label) * parameter * parameter, FLOAT_TOL);
  BOOST_CHECK_CLOSE(loss_expectile->first_derivative(&sd, prediction, label),
      loss_squared->first_derivative(&sd, prediction, label) * parameter, FLOAT_TOL);
  BOOST_CHECK_CLOSE(loss_expectile->second_derivative(&sd, prediction, label),
      loss_squared->second_derivative(&sd, prediction, label) * parameter, FLOAT_TOL);

  VW::finish(vw);
}


#include "sandBox.h"
#include "igl/edge_flaps.h"
#include "igl/collapse_edge.h"
#include "Eigen/dense"
#include <functional>
#include <Eigen/Core>
#include "igl/opengl/ViewerCore.h"
#include "igl/opengl/glfw/renderer.h"
#include "igl/decimate.h"
#include "igl/writeOBJ.h"
#include <igl/circulation.h>
#include <igl/shortest_edge_and_midpoint.h>
#include <igl/parallel_for.h>
#include <igl/opengl/glfw/Viewer.h>
#include <set>
#include "simplifier.h"
#include "igl/swept_volume_bounding_box.h"

SandBox::SandBox() : objectsData{ new std::vector<ObjectData*>{} }
{
	xVelocity = -0.005;
	yVelocity = 0;
}

SandBox::~SandBox()
{
    delete objectsData;
}

void SandBox::OnNewMeshLoad()
{
	ObjectData* od = new ObjectData();
	InitObjectData(*od, data().V, data().F);
	objectsData->push_back(od);
	data().AddBoundingBox(od->tree->m_box, Eigen::RowVector3d(0, 1, 0));
	data().dirty = 157; //this line prevents texture coordinates
}

void SandBox::Init(const std::string &config)
{

	std::string item_name;
	std::ifstream nameFileout;
	nameFileout.open(config);

	if (!nameFileout.is_open())
	{
		std::cout << "Can't open file " << config << std::endl;
	}
	else
	{
        double n = 0;
		while (nameFileout >> item_name)
		{
			std::cout << "openning " << item_name << std::endl;
			load_mesh_from_file(item_name);

			parents.push_back(-1);
			data().add_points(Eigen::RowVector3d(0, 0, 0), Eigen::RowVector3d(0, 0, 1));
			data().show_overlay_depth = false;
			data().point_size = 10;
			data().line_width = 2;
			data().set_visible(false, 1);

            ObjectData* od = new ObjectData();
            InitObjectData(*od, data().V, data().F);
            objectsData->push_back(od);

            double x = -1 + (n * 2);
            double y = -1 + (n * 2);
            MoveTo(x, y);
			data().AddBoundingBox(od->tree->m_box, Eigen::RowVector3d(0, 1, 0));
            data().dirty = 157; //this line prevents texture coordinates
            n++;
		}
		nameFileout.close();
	}
	MyTranslate(Eigen::Vector3d(0, 0, -1), true);
	data().set_colors(Eigen::RowVector3d(0.9, 0.1, 0.1));
	isActive = true;
}

void SandBox::InitObjectData(ObjectData& od, Eigen::MatrixXd& V, Eigen::MatrixXi& F)
{

    od.V = new Eigen::MatrixXd(V);
    od.F = new Eigen::MatrixXi(F);
    od.E = new Eigen::MatrixXi();
    od.EF = new Eigen::MatrixXi();
    od.EI = new Eigen::MatrixXi();
    od.EMAP = new Eigen::VectorXi();
    od.Q = new PriorityQueue();
    od.num_collapsed = 0;
    od.tree = new igl::AABB<Eigen::MatrixXd, 3>{};

    igl::edge_flaps(*od.F, *od.E, *od.EMAP, *od.EF, *od.EI);

    od.tree->init(*od.V, *od.F);

    od.C = new Eigen::MatrixXd(od.E->rows(), od.V->cols());

    ComputeNormals(od, data());

    ComputeQMatrices(od);

    ComputePriorityQueue(od);
}

void SandBox::ReInitObjectData(ObjectData& od)
{
    Eigen::MatrixXd V = *od.V;
    Eigen::MatrixXi F = *od.F;

    // Remove old object data
    ObjectData* odToRemove = objectsData->at(selected_data_index);
    ClearObjectData(*odToRemove);
    delete odToRemove;

    // Add new object data after collapsing edges
    ObjectData* odToAdd = new ObjectData();
    InitObjectData(*odToAdd, V, F);
    objectsData->at(selected_data_index) = odToAdd;
}

void SandBox::ClearObjectData(ObjectData& od)
{
    delete od.V;
    delete od.F;
    delete od.EMAP;
    delete od.E;
    delete od.EF;
    delete od.EI;
    delete od.Q;
    delete od.C;
    delete od.F_NORMALS;

    std::for_each(od.QMATRICES.begin(), od.QMATRICES.end(), [](Eigen::Matrix4d* m) -> void { delete m; });
    od.QMATRICES.clear();
}

void SandBox::Simplify()
{
    ObjectData* objectToSimplify = objectsData->at(selected_data_index);
    int num_to_collapse = std::ceil(0.05 * objectToSimplify->Q->size());
    Simplify(num_to_collapse, *objectToSimplify);
}

void SandBox::Simplify(int num_to_collapse, ObjectData& od)
{

    if (!od.Q->empty())
    {
        bool something_collapsed = false;
        for (int j = 0; j < num_to_collapse; j++)
        {
              if (!collapse_edge(od))
            {
                break;
            }
            something_collapsed = true;
            od.num_collapsed++;
        }

        if (something_collapsed)
        {
            data().clear();
            data().set_mesh(*od.V, *od.F);
            data().set_face_based(true);
            data().dirty = 157; //this line prevents texture coordinates
            ReInitObjectData(od);
        }
    }
}

void SandBox::MoveTo(double x, double y)
{
    data().TranslateInSystem(GetRotation(), Eigen::Vector3d(x, 0, 0));
    data().TranslateInSystem(GetRotation(), Eigen::Vector3d(0, y, 0));
    WhenTranslate();
}

void SandBox::Animate()
{
	if (isActive)
	{
        data().TranslateInSystem(GetRotation(), Eigen::Vector3d(xVelocity, yVelocity, 0));
		if (ObjectsCollide(objectsData->at(0)->tree, objectsData->at(1)->tree))
		{
			isActive = false;
		}
	}
}

bool SandBox::ObjectsCollide(igl::AABB<Eigen::MatrixXd, 3>* firstTree, igl::AABB<Eigen::MatrixXd, 3>* secondTree)
{
	if (firstTree == nullptr || secondTree == nullptr)
	{
		return false;
	}
 	if (BoxesIntersect(firstTree->m_box, secondTree->m_box)) {
		if (firstTree->is_leaf() && secondTree->is_leaf())
		{
			data_list.at(0).AddBoundingBox(firstTree->m_box, Eigen::RowVector3d(0, 1, 0));
			data_list.at(1).AddBoundingBox(secondTree->m_box, Eigen::RowVector3d(0, 1, 0));
			return true;
		}
		else if (!firstTree->is_leaf() && secondTree->is_leaf())
		{
			return	ObjectsCollide(firstTree->m_left, secondTree) ||
					ObjectsCollide(firstTree->m_right, secondTree);
		}
		else if (firstTree->is_leaf() && !secondTree->is_leaf())
		{

			return  ObjectsCollide(firstTree, secondTree->m_left) ||
					ObjectsCollide(firstTree, secondTree->m_right);
		}
		else if (!firstTree->is_leaf() && !secondTree->is_leaf())
		{
			return  ObjectsCollide(firstTree->m_left, secondTree->m_left) ||
					ObjectsCollide(firstTree->m_left, secondTree->m_right) ||
					ObjectsCollide(firstTree->m_right, secondTree->m_left) ||
					ObjectsCollide(firstTree->m_right, secondTree->m_right);
		}
	}
	return false;
}

bool SandBox::BoxesIntersect(Eigen::AlignedBox <double, 3>& firstBox, Eigen::AlignedBox <double, 3>& secondBox)
{
	Eigen::Matrix4d firstTrans = data_list.at(0).MakeTransd();
	Eigen::Matrix4d secondTrans = data_list.at(1).MakeTransd();

	Eigen::Vector3d firstBoxCenter = firstBox.center();
	Eigen::Vector3d secondBoxCenter = secondBox.center();

	Eigen::Vector4d firstMult = firstTrans * Eigen::Vector4d(firstBoxCenter(0), firstBoxCenter(1), firstBoxCenter(2), 1);
	Eigen::Vector4d secondMult = secondTrans * Eigen::Vector4d(secondBoxCenter(0), secondBoxCenter(1), secondBoxCenter(2), 1);

	Eigen::Vector3d C0(firstMult(0), firstMult(1), firstMult(2));
	Eigen::Vector3d C1(secondMult(0), secondMult(1), secondMult(2));

	Eigen::Vector3d D = C1 - C0;

	Eigen::Matrix3d A = data_list.at(0).GetRotation();
	Eigen::Matrix3d B = data_list.at(1).GetRotation();

	Eigen::Matrix3d A_matrix;
	A_matrix << A(0, 0), A(1, 0), A(2, 0),
				A(0, 1), A(1, 1), A(2, 1),
				A(0, 2), A(1, 2), A(2, 2);

	Eigen::Matrix3d B_matrix;
	B_matrix << B(0, 0), B(1, 0), B(2, 0),
				B(0, 1), B(1, 1), B(2, 1),
				B(0, 2), B(1, 2), B(2, 2);

	Eigen::RowVector3d a;
	a << firstBox.sizes()(0) / 2, firstBox.sizes()(1) / 2, firstBox.sizes()(2) / 2;

	Eigen::RowVector3d b;
	b << secondBox.sizes()(0) / 2, secondBox.sizes()(1) / 2, secondBox.sizes()(2) / 2;

	Eigen::Matrix3d C = A.transpose() * B;

	double R0, R1, R;

	// L = A0
	R0 = a(0);
	R1 = b(0) * abs(C(0, 0)) + b(1) * abs(C(0, 1)) + b(2) * abs(C(0, 2));
	R = (A_matrix.row(0) * D).norm();
	if (R > R0 + R1) return false;

	// L = A1
	R0 = a(1);
	R1 = b(0) * abs(C(1, 0)) + b(1) * abs(C(1, 1)) + b(2) * abs(C(1, 2));
	R = (A_matrix.row(1) * D).norm();
	if (R > R0 + R1) return false;

	// L = A2
	R0 = a(2);
	R1 = b(0) * abs(C(2, 0)) + b(1) * abs(C(2, 1)) + b(2) * abs(C(2, 2));
	R = (A_matrix.row(2) * D).norm();
	if (R > R0 + R1) return false;

	// L = B0
	R0 = a(0) * abs(C(0, 0)) + a(1) * abs(C(1, 0)) + a(2) * abs(C(2, 0));
	R1 = b(0);
	R = (B_matrix.row(0) * D).norm();
	if (R > R0 + R1) return false;

	// L = B1
	R0 = a(0) * abs(C(0, 1)) + a(1) * abs(C(1, 1)) + a(2) * abs(C(2, 1));
	R1 = b(1);
	R = (B_matrix.row(1) * D).norm();
	if (R > R0 + R1) return false;

	// L = B2
	R0 = a(0) * abs(C(0, 2)) + a(1) * abs(C(1, 2)) + a(2) * abs(C(2, 2));
	R1 = b(2);
	R = (B_matrix.row(2) * D).norm();
	if (R > R0 + R1) return false;

	// L = A0 x B0
	R0 = a(1) * abs(C(2, 0)) + a(2) * abs(C(1, 0));
	R1 = b(1) * abs(C(0, 2)) + b(2) * abs(C(0, 1));
	R = (C(1, 0) * A_matrix.row(2) * D - C(2, 0) * A_matrix.row(1) * D).norm();
	if (R > R0 + R1) return false;

	// L = A0 x B1
	R0 = a(1) * abs(C(2, 1)) + (a(2))*abs(C(1, 1));
	R1 = b(0) * abs(C(0, 2)) + b(2) * abs(C(0, 0));
	R = (C(1, 1) * A_matrix.row(2) * D - C(2, 1) * A_matrix.row(1) * D).norm();
	if (R > R0 + R1) return false;

	// L = A0 x B2
	R0 = a(1) * abs(C(2, 2)) + a(2) * abs(C(1, 2));
	R1 = b(0) * abs(C(0, 1)) + b(1) * abs(C(0, 0));
	R = (C(1, 2) * A_matrix.row(2) * D - C(2, 2) * A_matrix.row(1) * D).norm();
	if (R > R0 + R1) return false;

	// L = A1 x B0
	R0 = a(0) * abs(C(2, 0)) + a(2) * abs(C(0, 0));
	R1 = b(1) * abs(C(1, 2)) + b(2) * abs(C(1, 1));
	R = (C(2, 0) * A_matrix.row(0) * D - C(0, 0) * A_matrix.row(2) * D).norm();
	if (R > R0 + R1) return false;

	// L = A1 x B1
	R0 = a(0) * abs(C(2, 1)) + a(2) * abs(C(0, 1));
	R1 = b(0) * abs(C(1, 2)) + b(2) * abs(C(1, 0));
	R = (C(2, 1) * A_matrix.row(0) * D - C(0, 1) * A_matrix.row(2) * D).norm();
	if (R > R0 + R1) return false;

	/// L = A1 x B2
	R0 = a(0) * abs(C(2, 2)) + a(2) * abs(C(0, 2));
	R1 = b(0) * abs(C(1, 1)) + b(1) * abs(C(1, 0));
	R = (C(2, 2) * A_matrix.row(0) * D - C(0, 2) * A_matrix.row(2) * D).norm();
	if (R > R0 + R1) return false;

	// L = A2 x B0
	R0 = a(0) * abs(C(1, 0)) + a(1) * abs(C(0, 0));
	R1 = b(1) * abs(C(2, 2)) + b(2) * abs(C(2, 1));
	R = (C(0, 0) * A_matrix.row(1) * D - C(1, 0) * A_matrix.row(0) * D).norm();
	if (R > R0 + R1) return false;

	// L = A2 x B1
	R0 = a(0) * abs(C(1, 1)) + a(1) * abs(C(0, 1));
	R1 = b(0) * abs(C(2, 2)) + b(2) * abs(C(2, 0));
	R = (C(0, 1) * A_matrix.row(1) * D - C(1, 1) * A_matrix.row(0) * D).norm();
	if (R > R0 + R1) return false;

	// L = A2 x B2
	R0 = a(0) * abs(C(1, 2)) + a(1) * abs(C(0, 2));
	R1 = b(0) * abs(C(2, 1)) + b(1) * abs(C(2, 0));
	R = (C(0, 2) * A_matrix.row(1) * D - C(1, 2) * A_matrix.row(0) * D).norm();
	if (R > R0 + R1) return false;
	return true;
}

void SandBox::SetVelocity(double x, double y)
{
	xVelocity = x;
	yVelocity = y;
}


// Author of FLOAM: Wang Han
// Email wh200720041@gmail.com
// Homepage https://wanghan.pro
#ifndef _LIDAR_OPTIMIZATION_ANALYTIC_H_
#define _LIDAR_OPTIMIZATION_ANALYTIC_H_

#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>

Eigen::Matrix3d skew(Eigen::Vector3d& mat_in) {
  Eigen::Matrix<double, 3, 3> skew_mat;
  skew_mat.setZero();
  skew_mat(0, 1) = -mat_in(2);
  skew_mat(0, 2) = mat_in(1);
  skew_mat(1, 2) = -mat_in(0);
  skew_mat(1, 0) = mat_in(2);
  skew_mat(2, 0) = -mat_in(1);
  skew_mat(2, 1) = mat_in(0);
  return skew_mat;
}

void getTransformFromSe3(const Eigen::Matrix<double, 6, 1>& se3, Eigen::Quaterniond& q, Eigen::Vector3d& t) {
  Eigen::Vector3d omega(se3.data());
  Eigen::Vector3d upsilon(se3.data() + 3);
  Eigen::Matrix3d Omega = skew(omega);

  double theta = omega.norm();
  double half_theta = 0.5 * theta;

  double imag_factor;
  double real_factor = cos(half_theta);
  if (theta < 1e-10) {
    double theta_sq = theta * theta;
    double theta_po4 = theta_sq * theta_sq;
    imag_factor = 0.5 - 0.0208333 * theta_sq + 0.000260417 * theta_po4;
  } else {
    double sin_half_theta = sin(half_theta);
    imag_factor = sin_half_theta / theta;
  }

  q = Eigen::Quaterniond(real_factor, imag_factor * omega.x(), imag_factor * omega.y(), imag_factor * omega.z());

  Eigen::Matrix3d J;
  if (theta < 1e-10) {
    J = q.matrix();
  } else {
    Eigen::Matrix3d Omega2 = Omega * Omega;
    J = (Eigen::Matrix3d::Identity() + (1 - cos(theta)) / (theta * theta) * Omega + (theta - sin(theta)) / (pow(theta, 3)) * Omega2);
  }

  t = J * upsilon;
}

class PoseSE3Parameterization : public ceres::LocalParameterization {
public:
  PoseSE3Parameterization() {}
  virtual ~PoseSE3Parameterization() {}
  // 从这里看，参数块应该是[q,t]，且q是[x,y,z,w]
  virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const {
    Eigen::Map<const Eigen::Vector3d> trans(x + 4);

    Eigen::Quaterniond delta_q;
    Eigen::Vector3d delta_t;
    getTransformFromSe3(Eigen::Map<const Eigen::Matrix<double, 6, 1>>(delta), delta_q, delta_t);
    Eigen::Map<const Eigen::Quaterniond> quater(x);
    Eigen::Map<Eigen::Quaterniond> quater_plus(x_plus_delta);
    Eigen::Map<Eigen::Vector3d> trans_plus(x_plus_delta + 4);
    // TODO 不需要归一化吗？
    quater_plus = delta_q * quater;
    trans_plus = delta_q * trans + delta_t;

    return true;
  }
  // ref: https://github.com/wh200720041/floam/issues/50
  // 这是全局参数到局部参数的雅可比矩阵。全局是trans,quat，因此是7维，局部则是se3,因此是6维。
  // 答案中提到了J1*J2,其中J1是参数块对[t,q]求导来自于evaluate或者AD，J2则是这里的到李代数的参数化。
  virtual bool ComputeJacobian(const double* x, double* jacobian) const {
    Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> j(jacobian);
    (j.topRows(6)).setIdentity();
    (j.bottomRows(1)).setZero();

    return true;
  }
  virtual int GlobalSize() const { return 7; }
  virtual int LocalSize() const { return 6; }
};

#endif  // _LIDAR_OPTIMIZATION_ANALYTIC_H_