/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkLaplacianInfilling.cxx
  Author: Pierre Guilbert

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.  See the above copyright notice for more information.

=========================================================================*/

// LOCAL
#include "vtkLaplacianInfilling.h"

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

// VTK
#include <vtkObjectFactory.h>
#include <vtkImageData.h>
#include <vtkInformation.h>
#include <vtkInformationVector.h>
#include <vtkStreamingDemandDrivenPipeline.h>
#include <vtkXMLImageDataWriter.h>

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

// Eigen
#include <Eigen/Sparse>

// Implementation of the New function
vtkStandardNewMacro(vtkLaplacianInfilling)

//-----------------------------------------------------------------------------
int vtkLaplacianInfilling::RequestData(vtkInformation *vtkNotUsed(request),
  vtkInformationVector **inputVector, vtkInformationVector *outputVector)
{
  // Get the input
  vtkImageData * inputImage = vtkImageData::GetData(inputVector[0]->GetInformationObject(0));

  // Get the output
  vtkImageData* outputImage = vtkImageData::GetData(outputVector->GetInformationObject(0));
  outputImage->ShallowCopy(inputImage);

  int xBound = outputImage->GetDimensions()[0];
  int yBound = outputImage->GetDimensions()[1];
  int nParams = xBound * yBound;

  Eigen::SparseMatrix<double> Laplacian(nParams, nParams);
  Eigen::VectorXd Y(nParams); // The values of the laplacian required

  // Triplet of value: row, column and value
  std::vector<Eigen::Triplet<double> > nonZeroCoefficient;
  for (int x = 0; x < xBound; ++x)
  {
    for (int y = 0; y < yBound; ++y)
    {
      int flattenIndex = x + xBound * y;

      // check if the current pixel has a value
      double value = inputImage->GetScalarComponentAsDouble(x, y, 0, 0);
      if ((std::abs(value) > std::numeric_limits<double>::epsilon()))
      {
        // we don't want this value to be modified
        // contraint: xi = yi
        nonZeroCoefficient.push_back(Eigen::Triplet<double>(flattenIndex, flattenIndex, 1.0));
        Y(flattenIndex) = value;
      }
      else
      {
        // else fill it solving the laplace equation
        // using finite difference scheme and Dirichlet
        // boundary
        int validNeigh = 0;

        // Laplacian constraints matrix:
        // Neighbors contraint
        if (x != 0)
        {
          nonZeroCoefficient.push_back(Eigen::Triplet<double>(flattenIndex, flattenIndex - 1, 1));
          validNeigh++;
        }
        if (x != xBound - 1)
        {
          nonZeroCoefficient.push_back(Eigen::Triplet<double>(flattenIndex, flattenIndex + 1, 1));
          validNeigh++;
        }
        if (y != 0)
        {
          nonZeroCoefficient.push_back(Eigen::Triplet<double>(flattenIndex, flattenIndex - xBound, 1));
          validNeigh++;
        }
        if (y != yBound - 1)
        {
          nonZeroCoefficient.push_back(Eigen::Triplet<double>(flattenIndex, flattenIndex + xBound, 1));
          validNeigh++;
        }
        // Diagonal constraint
        nonZeroCoefficient.push_back(Eigen::Triplet<double>(flattenIndex, flattenIndex, -1.0 * static_cast<double>(validNeigh)));
        // We want the laplacian to be null
        Y(flattenIndex) = 0;
      }
    }
  }

  // Fill Laplacian constraints matrix
  Laplacian.setFromTriplets(nonZeroCoefficient.begin(), nonZeroCoefficient.end());

  // Solving:
  Eigen::SparseLU< Eigen::SparseMatrix<double> > solver(Laplacian);
  Eigen::MatrixXd X = solver.solve(Y);

  // X contains the Dirichlet solution function
  // values i.e: 0-values pixel are filled with
  // laplacian
  for (int x = 0; x < xBound; ++x)
  {
    for (int y = 0; y < yBound; ++y)
    {
      int flattendIndex = x + xBound * y;
      outputImage->SetScalarComponentFromDouble(x, y, 0, 0, X(flattendIndex));
    }
  }

  return 1;
}


/*
 * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
 *
 * This software may be modified and distributed under the terms of the
 * BSD 3-Clause license. See the accompanying LICENSE file for details.
 */

#include <BayesFilters/Gaussian.h>
#include <BayesFilters/GaussianFilter.h>
#include <BayesFilters/sigma_point.h>
#include <BayesFilters/SimulatedLinearSensor.h>
#include <BayesFilters/SimulatedStateModel.h>
#include <BayesFilters/UKFCorrection.h>
#include <BayesFilters/UKFPrediction.h>
#include <BayesFilters/utils.h>
#include <BayesFilters/WhiteNoiseAcceleration.h>

#include <string>

#include <Eigen/Dense>

using namespace bfl;
using namespace Eigen;

class UKFSimulation : public GaussianFilter
{
public:
    UKFSimulation
    (
        Gaussian& initial_state,
        std::unique_ptr<GaussianPrediction> prediction,
        std::unique_ptr<GaussianCorrection> correction,
        std::size_t simulation_steps
    ) noexcept :
        GaussianFilter(std::move(prediction), std::move(correction)),
        predicted_state_(initial_state.dim_linear, initial_state.dim_circular),
        corrected_state_(initial_state),
        simulation_steps_(simulation_steps)
    { }

protected:
    bool run_condition() override
    {
        if (step_number() < simulation_steps_)
            return true;
        else
            return false;
    }

    bool initialization_step() override
    {
        return true;
    }

    void filtering_step() override
    {
        prediction().predict(corrected_state_, predicted_state_);
        correction().freeze_measurements();
        correction().correct(predicted_state_, corrected_state_);

        log();
    }

    std::vector<std::string> log_file_names(const std::string& folder_path, const std::string& file_name_prefix) override
    {
        return {folder_path + "/" + file_name_prefix + "_pred_mean",
                folder_path + "/" + file_name_prefix + "_cor_mean"};
    }

    void log() override
    {
        logger(predicted_state_.mean().transpose(), corrected_state_.mean().transpose());
    }

private:
    Gaussian predicted_state_;

    Gaussian corrected_state_;

    std::size_t simulation_steps_;
};

int main(int argc, char* argv[])
{
    std::cout << "Running a UKF filter on a simulated target." << std::endl;

    const bool write_to_file = (argc > 1 ? std::string(argv[1]) == "ON" : false);
    if (write_to_file)
        std::cout << "Data is logged in the test folder with prefix testUKF." << std::endl;

    /* A set of parameters needed to run an unscented Kalman filter in a simulated environment. */
    Vector4d initial_simulated_state(10.0f, 0.0f, 10.0f, 0.0f);
    std::size_t simulation_time = 100;
    /* Initialize unscented transform parameters. */
    double alpha = 1.0;
    double beta = 2.0;
    double kappa = 0.0;

    /* Step 1 - Initialization */

    std::size_t state_size = 4;
    Gaussian initial_state(state_size);
    Vector4d initial_mean(4.0f, 0.04f, 15.0f, 0.4f);
    Matrix4d initial_covariance;
    initial_covariance << pow(0.05, 2), 0,            0,            0,
                          0,            pow(0.05, 2), 0,            0,
                          0,            0,            pow(0.01, 2), 0,
                          0,            0,            0,            pow(0.01, 2);
    initial_state.mean() = initial_mean;
    initial_state.covariance() = initial_covariance;

    /* Step 2 - Prediction */

    /* Step 2.1 - Define the state model. */

    /* Initialize a white noise acceleration state model. */
    double T = 1.0f;
    double tilde_q = 10.0f;

    std::unique_ptr<AdditiveStateModel> wna = utils::make_unique<WhiteNoiseAcceleration>(WhiteNoiseAcceleration::Dim::TwoD, T, tilde_q);

    /* Step 2.2 - Define the prediction step. */

    /* Initialize the unscented Kalman filter prediction step and pass the ownership of the state model */
    std::unique_ptr<UKFPrediction> ukf_prediction = utils::make_unique<UKFPrediction>(std::move(wna), state_size, alpha, beta, kappa);

    /* Step 3 - Correction */

    /* Step 3.1 - Define where the measurement are originated from (simulated in this case). */

    /* Initialize simulated target model with a white noise acceleration. */
    std::unique_ptr<AdditiveStateModel> target_model = utils::make_unique<WhiteNoiseAcceleration>(WhiteNoiseAcceleration::Dim::TwoD, T, tilde_q);
    std::unique_ptr<SimulatedStateModel> simulated_state_model = utils::make_unique<SimulatedStateModel>(std::move(target_model), initial_simulated_state, simulation_time);

    if (write_to_file)
        simulated_state_model->enable_log("./", "testUKF");

    /* Step 3.2 - Initialize a measurement model (a linear sensor reading x and y coordinates). */
    double sigma_x = 10.0;
    double sigma_y = 10.0;
    Eigen::MatrixXd R(2, 2);
    R << std::pow(sigma_x, 2.0),                    0.0,
                            0.0, std::pow(sigma_y, 2.0);

    std::unique_ptr<AdditiveMeasurementModel> simulated_linear_sensor = utils::make_unique<SimulatedLinearSensor>(std::move(simulated_state_model), SimulatedLinearSensor::LinearMatrixComponent{ 4, std::vector<std::size_t>{ 0, 2 } }, R);

    if (write_to_file)
        simulated_linear_sensor->enable_log("./", "testUKF");

    /* Step 3.3 - Initialize the unscented Kalman filter correction step and pass the ownership of the measurement model. */
    std::unique_ptr<UKFCorrection> ukf_correction = utils::make_unique<UKFCorrection>(std::move(simulated_linear_sensor), state_size, alpha, beta, kappa);

    /* Step 4 - Assemble the unscented Kalman filter. */
    std::cout << "Constructing unscented Kalman filter..." << std::flush;

    UKFSimulation ukf(initial_state, std::move(ukf_prediction), std::move(ukf_correction), simulation_time);

    if (write_to_file)
        ukf.enable_log("./", "testUKF");

    std::cout << "done!" << std::endl;

    /* Step 5 - Boot the filter. */
    std::cout << "Booting unscented Kalman filter..." << std::flush;

    ukf.boot();

    std::cout << "completed!" << std::endl;

    /* Step 6 - Run the filter and wait until it is closed. */
    /* Note that since this is a simulation, the filter will end upon simulation termination. */
    std::cout << "Running unscented Kalman filter..." << std::flush;

    ukf.run();

    std::cout << "waiting..." << std::flush;

    if (!ukf.wait())
        return EXIT_FAILURE;

    std::cout << "completed!" << std::endl;

    return EXIT_SUCCESS;
}


/*
 libs/numeric/odeint/examples/stochastic_euler.hpp

 Copyright 2012 Karsten Ahnert
 Copyright 2012 Mario Mulansky

 Stochastic euler stepper example and Ornstein-Uhlenbeck process

 Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <vector>
#include <iostream>
#include <boost/random.hpp>
#include <boost/array.hpp>

#include <boost/numeric/odeint.hpp>

/*
//[ stochastic_euler_class_definition
template< size_t N > class stochastic_euler
{
public:

    typedef boost::array< double , N > state_type;
    typedef boost::array< double , N > deriv_type;
    typedef double value_type;
    typedef double time_type;
    typedef unsigned short order_type;
    typedef boost::numeric::odeint::stepper_tag stepper_category;

    static order_type order( void ) { return 1; }

    // ...
};
//]
*/

/*
//[ stochastic_euler_do_step
template< size_t N > class stochastic_euler
{
public:

    // ...

    template< class System >
    void do_step( System system , state_type &x , time_type t , time_type dt ) const
    {
        deriv_type det , stoch ;
        system.first( x , det );
        system.second( x , stoch );
        for( size_t i=0 ; i<x.size() ; ++i )
            x[i] += dt * det[i] + sqrt( dt ) * stoch[i];
    }
};
//]
*/

//[ stochastic_euler_class
template< size_t N >
class stochastic_euler
{
public:

    typedef boost::array< double , N > state_type;
    typedef boost::array< double , N > deriv_type;
    typedef double value_type;
    typedef double time_type;
    typedef unsigned short order_type;

    typedef boost::numeric::odeint::stepper_tag stepper_category;

    static order_type order( void ) { return 1; }

    template< class System >
    void do_step( System system , state_type &x , time_type t , time_type dt ) const
    {
        deriv_type det , stoch ;
        system.first( x , det );
        system.second( x , stoch );
        for( size_t i=0 ; i<x.size() ; ++i )
            x[i] += dt * det[i] + sqrt( dt ) * stoch[i];
    }
};
//]

//[ stochastic_euler_ornstein_uhlenbeck_def
const static size_t N = 1;
typedef boost::array< double , N > state_type;

struct ornstein_det
{
    void operator()( const state_type &x , state_type &dxdt ) const
    {
        dxdt[0] = -x[0];
    }
};

struct ornstein_stoch
{
    boost::mt19937 m_rng;
    boost::normal_distribution<> m_dist;

    ornstein_stoch( double sigma ) : m_rng() , m_dist( 0.0 , sigma ) { }

    void operator()( const state_type &x , state_type &dxdt )
    {
        dxdt[0] = m_dist( m_rng );
    }
};
//]

struct streaming_observer
{
    template< class State >
    void operator()( const State &x , double t ) const
    {
        std::cout << t << "\t" << x[0] << "\n";
    }
};

int main( int argc , char **argv )
{
    using namespace std;
    using namespace boost::numeric::odeint;

    //[ ornstein_uhlenbeck_main
    double dt = 0.1;
    state_type x = {{ 1.0 }};
    integrate_const( stochastic_euler< N >() , make_pair( ornstein_det() , ornstein_stoch( 1.0 ) ) ,
            x , 0.0 , 10.0 , dt , streaming_observer() );
    //]
    return 0;
}


/// @file bealab/core/prelim/math.hpp
/// A number of math functions for real and complex numbers.

#ifndef _BEALAB_PRELIM_MATH_
#define	_BEALAB_PRELIM_MATH_

#include <cmath>
#include <boost/math/special_functions/erf.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include <boost/math/special_functions/sinc.hpp>
#include <boost/math/special_functions/binomial.hpp>

namespace bealab
{
/// @defgroup prelim_math Math functions
/// A number of math functions, most of them imported from STD.
/// - Classification
///   - isnan
///   - isinf
///   - isfinite
/// - Basic functions
///   - min
///   - max
///   - round
///   - trunc
///   - ceil
///   - floor
///   - mod
/// - Complex functions
///   - abs
///   - arg
///   - real
///   - imag
///   - conj
///   - polar
/// - Trigonometric functions
///   - sin
///   - cos
///   - tan
///   - asin
///   - acos
///   - atan
/// - Hyperbolic functions
///   - sinh
///   - cosh
///   - tanh
///   - asinh
///   - acosh
///   - atanh
/// - Exponential and power functions
///   - exp
///   - log
///   - log2
///   - log10
///   - pow
///   - sqrt
/// - Special functions
///   - sinc
///   - factorial
///   - erf
///   - erfc
///   - erf_inv
///   - erfc_inv
/// @{

/// @name Classification
using std::isnan;
using std::isinf;
using std::isfinite;
/// @}

/// @name Basic functions
using std::min;
using std::max;
#ifdef BEALAB_MACOSX
using ::round;
using ::trunc;
#else
using std::round;
using std::trunc;
#endif
using std::ceil;
using std::floor;

/// x modulo y
inline double mod( double x, double y ) { return x - floor( x / y ) * y; }
/// @}

/// @name Complex functions
using std::abs;
using std::arg;
using std::real;
using std::imag;
using std::conj;
using std::polar;
//inline double abs( const complex& x )  { return std::abs(_complex(x)); }
//inline double arg( const complex& x ) { return std::arg(_complex(x)); }
//inline double real( const complex& x ) { return std::real(_complex(x)); }
//inline double imag( const complex& x ) { return std::imag(_complex(x)); }
//inline complex conj( const complex& x ) { return std::conj(_complex(x)); }
//inline complex polar( double r, double theta=0 ) { return std::polar(r,theta); }
/// @}

/// @name Trigonometric functions
using std::sin;
using std::cos;
using std::tan;
using std::asin;
using std::acos;
using std::atan;
//inline complex sin( const complex& x ) { return std::sin(_complex(x)); }
//inline complex cos( const complex& x ) { return std::cos(_complex(x)); }
//inline complex tan( const complex& x ) { return std::tan(_complex(x)); }
//inline complex asin( const complex& x ) { return std::asin(_complex(x)); }
//inline complex acos( const complex& x ) { return std::acos(_complex(x)); }
//inline complex atan( const complex& x ) { return std::atan(_complex(x)); }
/// @}

/// @name Hyperbolic functions
using std::sinh;
using std::cosh;
using std::tanh;
using std::asinh;
using std::acosh;
using std::atanh;
//inline complex sinh( const complex& x ) { return std::sinh(_complex(x)); }
//inline complex cosh( const complex& x ) { return std::cosh(_complex(x)); }
//inline complex tanh( const complex& x ) { return std::tanh(_complex(x)); }
//inline complex asinh( const complex& x ) { return std::asinh(_complex(x)); }
//inline complex acosh( const complex& x ) { return std::acosh(_complex(x)); }
//inline complex atanh( const complex& x ) { return std::atanh(_complex(x)); }
/// @}

/// @name Exponential and power functions
using std::exp;
using std::log;
#ifdef BEALAB_MACOSX
using ::log2;
#else
using std::log2;
#endif
using std::log10;
//inline complex exp( const complex& x ) { return std::exp(_complex(x)); }
//inline complex log( const complex& x ) { return std::log(_complex(x)); }
//inline complex log2( const complex& x ) { return log(_complex(x))/log(2); }
//inline complex log10( const complex& x ) { return std::log10(_complex(x)); }
using std::pow;
//inline double pow( double x, int y ) { return std::pow( x, y ); }
//inline complex pow( double x, double y )  { return std::pow( _complex(x), y ); }
//inline complex pow( const complex& x, int y ) { return std::pow(_complex(x),y); }
//inline complex pow( const complex& x, double y ) { return std::pow(_complex(x),y); }
//inline complex pow( double x, const complex& y ) { return std::pow(x,_complex(y)); }
using std::sqrt;
//inline complex sqrt( const complex& x ) { return std::sqrt(_complex(x)); }
/// @}

/// @name Special functions

/// Sine cardinal function
//inline double sinc( double x ) { return x == 0 ? 1 : sin( pi * x ) / (pi * x); }
inline double sinc( double x ) { return boost::math::sinc_pi( pi * x ); }

/// Factorial
inline double factorial( int i ) { return boost::math::factorial<double>(i); }

/// Binomial coefficients
inline double binomial_coefficient( int n, int k )
	{ return boost::math::binomial_coefficient<double>(n,k); }

using boost::math::erf;
using boost::math::erfc;
using boost::math::erf_inv;
using boost::math::erfc_inv;

/// @}

/// @}
}
#endif


#include <glm/models/links/power.hpp>

#include <armadillo>

using namespace arma;

power_link::power_link(float lambda)
    : glm_link::glm_link( "power" )
{
    m_lambda = lambda;
    if( lambda < 1e-6 )
    {
        m_lambda = 0.0;
    }
    else if( lambda > 2.0 )
    {
        m_lambda = 2.0;
    }
}

vec
power_link::init_beta(const mat &X, const vec &y) const
{
    return 0;
}

vec
power_link::mu(const arma::vec &eta) const
{
    if( m_lambda == 0.0 )
    {
        return log( eta );
    }
    else if( m_lambda == 2.0 )
    {
        return exp( eta );
    }
    else if( m_lambda < 1.0 )
    {
        return ( pow( eta, m_lambda ) - 1 ) / m_lambda;
    }
    else
    {
        return pow( 1 + eta*(2-m_lambda), 1/(2-m_lambda) );
    }
}

vec
power_link::eta(const arma::vec &mu) const
{
    if( m_lambda == 0.0 )
    {
        return exp( mu );
    }
    else if( m_lambda == 2.0 )
    {
        return log( mu );
    }
    else if( m_lambda < 1.0 )
    {
        return pow( 1 + mu*m_lambda, 1/m_lambda);
    }
    else
    {
        return (pow( mu, 2 - m_lambda ) - 1) / (2 - m_lambda);
    }
}

vec
power_link::mu_eta(const arma::vec &mu) const
{
    if( m_lambda == 0.0 )
    {
        return exp( mu );
    }
    else if( m_lambda == 2.0 )
    {
        return 1 / mu;
    }
    else if( m_lambda < 1.0 )
    {
        return pow( 1 + mu * m_lambda, 1/m_lambda - 1 );
    }
    else
    {
        return pow( mu, 1 - m_lambda );
    }
}


#ifndef PYBNESIAN_UTIL_CHISQUARESUM_HPP
#define PYBNESIAN_UTIL_CHISQUARESUM_HPP

#include <Eigen/Dense>
#include <boost/math/special_functions/binomial.hpp>
#include <boost/math/distributions/gamma.hpp>
#include <util/rpoly.hpp>
#include <util/uniroot.hpp>

using boost::math::binomial_coefficient, boost::math::gamma_distribution, boost::math::cdf, boost::math::complement;
using Eigen::VectorXd, Eigen::Dynamic, Eigen::Matrix;

namespace util {

namespace detail {

template <typename VectorType>
VectorType chisquaresum_moments(VectorType& coeffs, int p) {
    using Scalar = typename VectorType::Scalar;
    VectorType cumulants(2 * p);

    cumulants(0) = coeffs.sum();
    cumulants(1) = 2 * coeffs.squaredNorm();

    // Start loop in r = 3, so 2^(r-1)*(r-1)! = 8
    Scalar fact_const = 8;
    for (int i = 2, end = 2 * p; i < end; ++i) {
        cumulants(i) = fact_const * coeffs.array().pow(i + 1).sum();
        fact_const *= 2 * (i + 1);
    }

    VectorType moments = cumulants;
    moments(1) += moments(0) * moments(0);
    for (int i = 2, end = 2 * p; i < end; ++i) {
        auto offset = cumulants(0) * moments(i - 1) + i * cumulants(1) * moments(i - 2);

        for (int j = 2; j < i; ++j) {
            offset += binomial_coefficient<Scalar>(i, j) * cumulants(j) * moments(i - j - 1);
        }

        moments(i) += offset;
    }

    return moments;
}

template <typename VectorType>
Matrix<typename VectorType::Scalar, Dynamic, Dynamic> delta_matrix_template(VectorType& moments, int size_matrix) {
    using Scalar = typename VectorType::Scalar;
    using MatrixType = Matrix<Scalar, Dynamic, Dynamic>;

    MatrixType t(size_matrix, size_matrix);

    t(0) = 1;
    t(0, 1) = t(1, 0) = moments(0);

    // Fill two first columns first
    for (int i = 2; i < size_matrix; ++i) {
        t(i, 0) = moments(i - 1);
    }

    for (int i = 1; i < size_matrix; ++i) {
        t(i, 1) = moments(i);
    }

    // Fill remaining columns
    for (int j = 2; j < size_matrix; ++j) {
        for (int i = 0; i < size_matrix; ++i) {
            t(i, j) = moments(i + j - 1);
        }
    }

    return t;
}

template <typename Scalar>
Matrix<Scalar, Dynamic, 1> delta_mult_coefficients(Scalar alpha, int size_matrix) {
    using VectorType = Matrix<Scalar, Dynamic, 1>;
    auto max_r = 2 * size_matrix - 2;

    VectorType mult_coefficients(max_r - 1);
    mult_coefficients(0) = 1 + alpha;
    for (int i = 1, end = max_r - 1; i < end; ++i) {
        mult_coefficients(i) = mult_coefficients(i - 1) * (1 + (i + 1) * alpha);
    }

    return mult_coefficients.cwiseInverse();
}

template <typename MatrixType>
void delta_apply_mult_coefficients(MatrixType& delta,
                                   Matrix<typename MatrixType::Scalar, Dynamic, 1>& mult_coefficients) {
    auto p = delta.rows();
    // Divide first two columns
    for (int i = 2; i < p; ++i) {
        delta(i, 0) *= mult_coefficients(i - 2);
    }

    for (int i = 1; i < p; ++i) {
        delta(i, 1) *= mult_coefficients(i - 1);
    }

    // Divide remaining columns.
    for (int j = 2; j < p; ++j) {
        for (int i = 0; i < p; ++i) {
            delta(i, j) *= mult_coefficients(i + j - 2);
        }
    }
}

template <typename Scalar>
struct DeltaMatrixDeterminant {
    using VectorType = Matrix<Scalar, Dynamic, 1>;
    using MatrixType = Matrix<Scalar, Dynamic, Dynamic>;
    Scalar operator()(Scalar alpha) {
        MatrixType copy = matrix;

        auto mult_coefficients = delta_mult_coefficients(alpha, matrix.rows());
        delta_apply_mult_coefficients(copy, mult_coefficients);

        return copy.determinant();
    }

    MatrixType matrix;
};

template <typename VectorType>
typename VectorType::Scalar lambda_tilde(VectorType& moments, int p) {
    using Scalar = typename VectorType::Scalar;

    // This is the closed solution for lambda_1
    Scalar last_lambda = moments(1) / (moments(0) * moments(0)) - 1;
    for (auto i = 2; i <= p; ++i) {
        DeltaMatrixDeterminant<Scalar> mdet{/*.matrix = */ delta_matrix_template(moments, i + 1)};
        last_lambda = util::uniroot(mdet, static_cast<Scalar>(0), last_lambda, static_cast<Scalar>(1e-9), 1000);
    }

    return last_lambda;
}

template <typename VectorType>
Matrix<typename VectorType::Scalar, Dynamic, 1> mu_roots(VectorType& moments,
                                                         typename VectorType::Scalar lambda_tilde,
                                                         int p) {
    using Scalar = typename VectorType::Scalar;
    using VecType = Matrix<Scalar, Dynamic, 1>;

    auto M = delta_matrix_template(moments, p + 1);
    auto mult_coefficients = delta_mult_coefficients(lambda_tilde, p + 1);
    delta_apply_mult_coefficients(M, mult_coefficients);

    VecType poly_coeffs(p + 1);

    M.col(p) = VectorType::Zero(p + 1);

    for (int i = p; i >= 0; --i) {
        M(i, p) = 1;
        poly_coeffs(p - i) = M.determinant();
        M(i, p) = 0;
    }

    VecType real_roots = VecType::Zero(p);
    VecType complex_roots = VecType::Zero(p);

    util::RPoly<Scalar> poly_solver;
    poly_solver.findRoots(poly_coeffs.data(), p, real_roots.data(), complex_roots.data());

    return real_roots;
}

template <typename VectorType>
VectorType mixture_proportions(VectorType& mu, VectorType& moments, typename VectorType::Scalar lambda_tilde, int p) {
    using Scalar = typename VectorType::Scalar;
    using MatrixType = Matrix<Scalar, Dynamic, Dynamic>;

    MatrixType vandermonde(p, p);

    vandermonde.row(0) = VectorType::Ones(p);
    vandermonde.row(1) = mu;
    vandermonde.row(2) = mu.cwiseProduct(mu);
    for (int i = 3; i < p; ++i) {
        vandermonde.row(i) = mu.array().pow(i);
    }

    VectorType delta_vec(p);
    delta_vec(0) = 1;
    delta_vec(1) = moments(0);
    delta_vec(2) = moments(1) / (1 + lambda_tilde);
    delta_vec(3) = moments(2) / ((1 + lambda_tilde) * (1 + 2 * lambda_tilde));

    auto mult_coeff = (1 + lambda_tilde) * (1 + 2 * lambda_tilde);
    for (int i = 4; i < p; ++i) {
        mult_coeff *= (1 + (i - 1) * lambda_tilde);
        delta_vec(i) = moments(i - 1) * (1. / mult_coeff);
    }

    return vandermonde.colPivHouseholderQr().solve(delta_vec);
}

template <typename VectorType>
typename VectorType::Scalar lpb4_cdf(VectorType& prop,
                                     VectorType& mu,
                                     typename VectorType::Scalar lambda_tilde,
                                     typename VectorType::Scalar quantile) {
    using Scalar = typename VectorType::Scalar;
    auto k = 1. / lambda_tilde;

    Scalar res = 0;
    for (int i = 0; i < prop.rows(); ++i) {
        auto theta = mu(i) * lambda_tilde;

        if (theta <= 0) {
            throw std::runtime_error("Wrong theta parameter.");
        }

        gamma_distribution<Scalar> gamma(k, theta);
        res += prop(i) * cdf(gamma, quantile);
    }

    return res;
}

template <typename VectorType>
typename VectorType::Scalar lpb4_cdf_complement(VectorType& prop,
                                                VectorType& mu,
                                                typename VectorType::Scalar lambda_tilde,
                                                typename VectorType::Scalar quantile) {
    using Scalar = typename VectorType::Scalar;
    auto k = 1. / lambda_tilde;
    Scalar res = 0;
    for (int i = 0; i < prop.rows(); ++i) {
        auto theta = mu(i) * lambda_tilde;
        gamma_distribution<Scalar> gamma(k, theta);
        res += prop(i) * cdf(complement(gamma, quantile));
    }

    return res;
}

}  // namespace detail

/**
 * A comparison of efficient approximations for a weighted sum of chi-squared random variables
 */
template <typename VectorType>
typename VectorType::Scalar lpb4(VectorType& coeffs, typename VectorType::Scalar quantile) {
    if (coeffs.rows() < 4) {
        throw std::invalid_argument("lbp4 requires at least 4 coefficients.");
    }

    auto p = 4;
    auto moments = detail::chisquaresum_moments(coeffs, p);
    auto ld_tilde = detail::lambda_tilde(moments, p);
    auto mu = detail::mu_roots(moments, ld_tilde, p);
    auto prop = detail::mixture_proportions(mu, moments, ld_tilde, p);
    return detail::lpb4_cdf(prop, mu, ld_tilde, quantile);
}

template <typename VectorType>
typename VectorType::Scalar lpb4_complement(VectorType& coeffs, typename VectorType::Scalar quantile) {
    if (coeffs.rows() < 4) {
        throw std::invalid_argument("lbp4 requires at least 4 coefficients.");
    }

    auto p = 4;
    auto moments = detail::chisquaresum_moments(coeffs, p);
    auto ld_tilde = detail::lambda_tilde(moments, p);
    auto mu = detail::mu_roots(moments, ld_tilde, p);
    auto prop = detail::mixture_proportions(mu, moments, ld_tilde, p);
    return detail::lpb4_cdf_complement(prop, mu, ld_tilde, quantile);
}

template <typename VectorType>
typename VectorType::Scalar hbe(VectorType& coeffs, typename VectorType::Scalar quantile) {
    using Scalar = typename VectorType::Scalar;
    auto k1 = coeffs.sum();
    auto squared = coeffs.array().square().matrix();
    auto k2 = 2 * squared.sum();
    auto k3 = 8 * (coeffs.dot(squared));

    auto nu = 8 * (k2 * k2 * k2) / (k3 * k3);

    auto statistic = std::sqrt(2 * nu / k2) * (quantile - k1) + nu;

    gamma_distribution<Scalar> gamma(nu / 2., 2);

    return cdf(gamma, statistic);
}

template <typename VectorType>
typename VectorType::Scalar hbe_complement(VectorType& coeffs, typename VectorType::Scalar quantile) {
    using Scalar = typename VectorType::Scalar;
    auto k1 = coeffs.sum();
    auto squared = coeffs.array().square().matrix();
    auto k2 = 2 * squared.sum();
    auto k3 = 8 * (coeffs.dot(squared));

    auto nu = 8 * (k2 * k2 * k2) / (k3 * k3);

    auto statistic = std::sqrt(2 * nu / k2) * (quantile - k1) + nu;

    gamma_distribution<Scalar> gamma(nu / 2., 2);

    return cdf(complement(gamma, statistic));
}

}  // namespace util

#endif  // PYBNESIAN_UTIL_CHISQUARESUM_HPP


// Copyright 2002 Rensselaer Polytechnic Institute

// Use, modification and distribution is subject to the Boost Software
// License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//  Authors: Lauren Foutz
//           Scott Hill

/*
  This file implements the functions

  template <class VertexListGraph, class DistanceMatrix,
    class P, class T, class R>
  bool floyd_warshall_initialized_all_pairs_shortest_paths(
    const VertexListGraph& g, DistanceMatrix& d,
    const bgl_named_params<P, T, R>& params)

  AND

  template <class VertexAndEdgeListGraph, class DistanceMatrix,
    class P, class T, class R>
  bool floyd_warshall_all_pairs_shortest_paths(
    const VertexAndEdgeListGraph& g, DistanceMatrix& d,
    const bgl_named_params<P, T, R>& params)
*/

#ifndef BOOST_GRAPH_FLOYD_WARSHALL_HPP
#define BOOST_GRAPH_FLOYD_WARSHALL_HPP

#include <boost/property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/relax.hpp>
#include <algorithm> // for std::min and std::max

namespace boost
{
  namespace detail
  {

    template<typename VertexListGraph, typename DistanceMatrix, typename BinaryPredicate, typename BinaryFunction, typename Infinity, typename Zero>
	bool floyd_warshall_dispatch(const VertexListGraph& g,  DistanceMatrix& d, const BinaryPredicate &compare, const BinaryFunction &combine,
						const Infinity& inf, const Zero& zero)
    {
      BOOST_USING_STD_MIN();

      typename graph_traits<VertexListGraph>::vertex_iteratorN
        i, lasti, j, lastj, k, lastk;

      /* main Floyd Warshall algorithm */
      for (tie(k, lastk) = vertices(g); k != lastk; k++)
        for (tie(i, lasti) = vertices(g); i != lasti; i++)
          for (tie(j, lastj) = vertices(g); j != lastj; j++)
          {
            d[*i][*j] = min BOOST_PREVENT_MACRO_SUBSTITUTION(d[*i][*j], combine(d[*i][*k], d[*k][*j]));
          }

		/* checks for negative weight cycle */
      for (tie(i, lasti) = vertices(g); i != lasti; i++)
      	if (compare(d[*i][*i], zero))
          return false;

	  return true;
    }

/* with predecessor map */
    template<typename VertexListGraph, typename DistanceMatrix, typename PredecessorMatrix, typename BinaryPredicate, typename BinaryFunction, typename Infinity, typename Zero>
	bool floyd_warshall_dispatch2(const VertexListGraph& g,  DistanceMatrix& d, PredecessorMatrix& p, const BinaryPredicate &compare, const BinaryFunction &combine,
						const Infinity& inf, const Zero& zero)
    {
      BOOST_USING_STD_MIN();

      typename graph_traits<VertexListGraph>::vertex_iteratorN i, lasti, j, lastj, k, lastk;

      // main Floyd Warshall algorithm
      for (tie(k, lastk) = vertices(g); k != lastk; k++)
        for (tie(i, lasti) = vertices(g); i != lasti; i++)
          for (tie(j, lastj) = vertices(g); j != lastj; j++)
          {
			if(d[*i][*j] > combine(d[*i][*k], d[*k][*j]))
				p[*i][*j] = p[*k][*j];

			d[*i][*j] = min BOOST_PREVENT_MACRO_SUBSTITUTION(d[*i][*j], combine(d[*i][*k], d[*k][*j]));
          }

		// checks for negative weight cycle
      for (tie(i, lasti) = vertices(g); i != lasti; i++)
      	if (compare(d[*i][*i], zero))
          return false;

	  return true;
    }
  }

  template <typename VertexListGraph, typename DistanceMatrix, typename BinaryPredicate, typename BinaryFunction,typename Infinity, typename Zero>
  bool floyd_warshall_initialized_all_pairs_shortest_paths(const VertexListGraph& g, DistanceMatrix& d, const BinaryPredicate& compare,
				    const BinaryFunction& combine, const Infinity& inf, const Zero& zero)
  {
    function_requires<VertexListGraphConcept<VertexListGraph> >();

    return detail::floyd_warshall_dispatch(g, d, compare, combine, inf, zero);
  }

  template <typename VertexAndEdgeListGraph, typename DistanceMatrix, typename WeightMap, typename BinaryPredicate, typename BinaryFunction, typename Infinity, typename Zero>
  bool floyd_warshall_all_pairs_shortest_paths( const VertexAndEdgeListGraph& g, DistanceMatrix& d, const WeightMap& w,
  					 const BinaryPredicate& compare, const BinaryFunction& combine, const Infinity& inf, const Zero& zero)
  {
    BOOST_USING_STD_MIN();

    function_requires<VertexListGraphConcept<VertexAndEdgeListGraph> >();
    function_requires<EdgeListGraphConcept<VertexAndEdgeListGraph> >();
    function_requires<IncidenceGraphConcept<VertexAndEdgeListGraph> >();

    typename graph_traits<VertexAndEdgeListGraph>::vertex_iteratorN 	firstv, lastv, firstv2, lastv2;
    typename graph_traits<VertexAndEdgeListGraph>::edge_iteratorN 	first, last;

    for(tie(firstv, lastv) = vertices(g); firstv != lastv; firstv++)
      for(tie(firstv2, lastv2) = vertices(g); firstv2 != lastv2; firstv2++)
	    d[*firstv][*firstv2] = inf;

    for(tie(firstv, lastv) = vertices(g); firstv != lastv; firstv++)
      d[*firstv][*firstv] = 0;

    for(tie(first, last) = edges(g); first != last; first++)
    {
      if (d[source(*first, g)][target(*first, g)] != inf)
        d[source(*first, g)][target(*first, g)] = min BOOST_PREVENT_MACRO_SUBSTITUTION(get(w, *first), d[source(*first, g)][target(*first, g)]);
      else
        d[source(*first, g)][target(*first, g)] = get(w, *first);
    }

    bool is_undirected = is_same<typename graph_traits<VertexAndEdgeListGraph>::directed_category, undirected_tag>::value;
    if (is_undirected)
    {
      for(tie(first, last) = edges(g); first != last; first++)
      {
        if (d[target(*first, g)][source(*first, g)] != inf)
          d[target(*first, g)][source(*first, g)] = min BOOST_PREVENT_MACRO_SUBSTITUTION(get(w, *first), d[target(*first, g)][source(*first, g)]);
        else
          d[target(*first, g)][source(*first, g)] = get(w, *first);
      }
    }

    return detail::floyd_warshall_dispatch(g, d, compare, combine, inf, zero);
  }

/* with predecessor map */
  template <typename VertexAndEdgeListGraph, typename DistanceMatrix, typename PredecessorMatrix, typename WeightMap, typename BinaryPredicate, typename BinaryFunction, typename Infinity, typename Zero>
  bool floyd_warshall_all_pairs_shortest_paths2( const VertexAndEdgeListGraph& g, DistanceMatrix& d, PredecessorMatrix& p, const WeightMap& w,
  					 const BinaryPredicate& compare, const BinaryFunction& combine, const Infinity& inf, const Zero& zero)
  {
    BOOST_USING_STD_MIN();

    function_requires<VertexListGraphConcept<VertexAndEdgeListGraph> >();
    function_requires<EdgeListGraphConcept<VertexAndEdgeListGraph> >();
    function_requires<IncidenceGraphConcept<VertexAndEdgeListGraph> >();

    typename graph_traits<VertexAndEdgeListGraph>::vertex_iteratorN 	firstv, lastv, firstv2, lastv2;
    typename graph_traits<VertexAndEdgeListGraph>::edge_iteratorN 	first, last;

    // initialize matrix: distance infinity
    for(tie(firstv, lastv) = vertices(g); firstv != lastv; firstv++)
      for(tie(firstv2, lastv2) = vertices(g); firstv2 != lastv2; firstv2++)
      {
	  	d[*firstv][*firstv2] = inf;
		p[*firstv][*firstv2] = *firstv;
		//cerr << "[FW::Initialization]  p: " << *firstv << " = " << p[*firstv][*firstv2] << endl;
	  }

    // initialize matrix: distance to itself i zero
    for(tie(firstv, lastv) = vertices(g); firstv != lastv; firstv++)
      d[*firstv][*firstv] = 0;

    for(tie(first, last) = edges(g); first != last; first++)
    {
      if (d[source(*first, g)][target(*first, g)] != inf)
	  {
	  	cerr << "[Floyd-Warshall] ERROR Predecessor map " << endl;
        d[source(*first, g)][target(*first, g)] = min BOOST_PREVENT_MACRO_SUBSTITUTION(get(w, *first), d[source(*first, g)][target(*first, g)]);
	  }
      else
	  {
        d[source(*first, g)][target(*first, g)] = get(w, *first);
		p[source(*first, g)][target(*first, g)] = source(*first, g); // enter predecessor map here
	    //cerr << "[FW::Initialization] (" << source(*first, g) << "," << target(*first, g) << ") Parent map: " << p[source(*first, g)][target(*first, g)];
		//cerr << " Weight Map: " << d[source(*first, g)][target(*first, g)] << endl;
	  }
    }

    bool is_undirected = is_same<typename graph_traits<VertexAndEdgeListGraph>::directed_category, undirected_tag>::value;
    if (is_undirected)
    {
      for(tie(first, last) = edges(g); first != last; first++)
      {
        if (d[target(*first, g)][source(*first, g)] != inf)
		{
		  cerr << "[Floyd-Warshall] Undirected: ERROR Predecessor map " << endl;
          d[target(*first, g)][source(*first, g)] = min BOOST_PREVENT_MACRO_SUBSTITUTION(get(w, *first), d[target(*first, g)][source(*first, g)]);
		}
        else
		{
          d[target(*first, g)][source(*first, g)] = get(w, *first);
		  p[target(*first, g)][source(*first, g)] = target(*first, g); // enter predecessor map here
		  //cerr << "[FW::Initialization] (" << target(*first, g) << "," << source(*first, g) << ") UndParent map: " << p[target(*first, g)][source(*first, g)];
		  //cerr << " Weight Map: " << d[target(*first, g)][source(*first, g)] << endl;
		}
      }
    }

    return detail::floyd_warshall_dispatch2(g, d, p, compare, combine, inf, zero);
  }

  namespace detail {
    template <class VertexListGraph, class DistanceMatrix, class WeightMap, class P, class T, class R>
    bool floyd_warshall_init_dispatch(const VertexListGraph& g, DistanceMatrix& d, WeightMap w, const bgl_named_params<P, T, R>& params)
    {
      typedef typename property_traits<WeightMap>::value_type WM;

      return floyd_warshall_initialized_all_pairs_shortest_paths(g, d,
        choose_param(get_param(params, distance_compare_t()),
          std::less<WM>()),
        choose_param(get_param(params, distance_combine_t()),
          closed_plus<WM>()),
        choose_param(get_param(params, distance_inf_t()),
          std::numeric_limits<WM>::max BOOST_PREVENT_MACRO_SUBSTITUTION()),
        choose_param(get_param(params, distance_zero_t()),
          WM()));
    }

    template <class VertexAndEdgeListGraph, class DistanceMatrix, class WeightMap, class P, class T, class R>
    bool floyd_warshall_noninit_dispatch(const VertexAndEdgeListGraph& g, DistanceMatrix& d, WeightMap w, const bgl_named_params<P, T, R>& params)
    {
      typedef typename property_traits<WeightMap>::value_type WM;

      return floyd_warshall_all_pairs_shortest_paths(g, d, w,
        choose_param(get_param(params, distance_compare_t()),
          std::less<WM>()),
        choose_param(get_param(params, distance_combine_t()),
          closed_plus<WM>()),
        choose_param(get_param(params, distance_inf_t()),
          std::numeric_limits<WM>::max BOOST_PREVENT_MACRO_SUBSTITUTION()),
        choose_param(get_param(params, distance_zero_t()),
          WM()));
    }

/* With predecessor map */
    template <class VertexAndEdgeListGraph, class DistanceMatrix, class PredecessorMatrix, class WeightMap, class P, class T, class R>
    bool floyd_warshall_noninit_dispatch2(const VertexAndEdgeListGraph& g, DistanceMatrix& d, PredecessorMatrix& p, WeightMap w, const bgl_named_params<P, T, R>& params)
    {
      typedef typename property_traits<WeightMap>::value_type WM;

      return floyd_warshall_all_pairs_shortest_paths2(g, d, p, w,
        choose_param(get_param(params, distance_compare_t()),
          std::less<WM>()),
        choose_param(get_param(params, distance_combine_t()),
          closed_plus<WM>()),
        choose_param(get_param(params, distance_inf_t()),
          std::numeric_limits<WM>::max BOOST_PREVENT_MACRO_SUBSTITUTION()),
        choose_param(get_param(params, distance_zero_t()),
          WM()));
    }

  }   // namespace detail

/* VertexListGraphs:   */
  template <class VertexListGraph, class DistanceMatrix, class P, class T, class R>
  bool floyd_warshall_initialized_all_pairs_shortest_paths(const VertexListGraph& g, DistanceMatrix& d, const bgl_named_params<P, T, R>& params)
  {
    return detail::floyd_warshall_init_dispatch(g, d, choose_const_pmap(get_param(params, edge_weight), g, edge_weight), params);
  }

  template <class VertexListGraph, class DistanceMatrix>
  bool floyd_warshall_initialized_all_pairs_shortest_paths(const VertexListGraph& g, DistanceMatrix& d)
  {
    bgl_named_params<int,int> params(0);
    return detail::floyd_warshall_init_dispatch(g, d, get(edge_weight, g), params);
  }

/* VertexAndEdgeListGraphs:   */
  template <class VertexAndEdgeListGraph, class DistanceMatrix, class P, class T, class R>
  bool floyd_warshall_all_pairs_shortest_paths(const VertexAndEdgeListGraph& g, DistanceMatrix& d, const bgl_named_params<P, T, R>& params)
  {
    return detail::floyd_warshall_noninit_dispatch(g, d, choose_const_pmap(get_param(params, edge_weight), g, edge_weight), params);
  }

  template <class VertexAndEdgeListGraph, class DistanceMatrix>
  bool floyd_warshall_all_pairs_shortest_paths(const VertexAndEdgeListGraph& g, DistanceMatrix& d)
  {
    bgl_named_params<int,int> params(0);
    return detail::floyd_warshall_noninit_dispatch(g, d, get(edge_weight, g), params);
  }

/* With predecessor map */
  template <class VertexAndEdgeListGraph, class DistanceMatrix, class PredecessorMatrix, class P, class T, class R>
  bool floyd_warshall_all_pairs_shortest_paths2(const VertexAndEdgeListGraph& g, DistanceMatrix& d, PredecessorMatrix& p, const bgl_named_params<P, T, R>& params)
  {
    return detail::floyd_warshall_noninit_dispatch2(g, d, p, choose_const_pmap(get_param(params, edge_weight), g, edge_weight), params);
    //return detail::floyd_warshall_noninit_dispatch(g, d, choose_const_pmap(get_param(params, edge_weight), g, edge_weight), params);
  }

} // namespace boost

#endif


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <sophus/se3.hpp>
#include <sophus/so3.hpp>
#include <chrono>

using namespace std;
using namespace cv;

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

void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          std::vector<KeyPoint> &keypoints_1,
                          std::vector<KeyPoint> &keypoints_2,
                          std::vector<DMatch> &matches) {
  Mat descriptors_1, descriptors_2;
  // used in OpenCV3
  Ptr<FeatureDetector> detector = ORB::create();
  Ptr<DescriptorExtractor> descriptor = ORB::create();
  // use this if you are in OpenCV2
  // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
  // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
  Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
  detector->detect(img_1, keypoints_1);
  detector->detect(img_2, keypoints_2);

  descriptor->compute(img_1, keypoints_1, descriptors_1);
  descriptor->compute(img_2, keypoints_2, descriptors_2);

  vector<DMatch> match;
  // BFMatcher matcher ( NORM_HAMMING );
  matcher->match(descriptors_1, descriptors_2, match);

  double min_dist = 10000, max_dist = 0;

  for (int i = 0; i < descriptors_1.rows; i++) {
    double dist = match[i].distance;
    if (dist < min_dist) min_dist = dist;
    if (dist > max_dist) max_dist = dist;
  }

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

  for (int i = 0; i < descriptors_1.rows; i++) {
    if (match[i].distance <= max(2 * min_dist, 30.0)) {
      matches.push_back(match[i]);
    }
  }
}

Point2d pixel2cam(const Point2d &p, const Mat &K);

typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;

// BA by gauss-newton
void bundleAdjustmentGaussNewton(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose
);

void pose_refinement_gauss_newton(const VecVector3d& points_3d, const VecVector2d& points_2d, const Eigen::Matrix3d& K, Sophus::SE3d& pose);

int main(int argc, char **argv) {
  // if (argc != 5) {
  //   cout << "usage: pose_estimation_3d2d img1 img2 depth1 depth2" << endl;
  //   return 1;
  // }
  string f1 = "../1.png"; //argv[1];
  string f2 = "../2.png"; //argv[2];
  string f3 = "../1_depth.png"; //argv[3];
  Mat img_1 = imread(f1, CV_LOAD_IMAGE_COLOR);
  Mat img_2 = imread(f2, CV_LOAD_IMAGE_COLOR);
  assert(img_1.data && img_2.data && "Can not load images!");

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

  Mat d1 = imread(f3, CV_LOAD_IMAGE_UNCHANGED);
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
  vector<Point3f> pts_3d;
  vector<Point2f> pts_2d;
  for (DMatch m:matches) {
    ushort d = d1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
    if (d == 0)   // bad depth
      continue;
    float dd = d / 5000.0;
    Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
    pts_3d.push_back(Point3f(p1.x * dd, p1.y * dd, dd));
    pts_2d.push_back(keypoints_2[m.trainIdx].pt);
  }

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

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  Mat r, t;
  solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false);
  Mat R;
  cv::Rodrigues(r, R);
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve pnp in opencv cost time: " << time_used.count() << " seconds." << endl;

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

  VecVector3d pts_3d_eigen;
  VecVector2d pts_2d_eigen;
  for (size_t i = 0; i < pts_3d.size(); ++i) {
    pts_3d_eigen.push_back(Eigen::Vector3d(pts_3d[i].x, pts_3d[i].y, pts_3d[i].z));
    pts_2d_eigen.push_back(Eigen::Vector2d(pts_2d[i].x, pts_2d[i].y));
  }

  cout << "calling bundle adjustment by gauss newton" << endl;
  Sophus::SE3d pose_gn;
  t1 = chrono::steady_clock::now();
  bundleAdjustmentGaussNewton(pts_3d_eigen, pts_2d_eigen, K, pose_gn);
  t2 = chrono::steady_clock::now();
  time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve pnp by gauss newton cost time: " << time_used.count() << " seconds." << endl;

  Eigen::Vector3d t_gn = pose_gn.translation();
  Eigen::Matrix3d R_gn = pose_gn.so3().unit_quaternion().toRotationMatrix();
  cout << "R_gn = " << endl << R_gn << endl;
  cout << "t_gn = " << endl << t_gn << endl;

  // cout << "calling bundle adjustment by g2o" << endl;
  // Sophus::SE3d pose_g2o;
  // t1 = chrono::steady_clock::now();
  // bundleAdjustmentG2O(pts_3d_eigen, pts_2d_eigen, K, pose_g2o);
  // t2 = chrono::steady_clock::now();
  // time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  // cout << "solve pnp by g2o cost time: " << time_used.count() << " seconds." << endl;

  Eigen::Matrix3d K_eigen;
  K_eigen << 520.9, 0, 325.1,
             0, 521.0, 249.7,
             0, 0, 1;
  cout << "Custom Gauss-Newton" << endl;
  Sophus::SE3d pose_gn2(Sophus::SO3d(Eigen::Matrix3d::Identity()), Eigen::Vector3d::Zero());
  // Sophus::SE3d pose_gn2(Sophus::SO3d(Eigen::Matrix3d::Identity()), Eigen::Vector3d(0.5, -0.1, 0.2));
  t1 = chrono::steady_clock::now();
  pose_refinement_gauss_newton(pts_3d_eigen, pts_2d_eigen, K_eigen, pose_gn2);
  t2 = chrono::steady_clock::now();
  time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve pnp with custom Gauss-Newton cost time: " << time_used.count() << " seconds." << endl;

  Eigen::Vector3d t_gn2 = pose_gn2.translation();
  Eigen::Matrix3d R_gn2 = pose_gn2.so3().unit_quaternion().toRotationMatrix();
  cout << "R_gn2 = " << endl << R_gn2 << endl;
  cout << "t_gn2 = " << endl << t_gn2 << endl;

  return 0;
}

void pose_refinement_gauss_newton(const VecVector3d& points_3d, const VecVector2d& points_2d, const Eigen::Matrix3d& K, Sophus::SE3d& pose)
{
  const int max_iter = 100;
  double fx = K(0, 0);
  double fy = K(1, 1);
  const int n = points_2d.size();
  double prev_cost = 0.0;
  for (int it = 0; it < max_iter; ++it)
  {
    Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
    Eigen::Matrix<double, 6, 1> b = Eigen::Matrix<double, 6, 1>::Zero();
    double cost = 0.0;
    for (int i = 0; i < n; ++i)
    {
      Eigen::Matrix<double, 2, 6> J = Eigen::Matrix<double, 2, 6>::Zero();
      const auto& X = points_3d[i];
      Eigen::Vector3d x = K * (pose * X);
      x /= x.z();
      Eigen::Vector2d err = points_2d[i] - x.head(2);
      cost += err.squaredNorm();
      double Z2 = std::pow(X.z(), 2);
      J(0, 0) = -fx / X.z();
      J(0, 2) = fx * X.x() / Z2;
      J(0, 3) = fx * X.x() * X.y() / Z2;
      J(0, 4) = -fx-fx * std::pow(X.x(), 2) / Z2;
      J(0, 5) = fx * X.y() / X.z();

      J(1, 1) = -fy / X.z();
      J(1, 2) = fy * X.y() / Z2;
      J(1, 3) = fy + fy * std::pow(X.y(), 2) / Z2;
      J(1, 4) = -fy * X.y() * X.x() / Z2;
      J(1, 5) = -fy * X.x() / X.z();

      H += J.transpose() * J;
      b += -J.transpose() * err;
    }

    std::cout << "iter " << it << ": cost = " << cost << "\n";
    Eigen::Matrix<double, 6, 1> delta_x = H.inverse() * b;
    // Eigen::Matrix<double, 6, 1> delta_x = H.ldlt().solve(b);

    if (it > 0 && prev_cost - cost < 1e-5 || delta_x.norm() < 1e-5)
    {
      break;
    }

    // update pose
    pose = Sophus::SE3d::exp(delta_x) * pose;
    prev_cost = cost;
  }

}

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

void bundleAdjustmentGaussNewton(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose) {
  typedef Eigen::Matrix<double, 6, 1> Vector6d;
  const int iterations = 10;
  double cost = 0, lastCost = 0;
  double fx = K.at<double>(0, 0);
  double fy = K.at<double>(1, 1);
  double cx = K.at<double>(0, 2);
  double cy = K.at<double>(1, 2);

  for (int iter = 0; iter < iterations; iter++) {
    Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
    Vector6d b = Vector6d::Zero();

    cost = 0;
    // compute cost
    for (int i = 0; i < points_3d.size(); i++) {
      Eigen::Vector3d pc = pose * points_3d[i];
      double inv_z = 1.0 / pc[2];
      double inv_z2 = inv_z * inv_z;
      Eigen::Vector2d proj(fx * pc[0] / pc[2] + cx, fy * pc[1] / pc[2] + cy);

      Eigen::Vector2d e = points_2d[i] - proj;

      cost += e.squaredNorm();
      Eigen::Matrix<double, 2, 6> J;
      J << -fx * inv_z,
        0,
        fx * pc[0] * inv_z2,
        fx * pc[0] * pc[1] * inv_z2,
        -fx - fx * pc[0] * pc[0] * inv_z2,
        fx * pc[1] * inv_z,
        0,
        -fy * inv_z,
        fy * pc[1] * inv_z2,
        fy + fy * pc[1] * pc[1] * inv_z2,
        -fy * pc[0] * pc[1] * inv_z2,
        -fy * pc[0] * inv_z;

      H += J.transpose() * J;
      b += -J.transpose() * e;
    }

    Vector6d dx;
    dx = H.ldlt().solve(b);

    if (isnan(dx[0])) {
      cout << "result is nan!" << endl;
      break;
    }

    if (iter > 0 && cost >= lastCost) {
      // cost increase, update is not good
      cout << "cost: " << cost << ", last cost: " << lastCost << endl;
      break;
    }

    // update your estimation
    pose = Sophus::SE3d::exp(dx) * pose;
    lastCost = cost;

    cout << "iteration " << iter << " cost=" << std::setprecision(12) << cost << endl;
    if (dx.norm() < 1e-6) {
      // converge
      break;
    }
  }

  // cout << "pose by g-n: \n" << pose.matrix() << endl;
}


extern "C" {
#include <umfpack.h>
}
#include <iostream>
    #include <boost/numeric/bindings/traits/ublas_vector.hpp>
    #include <boost/numeric/bindings/traits/ublas_sparse.hpp>
    #include <boost/numeric/bindings/umfpack/umfpack.hpp>
    #include <boost/numeric/ublas/io.hpp>

    namespace ublas = boost::numeric::ublas;
    namespace umf = boost::numeric::bindings::umfpack;

    int main() {

      ublas::compressed_matrix<double, ublas::column_major, 0,
       ublas::unbounded_array<int>, ublas::unbounded_array<double> > A (5,5,12);
      ublas::vector<double> B (5), X (5);

      A(0,0) = 2.; A(0,1) = 3;
      A(1,0) = 3.; A(1,2) = 4.; A(1,4) = 6;
      A(2,1) = -1.; A(2,2) = -3.; A(2,3) = 2.;
      A(3,2) = 1.;
      A(4,1) = 4.; A(4,2) = 2.; A(4,4) = 1.;

      B(0) = 8.; B(1) = 45.; B(2) = -3.; B(3) = 3.; B(4) = 19.;

      umf::symbolic_type<double> Symbolic;
      umf::numeric_type<double> Numeric;

      umf::symbolic (A, Symbolic);
      umf::numeric (A, Symbolic, Numeric);
      umf::solve (A, X, B, Numeric);

      std::cout << X << std::endl;  // output: [5](1,2,3,4,5)
	  return 0;
    }


#include "solv_plug.hpp"
#include <Eigen/SparseCholesky>
#include <Eigen/Geometry>
#include <set>
#include "mesh/meshinfo.hpp"
#include "tetelem.hpp"

struct Triplet      // for Eigen interface
{
    int Row;
    int Col;
    real Value;

    int row() const { return Row; }
    int col() const { return Col; }
    real value() const { return Value; }
};

static void ApplyBc(const MeshInfo* mesh,
                    const SolvParams& params,
                    SpMat& mat,
                    SpVec& rhs);
static std::string SolvStatusToStr(const Eigen::ComputationInfo st);

bool SolvPlugin::Solve_(std::string& log_buf)
{
    const auto node_lst = solv_info_->Mesh->NodeLst;
    const auto elem_lst = solv_info_->Mesh->ElemLst;
    const auto node_num = solv_info_->Mesh->NodeNum;
    const auto elem_num = solv_info_->Mesh->ElemNum;

    const auto dof_count = 3 * node_num;

    TetElem tet_el(log_buf);
    Mat K_loc;
    Mat node_mat = Mat::Zero(4, 3);

    const size_t K_lst_size = elem_num * 4 * 4 * 3 * 3;

    std::vector<Triplet> K_lst;
    try
    {
        K_lst.resize(K_lst_size);
    }
    catch (const std::bad_alloc&)
    {
        log_buf = "Cannot allocate memory for stiffness matrix (" +
                  std::to_string(K_lst_size * sizeof(Triplet)) + " bytes required).";
        return false;
    }

    for (uint i_el = 0, i_k_el = 0; i_el < elem_num; ++i_el)
    {
        // build local stiffness matrices

        for (int i_node = 0; i_node < 4; ++i_node)
            for(int i_coord = 0; i_coord < 3; ++i_coord)
                node_mat(i_node, i_coord) = node_lst[3*(elem_lst[4*i_el + i_node] - 1) + i_coord];

        if(!tet_el.Init(i_el, node_mat, solv_params_->Young, solv_params_->Poisn))
            return false;

        K_loc = tet_el.BuildStifMat();
        for (int i_node = 0; i_node < 4; ++i_node)
        {
            const int z1 = 3 * (elem_lst[4*i_el + i_node] - 1);
            for (int j_node = 0; j_node < 4; ++j_node)
            {
                const int z2 = 3 * (elem_lst[4*i_el + j_node] - 1);
                for (int i = 0; i < 3; ++i)
                    for (int j = 0; j < 3; ++j)
                    {
                        K_lst[i_k_el].Row = z1 + i;
                        K_lst[i_k_el].Col = z2 + j;
                        K_lst[i_k_el].Value = K_loc(i_node*3 + i, j_node*3 + j);
                        ++i_k_el;
                    }
            }
        }
    }

    SpMat K(dof_count, dof_count);
    K.setFromTriplets(K_lst.cbegin(), K_lst.cend());
    K_lst.clear();

    stats_buf += "Number of DOF: " + std::to_string(dof_count) + '\n';

    SpVec F(dof_count);

    ApplyBc(solv_info_->Mesh, *solv_params_, K, F);

    Eigen::SimplicialLDLT<SpMat> solver;

    solver.compute(K);
    if(auto st = solver.info(); st != Eigen::ComputationInfo::Success)
    {
        log_buf += "Could not decompose stiffness matrix: " + SolvStatusToStr(st);
        return false;
    }

    Vec sol = solver.solve(F);
    if(auto st = solver.info(); st != Eigen::ComputationInfo::Success)
    {
        log_buf += "Could not solve linear system: " + SolvStatusToStr(st);
        return false;
    }

    const auto siz = sol.size();
    for(int i = 0; i < siz; ++i)
        solv_info_->DispLst[i] = sol(i);

    return true;
}

void ApplyBc(const MeshInfo* mesh,
             const SolvParams& params,
             SpMat& mat,
             SpVec& rhs)
{
    const real big_num = 1.0e+28;
    std::set<MeshInfo::id_t> used_node_lst;

    for(auto i_face = 0u; i_face < mesh->ElemFaceNum; ++i_face)
    {
        for(const auto& bc: params.BcLst)
        {
            if(bc.first != mesh->ElemFaceMarkerLst[i_face])
                continue;

            if(bc.second.BcLst.size() == 1 && bc.second.BcLst.front().Tag[0] == 'p')
            {
                const MeshInfo::id_t nodes[] = { mesh->ElemFaceLst[3*i_face + 0] - 1,
                                                 mesh->ElemFaceLst[3*i_face + 1] - 1,
                                                 mesh->ElemFaceLst[3*i_face + 2] - 1 };
                const Vec3 n1_vec(mesh->NodeLst[3*nodes[0]+0], mesh->NodeLst[3*nodes[0]+1], mesh->NodeLst[3*nodes[0]+2]);
                const Vec3 n2_vec(mesh->NodeLst[3*nodes[1]+0], mesh->NodeLst[3*nodes[1]+1], mesh->NodeLst[3*nodes[1]+2]);
                const Vec3 n3_vec(mesh->NodeLst[3*nodes[2]+0], mesh->NodeLst[3*nodes[2]+1], mesh->NodeLst[3*nodes[2]+2]);
                const Vec3 surf_v = (n2_vec - n1_vec).cross(n3_vec - n1_vec);
                const Vec3 pr_vec = (bc.second.BcLst.front().Value/6) * surf_v;

                for(int i = 0; i < 3; ++i)
                    for(int j = 0; j < 3; ++j)
                    {
                        rhs.coeffRef(3*nodes[i]+j) += pr_vec(j);
                        //used_node_lst.insert(nodes[i]+j);  - do we have to do this???
                    }
            }
            else
            {
                for(int i_n = 0; i_n < 3; ++i_n)
                {
                    const auto node = mesh->ElemFaceLst[3*i_face + i_n] - 1;
                    for(const auto& b: bc.second.BcLst)
                    {
                        for(int i_ax = 0; i_ax < 3; ++i_ax)
                            if(b.Tag[i_ax+1] && !used_node_lst.count(3*node+i_ax))
                            {
                                if(b.Tag[0] == 'u')
                                    mat.coeffRef(3*node+i_ax, 3*node+i_ax) = big_num;
                                rhs.coeffRef(3*node+i_ax) = (b.Tag[0] == 'u' ? big_num : 1.0) * b.Value;
                                used_node_lst.insert(3*node+i_ax);
                            }
                    }
                }
            }

            break;
        }
    }
}

std::string SolvStatusToStr(const Eigen::ComputationInfo st)
{
    switch (st)
    {
    case Eigen::ComputationInfo::Success:       return "success";
    case Eigen::ComputationInfo::InvalidInput:  return "the inputs are invalid, or the algorithm has been improperly called";
    case Eigen::ComputationInfo::NoConvergence: return "iterative procedure did not converge";
    case Eigen::ComputationInfo::NumericalIssue:return "the provided data did not satisfy the prerequisites";
    }

    return "";
}


#ifndef STAN_MATH_FWD_SCAL_FUN_LOG_FALLING_FACTORIAL_HPP
#define STAN_MATH_FWD_SCAL_FUN_LOG_FALLING_FACTORIAL_HPP

#include <stan/math/fwd/meta.hpp>
#include <stan/math/fwd/core.hpp>

#include <stan/math/prim/scal/fun/log_falling_factorial.hpp>
#include <boost/math/special_functions/digamma.hpp>

namespace stan {
namespace math {

template <typename T>
inline fvar<T> log_falling_factorial(const fvar<T>& x, const fvar<T>& n) {
  using boost::math::digamma;

  return fvar<T>(log_falling_factorial(x.val_, n.val_),
                 (digamma(x.val_ + 1) - digamma(x.val_ - n.val_ + 1)) * x.d_
                     + digamma(x.val_ - n.val_ + 1) * n.d_);
}

template <typename T>
inline fvar<T> log_falling_factorial(double x, const fvar<T>& n) {
  using boost::math::digamma;

  return fvar<T>(log_falling_factorial(x, n.val_),
                 digamma(x - n.val_ + 1) * n.d_);
}

template <typename T>
inline fvar<T> log_falling_factorial(const fvar<T>& x, double n) {
  using boost::math::digamma;

  return fvar<T>(log_falling_factorial(x.val_, n),
                 (digamma(x.val_ + 1) - digamma(x.val_ - n + 1)) * x.d_);
}
}  // namespace math
}  // namespace stan
#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_MOD_N_INCLUDE
#define MTL_MOD_N_INCLUDE

#include <iostream>
#include <boost/operators.hpp>
#include <cassert>

#include <boost/config/concept_macros.hpp>
#ifdef __GXX_CONCEPTS__
#  include <bits/concepts.h>
#endif

#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/linear_algebra/is_invertible.hpp>
#include <boost/numeric/linear_algebra/inverse.hpp>
#include <boost/numeric/linear_algebra/operators.hpp>
#include <boost/numeric/linear_algebra/concepts.hpp>
#include <boost/numeric/meta_math/is_prime.hpp>

namespace mtl {

template<typename T, T N>
//  where std::Integral<T>
class mod_n_t
  : boost::totally_ordered< mod_n_t<T, N> >,
    boost::arithmetic< mod_n_t<T, N> >
{
    // or BOOST_STATIC_ASSERT((IS_INTEGRAL))

    T                 value;
 public:
    typedef T         value_type;
    typedef mod_n_t   self;

    static T const modulo= N;

    mod_n_t() : value(0) {}

    explicit mod_n_t(T const& v)
    {
	value= v >= 0 ? v%modulo : modulo - -v%modulo;
    }

    // modulo of negative numbers can be bizarre
    // better use constructor for T
    explicit mod_n_t(int v)
    {
	value= v >= 0 ? v%modulo : modulo - -v%modulo;
    }

    // copy constructor
    mod_n_t(const mod_n_t<T, N>& m): value(m.get()) {}

    // assignment
    mod_n_t<T, N>& operator= (const mod_n_t<T, N>& m)
    {
	value= m.value;
	return *this;
    }

    mod_n_t<T, N>& operator= (T const& v)
    {
	value= v >= 0 ? v%modulo : modulo - -v%modulo;
	return *this;
    }

    // conversion from other moduli must be called explicitly
    template<T OtherN>
    mod_n_t<T, N>& convert(const mod_n_t<T, OtherN>& m)
    {
	value= m.value >= 0 ? m.value%modulo : modulo - -m.value%modulo;
	return *this;
    }

    T get() const
    {
	return value;
    }

    bool operator==(self const& y) const
    {
	check(*this); check(y);
	return this->value == y.value;
    }

    bool operator<(self const& y) const
    {
	check(*this); check(y);
	return this->value < y.value;
    }

    self& operator+= (self const& y)
    {
	check(*this); check(y);
	this->value += y.value;
	this->value %= modulo;
	return *this;
    }

    self& operator-= (self const& y)
    {
	check(*this); check(y);
	// add n to avoid negative numbers esp. if T is unsigned
	this->value += modulo;
	this->value -= y.value;
	this->value %= modulo;
	return *this;
    }

    self& operator*= (self const& y)
    {
	check(*this); check(y);
	this->value *= y.value;
	this->value %= modulo;
	return *this;
    }

    self& operator/= (self const& y);

};

template<typename T, T N>
inline void check(const mod_n_t<T, N>& x)
{
    assert(x.get() >= 0 && x.get() < N);
}

template<typename T, T N>
inline std::ostream& operator<< (std::ostream& stream, const mod_n_t<T, N>& a)
{
    check(a);
    return stream << a.get();
}

// Extended Euclidian algorithm in vector notation
    // uu = (u1, u2, u3) := (1, 0, u)
    // vv = (v1, v2, v3) := (0, 1, v)
    // while (u3 % v3 != 0) {
    //   q= u3 / v3
    //   rr= uu - q * vv
    //   uu= vv
    //   vv= rr }
    //
    // with u = N and v = y
    // --> v2 * v mod u == gcd(u, v)
    // --> v2 * y mod N == 1
    // --> x * v2 == x / y
    // v1, u1, and r1 not used
template<typename T, T N>
inline mod_n_t<T, N>& mod_n_t<T, N>::operator/= (const mod_n_t<T, N>& y)
{
    check(*this); check(y);
    if (y.get() == 0) throw "Division by 0";

    // Goes wrong with unsigned b/c some values will be negative (even if the result isn't)
    // Something like remove_sign<T>::type would be cute
    int u= N, v= y.get(), /* u1= 1, */  u2= 0, /* v1= 0, */  v2= 1, q, r, /* r1, */  r2;

    while (u % v != 0) {
	q= u / v;

	r= u % v; /* r1= u1 - q * v1; */ r2= u2 - q * v2;
	u= v; /* u1= v1; */ u2= v2;
	v= r; /* v1= r1; */ v2= r2;
    }

    return *this *= mod_n_t<T, N>(v2);
}

inline int gcd(int u, int v)
{
    int r;
    while ((r= u % v) != 0) {
	u= v; v= r;
    }
    return v;
}

} // namespace mtl

namespace math {

    using mtl::mod_n_t;

    template<typename T, T N>
    struct identity_t< add< mod_n_t<T, N> >, mod_n_t<T, N> >
    {
	typedef mod_n_t<T, N> mod_t;
	mod_t operator() (add<mod_t> const&, mod_t const& v) const
	{
	    return mod_t(0);
	}
    };

    // Reverse definition, a little more efficient if / uses inverse
    template<typename T, T N>
    struct inverse_t< add< mod_n_t<T, N> >, mod_n_t<T, N> >
    {
	typedef mod_n_t<T, N> mod_t;
	mod_t operator() (add<mod_t> const& op, mod_t const& v) const
	{
	    return identity(op, v) - v;
	}
    };

    template<typename T, T N>
    struct is_invertible_t< add< mod_n_t<T, N> >, mod_n_t<T, N> >
    {
	typedef mod_n_t<T, N> mod_t;
	bool operator() (add<mod_t> const&, mod_t const& v) const
	{ return true; }
    };

    template<typename T, T N>
    struct identity_t< mult< mod_n_t<T, N> >, mod_n_t<T, N> >
    {
	typedef mod_n_t<T, N> mod_t;
	mod_t operator() (mult<mod_t> const&, mod_t const& v) const
	{
	    return mod_t(1);
	}
    };

    // Reverse definition, a little more efficient if / uses inverse
    template<typename T, T N>
    struct inverse_t< mult< mod_n_t<T, N> >, mod_n_t<T, N> >
    {
	typedef mod_n_t<T, N> mod_t;
	mod_t operator() (mult<mod_t> const&, mod_t const& v) const
	{
	    return mod_t(1) / v;
	}
    };

    template<typename T, T N>
    struct is_invertible_t< mult< mod_n_t<T, N> >, mod_n_t<T, N> >
    {
	typedef mod_n_t<T, N> mod_t;
	bool operator() (mult<mod_t> const&, mod_t const& v) const
	{
#           ifdef MTL_TRACE_MOD_N_INVERTIBILITY_DISPATCHING
                std::cout << "[slow mod n inversion test] ";
#           endif
	    T value = v.get();
	    return value != 0 && mtl::gcd(N, value) == 1;
	}
    };

# ifdef __GXX_CONCEPTS__

    // With Concept we can provide a faster invertibility test for prime numbers:
    //  only 0 is not invertible and gcd doesn't need to be called

    template<typename T, T N>
        where meta_math::Prime<N>
    struct is_invertible_t< mult< mod_n_t<T, N> >, mod_n_t<T, N> >
    {
	typedef mod_n_t<T, N> mod_t;
	bool operator() (mult<mod_t> const&, mod_t const& v) const
	{
#           ifdef MTL_TRACE_MOD_N_INVERTIBILITY_DISPATCHING
                std::cout << "[fast mod n inversion test] ";
#           endif
 	    return v.get() != 0;
	}
    };

// Concept mapping
// All modulo sets are commutative rings with identity
// but only if N is prime it is also a field
// Due to some mapping nesting trouble we define normally derived maps

template <typename T, T N>
concept_map CommutativeRingWithIdentity< mod_n_t<T, N> >
{
    // Why do we need the typedefs???

    typedef mod_n_t<T, N>& plus_assign_result_type;
    typedef mod_n_t<T, N>  addition_result_type;
    typedef mod_n_t<T, N>  unary_result_type;
    typedef mod_n_t<T, N>& minus_assign_result_type;
    typedef mod_n_t<T, N>  subtraction_result_type;

    typedef mod_n_t<T, N>& mult_assign_result_type;
    typedef mod_n_t<T, N>  mult_result_type;
    typedef mod_n_t<T, N>& divide_assign_result_type;
    typedef mod_n_t<T, N>  division_result_type;

    typedef mod_n_t<T, N>  inverse_result_type;
    typedef mod_n_t<T, N>  identity_result_type;
    typedef bool           is_invertible_result_type;
}

template <typename T, T N>
concept_map MultiplicativePartiallyInvertibleMonoid< mod_n_t<T, N> >
{
    // Why do we need the typedefs???

    typedef mod_n_t<T, N>& mult_assign_result_type;
    typedef mod_n_t<T, N>  mult_result_type;
    typedef mod_n_t<T, N>& divide_assign_result_type;
    typedef mod_n_t<T, N>  division_result_type;

    typedef mod_n_t<T, N>  inverse_result_type;
    typedef mod_n_t<T, N>  identity_result_type;
    typedef bool           is_invertible_result_type;
}

template <typename T, T N>
    where meta_math::Prime<N>
concept_map Field< mod_n_t<T, N> >
{
    // Why do we need the typedefs???

    typedef mod_n_t<T, N>& plus_assign_result_type;
    typedef mod_n_t<T, N>  addition_result_type;
    typedef mod_n_t<T, N>  unary_result_type;
    typedef mod_n_t<T, N>& minus_assign_result_type;
    typedef mod_n_t<T, N>  subtraction_result_type;

    typedef mod_n_t<T, N>& mult_assign_result_type;
    typedef mod_n_t<T, N>  mult_result_type;
    typedef mod_n_t<T, N>& divide_assign_result_type;
    typedef mod_n_t<T, N>  division_result_type;

    typedef mod_n_t<T, N>  inverse_result_type;
    typedef mod_n_t<T, N>  identity_result_type;
    typedef bool           is_invertible_result_type;
}

# endif // __GXX_CONCEPTS__

} // namespace math

#endif // MTL_MOD_N_INCLUDE


#include <mex.h>
#include <math.h>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <iostream>
#include <vector>
#include <time.h>
#include <tbb/tbb.h>

using namespace Eigen;
using namespace std;
using namespace tbb;

double *J_index, *JT_index, *J_value, *JT_value, *JTJ_info, *pu, *wu, *bu, *max_iter, *step_size;
double *b, *lambda, *pi, *Jx_in, *p_mod, *dummy;

int J_row;
int J_col;
int row_spy;
int col_spy;
int max_it;
double step;

class spmv_mul_add
{

public:
	spmv_mul_add(double* A_index, double* A_value, double* input, double* addition, double* output, int spy)
	{

		this->A_index = A_index;
		this->A_value = A_value;
		this->input = input;
        this->addition = addition;
		this->output = output;
		this->spy = spy;

	};

	~spmv_mul_add() {};

	void operator() (const blocked_range<int>& r) const
	{
		for (int i = r.begin(); i != r.end(); i++)
		{
			output[i] = 0;

			for (int j = 0; j < spy; j++)
			{
				int index = i * spy + j;

                if(A_index[index] >= 0)
                {
                    int idx = A_index[index];
                    output[i] += A_value[index] * input[idx];
                }
			}

            output[i] += addition[i];
		}
	};

	double* A_index;
	double* A_value;
	double* input;
    double* addition;
	double* output;
	int spy;

};

class jacobi_update
{

public:
	jacobi_update(double* dof, double* update, double* divisor, double alpha)
	{
		this->dof = dof;
		this->update = update;
		this->divisor = divisor;
        this->alpha = alpha;
	};

	~jacobi_update() {};

	void operator() (const blocked_range<int>& r) const
	{
		for (int i = r.begin(); i != r.end(); i++)
		{
            dof[i] = fmax(dof[i] - alpha * update[i] / divisor[i], 0.0);
			//dof[i] = dof[i] - alpha * update[i] / divisor[i];
		}
	};

	double* dof;
	double* update;
	double* divisor;
    double alpha;

};

class set_zero
{

public:
	set_zero(double* data)
	{
		this->data = data;
	};

	~set_zero() {};

	void operator() (const blocked_range<int>& r) const
	{
		for (int i = r.begin(); i != r.end(); i++)
		{
			data[i] = 0;
		}
	};

	double* data;

};

double get_Fischer_Burmeister(double *x, double *pj, int num)
{

    double fb = 0;

    for (int i = 0; i < num; i++)
    {
        double ent = x[i] + pj[i] - sqrt(x[i] * x[i] + pj[i] * pj[i]);
        fb += ent * ent;
    }

    return sqrt(fb);

}

void damped_Jacobi()
{

    task_scheduler_init init(8);

    parallel_for(blocked_range<int>(0, J_col), set_zero(lambda));

    parallel_for(blocked_range<int>(0, J_row), set_zero(dummy));

    parallel_for(blocked_range<int>(0, J_col), spmv_mul_add(JT_index, JT_value, pu, bu, b, col_spy));

    double pre_FB, post_FB;

    parallel_for(blocked_range<int>(0, J_row), spmv_mul_add(J_index, J_value, lambda, dummy, Jx_in, row_spy));

    parallel_for(blocked_range<int>(0, J_col), spmv_mul_add(JT_index, JT_value, Jx_in, b, pi, col_spy));

    post_FB = get_Fischer_Burmeister(lambda, pi, J_col);

    int iteration = -1;

    for(int outer = 0; outer < max_it; outer++)
    {
        iteration = outer;
        pre_FB = post_FB;

        parallel_for(blocked_range<int>(0, J_col), jacobi_update(lambda, pi, wu, step));

        ///////////////////////////////////////////////////////////////////////////////

        parallel_for(blocked_range<int>(0, J_row), spmv_mul_add(J_index, J_value, lambda, dummy, Jx_in, row_spy));

        parallel_for(blocked_range<int>(0, J_col), spmv_mul_add(JT_index, JT_value, Jx_in, b, pi, col_spy));

        ///////////////////////////////////////////////////////////////////////////////

        post_FB = get_Fischer_Burmeister(lambda, pi, J_col);

        if(abs(pre_FB) < 1e-6)
		{
			break;
		}

        if((pre_FB - post_FB) / pre_FB < 1e-3)
        {
            break;
        }

        /*if(abs(pre_FB - post_FB) < 1e-3 * abs(pre_FB))
        {
            break;
        }*/

    }

    //mexPrintf("%d\n", iteration);

    parallel_for(blocked_range<int>(0, J_row), spmv_mul_add(J_index, J_value, lambda, dummy, p_mod, row_spy));

}

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

    mxArray *output_mex;

    J_index = mxGetPr(prhs[0]);
    JT_index = mxGetPr(prhs[1]);
    J_value = mxGetPr(prhs[2]);
    JT_value = mxGetPr(prhs[3]);
    JTJ_info = mxGetPr(prhs[4]);
    pu = mxGetPr(prhs[5]);
    wu = mxGetPr(prhs[6]);
    bu = mxGetPr(prhs[7]);
    max_iter = mxGetPr(prhs[8]);
    step_size = mxGetPr(prhs[9]);

    // JTJ_info : dof_n, max_valence, tri_n, 3 * 2

    J_row = JTJ_info[0];
    J_col = JTJ_info[2];
    row_spy = JTJ_info[1];
    col_spy = JTJ_info[3];
    max_it = max_iter[0];
    step = step_size[0];

    b = (double*)malloc(J_col * sizeof(double));
    lambda = (double*)malloc(J_col * sizeof(double));
    pi = (double*)malloc(J_col * sizeof(double));
    Jx_in = (double*)malloc(J_row * sizeof(double));
    dummy = (double*)malloc(J_row * sizeof(double));

    output_mex = plhs[0] = mxCreateDoubleMatrix(J_row, 1, mxREAL);
    p_mod = mxGetPr(output_mex);

    damped_Jacobi();

    free(b);
    free(lambda);
    free(pi);
    free(Jx_in);
    free(dummy);

    return;

}


#include <stdio.h>
#include <stdlib.h>

#include <Eigen/Core>

#include "../libfovis/absolute_orientation_horn.hpp"

using namespace std;

static int
rand_int_range(int min, int max)
{
  return rand() % (max - min) + min;
}

static double
rand_double_range(double min, double max)
{
  double v = rand() / (double)RAND_MAX;
  return v * (max - min) + min;
}

void
rpy_to_quat (const double rpy[3], double q[4])
{
  double roll = rpy[0], pitch = rpy[1], yaw = rpy[2];

  double halfroll = roll / 2;
  double halfpitch = pitch / 2;
  double halfyaw = yaw / 2;

  double sin_r2 = sin (halfroll);
  double sin_p2 = sin (halfpitch);
  double sin_y2 = sin (halfyaw);

  double cos_r2 = cos (halfroll);
  double cos_p2 = cos (halfpitch);
  double cos_y2 = cos (halfyaw);

  q[0] = cos_r2 * cos_p2 * cos_y2 + sin_r2 * sin_p2 * sin_y2;
  q[1] = sin_r2 * cos_p2 * cos_y2 - cos_r2 * sin_p2 * sin_y2;
  q[2] = cos_r2 * sin_p2 * cos_y2 + sin_r2 * cos_p2 * sin_y2;
  q[3] = cos_r2 * cos_p2 * sin_y2 - sin_r2 * sin_p2 * cos_y2;
}

int main(int argc, char** argv)
{
  int num_trials = 1000;

  for(int trial=0; trial<num_trials; trial++) {

    // generate a random set of points
    int num_points = rand_int_range(5, 1000);

    Eigen::Matrix3Xd points(3, num_points);

    for(int col=0; col<num_points; col++) {
      points(0, col) = rand_double_range(-10, 10);
      points(1, col) = rand_double_range(-10, 10);
      points(2, col) = rand_double_range(-10, 10);
    }

    // generate a random transformation
    Eigen::Vector3d translation(rand_double_range(-10, 10),
        rand_double_range(-10, 10),
        rand_double_range(-10, 10));

    double rpy[3] = {
      rand_double_range(-M_PI, M_PI),
      rand_double_range(-M_PI, M_PI),
      rand_double_range(-M_PI, M_PI) };
    double rot_quat[4];
    rpy_to_quat(rpy, rot_quat);

    Eigen::Quaterniond rotation(rot_quat[0], rot_quat[1], rot_quat[2], rot_quat[3]);

    Eigen::Isometry3d trans;
    trans.setIdentity();
    trans.translate(translation);
    trans.rotate(rotation);

    // apply transformation to original random point set
    Eigen::Matrix3Xd transformed = trans * points;

    Eigen::Isometry3d estimated_transform;
    absolute_orientation_horn(points, transformed, &estimated_transform);

    // reproject points using estimated transformation
    Eigen::Matrix3Xd reprojected = estimated_transform * points;

    // compute reprojection error
    Eigen::Matrix3Xd reproject_err = reprojected - transformed;

    Eigen::Vector3d mean_err = reproject_err.rowwise().sum() / num_points;
    printf("%4d (%4d): mean reprojection error: %6.3f, %6.3f, %6.3f\n", trial, num_points, mean_err(0), mean_err(1), mean_err(2));
    if(fabs(mean_err(0)) > 1e-9 || fabs(mean_err(1)) > 1e-9 || fabs(mean_err(1)) > 1e-9) {
      fprintf(stderr, "FAIL!\n");
      exit(1);
    }
  }

  fprintf(stderr, "OK\n");
  return 0;
}


/*
   Copyright (C) 2015-2021 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>

   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.
*/

// Critical temperature of triangular lattice Ising model

// reference: J. Stephenson, J. of Math. Phys. 11, 420 (1970)

#pragma once

#include <cmath>
#include <stdexcept>
#include <boost/math/differentiation/autodiff.hpp>
#include <standards/newton.hpp>

namespace {

template<typename T>
struct func {
  func(T Ja, T Jb, T Jc) : Ja_(Ja), Jb_(Jb), Jc_(Jc) {}
  auto operator()(T beta) const -> decltype(boost::math::differentiation::make_fvar<T, 1>(beta)) {
    using std::exp;
    auto beta_fvar = boost::math::differentiation::make_fvar<T, 1>(beta);
    auto za = exp(-2 * beta_fvar * Ja_);
    auto zb = exp(-2 * beta_fvar * Jb_);
    auto zc = exp(-2 * beta_fvar * Jc_);
    return za * zb + zb * zc + zc * za - 1;
  }
  T Ja_, Jb_, Jc_;
};

}

namespace ising {
namespace tc {

template<typename T>
inline T triangular(T Ja, T Jb, T Jc) {
  if (Ja * Jb * Jc <= 0) throw(std::invalid_argument("Ja * Jb * Jc should be positive"));
  auto result = standards::newton_1d(func<T>(Ja, Jb, Jc), 1 / (2 * (Ja + Jb + Jc)));
  if (!result.second) throw(std::runtime_error("convergence error"));
  return 1 / result.first;
}

} // end namespace tc
} // end namespace ising


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../util/AlgorithmUtils.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <vector>

namespace fluid {
namespace algorithm {

class NMF
{

public:
  // pass iteration number; returns true if able to continue (i.e. not
  // cancelled)
  using ProgressCallback = std::function<bool(index)>;

  static void estimate(const RealMatrixView W, const RealMatrixView H,
                       index idx, RealMatrixView V)
  {
    using namespace Eigen;
    using namespace _impl;

    MatrixXd W1 = asEigen<Matrix>(W).transpose();
    MatrixXd H1 = asEigen<Matrix>(H).transpose();
    MatrixXd result = (W1.col(idx) * H1.row(idx)).transpose();
    V <<= asFluid(result);
  }

  // processFrame computes activations of a dictionary W in a given frame
  void processFrame(const RealVectorView x, const RealMatrixView W0,
                    RealVectorView out, index nIterations = 10,
                    RealVectorView v = RealVectorView(nullptr, 0, 0))
  {
    using namespace Eigen;
    using namespace _impl;
    index    rank = W0.extent(0);
    MatrixXd W = asEigen<Matrix>(W0).transpose();
    VectorXd h =
        MatrixXd::Random(rank, 1) * 0.5 + MatrixXd::Constant(rank, 1, 0.5);
    VectorXd v0 = asEigen<Matrix>(x);
    W = W.array().max(epsilon).matrix();
    h = h.array().max(epsilon).matrix();
    v0 = v0.array().max(epsilon).matrix();

    MatrixXd WT = W.transpose();
    W.colwise().normalize();
    VectorXd ones = VectorXd::Ones(x.extent(0));
    while (nIterations--)
    {
      ArrayXd  v1 = (W * h).array().max(epsilon);
      ArrayXXd hNum = (WT * (v0.array() / v1).matrix()).array();
      ArrayXXd hDen = (WT * ones).array();
      h = (h.array() * hNum / hDen.max(epsilon)).matrix();
      // VectorXd r = W * h;
      // double divergence = (v.cwiseProduct(v.cwiseQuotient(r)) - v + r).sum();
      // std::cout<<"Divergence "<<divergence<<std::endl;
    }
    out <<= asFluid(h);
    if (v.extent(0) > 0)
    {
      ArrayXd v2 = (W * h).array();
      v <<= asFluid(v2);
    }
  }

  void process(const RealMatrixView X, RealMatrixView W1, RealMatrixView H1,
               RealMatrixView V1, index rank, index nIterations, bool updateW,
               bool           updateH = false,
               RealMatrixView W0 = RealMatrixView(nullptr, 0, 0, 0),
               RealMatrixView H0 = RealMatrixView(nullptr, 0, 0, 0))
  {
    using namespace Eigen;
    using namespace _impl;
    index    nFrames = X.extent(0);
    index    nBins = X.extent(1);
    MatrixXd W;
    if (W0.extent(0) == 0 && W0.extent(1) == 0)
    {
      W = MatrixXd::Random(nBins, rank) * 0.5 +
          MatrixXd::Constant(nBins, rank, 0.5);
    }
    else
    {
      assert(W0.extent(0) == rank);
      assert(W0.extent(1) == nBins);
      W = asEigen<Matrix>(W0).transpose();
    }
    MatrixXd H;
    if (H0.extent(0) == 0 && H0.extent(1) == 0)
    {
      H = MatrixXd::Random(rank, nFrames) * 0.5 +
          MatrixXd::Constant(rank, nFrames, 0.5);
    }
    else
    {
      assert(H0.extent(0) == nFrames);
      assert(H0.extent(1) == rank);
      H = asEigen<Matrix>(H0).transpose();
    }
    MatrixXd V = asEigen<Matrix>(X).transpose();
    multiplicativeUpdates(V, W, H, nIterations, updateW, updateH);
    MatrixXd VT = V.transpose();
    MatrixXd WT = W.transpose();
    MatrixXd HT = H.transpose();

    V1 <<= asFluid(VT);
    W1 <<= asFluid(WT);
    H1 <<= asFluid(HT);
  }

  void addProgressCallback(ProgressCallback&& callback)
  {
    mCallbacks.emplace_back(std::move(callback));
  }

private:
  using MatrixXd = Eigen::MatrixXd;

  void multiplicativeUpdates(Eigen::Ref<MatrixXd> V, Eigen::Ref<MatrixXd> W,
                             Eigen::Ref<MatrixXd> H, index nIterations,
                             bool updateW, bool updateH)
  {
    using namespace Eigen;
    MatrixXd ones = MatrixXd::Ones(V.rows(), V.cols());
    H = H.array().max(epsilon).matrix();
    W = W.array().max(epsilon).matrix();
    W.colwise().normalize();
    H.rowwise().normalize();
    for (auto i = 0; i < nIterations; ++i)
    {
      if (updateW)
      {
        ArrayXXd V1 = (W * H).array().max(epsilon);
        ArrayXXd wnum = ((V.array() / V1).matrix() * H.transpose()).array();
        ArrayXXd wden = (ones * H.transpose()).array();
        W = (W.array() * wnum / wden.max(epsilon)).matrix();
        if (W.maxCoeff() > epsilon) W.colwise().normalize();
        assert(W.allFinite());
      }
      ArrayXXd V2 = (W * H).array().max(epsilon);
      if (updateH)
      {
        ArrayXXd hnum = (W.transpose() * (V.array() / V2).matrix()).array();
        ArrayXXd hden = (W.transpose() * ones).array();
        H = (H.array() * hnum / hden.max(epsilon)).matrix();
        assert(H.allFinite());
      }
      MatrixXd R = W * H;
      R = R.cwiseMax(epsilon);
      for (auto& cb : mCallbacks)
        if (!cb(i + 1)) return;
      // double divergence = (V.cwiseProduct(V.cwiseQuotient(R)) - V + R).sum();
      // divergenceCurve.push_back(divergence);
      // divergenceCurve(mIterations);
      // std::cout << "Divergence " << divergence << "\n";
    }
    V = W * H;
  }

  std::vector<ProgressCallback> mCallbacks;
};
} // namespace algorithm
} // namespace fluid


// Local private PANACEA includes
#include "matrix_eigen.hpp"

// Third party includes
#include <Eigen/Dense>

// Standard includes
#include <cassert>
#include <cmath>
#include <iostream>
#include <memory>

namespace panacea {

MatrixEigen::MatrixEigen() { matrix_ = std::make_unique<Eigen::MatrixXd>(); }

const MatrixType MatrixEigen::type() const { return MatrixType::Eigen; }

double MatrixEigen::getDeterminant() const {
  assert(matrix_->rows() > 0 || matrix_->cols() > 0);
  return matrix_->determinant();
}

bool MatrixEigen::isZero(const double threshold) const noexcept {
  assert(threshold > 0.0);
  // Because it is symmetric only need to check one half
  for (int i = 0; i < matrix_->rows(); ++i) {
    for (int j = i; j < matrix_->cols(); ++j) {
      if (std::fabs(matrix_->operator()(i, j)) > threshold)
        return false;
    }
  }
  return true;
}

void MatrixEigen::resize(const int rows, const int cols) {
  assert(rows >= 0);
  assert(cols >= 0);
  matrix_->resize(rows, cols);
}

MatrixEigen &MatrixEigen::operator=(const MatrixEigen &mat) {
  this->resize(mat.rows(), mat.cols());
  for (int row = 0; row < mat.rows(); ++row) {
    for (int col = 0; col < mat.cols(); ++col) {
      this->operator()(row, col) = mat(row, col);
    }
  }
  return *this;
}

MatrixEigen &MatrixEigen::operator=(const Matrix &mat) {
  this->resize(mat.rows(), mat.cols());
  for (int row = 0; row < mat.rows(); ++row) {
    for (int col = 0; col < mat.cols(); ++col) {
      this->operator()(row, col) = mat(row, col);
    }
  }
  return *this;
}

double &MatrixEigen::operator()(const int row, const int col) {
  assert(row >= 0);
  assert(col >= 0);
  assert(row < matrix_->rows());
  assert(col < matrix_->cols());
  return (*matrix_)(row, col);
}

double MatrixEigen::operator()(const int row, const int col) const {
  assert(row >= 0);
  assert(col >= 0);
  assert(row < matrix_->rows());
  assert(col < matrix_->cols());
  return (*matrix_)(row, col);
}

void MatrixEigen::makeIdentity() {
  for (int row = 0; row < matrix_->rows(); ++row) {
    this->operator()(row, row) = 1.0;
    for (int col = row + 1; col < matrix_->cols(); ++col) {
      this->operator()(row, col) = 0.0;
      this->operator()(col, row) = 0.0;
    }
  }
}

void MatrixEigen::setZero() { matrix_->setZero(); }

int MatrixEigen::rows() const { return matrix_->rows(); }

int MatrixEigen::cols() const { return matrix_->cols(); }

void MatrixEigen::print() const { std::cout << *matrix_ << std::endl; }

Eigen::MatrixXd MatrixEigen::pseudoInverse() const {
  return matrix_->completeOrthogonalDecomposition().pseudoInverse();
}

void pseudoInverse(Matrix &return_mat, const MatrixEigen &mat) {
  assert(return_mat.rows() == mat.rows());
  assert(return_mat.cols() == mat.cols());

  auto temp_mat = mat.pseudoInverse();
  for (int row = 0; row < temp_mat.rows(); ++row) {
    for (int col = 0; col < temp_mat.cols(); ++col) {
      return_mat(row, col) = temp_mat(row, col);
    }
  }
}
} // namespace panacea