#define _USE_MATH_DEFINES

#include <fstream>
#include <iostream>
#include <omp.h>
#include <cmath>
#include <complex>
#include <cstdio>
#include <chrono>
#include <string>
#include <armadillo>
#include <vector>
#include "wignerSymbols.h"
//#include "wigner/gaunt.hpp"

//using namespace std::chrono;
using namespace arma;

static const double kB = 1.3806504e-23;         // J/K
static const double NA = 6.02214179e23;         // 1/mol
static const double EHARTREE = 4.35974434e-18;  // J/Hartree
static const double AMU = 1.660538921e-27;      // kg/amu
static const double HBAR = 1.054571726e-34;     // J.s
static const double HBAR1 = HBAR / EHARTREE;    // Hartree.s
static const double HBAR2 = HBAR * 1e20 / AMU;  // amu.Å^2/s
static const double SCH4 = 186.25; // J/mol.K

Mat<complex<double>> getHamiltonian(int lmax, double Ix, double Iy, double Iz, std::vector<double>& a);
SpMat<complex<double>> getSparseHam(int lmax, double Ix, double Iy, double Iz, std::vector<double>& a);
//Col<double> getCoefficients(std::string sysname);
std::vector<double> getWignerCoeffs(std::string sysname, bool freeRotor);
std::vector<double> getMomentOfInertia(std::string sysname="METH-CHA");
std::string getDirectory(std::string sysname);
double getPartitionFunction(double T, Mat<complex<double>>& H, int sym=1);
double getSparseQ(double T, SpMat<complex<double>>& H, int sym=1);

int main(int argc, char** argv) {
    /* argv[0]  Program name
     * argv[1]  System name
     * argv[2]  Lmax (start)
     * argv[3]  free-rotor
     * argv[4]  temperature / K
     */
    if (argc != 5) {
        throw std::runtime_error("Enter systemName as it appears in data directory, Lmax, free-rotor flag, temperature in Kelvin");
    }

    auto start = std::chrono::high_resolution_clock::now();

    std::string sysname = argv[1];
    bool freeRotor = std::stoi(argv[3]);
    double T = std::stod(argv[4]);
    std::string dirname = getDirectory(sysname);
    std::cout << "Directory containing data is: " << dirname << std::endl;
    std::vector<double> a = getWignerCoeffs(sysname, freeRotor);
    int sigma = 1;

    std::vector<double> Ivec = getMomentOfInertia(sysname);
    std::cout << "Moments of Inertia:" << std::endl;
    for (double i : Ivec) {
        std::cout << i << '\t';
    }

    /*
     *  Rotational Constants + Classical Partition Function
     */
    double B = HBAR1*HBAR2/(2.0*Ivec[2]);
    double A = HBAR1*HBAR2/(2.0*Ivec[1]);
    double C = HBAR1*HBAR2/(2.0*Ivec[0]);
    //std::cout << "kB T / Hartree:\n" << kB * 298 / EHARTREE << std::endl;
    std::cout << "Qapprox = " << sqrt(M_PI)/sigma * sqrt(pow(kB*300/EHARTREE, 3) / (A*B*C)) << std::endl;

    /*
     *  Dense Matrix Implementation
     */
    bool converge = false;
    bool dense = true;
    int lmax = atoi(argv[2]);
    double Q;
    double Qprev = 0;
    std::cout << "Lmax\t\tQ\t\tTime\t\tdQ" << std::endl;
    do {
        auto qstart = std::chrono::high_resolution_clock::now();
        if (dense) {
            /****  Dense Matrix Implementation  ****/
            unsigned long long size = (lmax+1)*(2*lmax+1)*(2*lmax+3)/3.0;
            Mat<complex<double>> H = getHamiltonian(lmax, Ivec[0], Ivec[1], Ivec[2], a);
            if (!H.is_hermitian(1e-3)) {
                H.brief_print("H = ");
            }
            try {
                Q = getPartitionFunction(298.15, H, sigma);
            } catch (const std::logic_error& e) {
                //std::cout << "Required Memory is " << size*size*8*1e-9 << " Gb." << std::endl;
                //std::cout << "Memory problem caused failure. Switching to sparse implementation." << std::endl;
                //dense = false;
                throw e;
                break;
            }
            /* *********************************** */
        } else {
            break;
            /****  Sparse Matrix Implementation  ****/
            SpMat<complex<double>> H = getSparseHam(lmax, Ivec[0], Ivec[1], Ivec[2], a);
            Q = getSparseQ(298.15, H, sigma);
            /****************************************/
        }
        auto qend = std::chrono::high_resolution_clock::now();
        auto qduration = std::chrono::duration_cast<std::chrono::microseconds>(qend - qstart);
        double dQ = fabs(Q-Qprev);
        std::cout << lmax << "\t\t" << Q << "\t\t" << qduration.count()/1e6 << " sec." << "\t\t" << dQ << std::endl;
        if (dQ < 1e-4) {
            converge = true;
            std::cout << "DeltaQ = " << fabs(Q-Qprev) << std::endl;
            std::cout << "Convergence criterion met!" << std::endl;
            std::cout << "Lmax = " << lmax << std::endl;
        }
        lmax++;
        Qprev = Q;
    } while (!converge);
    std::cout << std::endl;
    std::cout << std::endl;
    auto stop = std::chrono::high_resolution_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
    std::cout << std::endl << duration.count()/1e6 << " secs" << std::endl;
    return 0;
}

double getPartitionFunction(double T, Mat<complex<double>>& H, int sym) {
    /* Solve the Eigenvalues
     * Inputs:  T;  the temperature [=] K
     *          H;  the Hamiltonian matrix [=] Hartree
     */
    double b = pow(kB * T, -1) * EHARTREE;
    double tr = 0;

    //std::cout << "Solving Matrix exponential" << std::endl;
    //Mat<complex<double>> expbH = expmat_sym(-b*H);
    vec eigval = eig_sym(H);
    for (double e : eigval) {
        tr += exp(-b*e);
    }
    //std::cout  <<  "Solved Matrix exponential" << std::endl;
    //double tr = trace(expmat_sym(-b*H));
    //std::cout << std::endl << "Q predicted by eig_sym: " << tr/sym << std::endl;
    return double(tr/sym);
    //return double(Q/sym);
}

Mat<complex<double>> getHamiltonian(int lmax, double Ix, double Iy, double Iz, std::vector<double>& a) {
    /* Construct the Hamiltonian Matrix
     * Inputs:  lmax;   the maximum quantum number forthe spherical basis
     *             I;   the gas-phase moments of inertia [=] amu*Å^2
     *             a;   coefficients for potential in the Wigner D Matrix basis
     * Outputs:    H;   the Hamiltonian matrix (Hermitian) [=] Hartree
     */
    long double size = (lmax+1)*(2*lmax+1)*(2*lmax+3)/3.0;
    Mat<complex<double>> H = zeros<mat>(size, size);

    // Define rotational constants:
    double B = HBAR1*HBAR2/(2.0*Iz);
    double A = HBAR1*HBAR2/(2.0*Iy);
    double C = HBAR1*HBAR2/(2.0*Ix);
    double kap = (A == B && A == C) ? 0 : (2.0*B - (A+C)) / (A-C);
    #pragma omp parallel
    {
        unsigned long long i = 0;
        #pragma omp for
        for (int el = 0; el < lmax+1; el++) {
            for (int m = -el; m <= el; m++) {
                for (int k = -el; k <= el; k++) {
                    //unsigned long long i = (4*el*el*el/3.0) + 2*el*el + (5*el/3.0) + 2*m*el + m + k;
                    //std::cout << j << '\t';
                    unsigned long long j = 0;
                    for (int ell = 0; ell < lmax+1; ell++) {
                        for (int mm = -ell; mm <= ell; mm++) {
                            for (int kk = -ell; kk <= ell; kk++) {
                                //unsigned long long j = (4*ell*ell*ell/3.0) + 2*ell*ell + (5*ell/3.0) + 2*mm*ell + mm + kk;
                                if (j > i) continue;
                                if (i == j) {
                                    try {
                                        H(i,j) += 0.5*(A+C)*el*(el+1) + 0.5*(A-C)*kap*k*k;
                                        if (k+2 <= el) {
                                            double val = 0.25*(C-A)*sqrt(el*(el+1)-k*(k+1))*sqrt(el*(el+1)-(k+1)*(k+2));
                                            H(i+2,j) += val;
                                            H(j,i+2) += val;
                                        }
                                    } catch (const std::exception& e) {
                                        std::cout << "Failure at index: " <<
                                            i << "\t(" << el << ',' << m << ',' <<
                                            k << ')' << std::endl;
                                    }
                                }
                                //if (a.size() == 1) continue;
                                //double Vij = 0;
                                //for (int L = 0; L < 8; L++) {
                                //    for (int M = -L; M <= L; M++) {
                                //        for (int K = -L; K <= L; K++) {
                                //            unsigned long long ind = (4*L*L*L/3.0) + 2*L*L + (5*L/3.0) + 2*M*L + M + K;
                                //            if (ind > a.size()-1 || a[ind] == 0) continue;
                                //            //double Clm = WignerSymbols::clebschGordan(L,ell,el,M,mm,m);
                                //            //double Clk = WignerSymbols::clebschGordan(L,ell,el,K,kk,k);
                                //            //double val = 8*M_PI*M_PI*Clm*Clk/(2.0*el+1.0);
                                //            double Wlm = WignerSymbols::wigner3j(L,ell,el,M,mm,-m);
                                //            double Wlk = WignerSymbols::wigner3j(L,ell,el,K,kk,-k);
                                //            double val = 8*M_PI*M_PI*pow(-1.0, -m-k)*Wlm*Wlk;
                                //            Vij += a[ind] * val;
                                //        }
                                //    }
                                //}
                                //H(i,j) += Vij;
                                //H(j,i) += Vij;
                                j++;
                            }
                        }
                    }
                    i++;
                }
            }
        }
    } // end parallel
    //H.print("H = ");
    //std::cout << "Constructed matrix with LMAX " << lmax << '.' << std::endl;
    return H;
}

std::vector<double> getWignerCoeffs(std::string sysname, bool freeRotor) {
    if (freeRotor) {
        std::vector<double> a = { 0.0 };
        return a;
    }
    std::string dirname = getDirectory(sysname);
    std::string filename = dirname+"/a.txt";
    std::ifstream is(filename);
    double val;
    std::vector<double> a;
    while (is) {
        if(!(is >> val)) {
            break;
        }
        a.push_back(val);
    }
    return a;
}

std::vector<double> getMomentOfInertia(std::string sysname) {
    std::string dirname = getDirectory(sysname);
    std::string filename = dirname+'/'+"I.txt";
    std::ifstream is(filename);
    double val;
    std::vector<double> Ivec;
    while (is) {
        if (!(is >> val)) {
            break;
        }
        Ivec.push_back(val);
    }
    return Ivec;
}

std::string getDirectory(std::string sysname) {
    //std::string dirname = "/Users/lancebettinson/Thesis/umrr/code/hamiltonian-cpp/data";
    std::string dirname = "/global/scratch/lbettins/rotational-hamiltonian/data";
    if (sysname == "") {
        std::string sysname;
        std::cout << "Enter system name:" << std::endl;
        std::cin >> sysname;
    }
    return dirname+'/'+sysname;
}

SpMat<complex<double>> getSparseHam(int lmax, double Ix, double Iy, double Iz, std::vector<double>& a) {
    /* Construct the Hamiltonian Matrix
     * Inputs:  lmax;   the maximum quantum number forthe spherical basis
     *             I;   the gas-phase moments of inertia [=] amu*Å^2
     * Outputs:    H;   the Hamiltonian matrix (Hermitian) [=] Hartree
     */
    unsigned long long size = (lmax+1)*(2*lmax+1)*(2*lmax+3)/3.0;
    SpMat<complex<double>> H = sp_mat(size, size);

    // Define rotational constants:
    double B = HBAR1*HBAR2/(2.0*Iz);
    double A = HBAR1*HBAR2/(2.0*Iy);
    double C = HBAR1*HBAR2/(2.0*Ix);

    double kap = (A == B && A == C) ? 0 : (2.0*B - (A+C)) / (A-C);
    #pragma omp parallel
    {
        #pragma omp for
        for (int el = 0; el < lmax+1; el++) {
            for (int m = -el; m <= el; m++) {
                for (int k = -el; k <= el; k++) {
                    unsigned long long i = (4*el*el*el/3.0) + 2*el*el + (5*el/3.0) + 2*m*el + m + k;
                    for (int ell = 0; ell < lmax+1; ell++) {
                        for (int mm = -ell; mm <= ell; mm++) {
                            for (int kk = -ell; kk <= ell; kk++) {
                                unsigned long long j = (4*ell*ell*ell/3.0) + 2*ell*ell + (5*ell/3.0)
                                    + 2*mm*ell + mm + kk;
                                if (j > i) continue;
                                if (i == j) {
                                    try {
                                        H(i,j) += 0.5*(A+C)*el*(el+1) + 0.5*(A-C)*kap*k*k;
                                        if (k+2 <= el) {
                                            double val = 0.25*(C-A)*sqrt(el*(el+1)-k*(k+1))*sqrt(el*(el+1)-(k+1)*(k+2));
                                            H(i+2,j) += val;
                                            H(j,i+2) += val;
                                        }
                                    } catch (const std::exception& e) {
                                        std::cout << "Failure at index: " <<
                                            i << "\t(" << el << ',' << m << ',' <<
                                            k << ')' << std::endl;
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
    } // end parallel
    return H;
}

double getSparseQ(double T, SpMat<complex<double>>& H, int sym) {
    /* Solve the Eigenvalues forSparse Matrix
     * Inputs:  T;  the temperature [=] K
     *          H;  the (sparse) Hamiltonian matrix [=] Hartree
     */
    double b = pow(kB * T, -1) * EHARTREE;
    vec eigval;
    mat eigvec;
    //std::cout << "Number of rows: " << H.n_rows << std::endl;
    eigs_sym(eigval, eigvec, H, H.n_rows-1);
    double Q = 0;
    //std::cout << "Eigenvalues: " << std::endl;
    for (double e : eigval) {
        //std::cout << e << '\t';
        Q += exp(-b * e);
    }
    //std::cout << "Q predicted by eigs_sym: " << Q/sym << std::endl;
    return double(Q/sym);
}

Col<double> getCoefficients(std::string sysname) {
    std::string dirname = getDirectory(sysname);
    std::string filename = dirname+'/'+"vdat.txt";
    std::ifstream is(filename);
    if (is.fail())
    {
        std::cout << "cannot open file " << filename;
    }
    double theta, phi, v;
    std::vector<double> my_vec;
    while (is) {
        if (!(is >> theta >> phi >> v)) {
            break;
        }
        //std::cout << theta << '\t' << phi << '\t' << v << std::endl;
        my_vec.push_back(v);
    }
    Col<double> cvec = conv_to<vec>::from(my_vec);
    //cvec.print();
    is.close();
    return cvec;
}


#include <kv/Heine.hpp>
#include <kv/qAiry.hpp>
#include <kv/qBessel.hpp>
#include <cmath>
#include <iostream>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
typedef kv::interval<double> itv;
typedef kv::complex< kv::interval<double> > cp;
using namespace std;
namespace ub = boost::numeric::ublas;
int main()
{
  cout.precision(17);
  int n=20;
  itv nu,q;
  ub::vector< itv > x(100);
  q="0.7";
  nu=1.5;
  x(0)=4.5;
  x(1)=4.45;
  for(int i=1;i<=n;i++){
    x(i+1)=x(i)-kv::Hahn_Exton(itv(x(i)),itv(nu),itv(q))*(x(i)-x(i-1))
      /(kv::Hahn_Exton(itv(x(i)),itv(nu),itv(q))-kv::Hahn_Exton(itv(x(i-1)),itv(nu),itv(q)));
  cout<<x(i+1)<<endl;
  cout<<"value of HE inf"<<kv::Hahn_Exton(itv(x(i+1).lower()),itv(nu),itv(q))<<endl;
  cout<<"value of HE sup"<<kv::Hahn_Exton(itv(x(i+1).upper()),itv(nu),itv(q))<<endl;
  cout<<"value of HE mid"<<kv::Hahn_Exton(itv(mid(x(i+1))),itv(nu),itv(q))<<endl;
  }
}


#ifndef GICP_COST_HPP
#define GICP_COST_HPP

#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include "litamin2/ceres_cost/PoseSE3Parameterization.hpp"
// L^T*error
struct GICP_FACTOR {
  GICP_FACTOR(Eigen::Vector3d p_mean, Eigen::Vector3d q_mean, Eigen::Matrix3d p_cov, Eigen::Matrix3d q_cov) : p_mean_(p_mean), q_mean_(q_mean), p_cov_(p_cov), q_cov_(q_cov) {}

  template <typename T>
  bool operator()(const T* const q, const T* const t, T* residuals) const {
    Eigen::Map<Eigen::Matrix<T, 3, 1>> residuals_map(residuals);
    Eigen::Matrix<T, 3, 1> p_m(p_mean_.cast<T>());
    Eigen::Matrix<T, 3, 1> q_m(q_mean_.cast<T>());
    Eigen::Matrix<T, 3, 3> p_c = p_cov_.cast<T>();
    Eigen::Matrix<T, 3, 3> q_c = q_cov_.cast<T>();

    Eigen::Quaternion<T> quat(q);
    Eigen::Matrix<T, 3, 1> translation(t);

    Eigen::Matrix<T, 3, 3> mahalanobis = (q_c + quat * p_c * quat.inverse()).inverse();
    Eigen::Matrix<T, 3, 3> LT = mahalanobis.llt().matrixL().transpose();
    residuals_map = LT * (q_m - (quat * p_m + translation));

    return true;
  }

  static ceres::CostFunction* Create(Eigen::Vector3d p_mean_, Eigen::Vector3d q_mean_, Eigen::Matrix3d p_cov_, Eigen::Matrix3d q_cov_) {
    // 分别是残差，q，t的维度
    return (new ceres::AutoDiffCostFunction<GICP_FACTOR, 3, 4, 3>(new GICP_FACTOR(p_mean_, q_mean_, p_cov_, q_cov_)));
  }

  Eigen::Vector3d p_mean_, q_mean_;
  Eigen::Matrix3d p_cov_, q_cov_;
};

class GICPAnalyticCostFunction : public ceres::SizedCostFunction<3, 7> {
public:
  GICPAnalyticCostFunction(Eigen::Vector3d p_mean, Eigen::Vector3d q_mean, Eigen::Matrix3d p_cov, Eigen::Matrix3d q_cov)
  : p_mean_(p_mean),
    q_mean_(q_mean),
    p_cov_(p_cov),
    q_cov_(q_cov) {}
  virtual ~GICPAnalyticCostFunction() {}
  // parameters是[0,0,0,1,x,y,z]
  virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const {
    // 默认已经归一化
    Eigen::Quaterniond q_last_curr(parameters[0]);
    Eigen::Vector3d t_last_curr(parameters[0] + 4);
    Eigen::Matrix3d LT = (q_cov_ + q_last_curr * q_cov_ * q_last_curr.inverse()).inverse().llt().matrixL().transpose();
    Eigen::Map<Eigen::Vector3d> residuals_map(residuals);
    Eigen::Vector3d p_mean_trans = q_last_curr * p_mean_ + t_last_curr;
    residuals_map = LT * (q_mean_ - p_mean_trans);

    if (jacobians != NULL) {
      if (jacobians[0] != NULL) {
        Eigen::Map<Eigen::Matrix<double, 3, 7, Eigen::RowMajor>> J_se3(jacobians[0]);
        J_se3.setZero();
        Eigen::Matrix<double, 3, 6> dp_by_se3;
        dp_by_se3.block<3, 3>(0, 0) = skew(p_mean_trans);
        dp_by_se3.block<3, 3>(0, 3) = -Eigen::Matrix3d::Identity();
        J_se3.block<3, 6>(0, 0) = LT * dp_by_se3;
      }
    }
    return true;
  }

  Eigen::Vector3d p_mean_, q_mean_;
  Eigen::Matrix3d p_cov_, q_cov_;
};

// 弃用
// 精度不高，而且耗时
// Z是R*Sigma_i*RT对李代数的雅可比矩阵，来自于d2d-ndt，具体如何计算，我不清楚
class GICPDoubleAnalyticCostFunction : public ceres::SizedCostFunction<3, 7> {
public:
  GICPDoubleAnalyticCostFunction(Eigen::Vector3d p_mean, Eigen::Vector3d q_mean, Eigen::Matrix3d p_cov, Eigen::Matrix3d q_cov)
  : p_mean_(p_mean),
    q_mean_(q_mean),
    p_cov_(p_cov),
    q_cov_(q_cov) {}
  virtual ~GICPDoubleAnalyticCostFunction() {}
  // parameters是[0,0,0,1,x,y,z]
  virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const {
    // 默认已经归一化
    Eigen::Quaterniond q_last_curr(parameters[0]);
    Eigen::Vector3d t_last_curr(parameters[0] + 4);
    Eigen::Matrix3d mahalanobis = (q_cov_ + q_last_curr * q_cov_ * q_last_curr.inverse()).inverse();
    // Eigen::Matrix3d LT = mahalanobis.llt().matrixL().transpose();
    Eigen::Map<Eigen::Vector3d> residuals_map(residuals);
    Eigen::Vector3d p_mean_trans = q_last_curr * p_mean_ + t_last_curr;
    residuals_map = mahalanobis * (q_mean_ - p_mean_trans);

    if (jacobians != NULL) {
      if (jacobians[0] != NULL) {
        Eigen::Map<Eigen::Matrix<double, 3, 7, Eigen::RowMajor>> J_se3(jacobians[0]);
        J_se3.setZero();
        Eigen::Matrix<double, 3, 6> dp_by_se3;
        Eigen::Matrix<double, 3, 6> BZBU(Eigen::Matrix<double,3,6>::Zero());
        dp_by_se3.block<3, 3>(0, 0) = skew(p_mean_trans);
        dp_by_se3.block<3, 3>(0, 3) = -Eigen::Matrix3d::Identity();
        Eigen::Matrix3d Z1, Z2, Z3;
        Z1 << 0, -p_cov_(0, 2), p_cov_(0, 1), -p_cov_(0, 2), -2 * p_cov_(1, 2), -p_cov_(2, 2) + p_cov_(1, 1), p_cov_(0, 1), -p_cov_(2, 2) + p_cov_(1, 1), 2 * p_cov_(1, 2);
        Z2 << 2 * p_cov_(0, 2), p_cov_(1, 2), -p_cov_(0, 0) + p_cov_(2, 2), p_cov_(1, 2), 0, -p_cov_(0, 1), -p_cov_(0, 0) + p_cov_(2, 2), -p_cov_(0, 1), -2 * p_cov_(0, 2);
        Z3 << -2 * p_cov_(0, 1), -p_cov_(1, 1) + p_cov_(0, 0), -p_cov_(1, 2), -p_cov_(1, 1) + p_cov_(0, 0), 2 * p_cov_(0, 1), p_cov_(0, 2), -p_cov_(1, 2), p_cov_(0, 2), 0;
        BZBU.block<3,1>(0,0) = mahalanobis * Z1 * mahalanobis * p_mean_trans;
        BZBU.block<3,1>(0,1) = mahalanobis * Z2 * mahalanobis * p_mean_trans;
        BZBU.block<3,1>(0,2) = mahalanobis * Z3 * mahalanobis * p_mean_trans;
        J_se3.block<3, 6>(0, 0) = -BZBU + mahalanobis * dp_by_se3;
      }
    }
    return true;
  }

  Eigen::Vector3d p_mean_, q_mean_;
  Eigen::Matrix3d p_cov_, q_cov_;
};

// 正常工作，但精度并不是特别的高
// TODO 删除无效代码
class ICPAnalyticCostFunction : public ceres::SizedCostFunction<3, 7> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  ICPAnalyticCostFunction(Eigen::Vector3d p_mean, Eigen::Vector3d q_mean, Eigen::Matrix3d p_cov, Eigen::Matrix3d q_cov)
  : p_mean_(p_mean),
    q_mean_(q_mean),
    p_cov_(p_cov),
    q_cov_(q_cov) {}
  virtual ~ICPAnalyticCostFunction() {}
  // parameters是[0,0,0,1,x,y,z]
  virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const {
    // 默认已经归一化
    Eigen::Quaterniond q_last_curr(parameters[0]);
    Eigen::Vector3d t_last_curr(parameters[0] + 4);
    // Eigen::Matrix3d mahalanobis = q_cov_ + q_last_curr * q_cov_ * q_last_curr.inverse();
    // Eigen::Matrix3d LT = mahalanobis.llt().matrixL().transpose();
    Eigen::Map<Eigen::Vector3d> residuals_map(residuals);
    Eigen::Vector3d p_mean_trans = q_last_curr * p_mean_ + t_last_curr;
    residuals_map = q_mean_ - p_mean_trans;

    if (jacobians != NULL) {
      if (jacobians[0] != NULL) {
        Eigen::Map<Eigen::Matrix<double, 3, 7, Eigen::RowMajor>> J_se3(jacobians[0]);
        J_se3.setZero();
        Eigen::Matrix<double, 3, 6> dp_by_se3;
        dp_by_se3.block<3, 3>(0, 0) = skew(p_mean_trans);
        dp_by_se3.block<3, 3>(0, 3) = -Eigen::Matrix3d::Identity();
        J_se3.block<3, 6>(0, 0) = dp_by_se3;
      }
    }
    return true;
  }

  Eigen::Vector3d p_mean_, q_mean_;
  Eigen::Matrix3d p_cov_, q_cov_;
};
#endif


//
//  gmatrix16float.cpp
//  GCommon
//
//  Created by David Coen on 2011 06 01
//  Copyright Pleasure seeking morons 2011. All rights reserved.
//

#include "gmatrix16float.h"

#include "gvector3float.h"
#include "gmathmatrix.h"
#include "gmath.h"
#include "gmatrix9float.h"

#include <boost/swap.hpp>

#define DSC_INLINE_MATRIX_MUL

/*
matching openGL documentation
		0_0, 0_1, 0_2, 0_3(x),
		1_0, 1_1, 1_2, 1_3(y),
		2_0, 2_1, 2_2, 2_3(z),
		3_0, 3_1, 3_2, 3_3
*/

/*static*/ const GMatrix16Float GMatrix16Float::sIdentity(1.0F, 0.0F, 0.0F, 0.0F,
														  0.0F, 1.0F, 0.0F, 0.0F,
														  0.0F, 0.0F, 1.0F, 0.0F,
														  0.0F, 0.0F, 0.0F, 1.0F);

//constructors
GMatrix16Float::GMatrix16Float(const GR32 in_data_0_0, const GR32 in_data_0_1, const GR32 in_data_0_2, const GR32 in_data_0_3,
	const GR32 in_data_1_0, const GR32 in_data_1_1, const GR32 in_data_1_2, const GR32 in_data_1_3,
	const GR32 in_data_2_0, const GR32 in_data_2_1, const GR32 in_data_2_2, const GR32 in_data_2_3,
	const GR32 in_data_3_0, const GR32 in_data_3_1, const GR32 in_data_3_2, const GR32 in_data_3_3
	)
{
	SetData(in_data_0_0, in_data_0_1, in_data_0_2, in_data_0_3,
		in_data_1_0, in_data_1_1, in_data_1_2, in_data_1_3,
		in_data_2_0, in_data_2_1, in_data_2_2, in_data_2_3,
		in_data_3_0, in_data_3_1, in_data_3_2, in_data_3_3
		);
	return;
}
GMatrix16Float::GMatrix16Float(const GR32* const in_data)
{
	for (GS32 index = 0; index < 16; ++index)
	{
		m_data[index] = in_data[index];
	}
	return;

}

GMatrix16Float::GMatrix16Float(const GMatrix16Float& in_src)
{
	(*this) = in_src;
	return;
}

GMatrix16Float::~GMatrix16Float()
{
	return;
}

//operators
const GMatrix16Float& GMatrix16Float::operator=(const GMatrix16Float& in_rhs)
{
	for (GS32 index = 0; index < 16; ++index)
	{
		m_data[index] = in_rhs.m_data[index];
	}
	return (*this);
}

const GMatrix16Float& GMatrix16Float::operator*=(const GR32 in_rhs)
{
	for (GS32 index = 0; index < 16; ++index)
	{
		m_data[index] *= in_rhs;
	}

	return (*this);
}

//public methods
void GMatrix16Float::Decompose(GVector3Float& out_position,
   GQuaternion4Float& out_rotation,
   GVector3Float& out_scale
   )
{
	return;
}

GMatrix16Float&  GMatrix16Float::TransposeSelf()
{
	std::swap(m_0_1, m_1_0);
	std::swap(m_0_2, m_2_0);
	std::swap(m_0_3, m_3_0);
	std::swap(m_1_2, m_2_1);
	std::swap(m_1_3, m_3_1);
	std::swap(m_2_3, m_3_2);

	return (*this);
}
GMatrix16Float& GMatrix16Float::InverseSelf()
{
	(*this) = ReturnInverse();
	return (*this);
}

const GMatrix16Float GMatrix16Float::ReturnInverse()const
{
	GMatrix16Float result;
	GMathMatrix<GR32>::Inverse4(&result.m_data[0], &m_data[0]);

	return result;
}

GMatrix9Float GMatrix16Float::GetRotation()const
{
	return GMatrix9Float(
		m_0_0, m_0_1, m_0_2,
		m_1_0, m_1_1, m_1_2,
		m_2_0, m_2_1, m_2_2
	);
}

void GMatrix16Float::SetRotation(const GMatrix9Float& in_rotation)
{
	m_0_0 = in_rotation.m_0_0;
	m_1_0 = in_rotation.m_1_0;
	m_2_0 = in_rotation.m_2_0;
	m_0_1 = in_rotation.m_0_1;
	m_1_1 = in_rotation.m_1_1;
	m_2_1 = in_rotation.m_2_1;
	m_0_2 = in_rotation.m_0_2;
	m_1_2 = in_rotation.m_1_2;
	m_2_2 = in_rotation.m_2_2;
	return;
}

//public accessors
void GMatrix16Float::SetData(const GR32 in_data_0_0, const GR32 in_data_0_1, const GR32 in_data_0_2, const GR32 in_data_0_3,
	const GR32 in_data_1_0, const GR32 in_data_1_1, const GR32 in_data_1_2, const GR32 in_data_1_3,
	const GR32 in_data_2_0, const GR32 in_data_2_1, const GR32 in_data_2_2, const GR32 in_data_2_3,
	const GR32 in_data_3_0, const GR32 in_data_3_1, const GR32 in_data_3_2, const GR32 in_data_3_3
	)
{
	m_0_0 = in_data_0_0;
	m_1_0 = in_data_1_0;
	m_2_0 = in_data_2_0;
	m_3_0 = in_data_3_0;

	m_0_1 = in_data_0_1;
	m_1_1 = in_data_1_1;
	m_2_1 = in_data_2_1;
	m_3_1 = in_data_3_1;

	m_0_2 = in_data_0_2;
	m_1_2 = in_data_1_2;
	m_2_2 = in_data_2_2;
	m_3_2 = in_data_3_2;

	m_0_3 = in_data_0_3;
	m_1_3 = in_data_1_3;
	m_2_3 = in_data_2_3;
	m_3_3 = in_data_3_3;

	return;
}

const GVector3Float GMatrix16Float::GetAt()const
{
	const GVector3Float result(
		m_0_2,
		m_1_2,
		m_2_2
		);
	return result;
}

const GVector3Float GMatrix16Float::GetUp()const
{
	const GVector3Float result(
		m_0_1,
		m_1_1,
		m_2_1
		);
	return result;
}

const GVector3Float GMatrix16Float::GetPosition()const
{
	const GVector3Float result(
		m_0_3,
		m_1_3,
		m_2_3
		);
	return result;
}

void GMatrix16Float::SetPosition(const GVector3Float& in_position)
{
	m_0_3 = in_position.m_x;
	m_1_3 = in_position.m_y;
	m_2_3 = in_position.m_z;
	return;
}

//global operators
const GMatrix16Float operator*(const GMatrix16Float& in_lhs, const GMatrix16Float& in_rhs)
{
	GR32 value[16];
#ifdef DSC_INLINE_MATRIX_MUL
	const GR32* const lhsData = in_lhs.GetData();
	const GR32* const rhsData = in_rhs.GetData();
	value[ 0] = (lhsData[ 0] * rhsData[ 0]) + (lhsData[ 1] * rhsData[ 4]) + (lhsData[ 2] * rhsData[ 8]) + (lhsData[ 3] * rhsData[12]);
	value[ 1] = (lhsData[ 0] * rhsData[ 1]) + (lhsData[ 1] * rhsData[ 5]) + (lhsData[ 2] * rhsData[ 9]) + (lhsData[ 3] * rhsData[13]);
	value[ 2] = (lhsData[ 0] * rhsData[ 2]) + (lhsData[ 1] * rhsData[ 6]) + (lhsData[ 2] * rhsData[10]) + (lhsData[ 3] * rhsData[14]);
	value[ 3] = (lhsData[ 0] * rhsData[ 3]) + (lhsData[ 1] * rhsData[ 7]) + (lhsData[ 2] * rhsData[11]) + (lhsData[ 3] * rhsData[15]);

	value[ 4] = (lhsData[ 4] * rhsData[ 0]) + (lhsData[ 5] * rhsData[ 4]) + (lhsData[ 6] * rhsData[ 8]) + (lhsData[ 7] * rhsData[12]);
	value[ 5] = (lhsData[ 4] * rhsData[ 1]) + (lhsData[ 5] * rhsData[ 5]) + (lhsData[ 6] * rhsData[ 9]) + (lhsData[ 7] * rhsData[13]);
	value[ 6] = (lhsData[ 4] * rhsData[ 2]) + (lhsData[ 5] * rhsData[ 6]) + (lhsData[ 6] * rhsData[10]) + (lhsData[ 7] * rhsData[14]);
	value[ 7] = (lhsData[ 4] * rhsData[ 3]) + (lhsData[ 5] * rhsData[ 7]) + (lhsData[ 6] * rhsData[11]) + (lhsData[ 7] * rhsData[15]);

	value[ 8] = (lhsData[ 8] * rhsData[ 0]) + (lhsData[ 9] * rhsData[ 4]) + (lhsData[10] * rhsData[ 8]) + (lhsData[11] * rhsData[12]);
	value[ 9] = (lhsData[ 8] * rhsData[ 1]) + (lhsData[ 9] * rhsData[ 5]) + (lhsData[10] * rhsData[ 9]) + (lhsData[11] * rhsData[13]);
	value[10] = (lhsData[ 8] * rhsData[ 2]) + (lhsData[ 9] * rhsData[ 6]) + (lhsData[10] * rhsData[10]) + (lhsData[11] * rhsData[14]);
	value[11] = (lhsData[ 8] * rhsData[ 3]) + (lhsData[ 9] * rhsData[ 7]) + (lhsData[10] * rhsData[11]) + (lhsData[11] * rhsData[15]);

	value[12] = (lhsData[12] * rhsData[ 0]) + (lhsData[13] * rhsData[ 4]) + (lhsData[14] * rhsData[ 8]) + (lhsData[15] * rhsData[12]);
	value[13] = (lhsData[12] * rhsData[ 1]) + (lhsData[13] * rhsData[ 5]) + (lhsData[14] * rhsData[ 9]) + (lhsData[15] * rhsData[13]);
	value[14] = (lhsData[12] * rhsData[ 2]) + (lhsData[13] * rhsData[ 6]) + (lhsData[14] * rhsData[10]) + (lhsData[15] * rhsData[14]);
	value[15] = (lhsData[12] * rhsData[ 3]) + (lhsData[13] * rhsData[ 7]) + (lhsData[14] * rhsData[11]) + (lhsData[15] * rhsData[15]);
#else
	GMathMatrix<GR32>::MatrixMul(
		in_lhs.GetData(),
		in_rhs.GetData(),
		4,
		4,
		4,
		&value[0]
		);
#endif
	return GMatrix16Float(&value[0]);
}

const GVector3Float GMatrix16FloatMultiplyNoTranslate(const GVector3Float& in_lhs, const GMatrix16Float& in_rhs)
{
#ifdef DSC_INLINE_MATRIX_MUL
	GR32 value[3];
	const GR32* const lhsData = in_lhs.GetData();
	const GR32* const rhsData = in_rhs.GetData();
	value[ 0] = (lhsData[ 0] * rhsData[ 0]) + (lhsData[ 1] * rhsData[ 4]) + (lhsData[ 2] * rhsData[ 8]);
	value[ 1] = (lhsData[ 0] * rhsData[ 1]) + (lhsData[ 1] * rhsData[ 5]) + (lhsData[ 2] * rhsData[ 9]);
	value[ 2] = (lhsData[ 0] * rhsData[ 2]) + (lhsData[ 1] * rhsData[ 6]) + (lhsData[ 2] * rhsData[10]);
#else
	GR32 source[4];
	GR32 value[4];
	//promote
	source[0] = in_lhs.m_x;
	source[1] = in_lhs.m_y;
	source[2] = in_lhs.m_z;
	source[3] = 0.0F;

	//< m, n > * < p, m > = < p, n >
	GMathMatrix<GR32>::MatrixMul(
		&source[0],
		in_rhs.GetData(),
		4, //m
		1, //n
		4, //p
		&value[0]
		);
#endif

	return GVector3Float(&value[0]);
}

//{
//	const GMatrix9Float subMatrix = in_lhs.GetRotation();
//	const GVector3Float result = subMatrix * in_rhs;
//	return result;
//}

const GVector3Float operator*(const GVector3Float& in_lhs, const GMatrix16Float& in_rhs)
{
#ifdef DSC_INLINE_MATRIX_MUL
	const GR32* const lhsData = in_lhs.GetData();
	const GR32* const rhsData = in_rhs.GetData();
	return GVector3Float(
		(lhsData[ 0] * rhsData[ 0]) + (lhsData[ 1] * rhsData[ 4]) + (lhsData[ 2] * rhsData[ 8]) + rhsData[12],
		(lhsData[ 0] * rhsData[ 1]) + (lhsData[ 1] * rhsData[ 5]) + (lhsData[ 2] * rhsData[ 9]) + rhsData[13],
		(lhsData[ 0] * rhsData[ 2]) + (lhsData[ 1] * rhsData[ 6]) + (lhsData[ 2] * rhsData[10]) + rhsData[14]
		);
#else
	GR32 source[4];
	GR32 value[4];
	//promote
	source[0] = in_lhs.m_x;
	source[1] = in_lhs.m_y;
	source[2] = in_lhs.m_z;
	source[3] = 1.0F;

	//< m, n > * < p, m > = < p, n >
	GMathMatrix<GR32>::MatrixMul(
		&source[0],
		in_rhs.GetData(),
		4, //m
		1, //n
		4, //p
		&value[0]
		);
	return GVector3Float(&value[0]);
#endif
}

/*
  from _Mathematics for computer graphics p.171
 mapping a pair of vectors onto another pair, u,x being one pair, a,y being another of unit vectors at same angle
 u.x = a.y = cos(\), with sin(\) != 0
 a roation matrix sending u,x to a,y given by
               a
 M = [u v w ][ b ]
               c
 d = | x * u | = | y * a | = | sin(\) |
 if 90deg = u to x, cos(\) = 0, sin(\) = 1
 v = ( x * u ) / d
 b = ( y * a ) / d
 w = ( x - u( cos(\) ) ) / d
 c = ( y - a( cos(\) ) ) / d
*/
const GMatrix16Float GMatrix16FloatConstructAtUp(
	const GVector3Float& in_targetAt,
	const GVector3Float& in_targetUp,
	const GVector3Float& in_baseAt,
	const GVector3Float& in_baseUp,
	const GVector3Float& in_position
	)
{
	////is this a stablity issue
	//const GR32 dotResultA = DotProduct(in_baseAt, in_targetAt);
	//const GR32 dotResultU = DotProduct(in_baseUp, in_targetUp);
	//if (((dotResultA < -0.999F) || (0.999F < dotResultA)) &&
	//	((dotResultU < -0.999F) || (0.999F < dotResultU)))
	//{
	//	GMatrix16Float result(GMatrix16Float::sIdentity);
	//	result.SetPosition(in_position);
	//	return result;
	//}

	const GVector3Float crossBaseUpAt = CrossProduct(in_baseUp, in_baseAt);
	const GVector3Float crossTargetUpAt = CrossProduct(in_targetUp, in_targetAt);

	return GMatrix16Float(
		(in_baseAt.m_x * in_targetAt.m_x) + (crossBaseUpAt.m_x * crossTargetUpAt.m_x) + (in_baseUp.m_x * in_targetUp.m_x),
		(in_baseAt.m_y * in_targetAt.m_x) + (crossBaseUpAt.m_y * crossTargetUpAt.m_x) + (in_baseUp.m_y * in_targetUp.m_x),
		(in_baseAt.m_z * in_targetAt.m_x) + (crossBaseUpAt.m_z * crossTargetUpAt.m_x) + (in_baseUp.m_z * in_targetUp.m_x),
		in_position.m_x,

		(in_baseAt.m_x * in_targetAt.m_y) + (crossBaseUpAt.m_x * crossTargetUpAt.m_y) + (in_baseUp.m_x * in_targetUp.m_y),
		(in_baseAt.m_y * in_targetAt.m_y) + (crossBaseUpAt.m_y * crossTargetUpAt.m_y) + (in_baseUp.m_y * in_targetUp.m_y),
		(in_baseAt.m_z * in_targetAt.m_y) + (crossBaseUpAt.m_z * crossTargetUpAt.m_y) + (in_baseUp.m_z * in_targetUp.m_y),
		in_position.m_y,

		(in_baseAt.m_x * in_targetAt.m_z) + (crossBaseUpAt.m_x * crossTargetUpAt.m_z) + (in_baseUp.m_x * in_targetUp.m_z),
		(in_baseAt.m_y * in_targetAt.m_z) + (crossBaseUpAt.m_y * crossTargetUpAt.m_z) + (in_baseUp.m_y * in_targetUp.m_z),
		(in_baseAt.m_z * in_targetAt.m_z) + (crossBaseUpAt.m_z * crossTargetUpAt.m_z) + (in_baseUp.m_z * in_targetUp.m_z),
		in_position.m_z,

		0.0F,
		0.0F,
		0.0F,
		1.0F
		);
}

void GMatrix16FloatDecomposeAtUp(
	GVector3Float& out_at,
	GVector3Float& out_up,
	GVector3Float& out_position,
	const GMatrix16Float& in_matrix
	)
{
	out_at = in_matrix.GetAt();
	out_up = in_matrix.GetUp();
	out_position = in_matrix.GetPosition();
	return;
}

/*
  http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/index.htm
	matrix[ HMatrixR4::IndexGet( 0, 0 ) ] = c + axis_x * axis_x * t;
	matrix[ HMatrixR4::IndexGet( 1, 1 ) ] = c + axis_y * axis_y * t;
	matrix[ HMatrixR4::IndexGet( 2, 2 ) ] = c + axis_z * axis_z * t;

	HREAL tmp1 = axis_x * axis_y * t;
	HREAL tmp2 = axis_z * s;
	matrix[ HMatrixR4::IndexGet( 1, 0 ) ] = tmp1 + tmp2;
	matrix[ HMatrixR4::IndexGet( 0, 1 ) ] = tmp1 - tmp2;
	tmp1 = axis_x * axis_z * t;
	tmp2 = axis_y * s;
	matrix[ HMatrixR4::IndexGet( 2, 0 ) ] = tmp1 - tmp2;
	matrix[ HMatrixR4::IndexGet( 0, 2 ) ] = tmp1 + tmp2;
	tmp1 = axis_y * axis_z * t;
	tmp2 = axis_x * s;
	matrix[ HMatrixR4::IndexGet( 2, 1 ) ] = tmp1 + tmp2;
	matrix[ HMatrixR4::IndexGet( 1, 2 ) ] = tmp1 - tmp2;
*/
const GMatrix16Float GMatrix16FloatConstructAxisAngle(const GVector3Float& in_axis, const GR32 in_angleRad)
{
	const GVector3Float localAxis = Normalise(in_axis);
	const GR32 axis_x = localAxis.m_x;
	const GR32 axis_y = localAxis.m_y;
	const GR32 axis_z = localAxis.m_z;

	const GR32 c = GMath::Cos( in_angleRad );
	const GR32 s = GMath::Sin( in_angleRad );
	const GR32 t = 1.0F - c;

	const GR32 tmp1_01 = axis_x * axis_y * t;
	const GR32 tmp2_01 = axis_z * s;

	const GR32 tmp1_02 = axis_x * axis_z * t;
	const GR32 tmp2_02 = axis_y * s;

	const GR32 tmp1_21 = axis_y * axis_z * t;
	const GR32 tmp2_21 = axis_x * s;

	return GMatrix16Float(
		c + axis_x * axis_x * t,		tmp1_01 - tmp2_01,			tmp1_02 + tmp2_02,			0.0F,
		tmp1_01 + tmp2_01,				c + axis_y * axis_y * t,	tmp1_21 - tmp2_21,			0.0F,
		tmp1_02 - tmp2_02,				tmp1_21 + tmp2_21,			c + axis_z * axis_z * t,	0.0F,
		0.0F,							0.0F,						0.0F,						1.0F
		);
}

const GBOOL Valid(const GMatrix16Float& in_data)
{
	const GR32* const pData = in_data.GetData();
	if ((!GMath::Valid(pData[0])) ||
		(!GMath::Valid(pData[1])) ||
		(!GMath::Valid(pData[2])) ||
		(!GMath::Valid(pData[3])) ||
		(!GMath::Valid(pData[4])) ||
		(!GMath::Valid(pData[5])) ||
		(!GMath::Valid(pData[6])) ||
		(!GMath::Valid(pData[7])) ||
		(!GMath::Valid(pData[8])) ||
		(!GMath::Valid(pData[9])) ||
		(!GMath::Valid(pData[10])) ||
		(!GMath::Valid(pData[11])) ||
		(!GMath::Valid(pData[12])) ||
		(!GMath::Valid(pData[13])) ||
		(!GMath::Valid(pData[14])) ||
		(!GMath::Valid(pData[15])))
	{
		return false;
	}
	return true;
}

//cheap invert, no skew or scale support
const GMatrix16Float GMatrix16FloatInvertOrthogonal(const GMatrix16Float& in_src)
{
	const GVector3Float position = in_src.GetPosition();
	GMatrix16Float transpose(in_src);
	transpose.SetPosition(GVector3Float::sZero);
	transpose.TransposeSelf();
	transpose.SetPosition(-position);

	return transpose;
}


#include "Truss.hpp"
#include <armadillo>
#include <iomanip>

using namespace arma;

Truss::Joint::Joint() {
	x=y=0;
	fixedX=fixedY=false;
	externalX=externalY=0;
	connectionLen = 0;
}

Truss::Member::Member(){
	id = -1;
	joint1 = joint2 = NULL;
	length = 0;
}

Truss::Truss(int numJoints, int numMembers) {
	this->numJoints = numJoints;
	this->numMembers = numMembers;
	this->joints = new Joint[this->numJoints];
	this->members = new Member[this->numMembers];
	this->pin = &joints[0];
	this->normalJoint = &joints[1];
}

Truss::~Truss() {
	delete[] joints;
	delete[] members;
}

Truss::Joint* Truss::getJoints() {
	return this->joints;
}

Truss::Member* Truss::getMembers() {
	return this->members;
}

void Truss::solveGeneralSystem() {
	// assumes normal reaction force is horizontally or vertically aligned
	double momentAtPin = 0; // counterclockwise is positive
	// calculate moments about every joint
	for (int i = 2; i < this->numJoints; i++) {
		// if the joint has an external x (i.e. has any external force
		if (joints[i].externalY != 0) {
			momentAtPin += joints[i].x*joints[i].externalY;
			// do not need to calculate moment generated by x forces since external forces are only in y direction
//			momentAtPin -= joints[i].y*joints[i].externalX;
		}
	}

	if (normalJoint->x == 0) { // the reactions are vertically aligned
		 normalJoint->externalX = momentAtPin/normalJoint->y;
		 pin->externalX = -normalJoint->externalX;
		 pin->externalY = 0;
		 for (int i = 2; i < this->numJoints; i++) {
			 pin->externalY -= joints[i].externalY;
		 }
	}
	else { // the reactions forces are horizontally aligned
		normalJoint->externalY = -momentAtPin/normalJoint->x;
		pin->externalX=0;
		pin->externalY = 0;
		for (int i = 1; i < this->numJoints; i++) {
			pin->externalY -= joints[i].externalY;
		}
	}
}

void Truss::initialSolve(){
	for (int i = 0; i < numMembers; i++){
		members[i].length = pow(pow(members[i].joint1->x - members[i].joint2->x, 2) + pow(members[i].joint1->y - members[i].joint2->y, 2), 0.5);
//		cout << "m" << i << "L: "<< members[i].length << endl;
	}

	for (int i = 0; i < numJoints; i++) {
		for (int k = 0; k < joints[i].connections.size(); k++) {
			joints[i].connectionLen += members[joints[i].connections[k]].length;
//			cout << "joint " << i << "connectionLen: " << joints[i].connectionLen << endl;
		}
	}

	solveGeneralSystem();

	arma::mat equations = arma::mat(numJoints*2, numMembers, arma::fill::zeros);
	arma::vec external = arma::vec(numJoints*2, arma::fill::zeros);

	for (int i = 0; i < numJoints; i++){
		Joint * j = &joints[i];
		external(i*2) = -j->externalX;
		external(i*2+1) = -j->externalY;

		for(int k = 0; k < joints[i].connections.size(); k++) {
			equations(i * 2, joints[i].connections[k]) = (members[joints[i].connections[k]].joint1->x + members[j->connections[k]].joint2->x - 2 * j->x) / members[j->connections[k]].length;
			equations(i * 2 + 1, joints[i].connections[k]) = (members[joints[i].connections[k]].joint1->y + members[j->connections[k]].joint2->y - 2 * j->y) / members[j->connections[k]].length;
		}
	}
	//external.print();
	//cout << endl;
	//system("pause");
//	equations.print();
//	cout << endl;
//	external.print();
//	cout << endl;
//	system("pause");
	arma::vec forces = solve(equations, external);
//	forces.print();
	for (int i = 0; i < numMembers; i++){
		validForces.push_back(forces(i));
	}
}

bool Truss::solveInternal() {
	//cout << "solving internal" << endl;
	solveGeneralSystem();

	arma::mat equations = arma::mat(numJoints*2, numMembers, arma::fill::zeros);
	arma::vec external = arma::vec(numJoints*2, arma::fill::zeros);

	for (int i = 0; i < numJoints; i++) {
		Joint * j = &joints[i];
		external(i * 2) = -j->externalX;
		external(i * 2 + 1) = -j->externalY;

		for (int k = 0; k < joints[i].connections.size(); k++) {
			equations(i * 2, joints[i].connections[k]) = (members[joints[i].connections[k]].joint1->x + members[j->connections[k]].joint2->x - 2 * j->x) / members[j->connections[k]].length;
			equations(i * 2 + 1, joints[i].connections[k]) = (members[joints[i].connections[k]].joint1->y + members[j->connections[k]].joint2->y - 2 * j->y) / members[j->connections[k]].length;
		}
	}

	//equations.print();
	//cout << endl << "external" << endl;
	/*external.print();
	cout << endl;
	*/
	//external.print();
	//cout << endl;
	//system("pause");
	arma::vec forces = solve(equations, external);

//	forces.print();
//	cout << endl;*/

    bool solveValid = true;
    for (int i = 0; i < numMembers; i++){
    	if(fabs(forces(i)) >= fabs(validForces[i]) && fabs(forces(i)) > MAX_MEMBER_FORCE){
			//cout << "invalid forces" << endl;
			solveValid = false;
    		break;
    	}
    }

    if (solveValid){
		for (int i = 0; i < numMembers; i++){
			validForces[i] = forces(i);
		}
        return true;
    } else {
        return false;
    }
}

double * Truss::checkIfBetterState(bool xDir, int jointNum, double increment) {
//	cout << "checking new state with joint " << jointNum << " moving " << xDir << " by " << increment << endl;
	bool betterState = true;
	Joint * joint = &joints[jointNum];
	int numCon = joint->connections.size();
	double * newLengths = new double[numCon];
	double * oldLengths = NULL;
	double totalLen = 0;

	//changes x or y value based on the direction given
	if(xDir) {
        joint->x += increment;
    } else {
	    joint->y += increment;
	}

	for (int i = 0; i < numCon; i++){
		newLengths[i] = pow( pow(members[joint->connections[i]].joint1->x - members[joint->connections[i]].joint2->x , 2) + pow(members[joint->connections[i]].joint1->y - members[joint->connections[i]].joint2->y, 2 ) , 0.5);
		if(newLengths[i] > members[joint->connections[i]].length  && newLengths[i] > 3.00000000){
			betterState = false;
			break;
		}
//		cout << "length " << i << newLengths[i] << endl;
        totalLen += newLengths[i];
	}
//	cout << "previos length: " << joint->connectionLen << endl;
    if (totalLen >= joint->connectionLen){
        betterState = false;
    }
	if (!betterState) {
		for (int i = 0; i < numMembers; i++) {
			if (fabs(validForces[i]) >= MAX_MEMBER_FORCE) {
				betterState = true;
				break;
			}
		}
	}
	// if new state is better, copy length values into old length and
	if (betterState){
//		cout << "state is better" << endl;
		joint->connectionLen = totalLen;
		oldLengths = new double[numCon];
		for (int i = 0; i< numCon; i++){
		    //change all the member geometry
			oldLengths[i] = members[joint->connections[i]].length;
			members[joint->connections[i]].length = newLengths[i];
//			cout << "m" << i << "L: " << members[joint->connections[i]].length << endl;
		}

	}
	//reverts x or y value based on direction given if new state is not better
	else {
	//	cout << "state is worse. new: " <<totalLen << "\told:" <<joint->connectionLen << endl;
        if(xDir) {
            joint->x -= increment;
        } else {
			joint->y -= increment;
		}
	}

	delete [] newLengths;
	newLengths = NULL;

	return oldLengths;
}

void Truss::revertLengths(int jointNum, double * oldLengths){
	for (int i = 0; i < joints[jointNum].connections.size(); i++){
		members[joints[jointNum].connections[i]].length = oldLengths[i];
	}
}

void Truss::optimize(){
	initialSolve();
	bool systemChanged = false;
	long double movementIncrement = 0.0001;
	double * oldLengths = NULL;

	while (movementIncrement > MIN_MOVEMENT_INCREMENT){
		systemChanged = true;
		while (systemChanged){
			systemChanged = false;
			for (int i  = 1; i < numJoints; i++){

				//testing vertical movement
				if (!joints[i].fixedY) {
					oldLengths = checkIfBetterState(false, i, -movementIncrement);
					if (oldLengths != NULL) {
						if (solveInternal()) {
							systemChanged = true;
						}
						else {
							joints[i].y += movementIncrement;
							revertLengths(i, oldLengths);//move node back and adjust dimensions
							for (int j = 0; j < numJoints; j++) {
								joints[j].connectionLen = 0;
								for (int con = 0; con < joints[j].connections.size(); con++) {
									joints[j].connectionLen += members[joints[j].connections[con]].length;
								}
							}
							solveGeneralSystem();
						}
					}
					else {
						oldLengths = checkIfBetterState(false, i, movementIncrement);
						if (oldLengths != NULL) {
							if (solveInternal()) {
								systemChanged = true;
							}
							else {
								joints[i].y -= movementIncrement;
								revertLengths(i, oldLengths);
								for (int j = 0; j < numJoints; j++) {
									joints[j].connectionLen = 0;
									for (int con = 0; con < joints[j].connections.size(); con++) {
										joints[j].connectionLen += members[joints[j].connections[con]].length;
									}
								}
								solveGeneralSystem();
							}
						}
					}
					if (oldLengths != NULL) {
						delete[] oldLengths;
						oldLengths = NULL;
					}

				}

				//testing horizontal movement
				if (!joints[i].fixedX){
					oldLengths = checkIfBetterState(true, i, movementIncrement*pow(-1, i));
					if (oldLengths != NULL) {
					    if(solveInternal()) {
                            systemChanged = true;
                        } else {
							joints[i].x -= movementIncrement* pow(-1, i);
					    	revertLengths(i, oldLengths);//move node back and adjust dimensions
							for (int j = 0; j < numJoints; j++) {
								joints[j].connectionLen = 0;
								for (int con = 0; con < joints[j].connections.size(); con++) {
									joints[j].connectionLen += members[joints[j].connections[con]].length;
								}
							}
							solveGeneralSystem();
					    }
					} else {
						oldLengths = checkIfBetterState(true, i, -movementIncrement* pow(-1, i));
                        if (oldLengths != NULL){
                            if (solveInternal()){
                                systemChanged = true;
                            } else {
                                joints[i].x += movementIncrement*pow(-1, i);
                            	revertLengths(i, oldLengths);
								for (int j = 0; j < numJoints; j++) {
									joints[j].connectionLen = 0;
									for (int con = 0; con < joints[j].connections.size(); con++) {
										joints[j].connectionLen += members[joints[j].connections[con]].length;
									}
								}
								solveGeneralSystem();
                            }
                        }
					}
					if(oldLengths != NULL){
						delete [] oldLengths;
						oldLengths = NULL;
					}

				}

				//both movement directions tested
				//output(cout);
//				system("pause");
			}

			//all joints have been moved
		}
		//system did not change on last iteration
		movementIncrement /= 2;
		cout << "Movement Increment Changed to: " << movementIncrement << endl;
	}
	//movement increment <0.01
}

void Truss::output(ostream & out) const{
	for(int i  = 0; i < numJoints; i++){
		out << "Joint " << i << ":  (" << joints[i].x << ", " << joints[i].y << ")" << endl;
	}
	out << endl;
	double totalLength = 0;
	for(int i  = 0; i < numMembers; i++){
		totalLength += members[i].length;
		out << "Member " << setw(4) << i << ": " << setw(20) << this->validForces[i] << " kN" << "\tLength: " << members[i].length << endl;
	}
	out << endl << "Total Length: " << totalLength << endl;
}

void Truss::makeCSV(ostream & out) const {
	for (int i = 0; i < numJoints; i++) {
		out <<  joints[i].x-0.1 << "," << joints[i].y << endl;
	}
}


/**
 * @date Wed Jan 30 17:25:28 CET 2013
 * @author Ivana Chingovska <ivana.chingovska@idiap.ch>
 *
 * @brief GLCMProp implementation
 *
 * Copyright (C) Idiap Research Institute, Martigny, Switzerland
 */

#include <bob.core/array_copy.h>
#include <bob.core/assert.h>
#include <boost/make_shared.hpp>

#include <bob.ip.base/GLCM.h>

static double sqr(const double x)
{
  return x*x;
}

bob::ip::base::GLCMProp::GLCMProp(){ }

bob::ip::base::GLCMProp::~GLCMProp() { }

const blitz::Array<double,3> bob::ip::base::GLCMProp::normalize_glcm(const blitz::Array<double,3>& glcm) const
{
   blitz::firstIndex i;
   blitz::secondIndex j;
   blitz::thirdIndex k;
   blitz::Array<double, 2> summations_temp(blitz::sum(glcm(i, k, j), k));
   blitz::Array<double, 1> summations(blitz::sum(summations_temp(j,i), j));
   blitz::Array<double,3> res(glcm / summations(k));
   return res;
}

const blitz::TinyVector<int,1> bob::ip::base::GLCMProp::get_prop_shape(const blitz::Array<double,3>& glcm) const
{
  blitz::TinyVector<int,1> res;
  res(0) = glcm.extent(2);
  return res;
}

void bob::ip::base::GLCMProp::angular_second_moment(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = blitz::pow2(glcm_norm(rall, rall, l));
    prop(l) = blitz::sum(mat); // angular second moment
  }
}

void bob::ip::base::GLCMProp::energy(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  //do the computation of the feature
  angular_second_moment(glcm, prop);
  prop = blitz::sqrt(prop);
}

void bob::ip::base::GLCMProp::variance(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    prop(l) = blitz::sum(sqr(i-blitz::mean(mat))*mat);
  }
}

void bob::ip::base::GLCMProp::contrast(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    prop(l) = blitz::sum((i-j)*(i-j)*mat);
  }
  /*
  //as done in [1]
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    double contrast = 0;
    for (int t=0; t < glcm_norm.extent(0) - 1; ++t) // iterate through all the levels
    {
      contrast += t*t*blitz::sum(blitz::where(abs(i-j)==t, mat, 0));
    }
    prop(l) = contrast;
  }
  */
}

void bob::ip::base::GLCMProp::auto_correlation(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    prop(l) = blitz::sum(i*j*mat);
  }
}

void bob::ip::base::GLCMProp::correlation(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    double mean_x = blitz::sum(i*mat);
    double mean_y = blitz::sum(j*mat);
    double std_x = sqrt(blitz::sum(sqr(i-mean_x)*mat));
    double std_y = sqrt(blitz::sum(sqr(j-mean_y)*mat));
    prop(l) = (blitz::sum(i*j*mat) - mean_x*mean_y) / (std_x * std_y);
  }
}

void bob::ip::base::GLCMProp::correlation_m(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    double mean_x = blitz::sum(i*mat);
    double mean_y = blitz::sum(j*mat);
    double std_x = sqrt(blitz::sum(sqr(i-mean_x)*mat));
    double std_y = sqrt(blitz::sum(sqr(j-mean_y)*mat));
    prop(l) = blitz::sum(((i-mean_x) * (j-mean_x) * mat) / (std_x * std_y));
  }
}

void bob::ip::base::GLCMProp::inv_diff_mom(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    prop(l) = blitz::sum(mat / (1 + sqr(i-j)));
  }
}

void bob::ip::base::GLCMProp::sum_avg(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    double sum_avg = 0;
    for (int t = 0; t < 2 * glcm_norm.extent(0) - 1; t++) // iterate through all the levels
    {
      sum_avg += t * blitz::sum(blitz::where(i+j==t, mat, 0));
    }
    prop(l) = sum_avg;
  }
}

void bob::ip::base::GLCMProp::sum_var(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  blitz::Array<double,1>& prop_sum_entropy(prop);
  sum_entropy(glcm, prop_sum_entropy);
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    double sum_var = 0;
    for (int t = 0; t < 2 * glcm_norm.extent(0) -1; t++) // iterate through all the levels
    {
      sum_var += sqr(t-prop_sum_entropy(l)) * blitz::sum(blitz::where(i+j==t, mat, 0));
    }
    prop(l) = sum_var;
  }
}

void bob::ip::base::GLCMProp::sum_entropy(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    double sum_entropy = 0;
    for (int t = 0; t < 2 * glcm_norm.extent(0) - 1; t++) // iterate through all grey levels
    {
      sum_entropy += blitz::sum(blitz::where(i+j==t, mat, 0)) * log(blitz::sum(blitz::where(i+j==t, mat, 0)) + std::numeric_limits<double>::min());
    }
    prop(l) = -sum_entropy;
  }
}

void bob::ip::base::GLCMProp::entropy(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    prop(l) = -blitz::sum(mat * blitz::log(mat + std::numeric_limits<double>::min())); // small numeric value is added to avoid 0 as an argument to the logarithm
  }
}

void bob::ip::base::GLCMProp::diff_var(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    double diff_var = 0;
    for (int t = 0; t < glcm_norm.extent(0); t++) // iterate through all grey levels
    {
      diff_var +=  t * t * blitz::sum(blitz::where(abs(i-j)==t, mat, 0));
    }
    prop(l) = diff_var;
  }
}

void bob::ip::base::GLCMProp::diff_entropy(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    double diff_entropy = 0;
    for (int t = 0; t < glcm_norm.extent(0); t++) // iterate through all grey levels
    {
      diff_entropy += blitz::sum(blitz::where(abs(i-j)==t, mat, 0)) * log(blitz::sum(blitz::where(abs(i-j)==t, mat, 0)) + std::numeric_limits<double>::min());
    }
    prop(l) = -diff_entropy;
  }
}

void bob::ip::base::GLCMProp::dissimilarity(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    prop(l) = blitz::sum(abs(i-j)*mat);
  }
}

void bob::ip::base::GLCMProp::homogeneity(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    prop(l) = blitz::sum(mat / (1 + abs(i-j)));
  }
}

void bob::ip::base::GLCMProp::cluster_prom(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    double mean_x = blitz::sum(i*mat);
    double mean_y = blitz::sum(j*mat);
    prop(l) = blitz::sum(pow(i + j - mean_x - mean_y, 4) * mat);
  }
}

void bob::ip::base::GLCMProp::cluster_shade(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    double mean_x = blitz::sum(i*mat);
    double mean_y = blitz::sum(j*mat);
    prop(l) = blitz::sum(pow(i + j - mean_x - mean_y, 3) * mat);
  }
}

void bob::ip::base::GLCMProp::max_prob(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    prop(l) = blitz::max(mat);
  }
}

void bob::ip::base::GLCMProp::inf_meas_corr1(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  blitz::Array<double,1>& prop_entropy(prop);
  entropy(glcm, prop_entropy); //calculate the entropy
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);

    blitz::Array<double,1> marg_prob_i(blitz::sum(mat,j)); // marginal probability of first dimension (i.e. row-wise sum)
    blitz::Array<double,1> marg_prob_j(blitz::sum(mat(j,i),j)); // marginal probability of second dimension (i.e. column-wise sum)

    double hxy1 = -blitz::sum(mat * blitz::log(marg_prob_i(i) * marg_prob_j(j) + std::numeric_limits<double>::min())); // small numeric value is added to avoid 0 as an argument to the logarithm
    double px_entropy = -blitz::sum(marg_prob_i * blitz::log(marg_prob_i + std::numeric_limits<double>::min()));
    double py_entropy = -blitz::sum(marg_prob_j * blitz::log(marg_prob_j + std::numeric_limits<double>::min()));
    prop(l) = (prop_entropy(l) - hxy1) / std::max(px_entropy, py_entropy);
  }
}

void bob::ip::base::GLCMProp::inf_meas_corr2(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{

  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  blitz::Array<double,1>& prop_entropy(prop);
  entropy(glcm, prop_entropy); //calculate the entropy
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    blitz::Array<double,1> marg_prob_i(blitz::sum(mat,j)); // marginal probability of first dimension (i.e. row-wise sum)
    blitz::Array<double,1> marg_prob_j(blitz::sum(mat(j,i),j)); // marginal probability of second dimension (i.e. column-wise sum)

    double hxy2 = -blitz::sum(marg_prob_i(i) * marg_prob_j(j) * blitz::log(marg_prob_i(i) * marg_prob_j(j) + std::numeric_limits<double>::min())); // small numeric value is added to avoid 0 as an argument to the logarithm
    prop(l) = sqrt(1 - exp(-2 * (hxy2 - prop_entropy(l))));
  }

}

void bob::ip::base::GLCMProp::inv_diff(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  homogeneity(glcm, prop);
}

void bob::ip::base::GLCMProp::inv_diff_norm(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    prop(l) = blitz::sum(mat / (1 + (abs(i-j) / (double)mat.extent(0)) ));
  }
}

void bob::ip::base::GLCMProp::inv_diff_mom_norm(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    prop(l) = blitz::sum(mat / (1 + (sqr(i-j) / sqr(mat.extent(0)))));
  }
}


#ifndef BFGS_HPP_
#define BFGS_HPP_

#include <Eigen/Core>

/**
 * @file BFGS.hpp
 */

namespace rwlibs { namespace algorithms {

    /** @addtogroup algorithms */
    /*@{*/

    /**
     * @brief BFGS is a class including the BFGS minimization algorithm.
     *
     * The BFGS minimization algorithm has been implemented as described in "Numerical Optimization
     * - by Jorge Nocedal and Stephen J. Wright" chapter 6+3. \sa \ref bfgsExample.cpp
     * "../example/bfgsApp/bfgsExample.cpp"
     */
    class BFGS
    {
      public:
        //! Vector type used in the minimazation algorithm.
        typedef Eigen::VectorXd vector;
        //! MAtrix type used in the minimazation algorithm.
        typedef Eigen::MatrixXd matrix;

        /**
         * @brief Minimisation function struct.
         */
        struct BFGS_function_struct
        {
            /** Function pointer to the static minimization function @f$f(vec{x})@f$. */
            double (*f) (const vector* x, void* params);
            /** Function pointer to the static minimization function @f$df(vec{x})@f$. */
            void (*df) (const vector* x, void* params, vector* g);
            /** Void pointer to optional data that the minimization function might require */
            void* params;
        };

        /**
         * @brief Optimization status.
         */
        enum OPTM_STATUS {
            /** Indicating a problem with the numerical precision when evaluating the gradient. */
            GRADIENTWARNING = 0,
            /** Indicating a successfully minimization. */
            SUCCESS
        };

        /**
         * @brief Minimize a function using the BFGS algorithm.
         * @param startguess Start guess for the minimizer parameters. Replaced with minima solution
         * at end of minimization.
         * @param function BFGS_function_struct including pointers to the minimization function f,
         * df and a void pointer to other data for the minimization function.
         * @param tolerance Indicating when an acceptable minima has been found by evaluating if @f$
         * tolerance>||\Delta f(x)||_2 @f$.
         * @param iterationLimit Maximum number of iterations for the BFGS algorithm.
         * @param initialStepsize Initial step size for the BFGS algorithm.
         * @param c1 Value used to ensure the "strong Wolfe conditions" are satisfied with the value
         * c1. See "Numerical Optimization - by Jorge Nocedal and Stephen J. Wright" chapter 3.
         * Typical value = 1e-4.
         * @param c2 Value used to ensure the "strong Wolfe conditions" are satisfied with the value
         * c2. See "Numerical Optimization - by Jorge Nocedal and Stephen J. Wright" chapter 3.
         * Typical value = 0.9.
         * @param alphamax Maximum stepsize used in iterations. Typical value of 1.0 is used to
         * produce superlinear convergence of the overall algorithm.
         * @return GRADIENTWARNING on numerically precision problems SUCCESS when a minima is found.
         */
        static int optimizer (vector& startguess, BFGS_function_struct function, double tolerance,
                              unsigned int iterationLimit, double initialStepsize = 1.0,
                              double c1 = 1e-4, double c2 = 0.9, double alphamax = 1.0);

      private:
        BFGS () {}

        static void colDotRow (vector& colvec, vector& rowvec, matrix& result);

        static double lineSearch (BFGS_function_struct function, vector& xk, vector& pk, double c1,
                                  double c2, double alphamax);

        static double phiGradient (BFGS_function_struct function, vector& xk, vector& pk,
                                   double alpha, vector& tempArray, double phi_alpha, double eps);

        static double zoom (double& alphalow, double& alphahigh, double& phi_alphalow,
                            double& dphi_alphalow, double& phi_alphahigh,
                            BFGS_function_struct function, vector& xk, vector& pk,
                            vector& tempArray, double phi_alpha_zero, double dphi_alpha_zero,
                            double c1, double c2, double eps);

        static double quadraticInterpolation (double phi_alpha_lo, double dphi_alpha_lo,
                                              double alpha_lo, double phi_alpha_hi,
                                              double alpha_hi);
    };
    /** \example bfgsExample.cpp
     * Example of using the BFGS optimization algorithm for finding a minimum in the Rosenbrock
     * function.
     *
     * The Rosenbrock function are defined by: @f$ f(x, y) = (1-x)^2 + 100(y-x^2)^2 @f$
     */

    /*@}*/
}}     // namespace rwlibs::algorithms
#endif /* BFGS_HPP_ */


/**
 * BRIValue (Boost Rational with Infinity Value) class for Discrete Event Simulation purposes
 * Copyright (C) 2016  Laouen Mayal Louan Belloli
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU 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/>.
 */

#ifndef BRIVALUE_H
#define BRIVALUE_H

#include <iostream>
#include <string>
#include <boost/rational.hpp>

using namespace std;

class BRIValue {

  private:
    boost::rational<int> _value;
    bool _inf;
    bool _possitive;

  public:
    BRIValue() : _inf(false) {}
    BRIValue(int n) : _value(n), _inf(false) {}
    BRIValue(int n, int d) : _value(n,d), _inf(false) {}

    static BRIValue infinity() noexcept {
      BRIValue f;
      f._inf=true;
      f._possitive=true;
      return f;
    }

    static BRIValue minusInfinity() noexcept {
      BRIValue f;
      f._inf=true;
      f._possitive=false;
      return f;
    }

    BRIValue& operator=(const BRIValue& o) noexcept {
      this->_value = o._value;
      this->_inf = o._inf;
      this->_possitive = o._possitive;
      return *this;
    }

    /* Aritmetical operators */

    BRIValue& operator+=(const BRIValue& o) noexcept {
      this->_value += o._value;
      if (o._inf) {
        this->_inf = o._inf;
        this->_possitive = o._possitive; // + * {+/-} = {+/-}
      };
      return *this;
    }

    BRIValue& operator-=(const BRIValue& o) noexcept {
      this->_value -= o._value;
      if (!this->_inf && o._inf) {
        this->_inf = o._inf;
        this->_possitive = !o._possitive; // - * {+/-} = {-/+}
      } else if (o._inf && (this->_possitive == o._possitive)) { //-inf-(-inf) = -inf+inf = 0 = inf-inf
        this->_inf = false;
        this->_value = boost::rational<int>(0);
      }
      return *this;
    }

    BRIValue& operator/=(const BRIValue& o) noexcept {
      if (!this->inf) {
        if (o._inf) {
          this->_value = boost::rational<int>(0);
        } else {
          this->_value /= o._value;
        }
      }
      return *this;
    }

    BRIValue& operator*=(const BRIValue& o) noexcept {
      if (!this->inf && o._inf) {
        this->_inf = o._inf;
        this->_possitive = o._possitive;
      } else if (o._inf && (this->_possitive == o._possitive)) { // (+ * + = -) and (- * - = +)
        this->_possitive = !this->_possitive;
      } else {
        this->_value *= o._value;
      }
      return *this;
    }

    BRIValue& operator--() noexcept {
      this->_value -= boost::rational<int>(1);
      return *this;
    }

    BRIValue& operator++() noexcept {
      this->_value += boost::rational<int>(1);
      return *this;
    }

    string naturalDisplay() {
      if (this->_inf) {
        if (this->_possitive)
          return "inf";
        else
          return "-inf";
      }

      return to_string(_value.numerator()) + "/" + to_string(_value.denominator());
    }
};

inline BRIValue operator+(const BRIValue lhs, const BRIValue& rhs) noexcept {
  BRIValue res = lhs;
  res += rhs;
  return res;
}

inline BRIValue operator-(const BRIValue lhs, const BRIValue& rhs) noexcept {
  BRIValue res = lhs;
  res -= rhs;
  return res;
}

inline BRIValue operator/(const BRIValue lhs, const BRIValue& rhs) noexcept {
  BRIValue res = lhs;
  res /= rhs;
  return res;
}

inline bool operator==(const BRIValue& lhs, const BRIValue& rhs) noexcept {

  if (lhs._inf && rhs._inf) return (lhs._possitive == rhs._possitive);
  else if (lhs._inf || rhs._inf) return false;
  return (lhs._value == rhs._value);
}

inline bool operator!=(const BRIValue& lhs, const BRIValue& rhs) noexcept {
  return !operator==(lhs,rhs);
}

inline bool operator<(const BRIValue& lhs, const BRIValue& rhs) noexcept {
  if (lhs._inf && lhs._possitive) return false;
  else if (lhs._inf && !lhs._possitive) return !(rhs._inf && !rhs._possitive);
  else if (rhs._inf && rhs._possitive) return true;
  else if (rhs._inf) return false;
  return (lhs._value < rhs._value);
}

inline bool operator>(const BRIValue& lhs, const BRIValue& rhs) noexcept {
  return  operator< (rhs,lhs);
}

inline bool operator<=(const BRIValue& lhs, const BRIValue& rhs) noexcept {
  return !operator> (lhs,rhs);
}

inline bool operator>=(const BRIValue& lhs, const BRIValue& rhs) noexcept {
  return !operator< (lhs,rhs);
}

inline std::ostream& operator<<(std::ostream& os, const BRIValue& t) noexcept {

  if (t._inf) {
    if (t._possitive)
      os << "inf";
    else
      os << "-inf";
  } else {
    os << t._value;
  }
  return os;
}

inline std::istream& operator>>std::istream& is, BRIValue& rhs) noexcept {
  string a;
  int n,d;
  is >> a;
  if (a == "inf") rhs = BRIValue::infinity();
  else if (a == "-inf") = BRIValue::minusInfinity();
  else {
    n = std::stoi(a.substr(0, a.find_last_of("/")));
    d = std::stoi(a.substr(a.find_last_of("/")+1));
    rhs = BRIValue(n,d);
  }
  return is;
}

  //TODO: Chack this specialization
  // Specialize numeric_limits
namespace std {
  template<>
  class numeric_limits<BRIValue>{
  public:
    static constexpr bool is_specialized = true;
    static BRIValue min() noexcept { return BRIValue(-1,1) * BRIValue{numeric_limits<int>::max(), numeric_limits<int>::min()}; }
    static BRIValue max() noexcept { return BRIValue{numeric_limits<int>::max(), numeric_limits<int>::min()}; }
    static BRIValue lowest() noexcept { return BRIValue(-1,1) * BRIValue{numeric_limits<int>::max(), numeric_limits<int>::min()}; }

    static constexpr bool is_signed = true;
    static constexpr bool is_integer = false;
    static constexpr bool is_exact = true;
    static BRIValue epsilon() noexcept { return BRIValue{1,1} - BRIValue{numeric_limits<int>::max(), numeric_limits<int>::max() - 1}; }
    static BRIValue round_error() noexcept { return BRIValue(0); }

    static constexpr int  min_exponent = numeric_limits<int>::min(); // trash_value
    static constexpr int  min_exponent10 = min_exponent/radix; // trash_value
    static constexpr int  max_exponent = numeric_limits<int>::max(); // trash_value
    static constexpr int  max_exponent10 = max_exponent/radix; // trash_value

    static constexpr bool has_infinity = true;
    static constexpr bool has_quiet_NaN = false;
    static constexpr bool has_signaling_NaN = false;
    static constexpr float_denorm_style has_denorm = denorm_indeterminate;
    static constexpr bool has_denorm_loss = false;
    static BRIValue infinity() noexcept { return BRIValue::infinity(); }

    static constexpr bool is_iec559 = false;
    static constexpr bool is_bounded = false;
    static constexpr bool is_modulo = false;

    static constexpr bool traps = false;
    static constexpr bool tinyness_before = false;
  };
}

#endif // BRIVALUE_H


//=========================================================================
//
// Copyright 2019 Kitware, Inc.
// Author: Guilbert Pierre (spguilbert@gmail.com)
// Data: 03-27-2019
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//=========================================================================

// LOCAL
#include "CameraProjection.h"
#include "vtkEigenTools.h"

// STD
#include <iostream>
#include <fstream>
#include <sstream>

// BOOST
#include <boost/algorithm/string.hpp>

//-----------------------------------------------------------------------------
Eigen::Vector3d GetRGBColourFromReflectivity(double v, double vmin, double vmax)
{
   Eigen::Vector3d c(1.0, 1.0, 1.0); // white
   double dv;
   if (v < vmin)
      v = vmin;
   if (v > vmax)
      v = vmax;
   dv = vmax - vmin;

   if (v < (vmin + 0.25 * dv)) {
      c[2] = 0;
      c[1] = 4 * (v - vmin) / dv;
   } else if (v < (vmin + 0.5 * dv)) {
      c[2] = 0;
      c[0] = 1 + 4 * (vmin + 0.25 * dv - v) / dv;
   } else if (v < (vmin + 0.75 * dv)) {
      c[2] = 4 * (v - vmin - 0.5 * dv) / dv;
      c[0] = 0;
   } else {
      c[1] = 1 + 4 * (vmin + 0.75 * dv - v) / dv;
      c[0] = 0;
   }
   return 255.0 * c;
}

//----------------------------------------------------------------------------
void LoadCameraParamsFromCSV(std::string filename, Eigen::VectorXd& W)
{
  // Load file and check that the file is opened
  std::ifstream file(filename.c_str());
  if (!file.is_open())
  {
    std::cout << "Error: could not load file: " << filename << std::endl;
    return;
  }

  std::string line;
  std::getline(file, line);
  std::vector<std::string> values;
  boost::algorithm::split(values, line, boost::is_any_of(","));
  // initialize the parameters
  W = Eigen::VectorXd(values.size(), 1);
  for (int i = 0; i < values.size(); ++i)
  {
    W(i) = std::atof(values[i].c_str());
  }
  return;
}

//----------------------------------------------------------------------------
void WriteCameraParamsCSV(std::string filename, Eigen::VectorXd& W)
{
  // Load file and check that the file is opened
  std::ofstream file(filename.c_str());
  if (!file.is_open())
  {
    std::cout << "Error: could not open file: " << filename << std::endl;
    return;
  }
  for (int i = 0; i < W.size(); ++i)
  {
    file << W(i) << ",";
  }
  file.close();
  return;
}

//----------------------------------------------------------------------------
Eigen::Vector2d FisheyeProjection(const Eigen::Matrix<double, 15, 1>& W,
                                  const Eigen::Vector3d& X,
                                  bool shouldClip)
{
  // Get rotation matrix
  Eigen::Matrix3d R = RollPitchYawToMatrix(W(0), W(1), W(2));
  Eigen::Vector3d T(W(3), W(4), W(5));

  // Express the 3D point in the camera reference frame
  Eigen::Vector3d Xcam = R.transpose() * (X - T);

  // check that the point is not behind the camera plane
  if (shouldClip && (Xcam(2) < 0))
  {
    return Eigen::Vector2d(-1, -1);
  }

  // Project the 3D point in the plan
  Eigen::Vector2d Xp1(Xcam(0) / Xcam(2), Xcam(1) / Xcam(2));

  // Undistorded the projected image
  double r = Xp1.norm();
  double theta = std::atan(r);
  double thetad = theta * (1 + W(11) * std::pow(theta, 2) + W(12) * std::pow(theta, 4) +
                           W(13) * std::pow(theta, 6) + W(14) * std::pow(theta, 8));
   Eigen::Vector2d Xp1d = (thetad / r) * Xp1;

   // Create current intrinsic parameters
   Eigen::Matrix3d K = Eigen::Matrix3d::Zero();
   K(0, 0) = W(6);
   K(1, 1) = W(7);
   K(0, 2) = W(8);
   K(1, 2) = W(9);
   K(0, 1) = W(10);
   K(2, 2) = 1;

   // Express the point in the pixel coordinates
   Eigen::Vector3d Xp1dh(Xp1d(0), Xp1d(1), 1);
   Eigen::Vector3d Xpix = K * Xp1dh;
   return Eigen::Vector2d(Xpix(0) / Xpix(2), Xpix(1) / Xpix(2));
}

//----------------------------------------------------------------------------
Eigen::Vector2d BrownConradyPinholeProjection(const Eigen::Matrix<double, 17, 1>& W,
                                              const Eigen::Vector3d& X,
                                              bool shouldClip)
{
  // Get rotation matrix
  Eigen::Matrix3d R = RollPitchYawToMatrix(W(0), W(1), W(2));
  Eigen::Vector3d T(W(3), W(4), W(5));

  // Express the 3D point in the camera reference frame
  Eigen::Vector3d Xcam = R.transpose() * (X - T);

  // check that the point is not behind the camera plane
  if (shouldClip && (Xcam(2) < 0))
  {
    return Eigen::Vector2d(-1, -1);
  }

  // Project the 3D point in the plan
  Eigen::Vector2d Xp1(Xcam(0) / Xcam(2), Xcam(1) / Xcam(2));

  // Undistorded the projected image
  double r = Xp1.norm();
  double k1 = W(11); double k2 = W(12);
  double p1 = W(13); double p2 = W(14);
  double p3 = W(15); double p4 = W(16);

  double xdist = Xp1(0) + Xp1(0) * (k1 * std::pow(r, 2) + k2 * std::pow(r, 4)) +
                 (p1 * (std::pow(r, 2) + 2 * std::pow(Xp1(0), 2)) +
                  2 * p2 * Xp1(0) * Xp1(1)) * (1 + p3 * std::pow(r, 2) + p4 * std::pow(r, 4));
  double ydist = Xp1(1) + Xp1(1) * (k1 * std::pow(r, 2) + k2 * std::pow(r, 4)) +
                 (2 * p1 * Xp1(0) * Xp1(1) + p2 * (std::pow(r, 2) + 2 * std::pow(Xp1(1), 2))) *
                 (1 + p3 * std::pow(r, 2) + p4 * std::pow(r, 4));
  Eigen::Vector2d Xp1d(xdist, ydist);

   // Create current intrinsic parameters
   Eigen::Matrix3d K = Eigen::Matrix3d::Zero();
   K(0, 0) = W(6);
   K(1, 1) = W(7);
   K(0, 2) = W(8);
   K(1, 2) = W(9);
   K(0, 1) = W(10);
   K(2, 2) = 1;

   // Express the point in the pixel coordinates
   Eigen::Vector3d Xp1dh(Xp1d(0), Xp1d(1), 1);
   Eigen::Vector3d Xpix = K * Xp1dh;
   return Eigen::Vector2d(Xpix(0) / Xpix(2), Xpix(1) / Xpix(2));
}


/********************************************************************************
*  This file is part of CinoLib                                                 *
*  Copyright(C) 2016: Marco Livesu                                              *
*                                                                               *
*  The MIT License                                                              *
*                                                                               *
*  Permission is hereby granted, free of charge, to any person obtaining a      *
*  copy of this software and associated documentation files (the "Software"),   *
*  to deal in the Software without restriction, including without limitation    *
*  the rights to use, copy, modify, merge, publish, distribute, sublicense,     *
*  and/or sell copies of the Software, and to permit persons to whom the        *
*  Software is furnished to do so, subject to the following conditions:         *
*                                                                               *
*  The above copyright notice and this permission notice shall be included in   *
*  all copies or substantial portions of the Software.                          *
*                                                                               *
*  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR   *
*  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,     *
*  FITNESS FOR A PARTICULAR PURPOSE AND NON INFRINGEMENT. IN NO EVENT SHALL THE *
*  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER       *
*  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING      *
*  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS *
*  IN THE SOFTWARE.                                                             *
*                                                                               *
*  Author(s):                                                                   *
*                                                                               *
*     Marco Livesu (marco.livesu@gmail.com)                                     *
*     http://pers.ge.imati.cnr.it/livesu/                                       *
*                                                                               *
*     Italian National Research Council (CNR)                                   *
*     Institute for Applied Mathematics and Information Technologies (IMATI)    *
*     Via de Marini, 6                                                          *
*     16149 Genoa,                                                              *
*     Italy                                                                     *
*********************************************************************************/
#include <cinolib/matrix.h>
#include <Eigen/Eigenvalues>

namespace cinolib
{

// http://www.math.harvard.edu/archive/21b_fall_04/exhibits/2dmatrices/index.html
//
CINO_INLINE
void eigen_decomposition_2x2(const float   a00,
                             const float   a01,
                             const float   a10,
                             const float   a11,
                                   vec2f  & v_min, // eigenvectors
                                   vec2f  & v_max,
                                   float & min,   // eigenvalues
                                   float & max)
{
    eigenvalues_2x2(a00,a01,a10,a11,min,max);

    if(std::fabs(a10)>1e-5)
    {
        v_max = vec2f(max-a11,a10);
        v_min = vec2f(min-a11,a10);
    }
    else if(std::fabs(a01)>1e-5)
    {
        v_max = vec2f(a01,max-a00);
        v_min = vec2f(a01,min-a00);
    }
    else
    {
        v_max = (a00>=a11) ? vec2f(1,0) : vec2f(0,1);
        v_min = (a00>=a11) ? vec2f(0,1) : vec2f(1,0);
    }

    v_max.normalize();
    v_min.normalize();
}

// http://www.math.harvard.edu/archive/21b_fall_04/exhibits/2dmatrices/index.html
//
CINO_INLINE
void eigenvalues_2x2(const float   a00,
                     const float   a01,
                     const float   a10,
                     const float   a11,
                           float & min,
                           float & max)
{
    float T = a00 + a11; // trace
    float D = determinant_2x2(a00,a01,a10,a11);

    min = T/2.0 - sqrt(T*T/4.0-D);
    max = T/2.0 + sqrt(T*T/4.0-D);
}

CINO_INLINE
void eigenvectors_2x2(const float   a00,
                      const float   a01,
                      const float   a10,
                      const float   a11,
                            vec2f  & v_min,
                            vec2f  & v_max)
{
    float min, max;
    eigen_decomposition_2x2(a00, a01, a10, a11, v_min, v_max, min, max);
}

CINO_INLINE
float determinant_2x2(const float a00, const float a01, const float a10, const float a11)
{
    return ((a00*a11) - (a10*a01));
}

CINO_INLINE
float determinant_2x2(const vec2f a0, const vec2f a1)
{
    return determinant_2x2(a0[0], a0[1], a1[0], a1[1]);
}

CINO_INLINE
void eigen_decomposition_3x3(const float   a[3][3],
                                   vec3f  & v_min, // eigenvectors
                                   vec3f  & v_mid,
                                   vec3f  & v_max,
                                   float & min,   // eigenvalues
                                   float & mid,
                                   float & max)
{
    eigen_decomposition_3x3(a[0][0], a[0][1], a[0][2],
                            a[1][0], a[1][1], a[1][2],
                            a[2][0], a[2][1], a[2][2],
                            v_min, v_mid, v_max,
                            min, mid, max);
}

CINO_INLINE
void eigen_decomposition_3x3(const float   a00,
                             const float   a01,
                             const float   a02,
                             const float   a10,
                             const float   a11,
                             const float   a12,
                             const float   a20,
                             const float   a21,
                             const float   a22,
                                   vec3f  & v_min, // eigenvectors
                                   vec3f  & v_mid,
                                   vec3f  & v_max,
                                   float & min,   // eigenvalues
                                   float & mid,
                                   float & max)
{
    Eigen::Matrix3d m;
    m << a00, a01, a02,
         a10, a11, a12,
         a20, a21, a22;

    bool symmetric = (a10==a01) && (a20==a02) && (a21==a12);

    if(symmetric)
    {
        // eigen decomposition for self-adjoint matrices
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eig(m);
        assert(eig.info() == Eigen::Success);

        v_min = vec3f(eig.eigenvectors()(0,0), eig.eigenvectors()(1,0), eig.eigenvectors()(2,0));
        v_mid = vec3f(eig.eigenvectors()(0,1), eig.eigenvectors()(1,1), eig.eigenvectors()(2,1));
        v_max = vec3f(eig.eigenvectors()(0,2), eig.eigenvectors()(1,2), eig.eigenvectors()(2,2));

        min = eig.eigenvalues()[0];
        mid = eig.eigenvalues()[1];
        max = eig.eigenvalues()[2];
    }
    else
    {
        // eigen decomposition for general matrices
        Eigen::EigenSolver<Eigen::Matrix3d> eig(m);
        assert(eig.info() == Eigen::Success);

        // WARNING: I am taking only the real part!
        v_min = vec3f(eig.eigenvectors()(0,0).real(), eig.eigenvectors()(1,0).real(), eig.eigenvectors()(2,0).real());
        v_mid = vec3f(eig.eigenvectors()(0,1).real(), eig.eigenvectors()(1,1).real(), eig.eigenvectors()(2,1).real());
        v_max = vec3f(eig.eigenvectors()(0,2).real(), eig.eigenvectors()(1,2).real(), eig.eigenvectors()(2,2).real());

        // WARNING: I am taking only the real part!
        min = eig.eigenvalues()[0].real();
        mid = eig.eigenvalues()[1].real();
        max = eig.eigenvalues()[2].real();
    }
}

CINO_INLINE
void eigenvalues_3x3(const float   a00,
                     const float   a01,
                     const float   a02,
                     const float   a10,
                     const float   a11,
                     const float   a12,
                     const float   a20,
                     const float   a21,
                     const float   a22,
                           float & min,
                           float & mid,
                           float & max)
{
    vec3f v_min, v_mid, v_max;
    eigen_decomposition_3x3(a00, a01, a02, a10, a11, a12, a20, a21, a22, v_min, v_mid, v_max, min, mid, max);
}

CINO_INLINE
void eigenvectors_3x3(const float   a00,
                      const float   a01,
                      const float   a02,
                      const float   a10,
                      const float   a11,
                      const float   a12,
                      const float   a20,
                      const float   a21,
                      const float   a22,
                            vec3f  & v_min,
                            vec3f  & v_mid,
                            vec3f  & v_max)
{
    float min, mid, max;
    eigen_decomposition_3x3(a00, a01, a02, a10, a11, a12, a20, a21, a22, v_min, v_mid, v_max, min, mid, max);
}

CINO_INLINE
float determinant_3x3(const float a00, const float a01, const float a02,
                       const float a10, const float a11, const float a12,
                       const float a20, const float a21, const float a22)
{
    return a00 * determinant_2x2(a11, a12, a21, a22) -
           a01 * determinant_2x2(a10, a12, a20, a22) +
           a02 * determinant_2x2(a10, a11, a20, a21);
}

CINO_INLINE
void from_std_3x3_to_Eigen_3x3(const float stdM[3][3], Eigen::Matrix3d & eigenM)
{
    eigenM.coeffRef(0,0) = stdM[0][0];  eigenM.coeffRef(0,1) = stdM[0][1];  eigenM.coeffRef(0,2) = stdM[0][2];
    eigenM.coeffRef(1,0) = stdM[1][0];  eigenM.coeffRef(1,1) = stdM[1][1];  eigenM.coeffRef(1,2) = stdM[1][2];
    eigenM.coeffRef(2,0) = stdM[2][0];  eigenM.coeffRef(2,1) = stdM[2][1];  eigenM.coeffRef(2,2) = stdM[2][2];
}

CINO_INLINE
void from_eigen_3x3_to_std_3x3(const Eigen::Matrix3d & eigenM, float stdM[3][3])
{
    stdM[0][0] = eigenM.coeffRef(0,0);  stdM[0][1] = eigenM.coeffRef(0,1);  stdM[0][2] = eigenM.coeffRef(0,2);
    stdM[1][0] = eigenM.coeffRef(1,0);  stdM[1][1] = eigenM.coeffRef(1,1);  stdM[1][2] = eigenM.coeffRef(1,2);
    stdM[2][0] = eigenM.coeffRef(2,0);  stdM[2][1] = eigenM.coeffRef(2,1);  stdM[2][2] = eigenM.coeffRef(2,2);
}
}