#include <iostream>
#include <typeinfo>
#define BOOST_TEST_MODULE SparseMIASolveTests
#include "MIAConfig.h"
#ifdef MIA_USE_HEADER_ONLY_TESTS
#include <boost/test/included/unit_test.hpp>
#else
#include <boost/test/unit_test.hpp>
#endif

#include "SparseMIA.h"
#include "DenseMIA.h"
#include "Index.h"
#include "LibMIAException.h"
#include "LibMIAUtil.h"

template<typename data_type, size_t _order>
void random_mia(LibMIA::DenseMIA<data_type,_order> & mia,double _prob,bool need_ranked=true)
{
    boost::uniform_real<> uni_dist(0,1);
    boost::variate_generator<boost::random::mt19937&, boost::uniform_real<> > uni(LibMIA::LibMIA_gen(), uni_dist);
    boost::uniform_real<> uni_dist2(-10,10);
    boost::variate_generator<boost::random::mt19937&, boost::uniform_real<> > uni2(LibMIA::LibMIA_gen(), uni_dist2);
    mia.zeros();
    for(auto it =mia.data_begin(); it<mia.data_end(); ++it)
    {

        if(uni()<_prob)
        {
            *it=uni2();
        }

    }
}

template<class _data_type>
void solve_work(size_t dim1, size_t dim2){

    LibMIA::MIAINDEX i;
    LibMIA::MIAINDEX j;
    LibMIA::MIAINDEX k;
    LibMIA::MIAINDEX l;
    LibMIA::MIAINDEX m;
    LibMIA::MIAINDEX n;
    //LibMIA::MIAINDEX o;
    //LibMIA::MIAINDEX p;

    LibMIA::DenseMIA<_data_type,4> dense_a(dim1,dim1,dim1,dim1);
    LibMIA::DenseMIA<_data_type,4> dense_a2(dim2,dim2,dim1,dim1);
    LibMIA::DenseMIA<_data_type,4> dense_b(dim1,dim1,dim1,dim1);
    LibMIA::DenseMIA<_data_type,4> dense_b2(dim2,dim2,dim1,dim1);
    LibMIA::DenseMIA<_data_type,4> dense_c(dim1,dim1,dim1,dim1);
    LibMIA::DenseMIA<_data_type,4> dense_d2(dim1,dim1,dim1,dim1);

    LibMIA::SparseMIA<_data_type,4> a(dim1,dim1,dim1,dim1);
    LibMIA::SparseMIA<_data_type,4> a2(dim2,dim2,dim1,dim1);
    LibMIA::SparseMIA<_data_type,4> b(dim1,dim1,dim1,dim1);
    LibMIA::SparseMIA<_data_type,4> b2(dim2,dim2,dim1,dim1);
    //result of a solution of sparse equations is set to dense
    LibMIA::DenseMIA<_data_type,4> c(dim1,dim1,dim1,dim1);

    double _prob=0.4;
    bool flag=true;
    random_mia(dense_b,_prob);
    while(flag){
        random_mia(dense_a,_prob);

        dense_c(i,j,m,n)=dense_a(i,j,k,l)|dense_b(k,l,m,n);
        if (dense_c.solveInfo()==LibMIA::FullyRanked)
            flag=false;

    }
    a=dense_a;
    b=dense_b;
    c(i,j,m,n)=a(i,j,k,l)|b(k,l,m,n);
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Inner/Outer Product Inverse 1 for ")+typeid(_data_type).name() );

    flag=true;
    while(flag){

        dense_c(i,j,m,n)=dense_a(i,k,j,l)|dense_b(k,l,m,n);
        if (dense_c.solveInfo()==LibMIA::FullyRanked)
            break;
        random_mia(dense_a,_prob);
    }
    a=dense_a;
    c(i,j,m,n)=a(i,k,j,l)|b(k,l,m,n);

    BOOST_CHECK_MESSAGE(c.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Inner/Outer Product Inverse 2 for ")+typeid(_data_type).name() );

    _prob=0.7;
    random_mia(dense_a,_prob);
    flag=true;
    while(flag){

        dense_c(i,j,l,m)=dense_a(i,!j,k,!l)|dense_b(k,!j,!l,m);
        if (dense_c.solveInfo()==LibMIA::FullyRanked)
            break;

        random_mia(dense_a,_prob);
    }
    a=dense_a;
    c(i,j,l,m)=a(i,!j,k,!l)|b(k,!j,!l,m);

    BOOST_CHECK_MESSAGE(c.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Inner/Outer/Inter Product Inverse 1 for ")+typeid(_data_type).name() );

    flag=true;
    while(flag){

        dense_c(i,j,l,m)=dense_a(!i,!j,k,l)|dense_b(m,!j,k,!i);
        if (dense_c.solveInfo()==LibMIA::FullyRanked)
            break;
        random_mia(dense_a,_prob);
    }
    a=dense_a;
    c(i,j,l,m)=a(!i,!j,k,l)|b(m,!j,k,!i);

    BOOST_CHECK_MESSAGE(c.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Inner/Outer/Inter Product Inverse 2 for ")+typeid(_data_type).name() );

//
//    c(i,j,m,n)=a2(k,l,i,j)|b2(k,l,m,n);
//    //test with normal equations
//    d(o,p,m,n)=a2(k,l,o,p)*a2(k,l,i,j)*c(i,j,m,n);
//    d2(i,j,m,n)=a2(k,l,i,j)*b2(k,l,m,n);
//    BOOST_CHECK_MESSAGE(d.fuzzy_equals(d2,test_precision<_data_type>()),std::string("Inner/Outer Product Least Squares 1 for ")+typeid(_data_type).name() );

}

BOOST_AUTO_TEST_CASE( SparseMIASolveTests )
{

    //solve_work<double>(10,10);
    solve_work<float>(10,10);

}


#include <cmath>
#include "conex/debug_macros.h"
#include "conex/divergence.h"
#include "gtest/gtest.h"
#include <Eigen/Dense>

namespace conex {

using Eigen::MatrixXd;

bool InLimits(double x, double lower, double upper) {
  return x >= lower && x <= upper;
}

double Divergence(Eigen::VectorXd gw, double k) {
  MatrixXd d = k * gw;
  d.array() -= 1;
  double dinf = d.array().abs().maxCoeff();
  return d.squaredNorm() / (1 - dinf);
}

GTEST_TEST(MuSelection, DivergenceBound) {
  int n = 3;
  MatrixXd gw = Eigen::VectorXd::Random(n, 1).array().abs();

  WeightedSlackEigenvalues p;
  p.frobenius_norm_squared = gw.squaredNorm();
  p.lambda_max = gw.maxCoeff();
  p.lambda_min = gw.minCoeff();
  p.trace = gw.sum();
  p.rank = gw.rows();

  // Div bound on one branch.
  double k_ref = 2.0 / (p.lambda_max + p.lambda_min) * .8;
  double hub_desired = Divergence(gw, k_ref);
  EXPECT_NEAR(hub_desired, DivergenceUpperBound(k_ref, p), 1e-8);
  double k = DivergenceUpperBoundInverse(hub_desired, p);
  EXPECT_NEAR(hub_desired, Divergence(gw, k), 1e-8);
  EXPECT_TRUE(k >= k_ref);

  // Div bound on the other branch.
  k_ref = 2.0 / (p.lambda_max + p.lambda_min) * 1.2;
  hub_desired = Divergence(gw, k_ref);
  EXPECT_NEAR(hub_desired, DivergenceUpperBound(k_ref, p), 1e-8);
  k = DivergenceUpperBoundInverse(hub_desired, p);
  EXPECT_NEAR(hub_desired, DivergenceUpperBound(k, p), 1e-8);
  EXPECT_TRUE(k >= k_ref);

  // Div bound undefined.
  k_ref = 1000000;
  hub_desired = Divergence(gw, k_ref);
  EXPECT_NEAR(hub_desired, DivergenceUpperBound(k_ref, p), 1e-8);
  k = DivergenceUpperBoundInverse(hub_desired, p);
  EXPECT_TRUE(DivergenceUpperBound(k, p) < 0);
  EXPECT_EQ(k, -1);
}

}  // namespace conex


#include <stan/math/prim.hpp>
#include <test/unit/math/prim/prob/vector_rng_test_helper.hpp>
#include <test/unit/math/prim/prob/util.hpp>
#include <gtest/gtest.h>
#include <boost/math/distributions.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <limits>
#include <vector>

class LoglogisticTestRig : public VectorRNGTestRig {
 public:
  LoglogisticTestRig()
      : VectorRNGTestRig(10000, 10, {2.5, 1.7, 0.2, 0.1, 2.0},
                         {3, 2, 1, 5, 10, 6}, {-2.5, -1.7, -0.2, -0.1, 0.0},
                         {-3, -2, -1, -4, -10, 0}, {0.1, 1.0, 2.5, 4.0},
                         {1, 2, 3, 4}, {-2.7, -1.5, -0.5, 0.0},
                         {-3, -2, -1, 0}) {}

  template <typename T1, typename T2, typename T3, typename T_rng>
  auto generate_samples(const T1& alpha, const T2& beta, const T3& unused,
                        T_rng& rng) const {
    return stan::math::loglogistic_rng(alpha, beta, rng);
  }
};

double icdf(double x, double alpha, double beta) {
  return alpha * pow(x / (1 - x), 1 / beta);
}

TEST(ProbDistributionsLoglogistic, errorCheck) {
  check_dist_throws_all_types(LoglogisticTestRig());
}

TEST(ProbDistributionsLoglogistic, error_check) {
  boost::random::mt19937 rng;
  EXPECT_NO_THROW(stan::math::loglogistic_rng(10.0, 2.0, rng));

  EXPECT_THROW(stan::math::loglogistic_rng(2.0, -1.0, rng), std::domain_error);
  EXPECT_THROW(stan::math::loglogistic_rng(-2.0, 1.0, rng), std::domain_error);
  EXPECT_THROW(
      stan::math::loglogistic_rng(10, stan::math::positive_infinity(), rng),
      std::domain_error);
  EXPECT_THROW(
      stan::math::loglogistic_rng(stan::math::positive_infinity(), 2, rng),
      std::domain_error);
}

TEST(ProbDistributionsLoglogistic, test_sampling_icdf) {
  for (double p : {0.0, 0.1, 0.2, 0.5, 0.7, 0.9, 0.99}) {
    for (double alpha : {1.11, 0.13, 1.2, 4.67}) {
      for (double beta : {0.11, 1.33, 2.0, 3.2}) {
        double x = icdf(p, alpha, beta);
        EXPECT_FLOAT_EQ(stan::math::loglogistic_cdf(x, alpha, beta), p);
      }
    }
  }
}

TEST(ProbDistributionsLoglogistic, chiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;
  int N = 10000;
  int K = stan::math::round(2 * std::pow(N, 0.4));

  std::vector<double> samples;
  for (int i = 0; i < N; ++i) {
    samples.push_back(stan::math::loglogistic_rng(1.2, 2.0, rng));
  }
  std::vector<double> quantiles;
  for (int i = 1; i < K; ++i) {
    double frac = static_cast<double>(i) / K;
    quantiles.push_back(icdf(frac, 1.2, 2.0));
  }
  quantiles.push_back(std::numeric_limits<double>::max());

  // Assert that they match
  assert_matches_quantiles(samples, quantiles, 1e-6);
}


/*
 * Copyright (c) 2016 Abhishek Agrawal (abhishek.agrawal@protonmail.com)
 * Distributed under the MIT License.
 * See accompanying file LICENSE.md or copy at http://opensource.org/licenses/MIT
 */

#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/generator_iterator.hpp>
#include <boost/random/mersenne_twister.hpp>

#include <vector>

#include "CppProject/randomGen.hpp"

namespace randomGen
{
	typedef double Real;
	typedef std::vector< Real > Vector2;
	typedef std::vector< Real > VectorLong;
	typedef boost::mt19937 generator_type; // mt19937 is a psuedo random number generator algorithm present in the boost library

	void randomGen( const Vector2 range, const int limit, VectorLong& output )
	{
		// generator type defined and initialized by a seed value
		generator_type generator( 900 ); // don't change the seed value while debugging otherwise the psuedo random generated numbers will change
		// define uniform random number distribution which produces type double values
		// between [min,max) for each orbital element. For more details please refer to
		// the following web link:
		// http://www.boost.org/doc/libs/1_60_0/libs/random/example/random_demo.cpp
		boost::uniform_real<> distribution( range[ 0 ], range[ 1 ] );
		boost::variate_generator<generator_type&, boost::uniform_real<> > uniform(generator, distribution);
		for(int i = 0; i < limit; i++)
		{
			output[ i ]	= uniform();
		}
	}

	void randomGenWithSeed( const Vector2 range, const int limit, VectorLong& output, const int seed )
	{
		// generator type defined and initialized by a seed value
		generator_type generator( seed );
		// define uniform random number distribution which produces type double values
		// between [min,max) for each orbital element. For more details please refer to
		// the following web link:
		// http://www.boost.org/doc/libs/1_60_0/libs/random/example/random_demo.cpp
		boost::uniform_real<> distribution( range[ 0 ], range[ 1 ] );
		boost::variate_generator<generator_type&, boost::uniform_real<> > uniform(generator, distribution);
		for(int i = 0; i < limit; i++)
		{
			output[ i ]	= uniform();
		}
	}
} // namespace randomGen


#include <mex.h>
#include <math.h>
#include <Eigen/Dense>
#include <iostream>
#include <stdio.h>

using namespace Eigen;
using namespace std;

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{

    mxArray *vers_mex, *tets_mex;

    char *name;
    double *vers, *tets;

    name = mxArrayToString(prhs[0]);

    mexPrintf("%s\n", name);

    int ver_num, tet_num;
    float x, y, z;
    int i1, i2, i3, i4;

    FILE* tet_file = fopen(name, "rt");

    fscanf(tet_file, "#num of vertex: %d\n", &ver_num);
    fscanf(tet_file, "#num of tetrahedra: %d\n\n", &tet_num);

    MatrixXd ver = MatrixXd::Zero(ver_num, 3);
    MatrixXi tet = MatrixXi::Zero(tet_num, 4);

    for(int i = 0; i < ver_num; i++)
    {
        fscanf(tet_file, "%f %f %f\n", &x, &y, &z);

        ver(i, 0) = x;
        ver(i, 1) = y;
        ver(i, 2) = z;
    }

    fscanf(tet_file, "\n");

    for(int i = 0; i < tet_num; i++)
    {
        fscanf(tet_file, "%d %d %d %d\n", &i1, &i2, &i3, &i4);

        tet(i, 0) = i1;
        tet(i, 1) = i2;
        tet(i, 2) = i3;
        tet(i, 3) = i4;
    }

    fclose(tet_file);

    vers_mex = plhs[0] = mxCreateDoubleMatrix(ver.rows() * 3, 1, mxREAL);
    tets_mex = plhs[1] = mxCreateDoubleMatrix(tet.rows() * 4, 1, mxREAL);

    vers = mxGetPr(vers_mex);
    tets = mxGetPr(tets_mex);

    for(int i = 0; i < ver.rows(); i++)
    {
        vers[i] = ver(i, 0);
        vers[ver.rows() + i] = ver(i, 1);
        vers[2 * ver.rows() + i] = ver(i, 2);
    }

    for(int i = 0; i < tet.rows(); i++)
    {
        tets[i] = tet(i, 0);
        tets[tet.rows() + i] = tet(i, 1);
        tets[2 * tet.rows() + i] = tet(i, 2);
        tets[3 * tet.rows() + i] = tet(i, 3);
    }

    return;

}


#ifndef HTOOL_SPMATRIX_HPP
#define HTOOL_SPMATRIX_HPP

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <cassert>
#include <complex>
#include <iostream>
#include <vector>
//#include "point.hpp"

namespace htool {
//================================//
//      DECLARATIONS DE TYPE      //
//================================//

//typedef vector<Cplx>    vectCplx;

//=================================================================//
//                         CLASS SPARSE MATRIX
/******************************************************************/ /**
* Class for sparse matrices (in coordinate list format).
* Its member objects are:
*   - I: vector of the row indices,
*   - J: vector of the column indices,
*   - K: vector of the (complex) coefficients of the matrix,
*   - nr: the number of rows,
*   - nc: the number of columns.
*********************************************************************/

class SpMatrix {

  private:
    std::vector<int> I, J;
    std::vector<Cplx> K;
    int nr;
    int nc;

  public:
    //! ### Default constructor
    /*!
	 Initializes the matrix to the size 0*0.
  */
    SpMatrix() : nr(0), nc(0) {}

    //! ### Another constructor
    /*!
	 Initializes the matrix with _nrp_ rows and _ncp_ columns,
     _Ip_ as vector of the row indices, _Jp_ as vector of the column indices,
     _Kp_ as vector of the coefficients of the matrix.
  */
    SpMatrix(const std::vector<int> &Ip, const std::vector<int> &Jp, std::vector<Cplx> &Kp, const int &nrp, const int &ncp) : I(Ip), J(Jp), K(Kp), nr(nrp), nc(ncp) {}

    //! ### Copy constructor
    /*!
  */
    SpMatrix(const SpMatrix &A) : I(A.I), J(A.J), K(A.K), nr(A.nr), nc(A.nc) {}

    //! ### Assignement operator with a sparse matrix input argument
    /*!
	 Copies the _I_, _J_, _K_ of the input _A_ argument
	 (which is a sparse matrix) into the vectors of
	 calling instance.
  */
    void operator=(const SpMatrix &A) {
        assert(nr == A.nr && nc == A.nc);
        I = A.I;
        J = A.J;
        K = A.K;
    }

    //! ### Matrix-vector product
    /*!
	 The input parameter _u_ is the input vector
	 (i.e. the right operand).
  */
    vectCplx operator*(const vectCplx &u) {
        int ncoef = I.size();
        vectCplx v(nr, 0.);
        for (int j = 0; j < ncoef; j++)
            v[I[j]] += K[j] * u[J[j]];
        return v;
    }

    //! ### Matrix-vector product
    /*!
	 Another instanciation of the matrix-vector product
	 that avoids the generation of temporary instance for the
	 output vector. This routine achieves the operation

	 lhs = m*rhs

	 The left and right operands (_lhs_ and _rhs_) are templated
	 and can then be of any type (not necessarily of type vectCplx).
  */
    template <typename LhsType, typename RhsType>
    friend void MvProd(LhsType &lhs, const SpMatrix &m, const RhsType &rhs) {
        int ncoef = m.I.size();
        for (int j = 0; j < ncoef; j++)
            lhs[m.I[j]] += m.K[j] * rhs[m.J[j]];
    }

    //! ### Modifies the size of the matrix
    /*!
     Changes the size of the matrix so that
     the number of rows is set to _nbr_ and
     the number of columns is set to _nbc_ and
     the sizes of the 3 member vectors are set to _nbcoef_.
     */
    void resize(const int nbr, const int nbc, const int nbcoef) {
        assert(nbcoef <= nbr * nbc);
        nr = nbr;
        nc = nbc;
        I.resize(nbcoef);
        J.resize(nbcoef);
        K.resize(nbcoef);
    }

    //! ### Access to row indices
    /*!
     Returns the _i_th row index of the input argument _A_.
     */
    int &I_(const int i) {
        assert(i < I.size());
        return I[i];
    }

    //! ### Access to column indices
    /*!
     Returns the _i_th column index of the input argument _A_.
     */
    int &J_(const int i) {
        assert(i < J.size());
        return J[i];
    }

    //! ### Access to coefficients
    /*!
     Returns the _i_th coefficients inside _K_ of the input argument _A_.
     */
    Cplx &K_(const int i) {
        assert(i < K.size());
        return K[i];
    }

    //! ### Access to number of rows
    /*!
	 Returns the number of rows of the input argument _A_.
  */
    friend const int &nb_rows(const SpMatrix &A) { return A.nr; }

    //! ### Access to number of columns
    /*!
	 Returns the number of columns of the input argument _A_.
  */
    friend const int &nb_cols(const SpMatrix &A) { return A.nc; }

    //! ### Access to number of non zero coefficients
    /*!
     Returns the number of non zero coefficients of the input argument _A_.
     */
    friend int nb_coeff(const SpMatrix &A) { return A.I.size(); }

    //! ### Compute the compression rate
    /*!
     1 - number of non zero coefficients/(nb_rows*nb_columns)
     */
    friend Real CompressionRate(const SpMatrix &A) {
        Real comp;
        comp = ((double)A.I.size()) / ((double)(A.nr * A.nc)); // number of non zero coefficients/(nb_rows*nb_columns)
        return (1 - comp);
    }
};

} // namespace htool
#endif


#include <mass.h>

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

BOOST_AUTO_TEST_SUITE(mass);

BOOST_AUTO_TEST_CASE(translate)
{

  mass::Properties<double> B = mass::compute_sphere(1.0,1.0);

  double const dx   = 1.0;
  double const dy   = 2.0;
  double const dz   = 3.0;

  BOOST_CHECK( B.is_body_space() );

  mass::Properties<double> M = mass::translate_to_model_frame( dx, dy, dz, B);

  BOOST_CHECK( M.is_model_space() );

  BOOST_CHECK(M.m_Ixx > B.m_Ixx);
  BOOST_CHECK(M.m_Iyy > B.m_Iyy);
  BOOST_CHECK(M.m_Izz > B.m_Izz);
  BOOST_CHECK(M.m_Ixy < B.m_Ixy);
  BOOST_CHECK(M.m_Ixz < B.m_Ixz);
  BOOST_CHECK(M.m_Iyz < B.m_Iyz);

  BOOST_CHECK_CLOSE(M.m_m, B.m_m, 0.01 );

  BOOST_CHECK_CLOSE(M.m_x, dx, 0.01 );
  BOOST_CHECK_CLOSE(M.m_y, dy, 0.01 );
  BOOST_CHECK_CLOSE(M.m_z, dz, 0.01 );

  BOOST_CHECK_CLOSE(M.m_Qs, 1.0, 0.01 );
  BOOST_CHECK_CLOSE(M.m_Qx, 0.0, 0.01 );
  BOOST_CHECK_CLOSE(M.m_Qy, 0.0, 0.01 );
  BOOST_CHECK_CLOSE(M.m_Qz, 0.0, 0.01 );

  mass::Properties<double> C = mass::translate_to_body_frame( M);

  BOOST_CHECK_CLOSE(C.m_Ixx , B.m_Ixx, 0.01);
  BOOST_CHECK_CLOSE(C.m_Iyy , B.m_Iyy, 0.01);
  BOOST_CHECK_CLOSE(C.m_Izz , B.m_Izz, 0.01);
  BOOST_CHECK_CLOSE(C.m_Ixy , B.m_Ixy, 0.01);
  BOOST_CHECK_CLOSE(C.m_Ixz , B.m_Ixz, 0.01);
  BOOST_CHECK_CLOSE(C.m_Iyz , B.m_Iyz, 0.01);
  BOOST_CHECK_CLOSE(C.m_m   , B.m_m  , 0.01 );
  BOOST_CHECK_CLOSE(C.m_x   , 0.0    , 0.01 );
  BOOST_CHECK_CLOSE(C.m_y   , 0.0    , 0.01 );
  BOOST_CHECK_CLOSE(C.m_z   , 0.0    , 0.01 );
  BOOST_CHECK_CLOSE(C.m_Qs  , 1.0    , 0.01 );
  BOOST_CHECK_CLOSE(C.m_Qx  , 0.0    , 0.01 );
  BOOST_CHECK_CLOSE(C.m_Qy  , 0.0    , 0.01 );
  BOOST_CHECK_CLOSE(C.m_Qz  , 0.0    , 0.01 );

  BOOST_CHECK( C.is_body_space() );
}

BOOST_AUTO_TEST_SUITE_END();


/*
 * Copyright (c) 2019 Opticks Team. All Rights Reserved.
 *
 * This file is part of Opticks
 * (see https://bitbucket.org/simoncblyth/opticks).
 *
 * 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.
 */

#pragma once

//#include <boost/random/mersenne_twister.hpp>
#include <boost/random/inversive_congruential.hpp>

#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>

#include <glm/fwd.hpp>

#include "NBBox.hpp"

#include "NPY_API_EXPORT.hh"

class NPY_API NGenerator
{
    //typedef boost::mt19937          RNG_t;
    typedef boost::hellekalek1995   RNG_t ;
    typedef boost::uniform_real<>   Distrib_t;
    typedef boost::variate_generator< RNG_t, Distrib_t > Generator_t ;

    public:
        NGenerator(const nbbox& bb);
        void operator()(nvec3& xyz);
        void operator()(glm::vec3& xyz);
    private:
        nbbox m_bb ;
        nvec3 m_side ;
        RNG_t m_rng;
        Distrib_t m_dist ;
        Generator_t m_gen ;
};


// author: Danny Rakita

#include <boost/python.hpp>
#include <boost/python/numpy.hpp>
#include <string>
#include <math.h>
#include <vector>
#include <Eigen/Dense>

using namespace std;
namespace np = boost::python::numpy;
namespace p = boost::python;

int main(int argc, char **argv) {
  Py_Initialize();
  np::initialize();
}

string greet() {
  return "hello!";
}

inline double SIGN(double x) {
	return (x >= 0.0f) ? +1.0 : -1.0;
}

inline double NORM(double a, double b, double c, double d) {
	return sqrt(a * a + b * b + c * c + d * d);
}
// quaternion = [w, x, y, z]'
vector<double> mRot2Quat(np::ndarray m) {
	double r11 = p::extract<double>(m[0][0]);
	double r12 = p::extract<double>(m[0][1]);
	double r13 = p::extract<double>(m[0][2]);
	double r21 = p::extract<double>(m[1][0]);
	double r22 = p::extract<double>(m[1][1]);
	double r23 = p::extract<double>(m[1][2]);
	double r31 = p::extract<double>(m[2][0]);
	double r32 = p::extract<double>(m[2][1]);
	double r33 = p::extract<double>(m[2][2]);
	double q0 = (r11 + r22 + r33 + 1.0) / 4.0;
	double q1 = (r11 - r22 - r33 + 1.0) / 4.0;
	double q2 = (-r11 + r22 - r33 + 1.0) / 4.0;
	double q3 = (-r11 - r22 + r33 + 1.0) / 4.0;
	if (q0 < 0.0) {
		q0 = 0.0;
	}
	if (q1 < 0.0) {
		q1 = 0.0;
	}
	if (q2 < 0.0) {
		q2 = 0.0;
	}
	if (q3 < 0.0) {
		q3 = 0.0;
	}
	q0 = sqrt(q0);
	q1 = sqrt(q1);
	q2 = sqrt(q2);
	q3 = sqrt(q3);
	if (q0 >= q1 && q0 >= q2 && q0 >= q3) {
		q0 *= +1.0;
		q1 *= SIGN(r32 - r23);
		q2 *= SIGN(r13 - r31);
		q3 *= SIGN(r21 - r12);
	}
	else if (q1 >= q0 && q1 >= q2 && q1 >= q3) {
		q0 *= SIGN(r32 - r23);
		q1 *= +1.0;
		q2 *= SIGN(r21 + r12);
		q3 *= SIGN(r13 + r31);
	}
	else if (q2 >= q0 && q2 >= q1 && q2 >= q3) {
		q0 *= SIGN(r13 - r31);
		q1 *= SIGN(r21 + r12);
		q2 *= +1.0;
		q3 *= SIGN(r32 + r23);
	}
	else if (q3 >= q0 && q3 >= q1 && q3 >= q2) {
		q0 *= SIGN(r21 - r12);
		q1 *= SIGN(r31 + r13);
		q2 *= SIGN(r32 + r23);
		q3 *= +1.0;
	}
	else {
		printf("coding error\n");
	}
	double r = NORM(q0, q1, q2, q3);
	q0 /= r;
	q1 /= r;
	q2 /= r;
	q3 /= r;

	vector<double> res(4);
	res[0] = q0; res[1] = q1; res[2] = q2; res[3] = q3;

	return res;
}

vector<double> quaternion_multiply(vector<double> q1, vector<double> q0) {
    vector<double> q(4);
    q[0] = -q1[1]*q0[1] - q1[2]*q0[2] - q1[3]*q0[3] + q1[0]*q0[0];
    q[1] =  q1[1]*q0[0] + q1[2]*q0[3] - q1[3]*q0[2] + q1[0]*q0[1];
    q[2] = -q1[1]*q0[3] + q1[2]*q0[0] + q1[3]*q0[1] + q1[0]*q0[2];
    q[3] =  q1[1]*q0[2] - q1[2]*q0[1] + q1[3]*q0[0] + q1[0]*q0[3];
    return q;
}

vector<double> quaternion_inverse(vector<double> q) {
    vector<double> q_i(4);
    q_i[0] = q[0];
    q_i[1] = -q[1];
    q_i[2] = -q[2];
    q_i[3] = -q[3];
    double dot = q_i[0]*q_i[0] + q_i[1]*q_i[1] + q_i[2]*q_i[2] + q_i[3]*q_i[3];
    q_i[0] = q_i[0] / dot;
    q_i[1] = q_i[1] / dot;
    q_i[2] = q_i[2] / dot;
    q_i[3] = q_i[3] / dot;
    return q_i;
}

vector<double> quaternion_log(vector<double> q) {
    vector<double> rot_vec(3);
    rot_vec[0] = q[1]; rot_vec[1] = q[2]; rot_vec[2] = q[3];
    if(abs(q[0]) < 1.0) {
        double a = acos(q[0]);
        double sina = sin(a);
        if (abs(sina) >= 0.05) {
           double c = a/sina;
           rot_vec[0] *= c;
           rot_vec[1] *= c;
           rot_vec[2] *= c;
        }
    }

    return rot_vec;

}

vector<double> quaternion_disp(vector<double> q1, vector<double> q2) {
    vector<double> inv = quaternion_inverse(q1);
    vector<double> m = quaternion_multiply(inv, q2);
    return quaternion_log(m);
}

double orientation_multiEE_obj(p::object frames, p::object goal_quats, p::list weights) {
    double num_ee = p::len(frames);

    double sum = 0.0;

    for(int q=0; q < num_ee; q++) {
        p::list f = p::extract<p::list>(frames[q]);

        p::list rot_mats = p::extract<p::list>(f[1]);
        int num_jts = p::len(rot_mats);

        np::ndarray ee_rot = p::extract<np::ndarray>(rot_mats[num_jts-1]);

        vector<double> ee_quat = mRot2Quat(ee_rot);
        vector<double> ee_quat2(4);
        ee_quat2[0] = -ee_quat[0];
        ee_quat2[1] = -ee_quat[1];
        ee_quat2[2] = -ee_quat[2];
        ee_quat2[3] = -ee_quat[3];

        // ee_quat2[0] = ee_quat[0];
        // ee_quat2[1] = ee_quat[1];
        // ee_quat2[2] = ee_quat[2];
        // ee_quat2[3] = ee_quat[3];

        p::object goal_quat_py = p::extract<p::object>(goal_quats[q]);
        vector<double> goal_quat(4);
        goal_quat[0] = p::extract<double>(goal_quat_py[0]);
        goal_quat[1] = p::extract<double>(goal_quat_py[1]);
        goal_quat[2] = p::extract<double>(goal_quat_py[2]);
        goal_quat[3] = p::extract<double>(goal_quat_py[3]);

        vector<double> r1 = quaternion_disp(goal_quat, ee_quat);
        vector<double> r2 = quaternion_disp(goal_quat, ee_quat2);

        double disp = sqrt(r1[0]*r1[0] + r1[1]*r1[1] + r1[2]*r1[2]);
        double disp2 = sqrt(r2[0]*r2[0] + r2[1]*r2[1] + r2[2]*r2[2]);

        sum += p::extract<double>(weights[q])*min(disp, disp2);

    }

    return sum;
}

double position_multiEE_obj(p::object frames, p::object eeGoals, p::list weights) {
    double num_ee = p::len(frames);

    double sum = 0.0;

    for(int i = 0; i < num_ee; i++) {
        p::list f = p::extract<p::list>(frames[i]);

        p::list positions = p::extract<p::list>(f[0]);
        int num_jts = p::len(positions);

        np::ndarray eePos = p::extract<np::ndarray>(positions[num_jts-1]);

        double eX = p::extract<double>(eePos[0]);
        double eY = p::extract<double>(eePos[1]);
        double eZ = p::extract<double>(eePos[2]);

        np::ndarray eeGoal = p::extract<np::ndarray>(eeGoals[i]);
        double gX = p::extract<double>(eeGoal[0]);
        double gY = p::extract<double>(eeGoal[1]);
        double gZ = p::extract<double>(eeGoal[2]);

        double val = pow(eX-gX, 2.0) + pow(eY-gY, 2.0) + pow(eZ-gZ, 2.0);
        val = sqrt(val);
        sum += p::extract<double>(weights[i]) * val;
    }

    return sum;
}

double min_jt_jerk_obj(np::ndarray x, p::object prev, p::object prev2, p::object prev3) {
    int vec_len = (int) p::len(x);

    double sum = 0.0;

    for(int i=0; i < vec_len; i++) {
        double v1 = p::extract<double>(x[i]);
        double v2 = p::extract<double>(prev[i]);
        double v3 = p::extract<double>(prev2[i]);
        double v4 = p::extract<double>(prev3[i]);

        double vel3 = v3 - v4;
        double vel2 = v2 - v3;
        double vel1 = v1 - v2;

        sum += pow((vel2 - vel3) - (vel1 - vel2), 2.0);
    }

    return sqrt(sum);
}

double min_jt_accel_obj(np::ndarray x, p::object prev, p::object prev2) {
    int vec_len = (int) p::len(x);

    double sum = 0.0;

    for(int i=0; i < vec_len; i++) {
        double v1 = p::extract<double>(x[i]);
        double v2 = p::extract<double>(prev[i]);
        double v3 = p::extract<double>(prev2[i]);

        sum += pow((v2 - v3) - (v1 - v2), 2.0);
    }

    return sqrt(sum);
}

double min_jt_vel_obj(np::ndarray x, p::object prev) {
    int vec_len = (int) p::len(x);

    double sum = 0.0;

    for(int i=0; i < vec_len; i++) {
        double v1 = p::extract<double>(x[i]);
        double v2 = p::extract<double>(prev[i]);

        sum += pow((v1-v2), 2.0);
    }
    return sqrt(sum);
}

double nloss(double x_val, double t, double d, double c, double f, double g) {
    return -exp( (- pow( (x_val - t), d) ) / (2.0 * pow(c,2.0))) + f * pow( (x_val - t), g);
}

BOOST_PYTHON_MODULE(objectives_ext)
{
    using namespace boost::python;
    def("greet", greet);
    def("orientation_multiEE_obj",orientation_multiEE_obj);
    def("position_multiEE_obj", position_multiEE_obj);
    def("min_jt_vel_obj", min_jt_vel_obj);
    def("min_jt_accel_obj", min_jt_accel_obj);
    def("min_jt_jerk_obj", min_jt_jerk_obj);
    def("nloss", nloss);
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2016 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "per_vertex_point_to_plane_quadrics.h"
#include "quadric_binary_plus_operator.h"
#include <Eigen/QR>
#include <cassert>
#include <cmath>

IGL_INLINE void igl::per_vertex_point_to_plane_quadrics(
  const Eigen::MatrixXd & V,
  const Eigen::MatrixXi & F,
  const Eigen::MatrixXi & EMAP,
  const Eigen::MatrixXi & EF,
  const Eigen::MatrixXi & EI,
  std::vector<
    std::tuple<Eigen::MatrixXd,Eigen::RowVectorXd,double> > & quadrics)
{
  using namespace std;
  typedef std::tuple<Eigen::MatrixXd,Eigen::RowVectorXd,double> Quadric;
  const int dim = V.cols();
  //// Quadrics per face
  //std::vector<Quadric> face_quadrics(F.rows());
  // Initialize each vertex quadric to zeros
  quadrics.resize(
    V.rows(),
    // gcc <=4.8 can't handle initializer lists correctly
    Quadric{Eigen::MatrixXd::Zero(dim,dim),Eigen::RowVectorXd::Zero(dim),0});
  Eigen::MatrixXd I = Eigen::MatrixXd::Identity(dim,dim);
  // Rather initial with zeros, initial with a small amount of energy pull
  // toward original vertex position
  const double w = 1e-10;
  for(int v = 0;v<V.rows();v++)
  {
    std::get<0>(quadrics[v]) = w*I;
    Eigen::RowVectorXd Vv = V.row(v);
    std::get<1>(quadrics[v]) = w*-Vv;
    std::get<2>(quadrics[v]) = w*Vv.dot(Vv);
  }
  // Generic nD qslim from "Simplifying Surfaces with Color and Texture
  // using Quadric Error Metric" (follow up to original QSlim)
  for(int f = 0;f<F.rows();f++)
  {
    int infinite_corner = -1;
    for(int c = 0;c<3;c++)
    {
      if(
         std::isinf(V(F(f,c),0)) ||
         std::isinf(V(F(f,c),1)) ||
         std::isinf(V(F(f,c),2)))
      {
        assert(infinite_corner == -1 && "Should only be one infinite corner");
        infinite_corner = c;
      }
    }
    // Inputs:
    //   p  1 by n row point on the subspace
    //   S  m by n matrix where rows coorespond to orthonormal spanning
    //     vectors of the subspace to which we're measuring distance (usually
    //     a plane, m=2)
    //   weight  scalar weight
    // Returns quadric triple {A,b,c} so that A-2*b+c measures the quadric
    const auto subspace_quadric = [&I](
      const Eigen::RowVectorXd & p,
      const Eigen::MatrixXd & S,
      const double  weight)->Quadric
    {
      // Dimension of subspace
      const int m = S.rows();
      // Weight face's quadric (v'*A*v + 2*b'*v + c) by area
      // e1 and e2 should be perpendicular
      Eigen::MatrixXd A = I;
      Eigen::RowVectorXd b = -p;
      double c = p.dot(p);
      for(int i = 0;i<m;i++)
      {
        Eigen::RowVectorXd ei = S.row(i);
        for(int j = 0;j<i;j++) assert(std::abs(S.row(j).dot(ei)) < 1e-10);
        A += -ei.transpose()*ei;
        b += p.dot(ei)*ei;
        c += -pow(p.dot(ei),2);
      }
      // gcc <=4.8 can't handle initializer lists correctly: needs explicit
      // cast
      return Quadric{ weight*A, weight*b, weight*c };
    };
    if(infinite_corner == -1)
    {
      // Finite (non-boundary) face
      Eigen::RowVectorXd p = V.row(F(f,0));
      Eigen::RowVectorXd q = V.row(F(f,1));
      Eigen::RowVectorXd r = V.row(F(f,2));
      Eigen::RowVectorXd pq = q-p;
      Eigen::RowVectorXd pr = r-p;
      // Gram Determinant = squared area of parallelogram
      double area = sqrt(pq.squaredNorm()*pr.squaredNorm()-pow(pr.dot(pq),2));
      Eigen::RowVectorXd e1 = pq.normalized();
      Eigen::RowVectorXd e2 = (pr-e1.dot(pr)*e1).normalized();
      Eigen::MatrixXd S(2,V.cols());
      S<<e1,e2;
      Quadric face_quadric = subspace_quadric(p,S,area);
      // Throw at each corner
      for(int c = 0;c<3;c++)
      {
        quadrics[F(f,c)] = quadrics[F(f,c)] + face_quadric;
      }
    }else
    {
      // cth corner is infinite --> edge opposite cth corner is boundary
      // Boundary edge vector
      const Eigen::RowVectorXd p = V.row(F(f,(infinite_corner+1)%3));
      Eigen::RowVectorXd ev = V.row(F(f,(infinite_corner+2)%3)) - p;
      const double length = ev.norm();
      ev /= length;
      // Face neighbor across boundary edge
      int e = EMAP(f+F.rows()*infinite_corner);
      int opp = EF(e,0) == f ? 1 : 0;
      int n =  EF(e,opp);
      int nc = EI(e,opp);
      assert(
        ((F(f,(infinite_corner+1)%3) == F(n,(nc+1)%3) &&
          F(f,(infinite_corner+2)%3) == F(n,(nc+2)%3)) ||
          (F(f,(infinite_corner+1)%3) == F(n,(nc+2)%3)
          && F(f,(infinite_corner+2)%3) == F(n,(nc+1)%3))) &&
        "Edge flaps not agreeing on shared edge");
      // Edge vector on opposite face
      const Eigen::RowVectorXd eu = V.row(F(n,nc)) - p;
      assert(!std::isinf(eu(0)));
      // Matrix with vectors spanning plane as columns
      Eigen::MatrixXd A(ev.size(),2);
      A<<ev.transpose(),eu.transpose();
      // Use QR decomposition to find basis for orthogonal space
      Eigen::HouseholderQR<Eigen::MatrixXd> qr(A);
      const Eigen::MatrixXd Q = qr.householderQ();
      const Eigen::MatrixXd N =
        Q.topRightCorner(ev.size(),ev.size()-2).transpose();
      assert(N.cols() == ev.size());
      assert(N.rows() == ev.size()-2);
      Eigen::MatrixXd S(N.rows()+1,ev.size());
      S<<ev,N;
      Quadric boundary_edge_quadric = subspace_quadric(p,S,length);
      for(int c = 0;c<3;c++)
      {
        if(c != infinite_corner)
        {
          quadrics[F(f,c)] = quadrics[F(f,c)] + boundary_edge_quadric;
        }
      }
    }
  }
}


#include <frovedis.hpp>
#include <frovedis/ml/glm/logistic_regression_with_lbfgs.hpp>

#include <boost/program_options.hpp>

using namespace boost;
using namespace frovedis;
using namespace std;

double to_double(std::string& line) {
  return boost::lexical_cast<double>(line);
}

void do_train(const string& input, const string& label, const string& output,
              size_t num_iteration, double alpha, size_t hist_size,
              const string& regularizer, double regParam, bool intercept,
              double convTol, MatType mType, bool binary) {

  RegType rt;
  if(regularizer == "ZERO") {
    rt = ZERO;
  } else if (regularizer == "L1") {
    rt = L1;
  } else if (regularizer == "L2") {
    rt = L2;
  } else {
    cerr << "invalid regularization type: " << regularizer << endl;
    exit(1);
  }
  if(binary) {
    time_spent t(DEBUG);
    auto mat = make_crs_matrix_loadbinary<double>(input);
    t.show("load matrix: ");
    auto lb = make_dvector_loadbinary<double>(label);
    t.show("load label: ");
    auto lm = logistic_regression_with_lbfgs::train(std::move(mat), lb,
                                                    num_iteration, alpha,
                                                    hist_size, regParam,
                                                    rt, intercept,convTol, mType);
    t.show("train time: ");
    lm.savebinary(output);
    t.show("save model time: ");
  } else {
    time_spent t(DEBUG);
    auto mat = make_crs_matrix_load<double>(input);
    t.show("load matrix: ");
    auto lb = make_dvector_loadline(label).map(to_double);
    t.show("load label: ");
    auto lm = logistic_regression_with_lbfgs::train(std::move(mat), lb,
                                                    num_iteration, alpha,
                                                    hist_size, regParam,
                                                    rt, intercept,convTol, mType);
    t.show("train time: ");
    lm.save(output);
    t.show("save model time: ");
  }
}

void do_predict(const string& input, const string& model, const string& output,
                bool prob, bool binary) {
  logistic_regression_model<double> lm;
  if(binary) {
    lm.loadbinary(model);
    auto mat = make_crs_matrix_local_loadbinary<double>(input);
    if(prob) {
      auto r = lm.predict_probability(mat);
      make_dvector_scatter(r).savebinary(output);
    } else {
      auto r = lm.predict(mat);
      make_dvector_scatter(r).savebinary(output);
    }
  } else {
    lm.load(model);
    auto mat = make_crs_matrix_local_load<double>(input);
    if(prob) {
      auto r = lm.predict_probability(mat);
      make_dvector_scatter(r).saveline(output);
    } else {
      auto r = lm.predict(mat);
      make_dvector_scatter(r).saveline(output);
    }
  }
}

int main(int argc, char* argv[]) {
  use_frovedis use(argc, argv);

  using namespace boost::program_options;

  options_description opt("option");
  opt.add_options()
    ("help,h", "print help")
    ("predict,p", "predict mode")
    ("predict-probability,y", "probability-predict mode")
    ("input,i", value<string>(), "input matrix")
    ("ell", "assume ell storage of input training data matrix")
    ("crs", "assume crs storage of input training data matrix (default for X86)")
    ("hybrid", "assume jds-crs hybrid storage of input training data matrix (default for SX)")
    ("label,l", value<string>(), "input label (for train)")
    ("model,m", value<string>(), "input model (for predict)")
    ("output,o", value<string>(), "output model or predict result")
    ("num-iteration,n", value<size_t>(), "number of iteration (default: 1000)")
    ("alpha,a", value<double>(), "learning rate (default:0.01) ")
    ("history-size,h", value<size_t>(), "size for lbfgs history vectors (default: 10)")
    ("regularizer,r", value<string>(), "regularizer (ZERO [default], L1, or L2)")
    ("regularization-parameter,e", value<double>(), "regularization parameter (default: 0.01)")
    ("convergence-tolerance,c", value<double>(), "a tolerance value to determine convergence (default: 0.001)")
    ("intercept,t", "use bias or not")
    ("verbose", "set loglevel to DEBUG")
    ("verbose2", "set loglevel to TRACE")
    ("binary,b", "use binary input/output");

  variables_map argmap;
  store(command_line_parser(argc,argv).options(opt).allow_unregistered().
        run(), argmap);
  notify(argmap);

  bool ispredict = false;
  bool predict_probability = false;
  string input, label, model, output;
  size_t num_iteration = 100;
  double alpha = 1.0;
  size_t hist_size = 10;
  double convTol = 0.001;
  string regularizer = "ZERO";
  double regParam = 0.01;
  bool intercept = false;
  bool binary = false;
  MatType mType = CRS;

  if(argmap.count("help")){
    cerr << opt << endl;
    exit(1);
  }

  if(argmap.count("predict")){
    ispredict = true;
  }

  if(argmap.count("predict-probability")){
    ispredict = true;
    predict_probability = true;
  }

  if(argmap.count("input")){
    input = argmap["input"].as<string>();
  } else {
    cerr << "input is not specified" << endl;
    cerr << opt << endl;
    exit(1);
  }

  if(!ispredict) {
    if(argmap.count("label")){
      label = argmap["label"].as<string>();
    } else {
      cerr << "label file is not specified" << endl;
      cerr << opt << endl;
      exit(1);
    }

    if(argmap.count("ell")) {
      mType = ELL;
    } else if(argmap.count("crs")) {
      mType = CRS;
    } else if(argmap.count("hybrid")) {
      mType = HYBRID;
    }
  }

  if(ispredict) {
    if(argmap.count("model")){
      model = argmap["model"].as<string>();
    } else {
      cerr << "model file is not specified" << endl;
      cerr << opt << endl;
      exit(1);
    }
  }

  if(argmap.count("output")){
    output = argmap["output"].as<string>();
  } else {
    cerr << "output is not specified" << endl;
    cerr << opt << endl;
    exit(1);
  }

  if(argmap.count("num-iteration")){
    num_iteration = argmap["num-iteration"].as<size_t>();
  }

  if(argmap.count("alpha")){
    alpha = argmap["alpha"].as<double>();
  }

  if(argmap.count("history-size")){
    hist_size = argmap["history-size"].as<size_t>();
  }

  if(argmap.count("regularizer")){
    regularizer = argmap["regularizer"].as<string>();
  }

  if(argmap.count("regularization-parameter")){
    regParam = argmap["regularization-parameter"].as<double>();
  }

  if(argmap.count("convergence-tolerance")){
    convTol = argmap["convergence-tolerance"].as<double>();
  }

  if(argmap.count("intercept")){
    intercept = true;
  }

  if(argmap.count("binary")){
    binary = true;
  }

  if(argmap.count("verbose")){
    set_loglevel(DEBUG);
  }

  if(argmap.count("verbose2")){
    set_loglevel(TRACE);
  }

  if(ispredict) do_predict(input, model, output, predict_probability, binary);
  else do_train(input, label, output, num_iteration, alpha, hist_size,
                regularizer, regParam, intercept, convTol, mType, binary);
}


////////////////////////////////////////////////////////////////////////////////
// IncompressibleBalloonEnergyWithHessProjection.hh
////////////////////////////////////////////////////////////////////////////////
/*! @file
//  Implementation of the incompressible neo-Hookean-based strain energy
//  density used in Skouras 2014: Designing Inflatable Structures, expressed
//  in terms of the 3x2 deformation gradient F instead of the 2x2 Cauchy-Green
//  deformation tensor F^T F. We also provide analytical expressions for the
//  eigenvalues and eigenvectors of the Hessian (wrt F), enabling Hessian
//  projection to resolve indefiniteness.
*/
//  Author:  Julian Panetta (jpanetta), julian.panetta@gmail.com
//  Created:  07/09/2019 10:56:53
////////////////////////////////////////////////////////////////////////////////
#ifndef INCOMPRESSIBLEBALLOONENERGYWITHHESSPROJECTION_HH
#define INCOMPRESSIBLEBALLOONENERGYWITHHESSPROJECTION_HH

#include <Eigen/Dense>
#include <array>

template<typename Real>
struct IncompressibleBalloonEnergyWithHessProjection {
    using V2d  = Eigen::Matrix<Real, 2, 1>;
    using M2d  = Eigen::Matrix<Real, 2, 2>;
    using M3d  = Eigen::Matrix<Real, 3, 3>;
    using M32d = Eigen::Matrix<Real, 3, 2>;

    IncompressibleBalloonEnergyWithHessProjection() { }

    template<typename Derived>
    IncompressibleBalloonEnergyWithHessProjection(const Eigen::MatrixBase<Derived> &F) { setF(F); }

    template<typename Derived>
    void setF(const Eigen::MatrixBase<Derived> &F) {
        static_assert((Derived::RowsAtCompileTime == 3) && (Derived::ColsAtCompileTime == 2), "F must be 3x2");
        m_F = F.template cast<Real>();

        Eigen::JacobiSVD<M32d> svd(m_F, Eigen::ComputeFullU | Eigen::ComputeFullV);
        m_sigma = svd.singularValues();
        m_V = svd.matrixV();
        m_U = svd.matrixU();

        m_det32_F = m_sigma.prod();
        m_det_C = m_det32_F * m_det32_F;

        m_d_det32_F = (m_U.col(0) * m_sigma[1] * m_V.col(0).transpose() +
                       m_U.col(1) * m_sigma[0] * m_V.col(1).transpose());

        m_trace_C = m_F.squaredNorm();

        m_d_psi_ddet = -2.0 / (m_det_C * m_det32_F);

        // Compute eigendecomposition of det32_F's Hessian
        /* static constexpr */ const Real inv_sqrt2 = 1.0 / std::sqrt(2.0); // std::sqrt is not constexpr!

        // T = U [0 -1; 1 0; 0 0] V^T / sqrt(2)
        m_eigmat[2] = (-inv_sqrt2 * m_U.col(0)) * m_V.col(1).transpose() +
                       (inv_sqrt2 * m_U.col(1)) * m_V.col(0).transpose();
        m_eigval[2] = 2.0 + m_d_psi_ddet;
        // L = U [0 1; 1 0; 0 0] V^T / sqrt(2)
        m_eigmat[3] = (inv_sqrt2 * m_U.col(0)) * m_V.col(1).transpose() +
                      (inv_sqrt2 * m_U.col(1)) * m_V.col(0).transpose();
        m_eigval[3] = 2.0 - m_d_psi_ddet;
        // NX = U [0 0; 0 0; 1 0] V^T
        m_eigmat[4] = m_U.col(2) * m_V.col(0).transpose();
        m_eigval[4] = 2.0 + m_d_psi_ddet * m_sigma[1] / m_sigma[0];
        // NY = U [0 0; 0 0; 0 1] V^T
        m_eigmat[5] = m_U.col(2) * m_V.col(1).transpose();
        m_eigval[5] = 2.0 + m_d_psi_ddet * m_sigma[0] / m_sigma[1];

        // The "R" and "P" modes [1 0; 0 1; 0 0] and [1 0; 0 -1; 0 0]
        // are not orthogonal to d_det32_F and thus will not
        // be eigenmatrices of the energy density Hessian (after addition of the
        // rank 1 term proportional to (d_det32_F otimes d_det32_F)).
        // We need to compute the compute the eigendecomposition of the Hessian
        // in this subspace by solving a 2x2 matrix eigenvalue problem.
        // We first probe the Hessian with an orthonormal basis for this space:
        //      d0 := U [1, 0; 0, 0; 0, 0] V^T and d1 := U [0, 1; 0, 0; 0, 0] V^T
        // to obtain the reduced Hessian:
        //      A_ij = di : d2psi / dF2 : dj
        //  A = 2 I + 1 / det(c)^2 [6 sigma_2^2  4 det32_F  ]
        //                         [4 det32_F    6 sigma_1^2]
        // Whose eigendecomposition is:
        Real e = 3 * (m_sigma[1] * m_sigma[1] - m_sigma[0] * m_sigma[0]),
             x = std::sqrt(16 * m_det_C + e * e);
        Eigen::Vector2d v0(e - x, 4 * m_det32_F),
                        v1(e + x, 4 * m_det32_F);
        v0.normalize();
        v1.normalize();
        m_eigmat[0] = (v0[0] * m_U.col(0)) * m_V.col(0).transpose() +
                      (v0[1] * m_U.col(1)) * m_V.col(1).transpose();
        m_eigmat[1] = (v1[0] * m_U.col(0)) * m_V.col(0).transpose() +
                      (v1[1] * m_U.col(1)) * m_V.col(1).transpose();
        m_eigval[0] = 2 + (3 * m_trace_C - x) / (m_det_C * m_det_C);
        m_eigval[1] = 2 + (3 * m_trace_C + x) / (m_det_C * m_det_C);
    }

    Real energy() const {
        return stiffness * (m_trace_C + 1.0 / m_det_C - 3.0);
    }

    M32d denergy() const {
        return 2 * m_F + m_d_psi_ddet * m_d_det32_F;
    }

    template<class DeltaF>
    Real denergy(const DeltaF &dF) const {
        const Real d_det32_F = m_sigma[1] * m_U.col(0).dot(dF * m_V.col(0)) +
                               m_sigma[0] * m_U.col(1).dot(dF * m_V.col(1));

        return 2.0 * doubleContract(m_F, dF) + m_d_psi_ddet * d_det32_F;
    }

    template<class DeltaF>
    M32d delta_denergy(const DeltaF &dF) const {
        M32d result(M32d::Zero());
        for (size_t i = 0; i < 6; ++i) {
            if (applyHessianProjection && (m_eigval[i] <= 0)) continue;
            result += (m_eigval[i] * doubleContract(m_eigmat[i], dF)) * m_eigmat[i];
        }

        return result;
    }

    template<class DeltaF>
    Real d2energy(const DeltaF &dF_a, const DeltaF &dF_b) const {
        return doubleContract(delta_denergy(dF_a), dF_b);
    }

    Real stiffness = 1.0;

    // Whether to project d2energy/denergy onto the space of positive semi-semidefinite tensors.
    bool applyHessianProjection = false;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
    // We need the singular value decomposition for our gradient/Hessian formulas.
    M32d m_F, m_d_det32_F;

    // Eigenmatrices of the determinant: "R", "P", "T", "L", "NX", and "NY"
    std::array<M32d, 6> m_eigmat;
    std::array<Real, 6> m_eigval;

    M2d  m_V;
    M3d  m_U;
    V2d  m_sigma;
    Real m_trace_C, m_det32_F, m_det_C, m_d_psi_ddet;
};

#endif /* end of include guard: INCOMPRESSIBLEBALLOONENERGYWITHHESSPROJECTION_HH */