#include <thread>
#include <complex>
#ifdef _OPENMP
#include <omp.h>
#endif

#define lapack_complex_float  std::complex<float>
#define lapack_complex_double std::complex<double>

#ifdef OpenBLAS_AVAILABLE
#include <cblas.h>
#include <openblas_config.h>
#endif

#ifdef MKL_AVAILABLE
#include <mkl_service.h>
#include <mkl.h>
#endif

#if __has_include(<mkl_lapacke.h>)
#include <mkl_lapacke.h>
#elif __has_include(<lapacke.h>)
#include <lapacke.h>
#endif

#include <Eigen/Core>
#include <general/nmspc_tensor_omp.h>
#include <h5pp/h5pp.h>
#include <math/class_eigsolver.h>

int eig_dsyevd(double *matrix2eigvecs, double * eigvals, int L){
    //These nice values are inspired from armadillo. The prefactors give good performance.
    int lwork  = 2 * (1 + 6*L + 2*(L*L));
    int liwork = 3 * (3 + 5*L);
    std::cout << " lwork  = " << lwork << std::endl;
    std::cout << " liwork = " << liwork << std::endl;

    int info   = 0;
    std::vector<double> work  ( lwork );
    std::vector<int   > iwork ( liwork );
    char jobz = 'V';
    info = LAPACKE_dsyevd_work(LAPACK_COL_MAJOR,jobz,'L',L,
                               matrix2eigvecs,
                               L,
                               eigvals,
                               work.data(),
                               lwork,
                               iwork.data(),
                               liwork);
    return info;
}

std::tuple<Eigen::VectorXd,Eigen::MatrixXd, int> eig_dsyevd(const double* matrix, int L){
//    using namespace eigutils::eigSetting;
    Eigen::VectorXd eigvals(L);
    Eigen::MatrixXd eigvecs(L,L);
    std::copy(matrix, matrix + L*L, eigvecs.data());
    int info = eig_dsyevd(eigvecs.data(),eigvals.data(), L);

    return std::make_tuple(eigvals,eigvecs,info);
}

int main(){

    #ifdef _OPENMP
        omp_set_num_threads(std::thread::hardware_concurrency());
        Eigen::setNbThreads(std::thread::hardware_concurrency());
        std::cout << "Using Eigen  with " << Eigen::nbThreads() << " threads" << std::endl;
        std::cout << "Using OpenMP with " << omp_get_max_threads() << " threads" << std::endl;

        #ifdef OpenBLAS_AVAILABLE
        openblas_set_num_threads(std::thread::hardware_concurrency());
                    std::cout << OPENBLAS_VERSION
                              << " compiled with parallel mode " << openblas_get_parallel()
                              << " for target " << openblas_get_corename()
                              << " with config " << openblas_get_config()
                              << " with multithread threshold " << OPENBLAS_GEMM_MULTITHREAD_THRESHOLD
                              << ". Running with " << openblas_get_num_threads() << " thread(s)" << std::endl;
        #endif

        #ifdef MKL_AVAILABLE
        mkl_set_num_threads(std::thread::hardware_concurrency());
        std::cout << "Using Intel MKL with " << mkl_get_max_threads() << " threads" << std::endl;
        #endif
    #endif

    Eigen::MatrixXd H_localA1, H_localA2;
    Eigen::MatrixXd H_localB1, H_localB2;

    Eigen::MatrixXd eigvecsA1, eigvecsA2;
    Eigen::MatrixXd eigvecsB1, eigvecsB2;
    Eigen::MatrixXd eigvalsA1, eigvalsA2;
    Eigen::MatrixXd eigvalsB1, eigvalsB2;
    Eigen::VectorXcd thetaA1, thetaA2;
    Eigen::VectorXcd thetaB1, thetaB2;

    Eigen::VectorXd overlapsA1, overlapsA2;
    Eigen::VectorXd overlapsB1, overlapsB2;

    h5pp::File fileA1(std::string(TEST_MATRIX_DIR) + "/lapacke_matrix-1A.h5", h5pp::FilePermission::READONLY);
    h5pp::File fileA2(std::string(TEST_MATRIX_DIR) + "/lapacke_matrix-2A.h5", h5pp::FilePermission::READONLY);
    h5pp::File fileB1(std::string(TEST_MATRIX_DIR) + "/lapacke_matrix-1B.h5", h5pp::FilePermission::READONLY);
    h5pp::File fileB2(std::string(TEST_MATRIX_DIR) + "/lapacke_matrix-2B.h5", h5pp::FilePermission::READONLY);
    fileA1.readDataset(H_localA1,"matrix");
    fileA2.readDataset(H_localA2,"matrix");
    fileB1.readDataset(H_localB1,"matrix");
    fileB2.readDataset(H_localB2,"matrix");

    fileA1.readDataset(eigvecsA1,"eigvecs");
    fileA2.readDataset(eigvecsA2,"eigvecs");
    fileB1.readDataset(eigvecsB1,"eigvecs");
    fileB2.readDataset(eigvecsB2,"eigvecs");

    fileA1.readDataset(eigvalsA1,"eigvals");
    fileA2.readDataset(eigvalsA2,"eigvals");
    fileB1.readDataset(eigvalsB1,"eigvals");
    fileB2.readDataset(eigvalsB2,"eigvals");

    fileA1.readDataset(thetaA1,"theta");
    fileA2.readDataset(thetaA2,"theta");
    fileB1.readDataset(thetaB1,"theta");
    fileB2.readDataset(thetaB2,"theta");

    fileA1.readDataset(overlapsA1,"overlaps");
    fileA2.readDataset(overlapsA2,"overlaps");
    fileB1.readDataset(overlapsB1,"overlaps");
    fileB2.readDataset(overlapsB2,"overlaps");
    using namespace eigutils::eigSetting;

    class_eigsolver solverA;
    class_eigsolver solverB;

    [[maybe_unused]] auto [eigvalsA1_lapacke,eigvecsA1_lapacke,infoA1] = eig_dsyevd(H_localA1.data(),H_localA1.rows());
    [[maybe_unused]] auto [eigvalsB1_lapacke,eigvecsB1_lapacke,infoB1] = eig_dsyevd(H_localB1.data(),H_localB1.rows());

    solverA.eig<Type::REAL, Form::SYMMETRIC>(H_localA1,true,false);
    solverB.eig<Type::REAL, Form::SYMMETRIC>(H_localB1,true,false);
    Eigen::VectorXd eigvalsA1_eig           = Eigen::Map<const Eigen::VectorXd> (solverA.solution.get_eigvals<Form::SYMMETRIC>().data()            ,solverA.solution.meta.cols);
    Eigen::MatrixXd eigvecsA1_eig           = Eigen::Map<const Eigen::MatrixXd> (solverA.solution.get_eigvecs<Type::REAL, Form::SYMMETRIC>().data(),solverA.solution.meta.rows,solverA.solution.meta.cols);
    Eigen::VectorXd eigvalsB1_eig           = Eigen::Map<const Eigen::VectorXd> (solverB.solution.get_eigvals<Form::SYMMETRIC>().data()            ,solverB.solution.meta.cols);
    Eigen::MatrixXd eigvecsB1_eig           = Eigen::Map<const Eigen::MatrixXd> (solverB.solution.get_eigvecs<Type::REAL, Form::SYMMETRIC>().data(),solverB.solution.meta.rows,solverB.solution.meta.cols);

    Eigen::VectorXd overlapsA1_read         = (thetaA1.adjoint() * eigvecsA1).cwiseAbs();
    Eigen::VectorXd overlapsA1_lapacke      = (thetaA1.adjoint() * eigvecsA1_lapacke).cwiseAbs();
    Eigen::VectorXd overlapsA1_eig          = (thetaA1.adjoint() * eigvecsA1_eig).cwiseAbs();
    Eigen::VectorXd overlapsB1_read         = (thetaB1.adjoint() * eigvecsB1).cwiseAbs();
    Eigen::VectorXd overlapsB1_lapacke      = (thetaB1.adjoint() * eigvecsB1_lapacke).cwiseAbs();
    Eigen::VectorXd overlapsB1_eig          = (thetaB1.adjoint() * eigvecsB1_eig).cwiseAbs();

    double max_overlapA1_read        = overlapsA1_read    .maxCoeff();
    double max_overlapA1_lapacke     = overlapsA1_lapacke .maxCoeff();
    double max_overlapA1_eig         = overlapsA1_eig     .maxCoeff();
    double max_overlapB1_read        = overlapsB1_read    .maxCoeff();
    double max_overlapB1_lapacke     = overlapsB1_lapacke .maxCoeff();
    double max_overlapB1_eig         = overlapsB1_eig     .maxCoeff();

    auto overlap_A1A1_eig     =     (eigvecsA1.adjoint() * eigvecsA1_eig    ).diagonal().cwiseAbs().mean();
    auto overlap_A1B1_eig     =     (eigvecsA1.adjoint() * eigvecsB1_eig    ).diagonal().cwiseAbs().mean();
    auto overlap_A1A1_lapacke =     (eigvecsA1.adjoint() * eigvecsA1_lapacke).diagonal().cwiseAbs().mean();
    auto overlap_A1B1_lapacke =     (eigvecsA1.adjoint() * eigvecsB1_lapacke).diagonal().cwiseAbs().mean();

    [[maybe_unused]] auto [eigvalsA2_lapacke,eigvecsA2_lapacke,infoA2] = eig_dsyevd(H_localA2.data(),H_localA2.rows());
    [[maybe_unused]] auto [eigvalsB2_lapacke,eigvecsB2_lapacke,infoB2] = eig_dsyevd(H_localB2.data(),H_localB2.rows());
    solverA.eig<Type::REAL, Form::SYMMETRIC>(H_localA2,true,false);
    solverB.eig<Type::REAL, Form::SYMMETRIC>(H_localB2,true,false);
    Eigen::VectorXd eigvalsA2_eig           = Eigen::Map<const Eigen::VectorXd> (solverA.solution.get_eigvals<Form::SYMMETRIC>().data()            ,solverA.solution.meta.cols);
    Eigen::MatrixXd eigvecsA2_eig           = Eigen::Map<const Eigen::MatrixXd> (solverA.solution.get_eigvecs<Type::REAL, Form::SYMMETRIC>().data(),solverA.solution.meta.rows,solverA.solution.meta.cols);
    Eigen::VectorXd eigvalsB2_eig           = Eigen::Map<const Eigen::VectorXd> (solverB.solution.get_eigvals<Form::SYMMETRIC>().data()            ,solverB.solution.meta.cols);
    Eigen::MatrixXd eigvecsB2_eig           = Eigen::Map<const Eigen::MatrixXd> (solverB.solution.get_eigvecs<Type::REAL, Form::SYMMETRIC>().data(),solverB.solution.meta.rows,solverB.solution.meta.cols);

    Eigen::VectorXd overlapsA2_read         = (thetaA2.adjoint() * eigvecsA2).cwiseAbs();
    Eigen::VectorXd overlapsA2_lapacke      = (thetaA2.adjoint() * eigvecsA2_lapacke).cwiseAbs();
    Eigen::VectorXd overlapsA2_eig          = (thetaA2.adjoint() * eigvecsA2_eig).cwiseAbs();
    Eigen::VectorXd overlapsB2_read         = (thetaB2.adjoint() * eigvecsB2).cwiseAbs();
    Eigen::VectorXd overlapsB2_lapacke      = (thetaB2.adjoint() * eigvecsB2_lapacke).cwiseAbs();
    Eigen::VectorXd overlapsB2_eig          = (thetaB2.adjoint() * eigvecsB2_eig).cwiseAbs();

    double max_overlapA2_read        = overlapsA2_read    .maxCoeff();
    double max_overlapA2_lapacke     = overlapsA2_lapacke .maxCoeff();
    double max_overlapA2_eig         = overlapsA2_eig     .maxCoeff();
    double max_overlapB2_read        = overlapsB2_read    .maxCoeff();
    double max_overlapB2_lapacke     = overlapsB2_lapacke .maxCoeff();
    double max_overlapB2_eig         = overlapsB2_eig     .maxCoeff();

    auto overlap_A2A2_eig     =  (eigvecsA2.adjoint() * eigvecsA2_eig    ).diagonal().cwiseAbs().mean();
    auto overlap_A2B2_eig     =  (eigvecsA2.adjoint() * eigvecsB2_eig    ).diagonal().cwiseAbs().mean();
    auto overlap_A2A2_lapacke =  (eigvecsA2.adjoint() * eigvecsA2_lapacke).diagonal().cwiseAbs().mean();
    auto overlap_A2B2_lapacke =  (eigvecsA2.adjoint() * eigvecsB2_lapacke).diagonal().cwiseAbs().mean();

    std::cout << std::setprecision(16);
    std::cout << "H_localA1         vs H_localB1           diff   =   " << std::boolalpha << (H_localA1.array() - H_localB1.array()).cwiseAbs().sum() << std::endl;
    std::cout << "thetaA1           vs thetaB1             diff   =   " << std::boolalpha << (thetaA1.array()   - thetaB1.array()).cwiseAbs().sum() << std::endl;
    std::cout << "<thetaA1          |  thetaB1 >            diff  =   " << std::boolalpha << (thetaA1.adjoint() * thetaB1).cwiseAbs().sum() << std::endl;
    std::cout << "overlapsA1        vs overlapsB1          diff   =   " << std::boolalpha << (overlapsA1.array()- overlapsB1.array()).cwiseAbs().sum() << std::endl;
    std::cout << "eigvecsA1_read    vs eigvecsA1_lapacke   diff   =   " << std::boolalpha << (eigvecsA1.adjoint()         * eigvecsA1_lapacke).diagonal().cwiseAbs().mean() << std::endl;
    std::cout << "eigvecsB1_read    vs eigvecsB1_lapacke   diff   =   " << std::boolalpha << (eigvecsB1.adjoint()         * eigvecsB1_lapacke).diagonal().cwiseAbs().mean() << std::endl;
    std::cout << "eigvecsA1_read    vs eigvecsA1_solver    diff   =   " << std::boolalpha << (eigvecsA1.adjoint()         * eigvecsA1_eig)    .diagonal().cwiseAbs().mean() << std::endl;
    std::cout << "eigvecsB1_read    vs eigvecsB1_solver    diff   =   " << std::boolalpha << (eigvecsB1.adjoint()         * eigvecsB1_eig)    .diagonal().cwiseAbs().mean() << std::endl;
    std::cout << "eigvecsA1_lapacke vs eigvecsA1_solver    diff   =   " << std::boolalpha << (eigvecsA1_lapacke.adjoint() * eigvecsA1_eig)    .diagonal().cwiseAbs().mean() << std::endl;
    std::cout << "eigvecsB1_lapacke vs eigvecsB1_solver    diff   =   " << std::boolalpha << (eigvecsB1_lapacke.adjoint() * eigvecsB1_eig)    .diagonal().cwiseAbs().mean() << std::endl;
    std::cout << "eigvalsA1_read    vs eigvalsA1_lapacke   diff   =   " << std::boolalpha << (eigvalsA1.array()           - eigvalsA1_lapacke .array())  .cwiseAbs().sum() << std::endl;
    std::cout << "eigvalsB1_read    vs eigvalsB1_lapacke   diff   =   " << std::boolalpha << (eigvalsB1.array()           - eigvalsB1_lapacke .array())  .cwiseAbs().sum() << std::endl;
    std::cout << "eigvalsA1_read    vs eigvalsA1_solver    diff   =   " << std::boolalpha << (eigvalsA1.array()           - eigvalsA1_eig     .array())  .cwiseAbs().sum() << std::endl;
    std::cout << "eigvalsB1_read    vs eigvalsB1_solver    diff   =   " << std::boolalpha << (eigvalsB1.array()           - eigvalsB1_eig     .array())  .cwiseAbs().sum() << std::endl;
    std::cout << "eigvalsA1_lapacke vs eigvalsA1_solver    diff   =   " << std::boolalpha << (eigvalsA1_lapacke.array()   - eigvalsA1_eig     .array())  .cwiseAbs().sum() << std::endl;
    std::cout << "eigvalsB1_lapacke vs eigvalsB1_solver    diff   =   " << std::boolalpha << (eigvalsB1_lapacke.array()   - eigvalsB1_eig     .array())  .cwiseAbs().sum() << std::endl;

    std::cout << "max_overlapA1_read     =  " << max_overlapA1_read     << std::endl;
    std::cout << "max_overlapA1_lapacke  =  " << max_overlapA1_lapacke  << std::endl;
    std::cout << "max_overlapA1_eig      =  " << max_overlapA1_eig      << std::endl;
    std::cout << "max_overlapB1_read     =  " << max_overlapB1_read     << std::endl;
    std::cout << "max_overlapB1_lapacke  =  " << max_overlapB1_lapacke  << std::endl;
    std::cout << "max_overlapB1_eig      =  " << max_overlapB1_eig      << std::endl;

    std::cout << "overlap_A1A1_eig       =  " << overlap_A1A1_eig       << std::endl;
    std::cout << "overlap_A1B1_eig       =  " << overlap_A1B1_eig       << std::endl;
    std::cout << "overlap_A1A1_lapacke   =  " << overlap_A1A1_lapacke   << std::endl;
    std::cout << "overlap_A1B1_lapacke   =  " << overlap_A1B1_lapacke   << std::endl;

    std::cout << "H_localA2         vs H_localB2           diff   =   " << std::boolalpha << (H_localA2.array() - H_localB2.array()).cwiseAbs().sum() << std::endl;
    std::cout << "thetaA2           vs thetaB2             diff   =   " << std::boolalpha << (thetaA2.array()   - thetaB2.array()).cwiseAbs().sum() << std::endl;
    std::cout << "<thetaA2          |  thetaB2 >            diff  =   " << std::boolalpha << (thetaA2.adjoint() * thetaB2).cwiseAbs().sum() << std::endl;
    std::cout << "overlapsA2        vs overlapsB2          diff   =   " << std::boolalpha << (overlapsA2.array()- overlapsB2.array()).cwiseAbs().sum() << std::endl;

    std::cout << "eigvecsA2_read    vs eigvecsA2_lapacke   diff   =   " << std::boolalpha << (eigvecsA2.adjoint()         * eigvecsA2_lapacke).diagonal().cwiseAbs().mean() << std::endl;
    std::cout << "eigvecsB2_read    vs eigvecsB2_lapacke   diff   =   " << std::boolalpha << (eigvecsB2.adjoint()         * eigvecsB2_lapacke).diagonal().cwiseAbs().mean() << std::endl;
    std::cout << "eigvecsA2_read    vs eigvecsA2_solver    diff   =   " << std::boolalpha << (eigvecsA2.adjoint()         * eigvecsA2_eig)    .diagonal().cwiseAbs().mean() << std::endl;
    std::cout << "eigvecsB2_read    vs eigvecsB2_solver    diff   =   " << std::boolalpha << (eigvecsB2.adjoint()         * eigvecsB2_eig)    .diagonal().cwiseAbs().mean() << std::endl;
    std::cout << "eigvecsA2_lapacke vs eigvecsA2_solver    diff   =   " << std::boolalpha << (eigvecsA2_lapacke.adjoint() * eigvecsA2_eig)    .diagonal().cwiseAbs().mean() << std::endl;
    std::cout << "eigvecsB2_lapacke vs eigvecsB2_solver    diff   =   " << std::boolalpha << (eigvecsB2_lapacke.adjoint() * eigvecsB2_eig)    .diagonal().cwiseAbs().mean() << std::endl;
    std::cout << "eigvalsA2_read    vs eigvalsA2_lapacke   diff   =   " << std::boolalpha << (eigvalsA2.array()           - eigvalsA2_lapacke .array())  .cwiseAbs().sum() << std::endl;
    std::cout << "eigvalsB2_read    vs eigvalsB2_lapacke   diff   =   " << std::boolalpha << (eigvalsB2.array()           - eigvalsB2_lapacke .array())  .cwiseAbs().sum() << std::endl;
    std::cout << "eigvalsA2_read    vs eigvalsA2_solver    diff   =   " << std::boolalpha << (eigvalsA2.array()           - eigvalsA2_eig     .array())  .cwiseAbs().sum() << std::endl;
    std::cout << "eigvalsB2_read    vs eigvalsB2_solver    diff   =   " << std::boolalpha << (eigvalsB2.array()           - eigvalsB2_eig     .array())  .cwiseAbs().sum() << std::endl;
    std::cout << "eigvalsA2_lapacke vs eigvalsA2_solver    diff   =   " << std::boolalpha << (eigvalsA2_lapacke.array()   - eigvalsA2_eig     .array())  .cwiseAbs().sum() << std::endl;
    std::cout << "eigvalsB2_lapacke vs eigvalsB2_solver    diff   =   " << std::boolalpha << (eigvalsB2_lapacke.array()   - eigvalsB2_eig     .array())  .cwiseAbs().sum() << std::endl;

    std::cout << "max_overlapA2_read     =  " << max_overlapA2_read     << std::endl;
    std::cout << "max_overlapA2_lapacke  =  " << max_overlapA2_lapacke  << std::endl;
    std::cout << "max_overlapA2_eig      =  " << max_overlapA2_eig      << std::endl;
    std::cout << "max_overlapB2_read     =  " << max_overlapB2_read     << std::endl;
    std::cout << "max_overlapB2_lapacke  =  " << max_overlapB2_lapacke  << std::endl;
    std::cout << "max_overlapB2_eig      =  " << max_overlapB2_eig      << std::endl;

    std::cout << "overlap_A2A2_eig       =  " << overlap_A2A2_eig       << std::endl;
    std::cout << "overlap_A2B2_eig       =  " << overlap_A2B2_eig       << std::endl;
    std::cout << "overlap_A2A2_lapacke   =  " << overlap_A2A2_lapacke   << std::endl;
    std::cout << "overlap_A2B2_lapacke   =  " << overlap_A2B2_lapacke   << std::endl;

    if(std::abs(overlap_A1A1_eig     - 1) > 1e-10 ) throw std::runtime_error("overlap_A1A1_eig     : " + std::to_string(overlap_A1A1_eig     ));
    if(std::abs(overlap_A1B1_eig     - 1) > 1e-10 ) throw std::runtime_error("overlap_A1B1_eig     : " + std::to_string(overlap_A1B1_eig     ));
    if(std::abs(overlap_A1A1_lapacke - 1) > 1e-10 ) throw std::runtime_error("overlap_A1A1_lapacke : " + std::to_string(overlap_A1A1_lapacke ));
    if(std::abs(overlap_A1B1_lapacke - 1) > 1e-10 ) throw std::runtime_error("overlap_A1B1_lapacke : " + std::to_string(overlap_A1B1_lapacke ));

    if(std::abs(overlap_A2A2_eig     - 1) > 1e-10 ) throw std::runtime_error("overlap_A2A2_eig     : " + std::to_string(overlap_A2A2_eig     ));
    if(std::abs(overlap_A2B2_eig     - 1) > 1e-10 ) throw std::runtime_error("overlap_A2B2_eig     : " + std::to_string(overlap_A2B2_eig     ));
    if(std::abs(overlap_A2A2_lapacke - 1) > 1e-10 ) throw std::runtime_error("overlap_A2A2_lapacke : " + std::to_string(overlap_A2A2_lapacke ));
    if(std::abs(overlap_A2B2_lapacke - 1) > 1e-10 ) throw std::runtime_error("overlap_A2B2_lapacke : " + std::to_string(overlap_A2B2_lapacke ));

//    std::cout << "theta * eigvecsA2         = \n" << std::fixed << (thetaA2.adjoint() * eigvecsA2).cwiseAbs().transpose() << std::endl;

}


//
// Copyright (c) 2012 Juan Palacios juan.palacios.puyana@gmail.com
// This file is part of minimathlibs.
// Subject to the BSD 2-Clause License
// - see < http://opensource.org/licenses/BSD-2-Clause>
//
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE TestRotation3D
#include <boost/test/unit_test.hpp>
#include <iostream>
#include <cmath>
#include "minimath/matrix.hpp"
#include "minimath/matrix_ops.hpp"
#include "minimath/rotation3d.hpp"
#include "minimath/point3d.hpp"
#include "minimath/geom3d_ops.hpp"
#include "TestRotation3DUtils.h"
#include "Defines.h"

using namespace minimath;

BOOST_AUTO_TEST_SUITE(TestRotation3D)

BOOST_AUTO_TEST_CASE(testInstantiation)
{
  rotation3d<double> rot1, rot2;
}

BOOST_AUTO_TEST_CASE(testDefaultEquality)
{
  BOOST_CHECK(rotation3d<double>() == rotation3d<double>());
}

BOOST_AUTO_TEST_CASE(testCopyConstruction)
{
  rotation3d<double> rot1;
  rotation3d<double> rot2(rot1);
  BOOST_CHECK(rot1 == rot2);
}

BOOST_AUTO_TEST_CASE(testAssignment)
{
  rotation3d<double> rot1, rot2;
  rot2 = rot1;
  BOOST_CHECK(rot1 == rot2);
}

BOOST_AUTO_TEST_CASE(testNullRotation)
{
  rotation3d<double> rot;
  pointxyzd pTest = rot*p100;
  BOOST_CHECK(pTest == pointxyzd(1., 0., 0.));

  pTest = rot*p010;
  BOOST_CHECK(pTest == pointxyzd(0., 1., 0.));

  pTest = rot*p001;
  BOOST_CHECK(pTest == pointxyzd(0., 0., 1.));
}

BOOST_AUTO_TEST_CASE(testRotatePoint45X)
{
  rotation3d<double> rot = rotation3dx<double>(PI/4.); // 45 degree rotation about X
  pointxyzd pTest = rot*p100;
  BOOST_CHECK(pTest == pointxyzd(1., 0., 0.));

  pTest = rot*p010;
  BOOST_CHECK(pTest == pointxyzd(0., cos45, cos45));

  pTest = rot*p001;
  BOOST_CHECK(pTest == pointxyzd(0., -cos45, cos45));
}

BOOST_AUTO_TEST_CASE(testRotatePoint90X)
{
  rotation3d<double> rot = rotation3dx<double>(PI/2.); // 90 degree rotation about X
  pointxyzd pTest = rot*p100;
  BOOST_CHECK(pTest == pointxyzd(1., 0., 0.));

  pTest = rot*p010;
  BOOST_CHECK(pTest == pointxyzd(0., 0., 1.));

  pTest = rot*p001;
  BOOST_CHECK(pTest == pointxyzd(0., -1., 0.));
}

BOOST_AUTO_TEST_CASE(testRotatePoint180X)
{
  rotation3d<double> rot = rotation3dx<double>(PI); // 180 degree rotation about X

  pointxyzd pTest = rot*p100;
  BOOST_CHECK(pTest == pointxyzd(1., 0., 0.));

  pTest = rot*p010;
  BOOST_CHECK(pTest == pointxyzd(0., -1., 0.));

  pTest = rot*p001;
  BOOST_CHECK(pTest == pointxyzd(0., 0., -1.));
}

BOOST_AUTO_TEST_CASE(testRotatePoint45Y)
{
  rotation3d<double> rot = rotation3dy<double> (PI/4.); // 45 degree rotation about X
  pointxyzd pTest = rot*p100;
  BOOST_CHECK(pTest == pointxyzd(cos45, 0., -cos45));

  pTest = rot*p010;
  BOOST_CHECK(pTest == pointxyzd(0., 1., 0));

  pTest = rot*p001;
  BOOST_CHECK(pTest == pointxyzd(cos45, 0, cos45));
}

BOOST_AUTO_TEST_CASE(testRotatePoint90Y)
{
  rotation3d<double> rot = rotation3dy<double> (PI/2.); // 90 degree rotation about X
  pointxyzd pTest = rot*p100;
  BOOST_CHECK(pTest == pointxyzd(0., 0., -1.));

  pTest = rot*p010;
  BOOST_CHECK(pTest == pointxyzd(0., 1., 0.));

  pTest = rot*p001;
  BOOST_CHECK(pTest == pointxyzd(1., 0., 0.));
}

BOOST_AUTO_TEST_CASE(testRotatePoint180Y)
{
  rotation3d<double> rot = rotation3dy<double> (PI); // 180 degree rotation about X

  pointxyzd pTest = rot*p100;
  BOOST_CHECK(pTest == pointxyzd(-1., 0., 0.));

  pTest = rot*p010;
  BOOST_CHECK(pTest == pointxyzd(0., 1., 0.));

  pTest = rot*p001;
  BOOST_CHECK(pTest == pointxyzd(0., 0., -1.));
}

BOOST_AUTO_TEST_CASE(testRotatePoint45Z)
{
  rotation3d<double> rot = rotation3dz<double>(PI/4.); // 45 degree rotation about Z
  pointxyzd pTest = rot*p100;
  BOOST_CHECK(pTest == pointxyzd(cos45, cos45, 0.));

  pTest = rot*p010;
  BOOST_CHECK(pTest == pointxyzd(-cos45, cos45, 0));

  pTest = rot*p001;
  BOOST_CHECK(pTest == pointxyzd(0, 0, 1));
}

BOOST_AUTO_TEST_CASE(testRotatePoint90Z)
{
  rotation3d<double> rot = rotation3dz<double>(PI/2.); // 90 degree rotation about Z
  pointxyzd pTest = rot*p100;
  BOOST_CHECK(pTest == pointxyzd(0., 1., 0.));

  pTest = rot*p010;
  BOOST_CHECK(pTest == pointxyzd(-1., 0., 0.));

  pTest = rot*p001;
  BOOST_CHECK(pTest == pointxyzd(0., 0., 1.));
}

BOOST_AUTO_TEST_CASE(testRotatePoint180Z)
{
  rotation3d<double> rot = rotation3dz<double>(PI); // 180 degree rotation about Z

  pointxyzd pTest = rot*p100;
  BOOST_CHECK(pTest == pointxyzd(-1., 0., 0.));

  pTest = rot*p010;
  BOOST_CHECK(pTest == pointxyzd(0., -1., 0.));

  pTest = rot*p001;
  BOOST_CHECK(pTest == pointxyzd(0., 0., 1.));
}

BOOST_AUTO_TEST_CASE(testInverse)
{
  BOOST_CHECK(TestUtils::testInverseRotation3D<rotation3dx<double> >());
  BOOST_CHECK(TestUtils::testInverseRotation3D<rotation3dy<double> >());
  BOOST_CHECK(TestUtils::testInverseRotation3D<rotation3dz<double> >());
}

BOOST_AUTO_TEST_CASE(testInvert)
{
  BOOST_CHECK(TestUtils::testInvertRotation3D<rotation3dx<double> >());
  BOOST_CHECK(TestUtils::testInvertRotation3D<rotation3dy<double> >());
  BOOST_CHECK(TestUtils::testInvertRotation3D<rotation3dz<double> >());
}

BOOST_AUTO_TEST_CASE(testFindTransformationRotX)
{
  TestUtils::testFindTransformationAxisRot<rotation3dx<double> >();
}

BOOST_AUTO_TEST_CASE(testFindTransformationRotY)
{
  TestUtils::testFindTransformationAxisRot<rotation3dy<double> >();
}

BOOST_AUTO_TEST_CASE(testFindTransformationRotZ)
{
  TestUtils::testFindTransformationAxisRot<rotation3dz<double> >();
}

BOOST_AUTO_TEST_SUITE_END()


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

    This is an example illustrating the use the general purpose non-linear
    least squares optimization routines from the dlib C++ Library.

    This example program will demonstrate how these routines can be used for data fitting.
    In particular, we will generate a set of data and then use the least squares
    routines to infer the parameters of the model which generated the data.
*/

#include <dlib/optimization.h>
#include <iostream>
#include <vector>

using namespace std;
using namespace dlib;

// ----------------------------------------------------------------------------------------

typedef matrix<double,1,1> input_vector;
typedef matrix<double,2,1> parameter_vector;
typedef std::vector<std::pair<input_vector, double> > data_samples;

const double sqrt2pi = std::sqrt(2*pi);
const double sqrtpi = std::sqrt(pi);
// ----------------------------------------------------------------------------------------

// We will use this function to generate data.  It represents a function of 2 variables
// and 3 parameters.   The least squares procedure will be used to infer the values of
// the 3 parameters based on a set of input/output pairs.
double model (
    const input_vector& input,
    const parameter_vector& params
)
{
    const double a = params(0);
    const double b = params(1);

    const double x = input(0);

    /*
               /               2 \
               |   (a - log(x))  |
    sqrt(2) exp| - ------------- |
               |           2     |
               \        2 b      /
    ------------------------------
            b x sqrt(pi) 2
     */

    const double logx = log(x);
    const double nominator = sqrt_2 * exp(- ((a - logx)*(a - logx))/(2*b*b));
    const double denominator = b * x * sqrtpi * 2;
    double out = nominator/denominator;
    return out;

}

// ----------------------------------------------------------------------------------------

// This function is the "residual" for a least squares problem.   It takes an input/output
// pair and compares it to the output of our model and returns the amount of error.  The idea
// is to find the set of parameters which makes the residual small on all the data pairs.
double residual (
    const std::pair<input_vector, double>& data,
    const parameter_vector& params
)
{
    return model(data.first, params) - data.second;
}

// ----------------------------------------------------------------------------------------

// This function is the derivative of the residual() function with respect to the parameters.
parameter_vector residual_derivative (
    const std::pair<input_vector, double>& data,
    const parameter_vector& params
)
{
    /*
    wrt a (mean)
             /               2 \
             |   (a - log(x))  |
  sqrt(2) exp| - ------------- | (2 a - 2 log(x))
             |           2     |
             \        2 b      /
- -----------------------------------------------
                   3
                  b  x sqrt(pi) 4

           /               2 \                            /               2 \
           |   (a - log(x))  |             2              |   (a - log(x))  |
sqrt(2) exp| - ------------- | (a - log(x))    sqrt(2) exp| - ------------- |
           |           2     |                            |           2     |
           \        2 b      /                            \        2 b      /
-------------------------------------------- - ------------------------------
                4                                       2
               b  x sqrt(pi) 2                         b  x sqrt(pi) 2
    */
    parameter_vector der;

    const double a = params(0);
    const double b = params(1);

    const double x = data.first(0);
    const double logx = log(x);
    const double a_logxsq = (a - logx)*(a - logx);
    const double sqb = b*b;
    const double exp_fraction = sqrt_2 * exp(- a_logxsq/(2*sqb));

    const double numeratorA = exp_fraction * (2 * a - 2 * logx);
    const double denominatorA = b*sqb * x * sqrtpi * 4;

    const double numeratorB = exp_fraction * a_logxsq;
    const double denominatorB = sqb*sqb * x * sqrtpi * 2;

    der(0) = -numeratorA/denominatorA;
    der(1) = numeratorB/denominatorB - exp_fraction/(sqb * x * sqrtpi * 2);

    return der;
}

// ----------------------------------------------------------------------------------------

bool read_inputfile(std::string filename, data_samples& DS, double& maxYPos, std::string& errmsg){
    bool out = false;
    std::ifstream datafile(filename.c_str());
    if (!datafile.good()) {
        errmsg = "Can't open the file" + filename;
    }
    else{
        std::string line;
        double y;
        double max_y = 0;
        double x = 1;
        input_vector input;
        while (getline(datafile, line)){
            if (line.size() > 0){
                std::istringstream inp(line.c_str());
                inp >> y;
                input(0) = x;
                DS.push_back(make_pair(input, y));
                if (y > max_y){
                    max_y = y;
                    maxYPos = x;
                }
                x = x + 1.0;
            }
        }
        out = true;
    }
    return out;
}

int main(int argc, char* argv[])
{
    if (argc == 1){
        std::cout << "No input file!" << std::endl;
        return 0;
    }

    std::string infile = argv[1];
    data_samples DS;
    double maxYpos = 1;
    std::string msg;
    bool tf = read_inputfile(infile, DS ,maxYpos,msg);

    parameter_vector params;
    params(0) = log(maxYpos);
    params(1) = 0.3;
    try
    {
        // randomly pick a set of parameters to use in this example
//        const parameter_vector params = 10*randm(3,1);
//        cout << "params: " << trans(params) << endl;

        // Now let's generate a bunch of input/output pairs according to our model.
//        std::vector<std::pair<input_vector, double> > data_samples;
//        input_vector input;
//        for (int i = 0; i < 1000; ++i)
//        {
//            input = 10*randm(2,1);
//            const double output = model(input, params);
//
//            // save the pair
//            data_samples.push_back(make_pair(input, output));
//        }

        // Before we do anything, let's make sure that our derivative function defined above matches
        // the approximate derivative computed using central differences (via derivative()).
        // If this value is big then it means we probably typed the derivative function incorrectly.
        //cout << "derivative error: " << length(residual_derivative(DS[18], params) -
        //                                       derivative(residual)(DS[18], params) ) << endl;

        // Now let's use the solve_least_squares_lm() routine to figure out what the
        // parameters are based on just the data_samples.
        parameter_vector x;
        x = params;

        //cout << "Use Levenberg-Marquardt" << endl;
        // Use the Levenberg-Marquardt method to determine the parameters which
        // minimize the sum of all squared residuals.
        solve_least_squares_lm(objective_delta_stop_strategy(1e-7),
                               residual,
                               residual_derivative,
                               DS,
                               x);

        // Now x contains the solution.  If everything worked it will be equal to params.
        //cout << "inferred parameters: "<< trans(x) << endl;
        //cout << "solution error:      "<< length(x - params) << endl;
        //cout << endl;
        cout << trans(x) << endl;

        /*

        x = params;
        cout << "Use Levenberg-Marquardt, approximate derivatives" << endl;
        // If we didn't create the residual_derivative function then we could
        // have used this method which numerically approximates the derivatives for you.
        solve_least_squares_lm(objective_delta_stop_strategy(1e-7).be_verbose(),
                               residual,
                               derivative(residual),
                               DS,
                               x);

        // Now x contains the solution.  If everything worked it will be equal to params.
        cout << "inferred parameters: "<< trans(x) << endl;
        cout << "solution error:      "<< length(x - params) << endl;
        cout << endl;

        x = params;
        cout << "Use Levenberg-Marquardt/quasi-newton hybrid" << endl;
        // This version of the solver uses a method which is appropriate for problems
        // where the residuals don't go to zero at the solution.  So in these cases
        // it may provide a better answer.
        solve_least_squares(objective_delta_stop_strategy(1e-7).be_verbose(),
                            residual,
                            residual_derivative,
                            DS,
                            x);

        // Now x contains the solution.  If everything worked it will be equal to params.
        cout << "inferred parameters: "<< trans(x) << endl;
        cout << "solution error:      "<< length(x - params) << endl;
        */

    }
    catch (std::exception& e)
    {
        cout << e.what() << endl;
    }
}


#include "average_case_relative_error.hpp"
#include "average_case_error.hpp"
#include "cudd_helpers.hpp"
#include <boost/multiprecision/cpp_int.hpp>

namespace abo::error_metrics {
    std::pair<boost::multiprecision::cpp_dec_float_100, boost::multiprecision::cpp_dec_float_100>
            average_relative_value(const Cudd &mgr, const std::vector<BDD> &f, const std::vector<BDD> &g) {

        BDD zero_so_far = mgr.bddOne();

        boost::multiprecision::cpp_dec_float_100 min_average_result = 0;
        boost::multiprecision::cpp_dec_float_100 max_average_result = 0;
        std::vector<BDD> max_one = abo::util::bdd_max_one(mgr, g);
        for (int i = int(g.size())-1;i>=0;i--) {
            std::vector<BDD> partial_result;
            partial_result.reserve(max_one.size());
            BDD modifier = zero_so_far & max_one[i];
            for (const BDD &b : f) {
                partial_result.push_back(b & modifier);
            }
            auto average = average_value(partial_result);
            min_average_result += average  / (std::pow(2.0, i + 1) - 1);
            max_average_result += average  / std::pow(2.0, i);

            zero_so_far &= !max_one[i];
        }

        return {min_average_result, max_average_result};
    }

    std::pair<boost::multiprecision::cpp_dec_float_100, boost::multiprecision::cpp_dec_float_100>
            average_relative_error_bounds(const Cudd &mgr, const std::vector<BDD> &f, const std::vector<BDD> &f_hat,
                                          const util::NumberRepresentation num_rep) {

        std::vector<BDD> absolute_difference = abo::util::bdd_absolute_difference(mgr, f, f_hat, num_rep);
        return average_relative_value(mgr, absolute_difference, abo::util::abs(mgr, f));
    }

    boost::multiprecision::cpp_dec_float_100 average_relative_error_add(const Cudd &mgr, const std::vector<BDD> &f, const std::vector<BDD> &f_hat,
                                                                        const NumberRepresentation num_rep) {

        ADD diff = abo::util::absolute_difference_add(mgr, f, f_hat, num_rep);
        ADD respective_diff = diff.Divide(abo::util::bdd_forest_to_add(mgr, abo::util::abs(mgr, f)).Maximum(mgr.addOne()));
        std::vector<std::pair<double, unsigned long>> terminal_values = abo::util::add_terminal_values(respective_diff);

        boost::multiprecision::cpp_dec_float_100 sum = 0;
        boost::multiprecision::uint256_t path_sum = 0;
        for (auto p : terminal_values) {
            boost::multiprecision::cpp_dec_float_100 value(p.first);
            sum += value * p.second;
            path_sum += p.second;
        }
        return boost::multiprecision::cpp_dec_float_100(sum) /
                boost::multiprecision::cpp_dec_float_100(path_sum);
    }

    boost::multiprecision::cpp_dec_float_100 average_relative_error_symbolic_division(const Cudd &mgr, const std::vector<BDD> &f, const std::vector<BDD> &f_hat,
                                                                                unsigned int num_extra_bits, const NumberRepresentation num_rep) {

        std::vector<BDD> absolute_difference = abo::util::bdd_absolute_difference(mgr, f, f_hat, num_rep);

        std::vector<BDD> no_zero = abo::util::bdd_max_one(mgr, abo::util::abs(mgr, f));
        std::vector<BDD> divided = abo::util::bdd_divide(mgr, absolute_difference, no_zero, num_extra_bits);

        return average_value(divided) / std::pow(2.0, num_extra_bits);
    }

}


/*
 * libcluster -- A collection of hierarchical Bayesian clustering algorithms.
 * Copyright (C) 2013 Daniel M. Steinberg (daniel.m.steinberg@gmail.com)
 *
 * This file is part of libcluster.
 *
 * libcluster is free software: you can redistribute it and/or modify it under
 * the terms of the GNU Lesser General Public License as published by the Free
 * Software Foundation, either version 3 of the License, or (at your option)
 * any later version.
 *
 * libcluster 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 Lesser General Public License
 * for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with libcluster. If not, see <http://www.gnu.org/licenses/>.
 */

#include "probutils.h"
#include <boost/math/special_functions.hpp>

//
// Namespaces
//

using namespace std;
using namespace Eigen;

//
// Local Constants
//

const double EIGCONTHRESH = 1.0e-8f;
const int    MAXITER      = 100;

//
// Public Functions
//

RowVectorXd probutils::mean (const MatrixXd& X)
{
  return X.colwise().sum()/X.rows();
}

RowVectorXd probutils::mean (const vector<MatrixXd>& X)
{
  const int J = X.size(),
            D = X[0].cols();
  int N = 0;
  RowVectorXd mean = RowVectorXd::Zero(D);

  for (int j = 0; j < J; ++j)
  {
    if (X[j].cols() != D)
      throw invalid_argument("X dimensions are inconsistent between groups!");

    mean += X[j].colwise().sum();
    N    += X[j].rows();
  }
  return mean / N;
}

RowVectorXd probutils::stdev (const MatrixXd& X)
{
  RowVectorXd meanX = mean(X);
  return ((X.rowwise() - meanX).array().square().colwise().sum()
          / (X.rows()-1)).sqrt();
}

MatrixXd probutils::cov (const MatrixXd& X)
{
  if (X.rows() <= 1)
    throw invalid_argument("Insufficient no. of observations.");

  MatrixXd X_mu = X.rowwise() - probutils::mean(X); // X - mu
  return (X_mu.transpose()*X_mu)/(X.rows()-1);      // (X-mu)'*(X-mu)/(N-1)
}

MatrixXd probutils::cov (const vector<MatrixXd>& X)
{
  const int J = X.size(),
            D = X[0].cols();
  int N = 0;
  const RowVectorXd mean = probutils::mean(X);
  MatrixXd cov = MatrixXd::Zero(D, D),
           X_mu;

  for (int j = 0; j < J; ++j)
  {
    if (X[j].rows() <= 1)
      throw invalid_argument("Insufficient no. of observations.");
    X_mu = X[j].rowwise() - mean;
    N   += X[j].rows();
    cov.noalias() += (X_mu.transpose() * X_mu); // (X_j-mu)'*(X_j-mu)
  }

  return cov / (N-1);
}

VectorXd probutils::mahaldist (
    const MatrixXd& X,
    const RowVectorXd& mu,
    const MatrixXd& A
    )
{
  // Check for same number of dimensions, D
  if((X.cols() != mu.cols()) || (X.cols() != A.cols()))
    throw invalid_argument("Arguments do not have the same dimensionality");

  // Check if A is square
  if (A.rows() != A.cols())
    throw invalid_argument("Matrix A must be square!");

  // Decompose A
  LDLT<MatrixXd> Aldl(A);

  // Check if A is PD
  if ((Aldl.vectorD().array() <= 0).any() == true)
    throw invalid_argument("Matrix A is not positive definite");

  // Do the Mahalanobis distance for each sample (N times)
  MatrixXd X_mu = (X.rowwise() - mu).transpose();
  return ((X_mu.array() * (Aldl.solve(X_mu)).array())
          .colwise().sum()).transpose();
}

VectorXd probutils::logsumexp (const MatrixXd& X)
{
  const VectorXd mx = X.rowwise().maxCoeff(); // Get max of each row

  // Perform the sum(exp(x - mx)) part
  ArrayXd se = ((X.colwise() - mx).array().exp()).rowwise().sum();

  // return total log(sum(exp(x))) - hoping for return value optimisation
  return (se.log()).matrix() + mx;
}

double probutils::eigpower (const MatrixXd& A, VectorXd& eigvec)
{
  // Check if A is square
  if (A.rows() != A.cols())
    throw invalid_argument("Matrix A must be square!");

  // Check if A is a scalar
  if (A.rows() == 1)
  {
    eigvec.setOnes(1);
    return A(0,0);
  }

  // Initialise working vectors
  VectorXd v = VectorXd::LinSpaced(A.rows(), -1, 1);
  VectorXd oeigvec(A.rows());

  // Initialise eigenvalue and eigenvectors etc
  double eigval = v.norm();
  double vdist = numeric_limits<double>::infinity();
  eigvec = v/eigval;

  // Loop until eigenvector converges or we reach max iterations
  for (int i=0; (vdist>EIGCONTHRESH) && (i<MAXITER); ++i)
  {
    oeigvec = eigvec;
    v.noalias() = A * oeigvec;
    eigval = v.norm();
    eigvec = v/eigval;
    vdist = (eigvec - oeigvec).norm();
  }

  return eigval;
}

double probutils::logdet (const MatrixXd& A)
{
  // Check if A is square
  if (A.rows() != A.cols())
    throw invalid_argument("Matrix A must be square!");

  VectorXd d = A.ldlt().vectorD();  // Get the diagonal from a Cholesky decomp.

  // Check if A is PD
  if ((d.array() <= 0).any() == true)
    throw domain_error("Matrix A is not positive definite.");

  return (d.array().log()).sum();   // ln(det(A)) = sum(log(d))
}

MatrixXd probutils::mxdigamma (const MatrixXd& X)
{
  const int I = X.rows(),
            J = X.cols();
  MatrixXd result(I, J);

  for (int i = 0; i < I; ++i)
    for (int j = 0; j < J; ++j)
      result(i,j) = boost::math::digamma(X(i, j));

  return result;
}

MatrixXd probutils::mxlgamma (const MatrixXd& X)
{
  const int I = X.rows(),
            J = X.cols();
  MatrixXd result(I, J);

  for (int i = 0; i < I; ++i)
    for (int j = 0; j < J; ++j)
      result(i, j) = boost::math::lgamma(X(i, j));

  return result;
}


#ifndef PDC_gauss_hh_included
#define PDC_gauss_hh_included

#include <dlib/optimization.h>
#include <iostream>
#include <vector>

#include "../classes/structs.hh"

typedef dlib::matrix<double,2,1> input_vector;
typedef dlib::matrix<double,3,1> parameter_vector;

// ----------------------------------------------------------------------------------------

// We will use this function to generate data.  It represents a function of 2 variables
// and 3 parameters.   The least squares procedure will be used to infer the values of
// the 3 parameters based on a set of input/output pairs.
double model (
    const input_vector& input,
    const parameter_vector& params
)
{
    const double p0 = params(0);
    const double p1 = params(1);
    const double p2 = params(2);

    const double i0 = input(0);
    const double i1 = input(1);

    const double temp = p0*i0 + p1*i1 + p2;

    return temp*temp;
}

// ----------------------------------------------------------------------------------------

// This function is the "residual" for a least squares problem.   It takes an input/output
// pair and compares it to the output of our model and returns the amount of error.  The idea
// is to find the set of parameters which makes the residual small on all the data pairs.
double residual (
    const std::pair<input_vector, double>& data,
    const parameter_vector& params
)
{
    return model(data.first, params) - data.second;
}

// ----------------------------------------------------------------------------------------

// This function is the derivative of the residual() function with respect to the parameters.
parameter_vector residual_derivative (
    const std::pair<input_vector, double>& data,
    const parameter_vector& params
)
{
    parameter_vector der;

    const double p0 = params(0);
    const double p1 = params(1);
    const double p2 = params(2);

    const double i0 = data.first(0);
    const double i1 = data.first(1);

    const double temp = p0*i0 + p1*i1 + p2;

    der(0) = i0*2*temp;
    der(1) = i1*2*temp;
    der(2) = 2*temp;

    return der;
}

PDC::gaussParameter fitGauss(std::vector<double>& xVals, std::vector<double>& yValsData, PDC::gaussParameter initialParams) {
    try {
        // randomly pick a set of parameters to use in this example
        const parameter_vector params = 10*dlib::randm(3,1);
        std::cout << "params: " << trans(params) << std::endl;

        // Now let's generate a bunch of input/output pairs according to our model.
        std::vector<std::pair<input_vector, double> > data_samples;
        input_vector input;
        for (int i = 0; i < 1000; ++i)
        {
            input = 10*dlib::randm(2,1);
            const double output = model(input, params);

            // save the pair
            data_samples.push_back(std::make_pair(input, output));
        }

        // Before we do anything, let's make sure that our derivative function defined above matches
        // the approximate derivative computed using central differences (via derivative()).
        // If this value is big then it means we probably typed the derivative function incorrectly.
        std::cout << "derivative error: " << length(residual_derivative(data_samples[0], params) -
                                               derivative(residual)(data_samples[0], params) ) << std::endl;

        // Now let's use the solve_least_squares_lm() routine to figure out what the
        // parameters are based on just the data_samples.
        parameter_vector x;
        x = 1;

        std::cout << "Use Levenberg-Marquardt" << std::endl;
        // Use the Levenberg-Marquardt method to determine the parameters which
        // minimize the sum of all squared residuals.
        dlib::solve_least_squares_lm(dlib::objective_delta_stop_strategy(1e-7).be_verbose(),
                               residual,
                               residual_derivative,
                               data_samples,
                               x);

        // Now x contains the solution.  If everything worked it will be equal to params.
        std::cout << "inferred parameters: "<< trans(x) << std::endl;
        std::cout << "solution error:      "<< length(x - params) << std::endl;
        std::cout << std::endl;

        x = 1;
        std::cout << "Use Levenberg-Marquardt, approximate derivatives" << std::endl;
        // If we didn't create the residual_derivative function then we could
        // have used this method which numerically approximates the derivatives for you.
        dlib::solve_least_squares_lm(dlib::objective_delta_stop_strategy(1e-7).be_verbose(),
                               residual,
                               derivative(residual),
                               data_samples,
                               x);

        // Now x contains the solution.  If everything worked it will be equal to params.
        std::cout << "inferred parameters: "<< trans(x) << std::endl;
        std::cout << "solution error:      "<< length(x - params) << std::endl;
        std::cout << std::endl;

        x = 1;
        std::cout << "Use Levenberg-Marquardt/quasi-newton hybrid" << std::endl;
        // This version of the solver uses a method which is appropriate for problems
        // where the residuals don't go to zero at the solution.  So in these cases
        // it may provide a better answer.
        dlib::solve_least_squares(dlib::objective_delta_stop_strategy(1e-7).be_verbose(),
                            residual,
                            residual_derivative,
                            data_samples,
                            x);

        // Now x contains the solution.  If everything worked it will be equal to params.
        std::cout << "inferred parameters: "<< trans(x) << std::endl;
        std::cout << "solution error:      "<< length(x - params) << std::endl;

    }
    catch (std::exception& e) {
        std::cout << "Error while fitting: " << e.what() << std::endl;
    }
}

#endif /* PDC_gauss_hh_included */


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

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Tridiagonalization<MatrixXf> tri;
MatrixXf X = MatrixXf::Random(4,4);
MatrixXf A = X + X.transpose();
tri.compute(A);
cout << "The matrix T in the tridiagonal decomposition of A is: " << endl;
cout << tri.matrixT() << endl;
tri.compute(2*A); // re-use tri to compute eigenvalues of 2A
cout << "The matrix T in the tridiagonal decomposition of 2A is: " << endl;
cout << tri.matrixT() << endl;

  return 0;
}


#include <iostream>
#include <cmath>
#include <string>
#include <random>
#include <chrono>
#include <fstream>
#include <tuple>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <unsupported/Eigen/SparseExtra>
#include "block_iterative_solvers.hpp"
#include "linear_algebra_addon.hpp"
using namespace std;
using namespace Eigen;
using namespace std::chrono;

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

    double tol= 1e-8;
    int itermax= 1000;

    std::srand((unsigned int) time(0));
    int L=atoi(argv[1]); // dimension of matrix A

    MatrixXcd A;

    switch (argc)
    {
    case 2:
        {
        // load A from a file in matrix market format .mtx
        cout << "please enter filename for A (.mtx): " ;
        string matfname;
        cin >> matfname;
        SparseMatrix<complex<double>> mat;
        //Matrix collections is available in   http://www.cise.ufl.edu/research/sparse/matrices/
        if(!loadMarket(mat,matfname)){
            cerr << "can't open file" << matfname << endl;
            exit(EXIT_FAILURE);
        }
        cout << "A was loaded from " <<  matfname << endl;
        A= MatrixXcd(mat);

        break;
        }
    case 3:
        {
        // Set A as a random matrix
        int n=atoi(argv[2]); // number of RHS vector
        A= MatrixXcd::Random(n,n);
        A= A.transpose()*A; // complex-symmetric
        cout << "A is a " << n << " x " << n << " square random matrix" << endl;
        break;
        }
    default:
        {

        break;
        }
    }

    MatrixXcd B= MatrixXcd::Random(A.rows(),L);

    {
        time_point<steady_clock> start= steady_clock::now();
        cout << "solving AX=B using bl_cocg_rq ..." << endl;
            MatrixXcd X= bl_cocg_rq(A, B, tol, itermax); // only applicable to complex-symmetric matrix A
        cout << " bl_cocg_rq relative error: " << (A*X-B).norm()/B.norm() << endl << endl;
        auto end= steady_clock::now();
        cout << "bl_cocg_rq computation time= " << duration_cast<seconds>((end - start)).count()  << " sec" << endl << endl<< endl;
    }

    {
        time_point<steady_clock> start= steady_clock::now();
        cout << "solving AX=B using bl_bicg_rq ..." << endl;
            MatrixXcd X= bl_bicg_rq(A, B, tol, itermax); // applicable to general matrix A but needs A.adjoint()
        cout << " bl_bicg_rq relative error: " << (A*X-B).norm()/B.norm() << endl << endl;
        auto end= steady_clock::now();
        cout << "bl_bicg_rq computation time= " << duration_cast<seconds>((end - start)).count() << " sec" << endl << endl<< endl;
    }

    {
        time_point<steady_clock> start= steady_clock::now();
        cout << "solving AX=B using bl_bicr_rq ..." << endl;
            MatrixXcd X= bl_bicr_rq(A, B, tol, itermax); // applicable to general matrix A but needs A.adjoint()
        cout << " bl_bicr_rq relative error: " << (A*X-B).norm()/B.norm() << endl << endl;
        auto end= steady_clock::now();
        cout << "bl_bicr_rq computation time= " << duration_cast<seconds>((end - start)).count() << " sec" << endl << endl<< endl;
    }

    {
        time_point<steady_clock> start= steady_clock::now();
        cout << "solving AX=B using bl_bicgstab_rq ..." << endl;
            MatrixXcd X= bl_bicgstab_rq(A, B, tol, itermax); // only applicable to complex-symmetric matrix A
        cout << " bl_bicgstab_rq relative error: " << (A*X-B).norm()/B.norm() << endl << endl;
        auto end= steady_clock::now();
        cout << "bl_bicgstab_rq computation time= " << duration_cast<seconds>((end - start)).count() << " sec" << endl << endl<< endl;
    }

    {
        time_point<steady_clock> start= steady_clock::now();
        cout << "solving AX=B using bl_bicgstab ..." << endl;
            MatrixXcd X= bl_bicgstab(A, B, tol, itermax); // only applicable to complex-symmetric matrix A
        cout << " bl_bicgstab relative error: " << (A*X-B).norm()/B.norm() << endl << endl;
        auto end= steady_clock::now();
        cout << "bl_bicgstab computation time= " << duration_cast<seconds>((end - start)).count() << " sec" << endl << endl<< endl;
    }

    {
        time_point<steady_clock> start= steady_clock::now();
        cout << "solving AX=B using bl_bicggr ..." << endl;
            MatrixXcd X= bl_bicggr(A, B, tol, itermax); // only applicable to complex-symmetric matrix A
        cout << " bl_bicggr relative error: " << (A*X-B).norm()/B.norm() << endl << endl;
        auto end= steady_clock::now();
        cout << "bl_bicggr computation time= " << duration_cast<seconds>((end - start)).count() << " sec" << endl << endl<< endl;
    }

}


#include <iostream>
#include <ctime>
using namespace std;

#include <Eigen/Core>
#include <Eigen/Dense>

#define MATRIX_SIZE 50

int main(int argc, char** argv) {
    Eigen::Matrix<float,2,3> matrix_23;
    Eigen::Vector3d v_3d;//Matrix<double,3,1>
    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero();//Matrix<double,3,3>
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_dynamic;
    Eigen::MatrixXd matric_x;

    matrix_23<<1,2,3,4,5,6;
    cout<<matrix_23<<endl;

    for (int i = 0; i < 1 ; i++)
        for (int j = 0; j < 2 ; j++)
            cout<<matrix_23(i,j)<<endl;

    v_3d<<3,2,1;
    Eigen::Matrix<double,2,1> result=matrix_23.cast<double>()*v_3d; //cast 转换类型
    cout<<result<<endl;

    matrix_33 = Eigen::Matrix3d::Random();
    cout <<matrix_33<<endl<<endl;

    cout<<matrix_33.transpose()<<endl;
    cout<<matrix_33.sum() <<endl;
    cout<<matrix_33.trace()<<endl;
    cout<<10*matrix_33<<endl;
    cout<<matrix_33.inverse()<<endl;
    cout<<matrix_33.determinant()<<endl;

    //特征值
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver (matrix_33);
    cout<< "Eigen values = "<<eigen_solver.eigenvalues() <<endl;
    cout<< "Eigen vectors = "<<eigen_solver.eigenvectors() <<endl;

    Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE>matrix_NN;
    matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);
    Eigen::Matrix<double, MATRIX_SIZE, 1> v_Nd;
    v_Nd =Eigen::MatrixXd::Random(MATRIX_SIZE,1);

    //计时
    clock_t time_stt=clock();
    Eigen::Matrix<double,MATRIX_SIZE,1> x=matrix_NN.inverse()*v_Nd;
    cout<<"time use in normal inverse is "<<1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<endl;

    time_stt=clock();
    x =matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout<<"time use in Qr composition is "<<1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<endl;

    return 0;
}


/********************************************************************
 * Implements multivariate regression analysis in an attempt to learn
 * the weight/hypothesis coefficients for a system of eqations.
 * Created: Jun 17 2020
 * Author: Ethan Patterson
 *******************************************************************/

#include <iostream>
#include <fstream>
#include <Eigen/Dense> // Linear Algebra library. Don't forget to add path to Makefile.

#include "HousingData.h" // For reading data files.

#ifndef ITR
#define ITR 2000
#endif

#ifndef ALPHA
#define ALPHA 0.01
#endif

// Made for convinance of managing
// the max and min values of the data.
struct Tuple{
    float x;
    float y;
};

void loadData(Eigen::MatrixXf &X, Eigen::VectorXf &Y, std::ifstream &fp){
    int r = 0;
    HousingData data;
    // Read data line by line and sotre it into X matrix.
    while (fp >> data){
        // Again this could be put into an array.
        // See HousingData.h for why it is currently structered this way.
        //X(r,0) = 1; // Dummy value.
        X(r,0) = std::stof(data.crim, NULL);
        X(r,1) = std::stof(data.zn, NULL);
        X(r,2) = std::stof(data.indus, NULL);
        X(r,3) = std::stof(data.chas, NULL);
        X(r,4) = std::stof(data.nox, NULL);
        X(r,5) = std::stof(data.rm, NULL);
        X(r,6) = std::stof(data.age, NULL);
        X(r,7) = std::stof(data.dis, NULL);
        X(r,8) = std::stof(data.rad, NULL);
        X(r,9) = std::stof(data.tax, NULL);
        X(r,10) = std::stof(data.ptratio, NULL);
        X(r,11) = std::stof(data.B, NULL);
        X(r,12) = std::stof(data.lstat, NULL);

        Y(r) = std::stof(data.medv, NULL); // Values of interest.
        r++;
    }

    fp.clear();
    fp.seekg(0, fp.beg);// Reset to head of file.
}
/**
 * @param fp is a file pointer.
 * Function gets the number of rows in a file.
 */
int getFileRowCount(std::ifstream &fp){
    int num_of_rows = 0;
    std::string rows;
    while (std::getline( fp, rows ))
        num_of_rows++;

    fp.clear();
    fp.seekg(0, fp.beg); // Reset to head of file.
    return num_of_rows;
}

/**
 * @param X Matrix of Quantitative Variables.
 * @param W Learned vector of weight coefficients
 * @param Y Vector of Response Variables.
 * Measures the Sum Squared Error of the learned weights.
 */
float leastSquares(Eigen::MatrixXf &X, Eigen::VectorXf &W, Eigen::VectorXf &Y){
    Eigen::VectorXf e( Y.size() );
    e = (Y - X*W);
    for (int i = 0; i < e.size(); i++)
        e[i] = e[i]*e[i]/Y.size();

    return (float)e.sum();
}
/**
 * @param X Matrix of Quantitative Variables.
 * @param W Learned vector of weight coefficients, hypothosis.
 * @param Y Vector of Response Variables.
 * @param alpha The learning rate to apply.
 * @param iter The number of iterations to complete.
 * Use batch gradient descent to minimize the cost/error of the system.
 * Returns the cost history of the system over iterations.
 */
float* BGD(Eigen::MatrixXf &X, Eigen::VectorXf &W, Eigen::VectorXf &Y, float alpha, int itr, const Tuple &maxmin){
    Eigen::VectorXf Hypo ( Y.size() );
    Eigen::VectorXf Loss ( Y.size() );
    Eigen::VectorXf Gradient ( Y.size() );
    float* costHistory = new float[itr];

    float cost = 0.0;
    for (int i = 0; i < itr; i++){
        Hypo = X*W;
        Loss = Hypo - Y;
        Gradient = (X.transpose() * Loss) / Y.size();
        W = W - alpha * Gradient;
        cost = leastSquares(X, W, Y);
        costHistory[i] = cost*(maxmin.x - maxmin.y) + maxmin.y;
    }
    return costHistory;
}
/**
 * @param X Matrix of Quantitative Variables.
 * @param Y Vector of Response Variables.
 * Nomralize the data between 0 and 1.
 * Returns a tuple of the max and min value.
 * Max is the x component and min is the y.
 */
Tuple normalize(Eigen::MatrixXf &X, Eigen::VectorXf &Y){
    float max_x, max_y, max;
    float min_x, min_y, min;
    Tuple maxmin;

    Eigen::MatrixXf::Index maxRow,maxCol;
    Eigen::MatrixXf::Index minRow,minCol;

    max_x = X.maxCoeff(&maxRow, &maxCol);
    max_y = Y.maxCoeff(&maxRow, &maxCol);

    min_x = X.minCoeff(&minRow, &minCol);
    min_y = Y.minCoeff(&minRow, &minCol);

    if (max_x < max_y)
        max = max_y;
    else
        max = max_x;

    if (min_x < min_y)
        min = min_x;
    else
        min = min_y;

    for (int i = 0; i < X.rows(); i++){
        for (int j = 0; j < X.cols(); j++){
            X(i,j) = (X(i,j) - min) / (max - min);
        }
    }

    for (int i = 0; i < Y.rows(); i++)
        Y(i) = (Y(i) - min) / (max - min);

    maxmin.x = max;
    maxmin.y = min;

    return maxmin;
}
/**
 * @param cost Dynamic array of cost history.
 * @param W Learned vector of weight coefficients, hypothesis.
 * @param Y Vector of Response Variables.
 * Saves data produced such as the cost history, weights/hypotheses, and normalization min max.
 */
void saveData(float *cost, const Tuple &maxmin, Eigen::VectorXf &W){
    std::ofstream weights("weights.csv");
    std::ofstream norm("normalize.csv");
    std::ofstream costHistory("cost_history.csv");

    if (weights.is_open()){
        for (int i = 0; i < W.size()-1; i++)
            weights << W(i) << ",";
        weights << W(W.size()-1);
    }
    weights.close();

    if (norm.is_open()){
        norm << "max" << "," << maxmin.x << "\n";
        norm << "min" << "," << maxmin.y << "\n";
    }
    norm.close();

    if(costHistory.is_open()){
        for (int i = 0; i < ITR; i++)
            if ( i % 2 ) // No need to see cost at every step.
                costHistory << i << "," << cost[i] << "\n";
    }
    costHistory.close();
}

int main()
{
    // Files in training and testing data.
    std::ifstream fp_train("./data/data/housing_train.csv");
    std::ifstream fp_test("./data/data/housing_test.csv");

    // Build maxtix for traning and testing data.
    Eigen::MatrixXf X(getFileRowCount(fp_train), COL_SIZE);
    Eigen::MatrixXf X_test(getFileRowCount(fp_test), COL_SIZE);

    // Build Vectors for target variables and weight/hypotheses.
    Eigen::VectorXf Y( X.rows() );
    Eigen::VectorXf Y_test( X_test.rows() );
    Eigen::VectorXf W ( X.cols() );// Size is the number of features/dimensions.

    Tuple maxmin; // Used to keep track of max and min value during normalization.
    float *costHistory; // Pointer for array to cost history.

    // Load data into matrices and vectors.
    loadData(X, Y, fp_train);
    loadData(X_test, Y_test, fp_test);
    fp_train.close(); // Don't need this anymore, data is now in the matrix.
    fp_test.close();

    // Set all initial weight/hypothesis values to 0.
    W = Eigen::VectorXf::Ones( X.cols() ) * 0;

    normalize(X, Y); // Normalize data.
    maxmin = normalize(X_test, Y_test); // Keep max and min values for future data sets.
    std::cout << "Max: " << maxmin.x << " Min: " << maxmin.y << std::endl;

    // Run batch gradient descent.
    costHistory = BGD(X, W, Y, ALPHA, ITR, maxmin);
    std::cout << "SSE for training set: " << leastSquares(X, W, Y) *
     (maxmin.x - maxmin.y) + maxmin.y << std::endl;
    std::cout << "SSE for testing set: " << leastSquares(X_test, W, Y_test) *
     (maxmin.x - maxmin.y) + maxmin.y << std::endl;

    saveData(costHistory, maxmin, W);
    delete costHistory;

    return 0;
}


/*
 * Copyright 2009-2020 The VOTCA Development Team (http://www.votca.org)
 *
 * 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.
 *
 */

#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE linalg_test

// Standard includes
#include <iostream>

// Third party includes
#include <boost/test/unit_test.hpp>

// Local VOTCA includes
#include "votca/tools/eigenio_matrixmarket.h"
#include "votca/tools/linalg.h"

using namespace votca::tools;

BOOST_AUTO_TEST_SUITE(linalg_test)

BOOST_AUTO_TEST_CASE(linalg_constrained_qrsolve_test) {

  Eigen::VectorXd b = Eigen::VectorXd::Zero(3);
  b(0) = 11;
  b(1) = -3;
  b(2) = 8;
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3, 3);
  A(0, 0) = 1;
  A(0, 1) = 1;
  A(0, 2) = 1;
  A(1, 0) = 1;
  A(1, 1) = -1;
  A(2, 1) = 1;
  A(2, 2) = 1;

  Eigen::MatrixXd B = Eigen::MatrixXd::Zero(1, 3);
  B(0, 1) = -1;
  B(0, 2) = 3;
  Eigen::VectorXd x = linalg_constrained_qrsolve(A, b, B);
  Eigen::VectorXd x_ref = Eigen::VectorXd::Zero(3);
  x_ref(0) = 3;
  x_ref(1) = 6;
  x_ref(2) = 2;

  bool equal = x_ref.isApprox(x, 1e-7);

  if (!equal) {
    std::cout << "result" << std::endl;
    std::cout << x << std::endl;
    std::cout << "ref" << std::endl;
    std::cout << x_ref << std::endl;
  }
  BOOST_CHECK_EQUAL(equal, true);
}

BOOST_AUTO_TEST_CASE(linalg_mkl_test) {

  votca::Index nmax = 10;

  Eigen::MatrixXd H = EigenIO_MatrixMarket::ReadMatrix(
      std::string(TOOLS_TEST_DATA_FOLDER) + "/linalg/H.mm");

  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(H);
  Eigen::MatrixXd V_ref = es.eigenvectors().block(0, 0, H.rows(), nmax);
  Eigen::VectorXd E_ref = es.eigenvalues().segment(0, nmax);

  EigenSystem result = linalg_eigenvalues(H, nmax);
  bool check_energy = E_ref.isApprox(result.eigenvalues(), 0.0001);

  bool check_vector =
      result.eigenvectors().cwiseAbs().isApprox(V_ref.cwiseAbs(), 0.0001);

  if (!check_vector || !check_energy) {
    std::cout << "E_ref" << std::endl;
    std::cout << E_ref << std::endl;
    std::cout << "E" << std::endl;
    std::cout << result.eigenvalues() << std::endl;
    std::cout << "V_ref" << std::endl;
    std::cout << V_ref << std::endl;
    std::cout << "V" << std::endl;
    std::cout << result.eigenvectors() << std::endl;
  }

  BOOST_CHECK_EQUAL(check_energy, 1);
  BOOST_CHECK_EQUAL(check_vector, 1);
}

BOOST_AUTO_TEST_SUITE_END()


#include "case.hpp"

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

using namespace Eigen;

Case1::Case1(const Ref<const MatrixXd> &matM, const Ref<const MatrixXd> &matD,
             const Ref<const MatrixXd> &matK)

    : ndim_(matM.cols()), matM_(matM), matD_(matD), matK_(matK),
      matC_(ndim_ * 2, ndim_ * 2), matG_(ndim_ * 2, ndim_ * 2),
      alphas(ndim_ * 2), betas(ndim_ * 2) {
  MatrixXd mzero = MatrixXd::Zero(ndim_, ndim_);
  MatrixXd mone = MatrixXd::Identity(ndim_, ndim_);
  matC_ << -matD_, -matK_, mone, mzero;
  matG_ << matM_, mzero, mzero, mone;

  GeneralizedEigenSolver<MatrixXd> ges(matC_, matG_, false);
  alphas = ges.alphas();
  betas = ges.betas();
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
Copyright (C) 2005, 2008 Klaus Spanderen
Copyright (C) 2007 StatPro Italia srl
Copyright (C) 2015 CompatibL

This file is part of QuantLib, a free-software/open-source library
for financial quantitative analysts and developers - http://quantlib.org/

QuantLib is free software: you can redistribute it and/or modify it
under the terms of the QuantLib license.  You should have received a
copy of the license along with this program; if not, please email
<quantlib-dev@lists.sf.net>. The license is also available online at
<http://quantlib.org/license.shtml>.

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 license for more details.
*/

// Based on batesmodel.cpp file from test-suite.

#ifndef cl_adjoint_bates_model_impl_hpp
#define cl_adjoint_bates_model_impl_hpp
#pragma once

#include "adjointbatesmodeltest.hpp"
#include "utilities.hpp"
#include "adjointtestutilities.hpp"
#include "adjointtestbase.hpp"
#include <ql/time/calendars/target.hpp>
#include <ql/processes/batesprocess.hpp>
#include <ql/processes/merton76process.hpp>
#include <ql/instruments/europeanoption.hpp>
#include <ql/time/daycounters/actualactual.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/termstructures/yield/zerocurve.hpp>
#include <ql/pricingengines/blackformula.hpp>
#include <ql/math/optimization/levenbergmarquardt.hpp>
#include <ql/pricingengines/vanilla/batesengine.hpp>
#include <ql/pricingengines/vanilla/jumpdiffusionengine.hpp>
#include <ql/pricingengines/vanilla/analyticeuropeanengine.hpp>
#include <ql/pricingengines/vanilla/mceuropeanhestonengine.hpp>
#include <ql/pricingengines/vanilla/fdbatesvanillaengine.hpp>
#include <ql/models/equity/batesmodel.hpp>
#include <ql/models/equity/hestonmodelhelper.hpp>
#include <ql/time/period.hpp>
#include <ql/quotes/simplequote.hpp>
#include <boost/make_shared.hpp>

using namespace QuantLib;
using namespace boost::unit_test_framework;

// Based on QuantLib\test-suite\batesmodel.cpp file

#define OUTPUT_FOLDER_NAME "AdjointBates"

/*!
AdjointBatesModelsTest is tested using adjoint differentiation for the class BatesProcess.

The Bates model (1996) combines the Merton and Heston approaches and proposes a model
with stochastic volatility and jumps governed by:
\f[
\begin{array}{rcl}
dS(t)  &=& (r-d-\lambda m) S dt +\sqrt{v} S dW_1 + (\exp^J - 1) S dN \\
dv(t)  &=& \kappa (\theta - v) dt + \sigma \sqrt{v} dW_2 \\
dW_1 dW_2 &=& \rho dt \\
\omega(J) &=& \frac{1}{\sqrt{2\pi \delta^2}}
\exp\left[-\frac{(J-\nu)^2}{2\delta^2}\right]
\end{array}
\f]
Here:
\f$ S(t) \f$                    is an asset (underlying) price,
\f$ v(t) \f$                    is volatility,
\f$ J \f$ and \f$ \omega(J) \f$ are a jump size and its PDF
\f$ r \f$                       is a risk-free (domestic) interest rate,
\f$ d \f$                       is a dividend (foreign interest) rate,
\f$ m \f$                       is an average jump amplitude
\f$ \sigma \f$                  is a volatility of volatility,
\f$ \kappa \f$                  is a mean-reverting rate,
\f$ \theta \f$                  is a long-term variance,
\f$ \rho \f$                    is correlation between price and variance (Wiener processes \f$ dW_1 \f$ and \f$ dW_2 \f$),
\f$ \lambda \f$, \f$ \nu \f$, \f$ \delta \f$ are jump intensity, mean and volatility for the Merton jump-diffusion, respectively.

References:
A. Sepp, Pricing European-Style Options under Jump Diffusion
Processes with Stochastic Volatility: Applications of Fourier
Transform (<http://math.ut.ee/~spartak/papers/stochjumpvols.pdf>)

In this test, the dependence of the NPV of a portfolio consisted of several options
on option strike prices is calculated using adjoint and finite difference differentiation.
In addition, the NPV is checked with the Black formula
(the parameters of the Bates model are chosen to produce results identical to the Black formula).
*/

namespace {
    enum
    {
#if defined CL_GRAPH_GEN
        // Number of points for dependency plots.
        pointNo = 100,
        // Number of points for performance plot.
        iterNo = 30,
        // Step for portfolio size for performance testing .
        step = 1,
#else
        // Number of points for dependency plots.
        pointNo = 0,
        // Number of points for performance plot.
        iterNo = 0,
        // Step for portfolio size for performance testing .
        step = 1,
#endif
        // Defines performance accuracy. Its value is a minimum number
        // of calling of O(1) complexity methods per one performance test.
        iterNumFactor = 0,
    };

    // This structure contains all constant variables
    // Parameters of the Bates model are chosen to produce results identical to the Black formula
    // (based on BatesModelTest::testAnalyticVsBlack() from QuantLib\test-suite\batesmodel.cpp)
    struct CommonVars
    {
        CommonVars()
        : settlementDate_(Date(1, September, 2005))
        , exerciseDate_(settlementDate_ + 6 * Months)
        , riskfreeRate_(0.10)
        , dividentRate_(0.04)
        , spotPrice_(30.0)
        , volatility_(0.05)
        , kappa_(5.0)
        , theta_(0.05)
        , sigma_(1.0e-4)
        , rho_(0.0)
        , lambda_(0.0001)
        , nu_(0.0)
        , delta_(0.0001)
        , exercise_(boost::make_shared<EuropeanExercise>(exerciseDate_))
        , strikePrice_(18.0)
        {
        }

        // global data
        const Date settlementDate_;
        const Date exerciseDate_;
        const Real riskfreeRate_;
        const Real dividentRate_;
        const Real spotPrice_;
        const Real volatility_;
        // Bates model specific data
        const Real kappa_;
        const Real theta_;
        const Real sigma_;
        const Real rho_;
        const Real lambda_;
        const Real nu_;
        const Real delta_;
        // variables and objects needed for calculations
        boost::shared_ptr<Exercise> exercise_;
        const Real strikePrice_;
        Handle<YieldTermStructure> riskFreeTS_;
    };

    // This structure contains all constant variables from CommonVars + strike price
    // In addition it provides methods for calcualtions
    struct BatesData
        : public CommonVars
    {
        explicit BatesData()
        : CommonVars()
        {
        }

        // Provides payoffs that are different only by their strike price
        boost::shared_ptr<StrikedTypePayoff> makePayoff(Rate strikePrice)
        {
            boost::shared_ptr<StrikedTypePayoff> payoff(
                new PlainVanillaPayoff(Option::Put, strikePrice));
            return payoff;
        }

        // Calculate forward price for given divident rate
        Real CalculateForwardPrice(Real dividentRate)
        {
            Settings::instance().evaluationDate() = settlementDate_;
            DayCounter dayCounter = ActualActual();
            Handle<Quote> s0(boost::make_shared<SimpleQuote>(spotPrice_));
            Real yearFraction = dayCounter.yearFraction(settlementDate_, exerciseDate_);
            Real forwardPrice = s0->value()*std::exp((riskfreeRate_ - dividentRate)*yearFraction);
            return forwardPrice;
        }

        // Provides options that are different only by their strike price
        boost::shared_ptr<VanillaOption> makeOptionForStrikePrice(Real strikePrice)
        {
            Settings::instance().evaluationDate() = settlementDate_;
            DayCounter dayCounter = ActualActual();

            boost::shared_ptr<StrikedTypePayoff> payoff = makePayoff(strikePrice);
            boost::shared_ptr<VanillaOption> option(new VanillaOption(payoff, exercise_));
            Handle<Quote> s0(boost::make_shared<SimpleQuote>(spotPrice_));

            Handle<YieldTermStructure> riskFreeTS(flatRate(riskfreeRate_, dayCounter));
            Handle<YieldTermStructure> dividendTS(flatRate(dividentRate_, dayCounter));
            boost::shared_ptr<BatesProcess> process(
                new BatesProcess(riskFreeTS, dividendTS, s0, volatility_,
                kappa_, theta_, sigma_, rho_, lambda_, nu_, delta_));

            boost::shared_ptr<PricingEngine> engine(new BatesEngine(
                boost::shared_ptr<BatesModel>(new BatesModel(process)), 64));

            option->setPricingEngine(engine);

            return option;
        }

        // Returns net present value (NPV) of option with given fixed rate strike and other
        // parameters determined from class instance.
        Real optionNPVForStrikePrice(Real strikePrice)
        {
            auto option = makeOptionForStrikePrice(strikePrice);
            return option->NPV();
        }

        // Black-Scholes result (for cross check)
        Real optionNPVForStrikePrice_BS(Real strikePrice)
        {
            Settings::instance().evaluationDate() = settlementDate_;
            DayCounter dayCounter = ActualActual();
            Real yearFraction = dayCounter.yearFraction(settlementDate_, exerciseDate_);
            boost::shared_ptr<StrikedTypePayoff> payoff = makePayoff(strikePrice);
            Real forwardPrice = CalculateForwardPrice(dividentRate_);
            Real NPV = blackFormula(payoff->optionType(), payoff->strike(),
                forwardPrice, std::sqrt(volatility_*yearFraction)) *
                std::exp(-1 * riskfreeRate_*yearFraction);
            return NPV;
        }

        // Provides options that are different only by the divident rate
        boost::shared_ptr<VanillaOption> makeOptionForDividentRate(Real dividentRate)
        {
            Settings::instance().evaluationDate() = settlementDate_;
            DayCounter dayCounter = ActualActual();

            boost::shared_ptr<StrikedTypePayoff> payoff = makePayoff(strikePrice_);
            boost::shared_ptr<VanillaOption> option(new VanillaOption(payoff, exercise_));
            Handle<Quote> s0(boost::make_shared<SimpleQuote>(spotPrice_));

            Handle<YieldTermStructure> riskFreeTS(flatRate(riskfreeRate_, dayCounter));
            Handle<YieldTermStructure> dividendTS(flatRate(dividentRate, dayCounter));
            boost::shared_ptr<BatesProcess> process(
                new BatesProcess(riskFreeTS, dividendTS, s0, volatility_,
                kappa_, theta_, sigma_, rho_, lambda_, nu_, delta_));

            boost::shared_ptr<PricingEngine> engine(new BatesEngine(
                boost::shared_ptr<BatesModel>(new BatesModel(process)), 64));

            option->setPricingEngine(engine);

            return option;
        }

        // Returns net present value (NPV) of option with given fixed rate strike and other
        // parameters determined from class instance.
        Real optionNPVForDividentRate(Real dividentRate)
        {
            auto option = makeOptionForDividentRate(dividentRate);
            return option->NPV();
        }

        // Black-Scholes result (for cross check)
        Real optionNPVForDividentRate_BS(Real dividentRate)
        {
            Settings::instance().evaluationDate() = settlementDate_;
            DayCounter dayCounter = ActualActual();
            Real yearFraction = dayCounter.yearFraction(settlementDate_, exerciseDate_);
            boost::shared_ptr<StrikedTypePayoff> payoff = makePayoff(strikePrice_);
            Real forwardPrice = CalculateForwardPrice(dividentRate);
            Real NPV = blackFormula(payoff->optionType(), payoff->strike(),
                forwardPrice, std::sqrt(volatility_*yearFraction)) *
                std::exp(-1*riskfreeRate_*yearFraction);
            return NPV;
        }
    };

    // Struct for graphics recording of NPV dependence on anything
    // (taken from adjointbermudanswaptionimpl.hpp)
    struct NPVonSmth
    {
        static std::deque<std::string > get_columns()
        {
            static std::deque<std::string > columns =
            {
                "Independent variable", ""
            };

            return columns;
        }

        template <typename stream_type>
        friend inline stream_type&
            operator << (stream_type& stm, NPVonSmth& v)
        {
                stm << v.var_
                    << ";" << v.sensitivity_
                    << std::endl;
                return stm;
            }

        Real var_;
        Real sensitivity_;
    };

    // Bates test structure (dependence on strike rate)
    struct BatesTestStrikePrice
        : public cl::AdjointTest<BatesTestStrikePrice>
    {
        // Create test with a portfolio given by the sum of size options with strike prices in the range:
        //                                                    data_->strikePrice_ + 0.5 * i, 0 < i < size
        BatesTestStrikePrice(Size size, BatesData* data, cl::tape_empty_test_output* logger = nullptr)
        : AdjointTest()
        , size_(size)
        , data_(data)
        , strikes_(size)
        , NPV_(size)
        , totalNPV_()
        {
            setLogger(logger);

            for (Size i = 0; i < size_; i++)
            {
                strikes_[i] = data_->strikePrice_ + 0.5 * i;
            }
        }

        // Do tape recording for adjoint calculation
        void recordTape()
        {
            cl::Independent(strikes_);
            calculateTotalNPV();
            f_ = std::make_unique<cl::tape_function<double>>(strikes_, totalNPV_);
        }

        // Calculates price of  each option and portfolio (the sum)
        Real calculateTotalNPV()
        {
            totalNPV_.resize(1, 0);
            NPV_.resize(size_);
            for (Size i = 0; i < size_; i++)
            {
                NPV_[i] = data_->optionNPVForStrikePrice(strikes_[i]);
                totalNPV_.front() += NPV_[i];
            }
            return totalNPV_.front();
        }

        // Calculates price of  each option and portfolio (the sum) using Black formula (for cross check)
        Real calculateTotalNPV_BS()
        {
            Real sum = 0.0;
            for (Size i = 0; i < size_; i++)
            {
                sum += data_->optionNPVForStrikePrice_BS(strikes_[i]);
            }
            return sum;
        }

        // Check with BS formula
        bool CheckBatesVsBlack()
        {
            Real calculated = this->calculateTotalNPV();
            Real expected = this->calculateTotalNPV_BS();
            Real maxabs = std::max(std::abs(calculated), std::abs(expected));
            Real tol = std::max(this->relativeTol() * maxabs, this->absTol());
            return (std::abs(expected - calculated) < tol);
        }

        // Calculates derivatives using finite difference method
        void calcAnalytical()
        {
            const Real diff = 1e-6;
            Real h = diff * data_->strikePrice_;  // shift for finite diff. method
            analyticalResults_.resize(size_);
            for (Size i = 0; i < size_; i++)
            {
                analyticalResults_[i] = (data_->optionNPVForStrikePrice(strikes_[i] + h)
                    - data_->optionNPVForStrikePrice(strikes_[i] - h)) / (2 * h);
            }
        }

        // get relative tolerance
        double relativeTol() const { return 1e-5; }

        // get absolute tolerance
        double absTol() const { return 1e-7; }

        // get number of independent variables
        Size indepVarNumber() { return size_; }

        Size size_;
        BatesData* data_;
        std::vector<cl::tape_double> strikes_;
        std::vector<Real> NPV_;
        std::vector<cl::tape_double> totalNPV_;
    };

    // Bates test structure (dependence on divident rate)
    struct BatesTestDividentRate
        : public cl::AdjointTest<BatesTestDividentRate>
    {
        // Create test with a portfolio given by the sum of options on assets with divident rates in the range:
        //                                                    data_->dividentRate_ + 0.01 * i, 0 < i < size
        BatesTestDividentRate(Size size, BatesData* data, cl::tape_empty_test_output* logger = nullptr)
        : AdjointTest()
        , size_(size)
        , data_(data)
        , dividents_(size)
        , NPV_(size)
        , totalNPV_()
        {
            setLogger(logger);

            for (Size i = 0; i < size_; i++)
            {
                dividents_[i] = data_->dividentRate_ + 0.005 * i;
            }
        }

        // Do tape recording for adjoint calculation
        void recordTape()
        {
            cl::Independent(dividents_);
            calculateTotalNPV();
            f_ = std::make_unique<cl::tape_function<double>>(dividents_, totalNPV_);
        }

        // Calculates price of  each option and portfolio (the sum)
        Real calculateTotalNPV()
        {
            totalNPV_.resize(1, 0);
            NPV_.resize(size_);
            for (Size i = 0; i < size_; i++)
            {
                NPV_[i] = data_->optionNPVForDividentRate(dividents_[i]);
                totalNPV_.front() += NPV_[i];
            }
            return totalNPV_.front();
        }

        // Calculates price of each option and portfolio (the sum) using Black formula (for cross check)
        Real calculateTotalNPV_BS()
        {
            Real sum = 0.0;
            for (Size i = 0; i < size_; i++)
            {
                sum += data_->optionNPVForDividentRate_BS(dividents_[i]);
            }
            return sum;
        }

        // Check with BS formula
        bool CheckBatesVsBlack()
        {
            Real calculated = this->calculateTotalNPV();
            Real expected = this->calculateTotalNPV_BS();
            Real maxabs = std::max(std::abs(calculated), std::abs(expected));
            Real tol = std::max(this->relativeTol() * maxabs, this->absTol());
            return (std::abs(expected - calculated) < tol);
        }

        // Calculates derivatives using finite difference method
        void calcAnalytical()
        {
            const Real diff = 1e-4;
            Real h = diff * data_->dividentRate_;  // shift for finite diff. method
            analyticalResults_.resize(size_);
            for (Size i = 0; i < size_; i++)
            {
                analyticalResults_[i] = (data_->optionNPVForDividentRate(dividents_[i] + h)
                    - data_->optionNPVForDividentRate(dividents_[i] - h)) / (2 * h);
            }
        }

        // get relative tolerance
        double relativeTol() const { return 1e-5; }

        // get absolute tolerance
        double absTol() const { return 1e-7; }

        // get number of independent variables
        Size indepVarNumber() { return size_; }

        Size size_;
        BatesData* data_;
        std::vector<cl::tape_double> dividents_;
        std::vector<Real> NPV_;
        std::vector<cl::tape_double> totalNPV_;
    };

    // BatesData structure extended for making plots with dependence on the divident rate
    struct TestDataDividentRate
        : public BatesData
    {
        explicit TestDataDividentRate()
        : outPerform_(OUTPUT_FOLDER_NAME "//" + std::string("DividentRate"), {
            { "title", "Bates NPV differentiation performance with respect to divident rate" }
            , { "not_clear", "Not" }
            , { "ylabel", "Time (s)" }
            , { "xlabel", "Number of divident rates" }
            , { "line_box_width", "-5" }
            , { "cleanlog", "true" }
            , { "smooth", "default" }
        })
            , outAdjoint_(OUTPUT_FOLDER_NAME "//" + std::string("DividentRate"), {
                { "title", "Bates NPV adjoint differentiation with respect to fixed divident rate" }
            , { "filename", "Adjoint" }
            , { "not_clear", "Not" }
            , { "ylabel", "Time (s)" }
            , { "xlabel", "Number of divident rates" }
            , { "cleanlog", "false" }
            , { "smooth", "default" }
        })
            , outSize_(OUTPUT_FOLDER_NAME "//" + std::string("DividentRate"), {
                { "title", "Tape size dependence on number of divident rates" }
            , { "filename", "TapeSize" }
            , { "not_clear", "Not" }
            , { "ylabel", "Memory (MB)" }
            , { "cleanlog", "false" }
        })
            , out_(OUTPUT_FOLDER_NAME "//" + std::string("DividentRate") + "//output", {
                { "filename", "NPVonDividentRate" }
            , { "not_clear", "Not" }
            , { "title", "Option NPV dependence on fixed divident rate" }
            , { "ylabel", "Option NPV" }
            , { "xlabel", "Divident rate" }
            , { "cleanlog", "false" }
        })
        { }

        // main method to produce output
        bool makeOutput()
        {
            bool ok = true;
            if (pointNo > 0)
            {
                ok &= recordDependencePlot(pointNo);
                ok &= cl::recordPerformance(*this, iterNo, step);
            }
            return ok;
        }

        std::shared_ptr<BatesTestDividentRate> getTest(size_t size)
        {
            return std::make_shared<BatesTestDividentRate>(size, this, &outPerform_);
        }

        // Makes graphics for divident sensitivity dependence
        bool recordDependencePlot(size_t pointNo)
        {
            bool ok = true;
            std::vector<NPVonSmth> outData(pointNo);
            auto test = getTest(pointNo);
            test->recordTape();
            for (Size i = 0; i < pointNo; i++)
            {
                outData[i] = { test->dividents_[i], test->NPV_[i] };
            }
            out_ << outData;
            return ok;
        }

        cl::tape_empty_test_output outPerform_;
        cl::tape_empty_test_output outAdjoint_;
        cl::tape_empty_test_output outSize_;
        cl::tape_empty_test_output out_;
    };
}

#endif


#include "matrix.h"
#include "mex.h"

#include <iostream>
#include <math.h>
#include <chrono>
#include <random>
#include <vector>
#include <Eigen\Dense>
#include <Eigen\SVD>

#include "estimator.h"
#include "prosac.h"
#include "mlesac.h"
#include "ransac.h"

using namespace std;
using namespace theia;
using namespace Eigen;

// Structure for a 2-D point
struct Point
{
	double x;
	double y;
	Point() {}
	Point(double _x, double _y) : x(_x), y(_y) {}
	Point(const Point& p)
	{
		x = p.x;
		y = p.y;
	}
};

// A pair of points that correspond to each other. These two points are two keypoints in to separate images that have been matched to each other
struct Correspondence
{
	Point p1;
	Point p2;
	Correspondence() {}
	Correspondence(double x1, double y1, double x2, double y2)
	{
		p1.x = x1;
		p1.y = y1;
		p2.x = x2;
		p2.y = y2;
	}
	Correspondence(Point& _p1, Point& _p2) : p1(_p1), p2(_p2) {}
	Correspondence(const Correspondence& other) : p1(other.p1), p2(other.p2) { }
};

// Homography matrix is a 3-by-3 matrix which is the model we are trying to estimate
struct Homography
{
	double Matrix[9];
	Homography() {}
	Homography(const Homography& other)
	{
		for (size_t i = 0; i < 9; i++)
		{
			Matrix[i] = other.Matrix[i];
		}
	}
};

// This is the estimator class for estimating a homography matrix between two images. A model estimation method and error calculation method are implemented
class HomographyEstimator : public Estimator < Correspondence, Homography >
{
public:
	HomographyEstimator() {}
	~HomographyEstimator() {}

	int SampleSize() const { return 4; }
	bool EstimateModel(const vector<Correspondence>& data, vector<Homography>* models) const
	{
		Homography model;

		//calculate homography -- see gc_lecture_notes pp. 17-19
		MatrixXd Q = MatrixXd::Zero(2 * data.size(), 9);
		for (size_t i = 0; i < data.size(); i++)
		{
			Q(i, 0) = data[i].p2.x;
			Q(i, 1) = data[i].p2.y;
			Q(i, 2) = 1.0;
			Q(i, 6) = -1 * data[i].p2.x * data[i].p1.x;
			Q(i, 7) = -1 * data[i].p2.y * data[i].p1.x;
			Q(i, 8) = -1 * data[i].p1.x;

			Q(i + data.size(), 3) = data[i].p2.x;
			Q(i + data.size(), 4) = data[i].p2.y;
			Q(i + data.size(), 5) = 1.0;
			Q(i + data.size(), 6) = -1.0 * data[i].p2.x * data[i].p1.y;
			Q(i + data.size(), 7) = -1.0 * data[i].p2.y * data[i].p1.y;
			Q(i + data.size(), 8) = -1.0 * data[i].p1.y;
		}

		MatrixXd qtq = Q.transpose() * Q;
		JacobiSVD<MatrixXd> svd(qtq, ComputeThinV);
		svd.computeV();
		MatrixXd minEigenVector = svd.matrixV().col(8);
		for (size_t i = 0; i < 9; i++)
		{
			model.Matrix[i] = minEigenVector(i);
		}
		models->push_back(model);
		return true;
	}

	double Error(const Correspondence& correspondence, const Homography& model) const
	{
		// calculate reporojection error
		double projectionPointZ = model.Matrix[6] * correspondence.p1.x + model.Matrix[7] * correspondence.p1.y + model.Matrix[8];
		double projectionPointX = (model.Matrix[0] * correspondence.p1.x + model.Matrix[1] * correspondence.p1.y + model.Matrix[2]) / projectionPointZ;
		double projectionPointY = (model.Matrix[3] * correspondence.p1.x + model.Matrix[4] * correspondence.p1.y + model.Matrix[5]) / projectionPointZ;

		return (double) (fabs(projectionPointX - correspondence.p2.x) + fabs(projectionPointY - correspondence.p2.y));
	}
};

// This is the entry point from MATLAB to this routine
// plhs and prhs represent a pointer to the left hand side (output) and a pointer to the right hand side (input) respectively
// matlab function signature: H = homography_estimator(points1, points2, 'mode')
// H -> 9-by-9 homography matrix as output
// points1 -> n-by-2 matrix, coordinates of the keypoints of the first image
// points2 -> n-by-2 matrix, coordinates of the keypoints of the second image
// 'mode' -> {'ransac', 'prosac', 'mlesac'}
void mexFunction(int nlhs, mxArray *plhs [], int nrhs, const mxArray*prhs [])
{
	if (nlhs != 1)
	{
		mexPrintf("Only one output expected\n");
		return;
	}
	if (nrhs != 3)
	{
		mexPrintf("Only three inputs expected\n");
		return;
	}

	const mwSize *dims1, *dims2;
	char* modeStr;
	double *points1, *points2;

	dims1 = mxGetDimensions(prhs[0]);
	dims2 = mxGetDimensions(prhs[1]);

	int dim1y = (int) dims1[0]; int dim1x = (int) dims1[1];
	int dim2y = (int) dims2[0]; int dim2x = (int) dims2[1];

	if (dim1x != dim2x || dim1y != dim2y)
	{
		mexPrintf("Error! two input points arrays must be of the same size\n");
		return;
	}

	if (dim1x != 2 || dim2x != 2)
	{
		mexPrintf("Error! Points must be 2-D!\n");
		return;
	}

	modeStr = mxArrayToString(prhs[2]);

	if (!modeStr || mxGetDimensions(prhs[2])[0] != 1)
	{
		mexPrintf("Error! Last argument must be an string!\n");
		return;
	}

	mexPrintf("string:\n%s\n", modeStr);
	vector<Correspondence> correspondences(dim1y);
	points1 = mxGetPr(prhs[0]);
	points2 = mxGetPr(prhs[1]);
	for (size_t j = 0; j < dim1y; j++)
	{
		/*Correspondence corr;
		corr.p1.x = points1[j];
		corr.p2.x = points2[j];
		corr.p1.y = points1[dim1y + j];
		corr.p2.y = points2[dim1y + j];*/
		correspondences[j] = Correspondence(points1[j], points1[dim1y + j], points2[j], points2[dim1y + j]);
	}
	Homography homographyModel;
	RansacParameters params;
	params.error_thresh = 0.5;
	HomographyEstimator homographyEstimator;
	SampleConsensusEstimator<HomographyEstimator> *sacEstimator;
	// RANSAC mode
	if (!strcmp("ransac", modeStr))
	{
		sacEstimator = new Ransac<HomographyEstimator>(params, homographyEstimator);
	}
	// MLESAC mode
	else if (!strcmp("mlesac", modeStr))
	{
		sacEstimator = new Mlesac<HomographyEstimator>(params, homographyEstimator);
	}
	// PROSAC mode
	else if (!strcmp("prosac", modeStr))
	{
		sacEstimator = new Prosac<HomographyEstimator>(params, homographyEstimator);
	}
	// Unknown mode
	else
	{
		mexPrintf("Error! unknown estimation mode selected!\n");
		return;
	}

	sacEstimator->Initialize();
	RansacSummary summary;
	sacEstimator->Estimate(correspondences, &homographyModel, &summary);

	auto outArray = mxCreateDoubleMatrix(3, 3, mxREAL);
	double *outp = mxGetPr(outArray);
	// Copy the estimated homography matrix to the output matrix, note the difference between matrix's column-wise arrays vs. C's row-wise arrays convention
	auto lastElement = homographyModel.Matrix[8];
	for (size_t i = 0; i < 3; i++)
	{
		for (size_t j = 0; j < 3; j++)
		{
			outp[i * 3 + j] = homographyModel.Matrix[j * 3 + i] / lastElement;
		}
	}

	plhs[0] = outArray;
}