/*
 * Copyright 2019 Denis Yaroshevskiy
 *
 * 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.
 */

#include "algo/nth_permutation.h"

#include <algorithm>
#include <cstdint>
#include <numeric>
#include <vector>

#include <boost/multiprecision/cpp_int.hpp>

#include "algo/factorial.h"

#include "test/catch.h"

namespace algo {
namespace {

using big_int = boost::multiprecision::cpp_int;

TEST_CASE("algorithm.nth_permutation.5ints", "[algorithm]") {
  std::vector<int> sorted(5);
  std::iota(sorted.begin(), sorted.end(), 0);

  std::vector<int> actual = sorted;

  for (std::int64_t i = 0;; ++i) {
    std::vector<int> expected(sorted.size());

    INFO("permutation number: " << i);
    nth_permutation(sorted.begin(), sorted.end(), expected.begin(), i);

    REQUIRE(expected == actual);
    if (!std::next_permutation(actual.begin(), actual.end())) break;
  }
}

TEST_CASE("algorithm.nth_permutation.special_cases", "[algorithm]") {
  {
    std::vector<int> v1, v2;
    nth_permutation(v1.begin(), v1.end(), v2.begin(), 0);
  }
  {
    std::vector<int> v1{1}, v2{1};
    nth_permutation(v1.begin(), v1.end(), v2.begin(), 0);
  }
  {
    static constexpr size_t size = 1000;
    std::vector<int> sorted(size), expected(size), actual(size);

    std::iota(sorted.begin(), sorted.end(), 0);
    std::reverse_copy(sorted.begin(), sorted.end(), expected.begin());

    big_int last_permuation_number =
        factorial<big_int>(static_cast<int>(size)) - 1;

    nth_permutation(sorted.begin(), sorted.end(), actual.begin(),
                    std::move(last_permuation_number));
    REQUIRE(expected == actual);
  }
}

}  // namespace
}  // namespace algo


/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

#ifndef NDEBUG
  #define NDEBUG
#endif

//#define VIENNACL_DEBUG_ALL
#include <iostream>
#include <fstream>
#include <stdexcept>
#include <vector>

#include "viennacl/linalg/prod.hpp"
#include "viennacl/linalg/qr-method.hpp"

#include <boost/numeric/ublas/matrix.hpp>

namespace ublas = boost::numeric::ublas;

typedef float ScalarType;

void initialize(viennacl::matrix<ScalarType>& A, std::vector<ScalarType>& v);
void vector_print(std::vector<ScalarType>& v );
void matrix_print(viennacl::matrix<ScalarType>& A_orig);

void qr_method()
{
    /*
     *                      Tutorial for the qr-method
     *
     * The eigenvalues and eigenvectors of a symmetric 9 by 9 matrix are calculated
     * by the QR-method.
     *
     */

    std::cout << "Testing matrix of size " << 9 << "-by-" << 9 << std::endl;

    viennacl::matrix<ScalarType> A_input(9,9);
    viennacl::matrix<ScalarType> Q(9, 9);
    std::vector<ScalarType> eigenvalues_ref(9);
    std::vector<ScalarType> eigenvalues(9);

    initialize(A_input, eigenvalues_ref);  //initialize with data for tutorial

    std::cout << std::endl <<"Input matrix: " << std::endl;
    matrix_print(A_input);

    std::cout << std::endl << "Starting QR-method" << std::endl;
    std::cout << "Calculation..." << std::endl;

    /*
     * Call function qr_method_sym to calculate eigenvalues and eigenvectors
     * Parameters:
     *       A_input     - input matrix to find eigenvalues and eigenvectors from
     *       Q           - matrix, where the calculated eigenvectors will be stored in
     *       eigenvalues - vector, where the calculated eigenvalues will be stored in
     */

    viennacl::linalg::qr_method_sym(A_input, Q, eigenvalues);

    std::cout << std::endl << "Eigenvalues:" << std::endl;
    vector_print(eigenvalues);
    std::cout << std::endl << "Reference eigenvalues:" << std::endl;
    vector_print(eigenvalues_ref);
    std::cout << std::endl << "Eigenvectors - each column is an eigenvector" << std::endl;

    matrix_print(Q);

}

int main()
{

  qr_method();

  std::cout << std::endl;
  std::cout << "------- Tutorial completed --------" << std::endl;
  std::cout << std::endl;

  return EXIT_SUCCESS;
}

//initialize vector and matrix
void initialize(viennacl::matrix<ScalarType>& A, std::vector<ScalarType>& v)
{
    ScalarType M[9][9] = {{4, 1, -2, 2, -7, 3, 9, -6, -2}, {1, -2, 0, 1, -1, 5, 4, 7, 3}, {-2, 0, 3, 2, 0, 3, 6, 1, -1},   {2, 1, 2, 1, 4, 5, 6, 7, 8},
               {-7, -1, 0, 4, 5, 4, 9, 1, -8},  {3, 5, 3, 5, 4, 9, -3, 3, 3}, {9, 4, 6, 6, 9, -3, 3, 6, -7},   {-6, 7, 1, 7, 1, 3, 6, 2, 6},
               {-2, 3, -1, 8, -8, 3, -7, 6, 1}};

    for(int i = 0; i < 9; i++)
        for(int j = 0; j < 9; j++)
            A(i, j) = M[i][j];

    ScalarType V[9] = {12.6005, 19.5905, 8.06067, 2.95074, 0.223506, 24.3642, -9.62084, -13.8374, -18.3319};

    for(int i = 0; i < 9; i++)
        v[i] = V[i];
}

void matrix_print(viennacl::matrix<ScalarType>& A_orig)
{
    ublas::matrix<ScalarType> A(A_orig.size1(), A_orig.size2());
    viennacl::copy(A_orig, A);
    for (unsigned int i = 0; i < A.size1(); i++) {
        for (unsigned int j = 0; j < A.size2(); j++)
           std::cout << A(i, j) << "\t";
        std::cout << std::endl;
    }
}

void vector_print(std::vector<ScalarType>& v )
{
  for (unsigned int i = 0; i < v.size(); i++)
      std::cout << std::setprecision(6) << std::fixed << v[i] << "\t";
    std::cout << std::endl;
}


/*
 * NOTE: this is a "power set" problem
 */
#include <fstream>
#include <iostream>
#include <iterator>
#include <numeric>
#include <string>
#include <vector>

#include <boost/range/counting_range.hpp>

template<typename T>
auto get_vector_from_file(std::fstream& file) {
	return std::vector<T>{
		std::istream_iterator<T>{file},
		std::istream_iterator<T>{}
	};
}

int main() {

	const auto filename = std::string{"containers.txt"};
	auto file = std::fstream{filename};

	if(file.is_open()) {

		const auto containers = get_vector_from_file<std::uint64_t>(file);

		const auto target_volume = std::uint64_t{150};

		auto count = std::uint64_t{};

		/*
		 * NOTE: the following algorithm was adapted
		 * from this stackoverflow.com answer:
		 * https://stackoverflow.com/a/19891145/699211
		 */

		/* compute power set */
		const auto size = containers.size();

		auto powerset = std::vector<std::vector<std::uint64_t>>(1 << size);

		powerset[0] = {};

		for(const auto i : boost::counting_range({}, size)) {

			const auto subsize = (1 << i); // doubling size of subset

			for(const auto j : boost::counting_range({}, subsize)) {

				const auto& source = powerset[j];

				const auto srcsize = source.size();

				auto& destination = powerset[subsize+j] = std::vector<std::uint64_t>(srcsize + 1);

				for(const auto k : boost::counting_range({}, srcsize)) {
					destination[k] = source[k];
				}

				destination[srcsize] = containers[i];

				const auto new_volume = std::accumulate(destination.begin(), destination.end(), std::uint64_t{});

				if(new_volume == target_volume) {
					++count;
				}
			}
		}

		std::cout << count << std::endl;

	} else {
		std::cerr << "Error! Could not open \"" << filename << "\"!" << std::endl;
	}

	return 0;
}


/*
 Copyright (C) 2017 Sascha Meiers
 Distributed under the MIT software license, see the accompanying
 file LICENSE.md or http://www.opensource.org/licenses/mit-license.php.
 */

#ifndef distribution_h
#define distribution_h

#include <iostream>
#include <vector>
#include <cassert>
#include "utils.hpp"
#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/negative_binomial.hpp>
#include <boost/math/distributions/binomial.hpp>

namespace hmm {

    static const double MIN_EMISSION_LOG = -1e4;
    // TODO: check for p=0 or 1 in all pdfs --> take special care for log then!

    /**
     *  NegativeBinomial
     *  ----------------
     *  p(k) = {\Gamma(n + k) \over \Gamma(k+1) \Gamma(n) } p^n (1-p)^k
     *  Number of failures k before n-th success.
     *  Mean :=  n * (1-p)/p
     *  Variance := Mean/p
     */
    class NegativeBinomial
    {
    public:
        typedef unsigned TEmission;
        static const uint8_t dim = 1;
        boost::math::negative_binomial nb;

        NegativeBinomial(double p, double n) : nb(n,p)
        {}

        inline double calc_log_emission(std::vector<unsigned>::const_iterator iter) const
        {
            double pr = log(calc_emission(iter));
            if (pr < MIN_EMISSION_LOG)
                pr = MIN_EMISSION_LOG;
            return pr;
        }

        inline double calc_emission(std::vector<unsigned>::const_iterator iter) const
        {
            return boost::math::pdf(nb, *iter);
        }
    };

    std::ostream& operator<<(std::ostream& os, const NegativeBinomial& obj)
    {
        os << "Negative Binomial Distribution:" << std::endl;
        os << "       p = " << obj.nb.success_fraction() << std::endl;
        os << "       r = " << obj.nb.successes() << std::endl;
        os << "    mean = " << mean(obj.nb) << std::endl;
        os << "     var = " << variance(obj.nb) << std::endl;
        return os;
    }

    /**
     *  MultiVariate
     *  ------------
     *  A quick wrapper to sum up multiple 1-dim distirbutions of the same type into
     *  one "multivariate" one, in which components are independent.
     */
    template <typename TDistribution>
    class MultiVariate
    {
    public:
        uint8_t dim;
        std::vector<TDistribution> inner;
        double log_prior;

        MultiVariate(std::vector<TDistribution> const & distributions, double _prior = 1) :
        inner(distributions), dim(distributions.size()), log_prior(log(_prior))
        {
            assert(distributions.size() > 0 && distributions.size() < 256);
            for (auto dist : distributions)
                assert(dist.dim == (uint8_t)1);
        }

        inline double calc_log_emission(typename std::vector<typename TDistribution::TEmission>::const_iterator iter) const
        {
            double logp = 0;
            for (unsigned i=0; i<dim; ++i, ++iter)
                logp += inner[i].calc_log_emission(iter);
            logp += log_prior;
            if (logp < MIN_EMISSION_LOG)
                logp = MIN_EMISSION_LOG;
            return logp;
        }

        inline double calc_emission(std::vector<unsigned>::const_iterator iter) const
        {
            return exp(calc_log_emission(iter));
        }
    };

    template <typename TDistribution>
    std::ostream& operator<<(std::ostream& os, const MultiVariate<TDistribution>& obj)
    {
        os << "Multivariate distribution with " << (int)obj.dim << " dimensions:" << std::endl;
        for (unsigned i=0; i<obj.inner.size(); ++i)
            os << ">> " <<  i << ") " <<  obj.inner[i];
        os << std::endl;
        return os;
    }

    /**
     *  CombinedNegBinAndBinomial
     *  ----------------
     *  Model total coverage (w+c) by Negative Binomial and fraction c/(w+c) by
     *  a binomial distribution.
     */
    class CombinedNegBinAndBinomial
    {
    public:
        typedef unsigned TEmission;
        static const uint8_t dim = 2;
        boost::math::negative_binomial nb;
        double ratio;
        double prior;

        CombinedNegBinAndBinomial(double nb_p, double nb_r, double strand_ratio, double prior = 1) :
        nb(nb_r,nb_p), ratio(strand_ratio), prior(prior)
        {}

        inline double calc_log_emission(std::vector<unsigned>::const_iterator iter) const
        {
            double pr = log(calc_emission(iter));
            if (pr < MIN_EMISSION_LOG)
                pr = MIN_EMISSION_LOG;
            return pr;
        }

        inline double calc_emission(std::vector<unsigned>::const_iterator iter) const
        {
            unsigned c = *iter++;
            unsigned w = *iter++;
            boost::math::binomial_distribution<> binomial(c+w, ratio);
            return prior * boost::math::pdf(nb, w+c) * boost::math::pdf(binomial, w);
        }
    };

}
#endif /* distribution_h */


#include "correlation.hpp"
#include <cmath>
#include <algorithm>
#include <iterator>
#include <stdexcept>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/predicate.hpp>
#include "global.hpp"
#include "lua.hpp"
#include <boost/numeric/ublas/io.hpp>

correlation_fn makeGaussianCorrelation( double corrlength )
{
	double scale = -1.0 / corrlength / corrlength;
	auto f = [scale](const gen_vect& v) -> double
	{
		using boost::numeric::ublas::inner_prod;
		return std::exp(inner_prod(v, v) * scale);
	};
	return f;
}

correlation_fn makeAnisotropicGaussianCorrelation( double corrlength, gen_vect ani )
{
	// scale anisotropy factor with global correlation length and precalculate the squares
	for( auto& v : ani )
		v = v*v/corrlength/corrlength;

	auto f = [ani](const gen_vect& v) -> double
	{
		using boost::numeric::ublas::inner_prod;
		double sum = 0;
		for( unsigned i = 0; i < v.size(); ++i )
			sum -= v[i] * v[i] * ani[i];

		return std::exp( sum );
	};
	return f;
}

correlation_fn makeSechCorrelation( double corrlength )
{
	double scale = 1.0 / corrlength;
	auto f = [scale](const gen_vect& v) -> double
	{
		using boost::numeric::ublas::inner_prod;
		double l = std::sqrt(inner_prod(v, v)) * scale;
		// sech(x) = 1 / cosh(x)
		return 1.0 / std::cosh(l);
	};
	return f;
}

correlation_fn makePowerCorrelation( double corrlength, double alpha )
{
	double scale = 1.0 / corrlength / corrlength;
	auto f = [scale, alpha](const gen_vect& v) -> double
	{
		using boost::numeric::ublas::inner_prod;
		double l = 1 + inner_prod(v, v) * scale;
		return std::pow(l, -alpha);
	};
	return f;
}

correlation_fn makeLuaCorrelation( double corrlength, std::string scriptfile, const std::vector<std::string>& vars )
{
	auto make_lua = [=]() {
		// create state, load and compile file
		lua_State* state = luaL_newstate();
		// open math library
		luaL_requiref(state, LUA_MATHLIBNAME, luaopen_math, 1);
		lua_pop(state, 1);
		// open file
		luaL_dofile(state, scriptfile.c_str());

		// set the variables
		for (unsigned i = 0; i < vars.size(); i += 2) {
			auto name = vars.at(i);
			auto value = boost::lexical_cast<double>(vars.at(i + 1));
			lua_pushnumber(state, value);
			lua_setglobal(state, name.c_str());
		}

		// check that correlation function is set
		lua_getglobal(state, "c");
		if (!lua_isfunction(state, -1))
		THROW_EXCEPTION(std::runtime_error, "lua script does not contain a function named c");
		lua_pop(state, 1);

		return state;
	};

	// now the correlation function
	auto f = [make_lua, corrlength](const gen_vect& v) -> double
	{
        // ensure that we have a one lua interpreter per thread
		thread_local lua_State* state = make_lua();
		// get the lua function reference
		/// \todo is it possible to cache this?
		lua_getglobal(state, "c");
		assert(lua_isfunction(state, -1));

		// push the scaled vector
		for(unsigned i = 0; i < v.size(); ++i)
			lua_pushnumber(state, v[i] / corrlength);

		// call the function and handle any errors
		if(lua_pcall(state, v.size(), 1, 0))
		{
			const char* error = lua_tostring(state, -1);
			THROW_EXCEPTION( std::runtime_error, error );
		}

		// get the result and clear the stack
		double result = lua_tonumber(state, -1);
		lua_pop(state, 1);
		return result;
	};
	return f;
}

correlation_fn makeTransformedCorrelation( correlation_fn original, trafo_matrix_t matrix )
{
	auto f = [original, matrix](const gen_vect& v) -> double
	{
		return original( boost::numeric::ublas::prod(matrix, v) );
	};
	return f;
}

// make correlation function without any trafo.
correlation_fn makeCorrelation( const std::vector<std::string>& specs, double length )
{
	assert( !specs.empty() );
	std::string corr_type = specs[0];
	if(corr_type == "gauss" || corr_type == "gaussian")
		if( specs.size() == 1 )	// no further parameters: use isotropic gaussian
			return makeGaussianCorrelation( length );
		else
		{
			gen_vect ani( specs.size() - 1);
			for(unsigned i = 1; i < specs.size(); ++i)
				ani[i-1] = boost::lexical_cast<double>( specs[i] );
			return makeAnisotropicGaussianCorrelation( length, ani );
		}
	else if(corr_type == "sech" )
		return makeSechCorrelation( length );
	else if(corr_type == "pow" || corr_type == "power")
		return makePowerCorrelation( length, boost::lexical_cast<double>(specs.at(1)));
	else if(corr_type == "lua")
	{
		if( specs.size() < 2 )
			THROW_EXCEPTION( std::runtime_error, "No script file specified for lua correlation" );

		// load specifications for variables that are set in lua
		std::vector<std::string> vars(specs.begin() + 2, specs.end());
		if( vars.size() % 2 != 0 )
			THROW_EXCEPTION( std::runtime_error, "invalid variables for lua script. Use \"lua filename var1 value1 var2 value2\"" );
		return makeLuaCorrelation( length, specs.at(1), vars);
	}
	else
		THROW_EXCEPTION( std::runtime_error, "correlation type %1% not valid", corr_type);

}

// convert a vector of strings to a ublas matrix
trafo_matrix_t matrix_from_string_vector(const std::vector<std::string>& source)
{
	std::size_t dim = 0;
	switch(source.size())
	{
	case 1:
		dim = 1;
		break;
	case 4:
		dim = 2;
		break;
	case 9:
		dim = 3;
		break;
	default:
		THROW_EXCEPTION(std::runtime_error, "transformation matrix is required to be square with dim <= 3. Got %1% elements.", source.size());
	}

	std::vector<double> converted;
	trafo_matrix_t matrix(dim, dim);
	std::transform( begin(source), end(source), std::back_inserter(converted), [](const std::string& s){ return boost::lexical_cast<double>(s); } );
	for(unsigned i = 0; i < dim; ++i)
	{
		for(unsigned j = 0; j < dim; ++j)
		{
			matrix(i, j) = converted[dim*i + j];
		}
	}
	return matrix;
}

correlation_fn makeCorrelation( const std::vector<std::string>& specs, double length, std::string trafo )
{
	// easy case: no trafo
	if(trafo.empty())
		return makeCorrelation(specs, length);

	// otherwise, start out with untrafo'ed
	// in case we get quotes from the command line, remove them.
	auto base = makeCorrelation(specs, length);
	if(boost::starts_with(trafo, "\""))
		trafo = trafo.substr(1);
	if(boost::ends_with(trafo, "\""))
		trafo = trafo.substr(0, trafo.size() - 1);

	// now split
	std::vector<std::string> split;
	boost::split(split, trafo, [](char c){ return std::isspace(c); });
	trafo_matrix_t matrix = matrix_from_string_vector(split);

	return makeTransformedCorrelation(base, matrix);
}


/*!
 * \file QBezierTriangle.hpp
 * \author Jun Yoshida
 * \copyright (c) 2019 Jun Yoshida.
 * The project is released under the MIT License.
 * \date Descember 6, 2019: created
 */

#pragma once

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

#include "../math/Bezier.hpp"
#include "PathFigure3D.hpp"

class QBezierTriangle : public PathFigure3D
{
private:
    std::array<Eigen::Vector3d,3> m_vert, m_edge;
    std::vector<Bezier<Eigen::Vector3d,3> > m_ridges;
    bool m_reliable;

public:
    QBezierTriangle(std::array<vertex_type,3> const &vert, std::array<vertex_type,3> const &edge)
        : m_vert{ Eigen::Vector3d(vert[0][0], vert[0][1], vert[0][2]),
                  Eigen::Vector3d(vert[1][0], vert[1][1], vert[1][2]),
                  Eigen::Vector3d(vert[2][0], vert[2][1], vert[2][2]) },
          m_edge{ Eigen::Vector3d(edge[0][0], edge[0][1], edge[0][2]),
                  Eigen::Vector3d(edge[1][0], edge[1][1], edge[1][2]),
                  Eigen::Vector3d(edge[2][0], edge[2][1], edge[2][2]) },
          m_ridges(),
          m_reliable(true)
    {}

    QBezierTriangle(std::array<double,3> const &v0, std::array<double,3> const &v1, std::array<double,3> const &v2, std::array<double,3> const &e0, std::array<double,3> const &e1, std::array<double,3> const &e2)
        : m_vert{ Eigen::Vector3d(v0[0], v0[1], v0[2]),
                  Eigen::Vector3d(v1[0], v1[1], v1[2]),
                  Eigen::Vector3d(v2[0], v2[1], v2[2]) },
          m_edge{ Eigen::Vector3d(e0[0], e0[1], e0[2]),
                  Eigen::Vector3d(e1[0], e1[1], e1[2]),
                  Eigen::Vector3d(e2[0], e2[1], e2[2]) },
          m_ridges(),
          m_reliable(true)
    {}
    virtual ~QBezierTriangle() = default;

    Eigen::Vector3d eval(double t0, double t1, double t2) {
        return t0*t0*m_vert[0]
            + t1*t1*m_vert[1]
            + t2*t2*m_vert[2]
            + 2.0*t0*t1*m_edge[2]
            + 2.0*t1*t2*m_edge[0]
            + 2.0*t2*t0*m_edge[1];
    }

    Eigen::Vector3d eval(double s1, double s2) {
        return eval(1.0-s1-s2, s1, s2);
    }

    /**!
     * \section Member functions derived from PathFigure
     **/
    virtual void draw(SchemeType &scheme) const override;

    /**!
     * \section Member functions derived from PathFigure3d
     **/
    virtual void updateProjector(Eigen::Matrix<double,2,3> const &proj) override;
};


/*
This example shows how to convert to/from transformation matrix (rotation matrix + translation vector).

Zivid primarily operate with a (4x4) transformation matrix. This example shows how to use Eigen to
convert to and from: AxisAngle, Rotation Vector, Roll-Pitch-Yaw, Quaternion

The convenience functions from this example can be reused in applicable applications. The YAML files for this sample can
be found under the main instructions for Zivid samples.
*/

#include <Zivid/Zivid.h>

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

#include <opencv2/core/core.hpp>
#include <opencv2/core/eigen.hpp>

#include <iomanip>
#include <iostream>

namespace
{
    enum class RotationConvention
    {
        zyxIntrinsic,
        xyzExtrinsic,
        xyzIntrinsic,
        zyxExtrinsic,
        nofROT
    };
    constexpr size_t nofRotationConventions = static_cast<size_t>(RotationConvention::nofROT);

    struct RollPitchYaw
    {
        RotationConvention convention;
        Eigen::Array3d rollPitchYaw;
    };

    std::string toString(RotationConvention convention)
    {
        switch(convention)
        {
            case RotationConvention::xyzIntrinsic: return "xyzIntrinsic";
            case RotationConvention::xyzExtrinsic: return "xyzExtrinsic";
            case RotationConvention::zyxIntrinsic: return "zyxIntrinsic";
            case RotationConvention::zyxExtrinsic: return "zyxExtrinsic";
            case RotationConvention::nofROT: break;
        }

        throw std::invalid_argument("Invalid RotationConvention");
    }

    // The following function converts Roll-Pitch-Yaw angles in radians to Rotation Matrix.
    // This function takes an array of roll, pitch and yaw angles, and a rotation convention, as input parameters.
    // For Roll-Pitch-Yaw we define that roll is a rotation about x-axis, pitch is a rotation about y-axis
    // and yaw is a rotation about z-axis.
    // Whether the axes are moving (intrinsic) or fixed (extrinsic) is defined by the rotation convention.
    // The array is ordered by Roll, Pitch and then Yaw.
    Eigen::Matrix3d rollPitchYawToRotationMatrix(const Eigen::Array3d &rollPitchYaw, const RotationConvention &rotation)
    {
        switch(rotation)
        {
            case RotationConvention::xyzIntrinsic:
            case RotationConvention::zyxExtrinsic:
                return (Eigen::AngleAxisd(rollPitchYaw[0], Eigen::Vector3d::UnitX())
                        * Eigen::AngleAxisd(rollPitchYaw[1], Eigen::Vector3d::UnitY())
                        * Eigen::AngleAxisd(rollPitchYaw[2], Eigen::Vector3d::UnitZ()))
                    .matrix();
            case RotationConvention::zyxIntrinsic:
            case RotationConvention::xyzExtrinsic:
                return (Eigen::AngleAxisd(rollPitchYaw[2], Eigen::Vector3d::UnitZ())
                        * Eigen::AngleAxisd(rollPitchYaw[1], Eigen::Vector3d::UnitY())
                        * Eigen::AngleAxisd(rollPitchYaw[0], Eigen::Vector3d::UnitX()))
                    .matrix();
            case RotationConvention::nofROT: break;
        }

        throw std::invalid_argument("Invalid orientation");
    }

    void rollPitchYawListToRotationMatrix(const std::vector<RollPitchYaw> &rpyList)
    {
        Eigen::IOFormat matrixFmt(4, 0, ", ", "\n", "[", "]", "[", "]");
        for(const auto &rotation : rpyList)
        {
            std::cout << "Rotation Matrix from Roll-Pitch-Yaw angles (" << toString(rotation.convention)
                      << "):" << std::endl;
            const auto rotationMatrixFromRollPitchYaw =
                rollPitchYawToRotationMatrix(rotation.rollPitchYaw, rotation.convention);
            std::cout << rotationMatrixFromRollPitchYaw.format(matrixFmt) << std::endl;
        }
    }

    // The following function converts Rotation Matrix to Roll-Pitch-Yaw angles in radians.
    // The rotation convention we use here is that Roll is a rotation about x-axis,
    // Pitch is a rotation about y-axis and Yaw is a rotation about z-axis.
    // Whether the axes are moving (intrinsic) or fixed (extrinsic) is defined by the rotation convention.
    // The array is ordered by Roll, Pitch and then Yaw.
    Eigen::Array3d rotationMatrixToRollPitchYaw(const Eigen::Matrix3d &rotationMatrix,
                                                const RotationConvention &convention)
    {
        switch(convention)
        {
            case RotationConvention::zyxExtrinsic:
            case RotationConvention::xyzIntrinsic: return rotationMatrix.eulerAngles(0, 1, 2);
            case RotationConvention::xyzExtrinsic:
            case RotationConvention::zyxIntrinsic: return rotationMatrix.eulerAngles(2, 1, 0).reverse();
            case RotationConvention::nofROT: break;
        }

        throw std::invalid_argument("Invalid rotation");
    }

    std::vector<RollPitchYaw> rotationMatrixToRollPitchYawList(const Eigen::Matrix3d &rotationMatrix)
    {
        const Eigen::IOFormat vectorFmt(4, 0, ", ", "", "", "", "[", "]");
        std::vector<RollPitchYaw> rpyList;
        for(size_t i = 0; i < nofRotationConventions; i++)
        {
            RotationConvention convention{ static_cast<RotationConvention>(i) };
            std::cout << "Roll-Pitch-Yaw angles (" << toString(convention) << "):" << std::endl;
            rpyList.push_back({ convention, rotationMatrixToRollPitchYaw(rotationMatrix, convention) });
            std::cout << rpyList[i].rollPitchYaw.format(vectorFmt) << std::endl;
        }
        return rpyList;
    }

    Eigen::MatrixXd cvToEigen(const cv::Mat &cvMat)
    {
        Eigen::MatrixXd eigenMat(cvMat.rows, cvMat.cols);

        cv::cv2eigen(cvMat, eigenMat);

        return eigenMat;
    }

    Eigen::Affine3d getTransformationMatrixFromYAML(const std::string &path)
    {
        cv::FileStorage fileStorageIn;
        if(!fileStorageIn.open(path, cv::FileStorage::Mode::READ))
        {
            throw std::runtime_error("Could not open " + path + ". Please run this sample from the build directory");
        }
        const auto poseStateNode = fileStorageIn["PoseState"];
        std::cout << "Getting PoseState:" << std::endl;
        if(poseStateNode.empty())
        {
            fileStorageIn.release();
            throw std::runtime_error("PoseState node not found in file");
        }
        auto transformationMatrix = Eigen::Affine3d(static_cast<Eigen::Matrix4d>(cvToEigen(poseStateNode.mat())));
        fileStorageIn.release();

        return transformationMatrix;
    }

    cv::Mat eigenToCv(const Eigen::MatrixXd &eigenMat)
    {
        // NOLINTNEXTLINE(hicpp-signed-bitwise)
        cv::Mat cvMat(static_cast<int>(eigenMat.rows()), static_cast<int>(eigenMat.cols()), CV_64FC1, cv::Scalar(0));

        cv::eigen2cv(eigenMat, cvMat);

        return cvMat;
    }

    void saveTransformationMatrixToYAML(const Eigen::Affine3d &transformationMatrix, const std::string &path)
    {
        // Save Transformation Matrix to .YAML file
        cv::FileStorage fileStorageOut;
        if(!fileStorageOut.open(path, cv::FileStorage::Mode::WRITE))
        {
            throw std::runtime_error("Could not open robotTransformOut.yaml for writing");
        }
        fileStorageOut.write("TransformationMatrixFromQuaternion", eigenToCv(transformationMatrix.matrix()));
        fileStorageOut.release();
    }

    Eigen::Vector3d rotationMatrixToRotationVector(const Eigen::Matrix3d &rotationMatrix)
    {
        const Eigen::AngleAxisd axisAngle(rotationMatrix);
        return axisAngle.angle() * axisAngle.axis();
    }

    Eigen::Matrix3d rotationVectorToRotationMatrix(const Eigen::Vector3d &rotationVector)
    {
        Eigen::AngleAxisd axisAngle(rotationVector.norm(), rotationVector.normalized());
        return axisAngle.toRotationMatrix();
    }

    void printHeader(const std::string &txt)
    {
        const std::string asterixLine = "****************************************************************";
        std::cout << asterixLine << "\n* " << txt << std::endl << asterixLine << std::endl;
    }
} // namespace

int main()
{
    try
    {
        Zivid::Application zivid;

        std::cout << std::setprecision(4);
        Eigen::IOFormat matrixFormatRules(4, 0, ", ", "\n", "[", "]", "[", "]");
        Eigen::IOFormat vectorFormatRules(4, 0, ", ", "", "", "", "[", "]");
        printHeader("This example shows conversions to/from Transformation Matrix");

        const auto transformationMatrix =
            getTransformationMatrixFromYAML(std::string(ZIVID_SAMPLE_DATA_DIR) + "/RobotTransform.yaml");
        std::cout << transformationMatrix.matrix().format(matrixFormatRules) << std::endl;

        // Extract Rotation Matrix and Translation Vector from Transformation Matrix
        std::cout << "RotationMatrix:\n" << transformationMatrix.linear().format(matrixFormatRules) << std::endl;
        std::cout << "TranslationVector:\n"
                  << transformationMatrix.translation().format(vectorFormatRules) << std::endl;

        /*
         * Convert from Rotation Matrix (Zivid) to other representations of orientation (Robot)
         */
        printHeader("Convert from Zivid (Rotation Matrix) to Robot");
        const Eigen::AngleAxisd axisAngle(transformationMatrix.linear());
        std::cout << "AxisAngle:\n"
                  << axisAngle.axis().format(vectorFormatRules) << ", " << axisAngle.angle() << std::endl;
        const auto rotationVector = rotationMatrixToRotationVector(transformationMatrix.linear());
        std::cout << "Rotation Vector:\n" << rotationVector.format(vectorFormatRules) << std::endl;
        const Eigen::Quaterniond quaternion(transformationMatrix.linear());
        std::cout << "Quaternion:\n" << quaternion.coeffs().format(vectorFormatRules) << std::endl;
        const auto rpyList = rotationMatrixToRollPitchYawList(transformationMatrix.linear());

        /*
         * Convert to Rotation Matrix (Zivid) from other representations of orientation (Robot)
         */
        printHeader("Convert from Robot to Zivid (Rotation Matrix)");
        const auto rotationMatrixFromAxisAngle = axisAngle.toRotationMatrix();
        std::cout << "Rotation Matrix from Axis Angle:\n"
                  << rotationMatrixFromAxisAngle.format(matrixFormatRules) << std::endl;
        const auto rotationMatrixFromRotationVector = rotationVectorToRotationMatrix(rotationVector);
        std::cout << "Rotation Matrix from Rotation Vector:\n"
                  << rotationMatrixFromRotationVector.format(matrixFormatRules) << std::endl;
        const auto rotationMatrixFromQuaternion = quaternion.toRotationMatrix();
        std::cout << "Rotation Matrix from Quaternion:\n"
                  << rotationMatrixFromQuaternion.format(matrixFormatRules) << std::endl;
        rollPitchYawListToRotationMatrix(rpyList);

        // Combine Rotation Matrix with Translation Vector to form Transformation Matrix
        Eigen::Affine3d transformationMatrixFromQuaternion(rotationMatrixFromQuaternion);
        transformationMatrixFromQuaternion.translation() = transformationMatrix.translation();
        saveTransformationMatrixToYAML(transformationMatrixFromQuaternion, "RobotTransformOut.yaml");
    }

    catch(const std::exception &e)
    {
        std::cerr << "Error: " << e.what() << std::endl;
        std::cout << "Press enter to exit." << std::endl;
        std::cin.get();
        return EXIT_FAILURE;
    }
}


/*
 * Maximum Likelihood estimator using Google's ceres-solver
 *
 */

#include <iomanip>
#include <fstream>
#include <iterator>
#include <ceres/ceres.h>
#include <glog/logging.h>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>

using namespace boost::accumulators;

// Log likelihood of normal distribution
double llfunc(double x, double mu, double sigma) {
    return -log(1 / (sigma * sqrt(2 * M_PI))) +
           (x - mu) * (x - mu) / (2 * sigma * sigma);
}

// Manual derivative with respect to mu
double llfunc_div_mu(double x, double mu, double sigma) {
    return (mu - x) / (sigma * sigma);
}

// Manual derivative with respect to sigma
double llfunc_div_sigma(double x, double mu, double sigma) {
    return -(mu * mu - 2 * mu * x - sigma * sigma + x * x) /
           (sigma * sigma * sigma);
}

/*
 * Class representing the log likelihood for a certain dataset
 * OpenMP is used to process parts of the dataset in parallel
 */
class LogLikelihood : public ceres::FirstOrderFunction {
public:
    LogLikelihood(std::vector<double> input) : data(input) {}

    virtual ~LogLikelihood() {}

    virtual bool Evaluate(const double* parameters, double* cost,
                          double* gradient) const {
        const double mu = parameters[0];
        const double sigma = parameters[1];

        if (mu < -10 || mu > 10 || sigma <= 0) {
            return false;
        }

        // Calculate cost
        double c = 0;
        #pragma omp parallel for reduction(+ : c)
        for (size_t i = 0; i < data.size(); i++) {
            c += llfunc(data[i], mu, sigma);
        }
        cost[0] = c;

        // Calculate gradient
        if (gradient != NULL) {
            double g0 = 0;
            double g1 = 0;
            #pragma omp parallel for reduction(+ : g0, g1)
            for (size_t i = 0; i < data.size(); i++) {
                g0 += llfunc_div_mu(data[i], mu, sigma);
                g1 += llfunc_div_sigma(data[i], mu, sigma);
            }
            gradient[0] = g0;
            gradient[1] = g1;
        }
        return true;
    }

    virtual int NumParameters() const { return 2; }

private:
    std::vector<double> data;
};

int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);

    double parameters[2] = {8, 8.0};

    // Generate data
    std::random_device rd;
    std::mt19937 gen(rd());
    std::normal_distribution<> dis(0, 1);
    std::vector<double> input;
    std::cout << "Generating data..." << std::endl;
    for (int i = 0; i < 100000; i++) {
        input.push_back(dis(gen));
    }

    // Perform fit
    std::cout << "Running fit..." << std::endl;
    ceres::GradientProblemSolver::Options options;
    options.minimizer_progress_to_stdout = true;
    options.max_num_iterations = 300;
    options.function_tolerance = 1e-15;

    ceres::GradientProblemSolver::Summary summary;
    ceres::GradientProblem problem(new LogLikelihood(input));
    ceres::Solve(options, problem, parameters, &summary);

    //std::cout << summary.FullReport() << std::endl;

    std::cout << std::setprecision(16)
              << "Final    mu: " << parameters[0] << " sigma: " << parameters[1]
              << std::endl;

    // Compare with analytic estimators (mean and std)
    accumulator_set<double, features<tag::mean, tag::variance>> acc;
    for_each(input.begin(), input.end(), [&acc](double x) { acc(x); });

    std::cout << std::setprecision(16)
              << "Analytic mu: " << mean(acc) << " sigma: " << sqrt(variance(acc))
              << std::endl;

    std::ofstream data("./data.txt");
    std::ostream_iterator<double> output_iterator(data, "\n");
    std::copy(input.begin(), input.end(), output_iterator);

    std::ofstream params("./params.txt");
    params << parameters[0] << std::endl;
    params << parameters[1] << std::endl;

    return 0;
}


#include <vector>
#include "boost/random.hpp"
#include "boost/generator_iterator.hpp"
#include <boost/random/normal_distribution.hpp>
#include <algorithm>

typedef boost::mt19937 RNGType; ///< mersenne twister generator

int main() {
    RNGType rng;
    boost::normal_distribution<> rdist(1.0,0.5); /**< normal distribution
                           with mean of 1.0 and standard deviation of 0.5 */

    boost::variate_generator< RNGType, boost::normal_distribution<> >
                    get_rand(rng, rdist);

    std::vector<double> v(1000);
    generate(v.begin(),v.end(),get_rand);
    return 0;
}


#pragma once

#include <iostream>

#include <Eigen/Dense>

namespace dg {

using Real = double;

using Matrix2 = Eigen::Matrix<Real,2,2>;
using Matrix3 = Eigen::Matrix<Real,3,3>;
using Matrix4 = Eigen::Matrix<Real,4,4>;

using Vector2 = Eigen::Matrix<Real,2,1>;
using Vector3 = Eigen::Matrix<Real,3,1>;
using Vector4 = Eigen::Matrix<Real,4,1>;

using Matrix2f = Eigen::Matrix<float,2,2>;
using Matrix3f = Eigen::Matrix<float,3,3>;
using Matrix4f = Eigen::Matrix<float,4,4>;

using Vector2f = Eigen::Matrix<float,2,1>;
using Vector3f = Eigen::Matrix<float,3,1>;
using Vector4f = Eigen::Matrix<float,4,1>;

using Quaternion = Eigen::Quaternion<Real>;
using AngleAxis  = Eigen::AngleAxis<Real>;

using Quaternionf = Eigen::Quaternion<float>;
using AngleAxisf  = Eigen::AngleAxis<float>;

using Transform  = Matrix4;
using Transformf = Matrix4f;

namespace math {

inline Transform transformFromScaleRotTrans(const Vector3& scale, const Quaternion& q, const Vector3& t)
{
    // Ordering:
    //    1. Scale
    //    2. Rotate
    //    3. Translate

    Transform m;

    Matrix3 rot3x3 = q.toRotationMatrix();

    m(0,0) = scale.x() * rot3x3(0,0); m(0,1) = scale.y() * rot3x3(0,1); m(0,2) = scale.z() * rot3x3(0,2); m(0,3) = t.x();
    m(1,0) = scale.x() * rot3x3(1,0); m(1,1) = scale.y() * rot3x3(1,1); m(1,2) = scale.z() * rot3x3(1,2); m(1,3) = t.y();
    m(2,0) = scale.x() * rot3x3(2,0); m(2,1) = scale.y() * rot3x3(2,1); m(2,2) = scale.z() * rot3x3(2,2); m(2,3) = t.z();
    m(3,0) = 0; m(3,1) = 0; m(3,2) = 0; m(3,3) = 1;

    return m;
}

} }


#include "Parameterization.h"

#include <unordered_set>

#include <Eigen/Eigen>

#include <iostream>

namespace Chaf
{
	struct pair_hash {
		template <class T1, class T2>
		std::size_t operator () (const std::pair<T1, T2>& p) const {
			auto h1 = std::hash<T1>{}(p.first);
			auto h2 = std::hash<T2>{}(p.second);
			return h1 ^ h2;
		}
	};

	Ref<TriMesh> Parameterization::parameterize(const Ref<TriMesh>& mesh, ParameterizationMethod method)
	{
		std::vector<Vertex> raw_vertices = mesh->m_Vertices;
		std::vector<uint32_t> raw_indices = mesh->m_Indices;

		std::vector<Vertex> vertices;
		std::vector<uint32_t> indices;

		//optimize mesh
		std::unordered_map<glm::vec3, size_t> vertices_map;
		for (auto& v : raw_vertices)
		{
			vertices_map[v.m_Position] = 0;
		}

		for (auto& [key, index] : vertices_map)
		{
			index = vertices.size();
			Vertex v = {};
			v.m_Position = key;
			vertices.push_back(v);
		}

		for (auto& idx : raw_indices)
		{
			indices.push_back(vertices_map[raw_vertices[idx].m_Position]);
		}

		std::vector<std::unordered_set<size_t>> neigborhood(vertices.size());
		std::unordered_map<std::pair<size_t, size_t>, size_t, pair_hash> triangle_map;
		for (size_t i = 0; i < indices.size(); i += 3)
		{
			neigborhood[indices[i]].insert(indices[i + 1]);
			neigborhood[indices[i + 1]].insert(indices[i + 2]);
			neigborhood[indices[i + 2]].insert(indices[i]);
			triangle_map[std::make_pair(indices[i], indices[i + 1])] = i;
			triangle_map[std::make_pair(indices[i + 1], indices[i + 2])] = i;
			triangle_map[std::make_pair(indices[i + 2], indices[i])] = i;
		}

		auto& boundaries = findBoundary(vertices, indices);
		if (boundaries.empty())
		{
			return mesh;
		}
		auto& boundary = boundaries[0];

		Eigen::SparseMatrix<float> Laplace_matrix(vertices.size(), vertices.size());
		Eigen::MatrixXf b(vertices.size(), 2);

		Laplace_matrix.setZero();
		b.setZero();

		std::vector<Eigen::Triplet<float>> Lij;

		for (size_t i = 0; i < vertices.size(); i++)
		{
			Lij.push_back(Eigen::Triplet<float>(i, i, 1.f));

			if (std::find(boundary.begin(), boundary.end(), i) == boundary.end())
			{
				std::unordered_map<size_t, float> wi;
				for (auto& idx : neigborhood[i])
				{
					if (method == ParameterizationMethod::Uniform)
					{
						wi[idx] = 1.f;
					}
					else
					{
						size_t t1 = triangle_map[std::make_pair(i, idx)];
						size_t t2 = triangle_map[std::make_pair(idx, i)];

						glm::vec3 v = vertices[i].m_Position;
						glm::vec3 vi = vertices[idx].m_Position;

						glm::vec3 vi1, vi2;

						for (size_t k = 0; k < 3; k++)
						{
							if (vertices[indices[t1 + k]].m_Position != v && vertices[indices[t1 + k]].m_Position != vi)
							{
								vi1 = vertices[indices[t1 + k]].m_Position;
							}
							if (vertices[indices[t2 + k]].m_Position != v && vertices[indices[t2 + k]].m_Position != vi)
							{
								vi2 = vertices[indices[t2 + k]].m_Position;
							}
						}

						if (method == ParameterizationMethod::WP)
						{
							float r = glm::length(v - vi);
							float cos1 = glm::dot(v - vi, vi1 - vi);
							float cos2 = glm::dot(v - vi, vi2 - vi);

							float cot1 = cos1 / (sqrtf(1 - cos1 * cos1));
							float cot2 = cos2 / (sqrtf(1 - cos2 * cos2));

							wi[idx] = (cot1 + cot2) / (r * r);
						}
						else if (method == ParameterizationMethod::MV)
						{
							float r = glm::length(v - vi);
							float cos1 = glm::dot(v - vi, v - vi1);
							float cos2 = glm::dot(v - vi, v - vi2);
							float alpha1 = acosf(cos1);
							float alpha2 = acosf(cos2);

							wi[idx] = (tanf(alpha1 / 2.f) + tanf(alpha2 / 2.f)) / r;
						}
						else if (method == ParameterizationMethod::DH)
						{
							float cos1 = glm::dot(vi1 - v, vi1 - vi);
							float cos2 = glm::dot(vi2 - v, vi2 - vi);
							float alpha1 = acosf(cos1);
							float alpha2 = acosf(cos2);

							wi[idx] = (1.f / tanf(alpha1) + tanf(1.f / alpha2));
						}
					}
				}
				float wi_sum = 0.f;
				for (auto& [idx, w] : wi)
				{
					wi_sum += w;
				}
				for (auto& [idx, w] : wi)
				{
					Lij.push_back(Eigen::Triplet<float>(i, idx, -w / wi_sum));
				}
			}
		}

		for (uint32_t i = 0; i < boundary.size(); i++)
		{
			b(boundary[i], 0) = std::cos((float)i * glm::pi<float>() * 2.f / static_cast<float>(boundary.size()));
			b(boundary[i], 1) = std::sin((float)i * glm::pi<float>() * 2.f / static_cast<float>(boundary.size()));
		}

		Laplace_matrix.setFromTriplets(Lij.begin(), Lij.end());

		Eigen::SparseLU<Eigen::SparseMatrix<float>> solver;

		solver.compute(Laplace_matrix);

		Eigen::MatrixXf result = solver.solve(b);

		std::vector<Vertex> new_vertices(vertices.size());

		for (size_t i = 0; i < vertices.size(); i++)
		{
			new_vertices[i].m_Position.x = result(i, 0);
			new_vertices[i].m_Position.y = result(i, 1);
			new_vertices[i].m_Position.z = 0.f;
		}

		return CreateRef<TriMesh>(std::move(new_vertices), std::move(indices));
	}

	std::vector<std::vector<size_t>> Parameterization::findBoundary(const std::vector<Vertex>& vertices, const std::vector<uint32_t>& indices)
	{
		std::vector<std::vector<size_t>> mesh_graph(vertices.size(), std::vector<size_t>(vertices.size()));
		for (auto& g : mesh_graph)
		{
			std::fill(g.begin(), g.end(), 0);
		}

		for (size_t i = 0; i < indices.size(); i += 3)
		{
			mesh_graph[indices[i]][indices[i + 1]]++;
			mesh_graph[indices[i + 1]][indices[i]] ++;
			mesh_graph[indices[i + 1]][indices[i + 2]] ++;
			mesh_graph[indices[i + 2]][indices[i + 1]] ++;
			mesh_graph[indices[i + 2]][indices[i]]++;
			mesh_graph[indices[i]][indices[i + 2]] ++;
		}

		std::unordered_set<size_t> found;
		std::vector<std::vector<size_t>> boundaries;

		for (size_t i = 0; i < vertices.size(); i++)
		{
			size_t currect_vertex = i;
			std::vector<size_t> boundary;
			while (true)
			{
				bool has = false;
				for (size_t j = 0; j < vertices.size(); j++)
				{
					if (j != currect_vertex && mesh_graph[currect_vertex][j] == 1 && found.find(j) == found.end())
					{
						found.insert(j);
						boundary.push_back(j);
						currect_vertex = j;
						has = true;
					}
				}
				if (currect_vertex == i || !has)
				{
					break;
				}
			}
			if (!boundary.empty())
			{
				boundaries.push_back(boundary);
			}
		}

		std::sort(boundaries.begin(), boundaries.end(), [](const std::vector<size_t>& lhs, const std::vector<size_t>& rhs) {return lhs.size() > rhs.size(); });
		return boundaries;
	}
}


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Vincent Rouvreau
 *
 *    Copyright (C) 2018 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#include <gudhi/graph_simplicial_complex.h>
#include <gudhi/distance_functions.h>
#include <gudhi/Simplex_tree.h>
#include <gudhi/Points_off_io.h>

#include <gudhi/Miniball.hpp>

#include <boost/program_options.hpp>

#include <string>
#include <vector>
#include <limits>   // infinity
#include <utility>  // for pair
#include <map>

// ----------------------------------------------------------------------------
// rips_persistence_step_by_step is an example of each step that is required to
// build a Rips over a Simplex_tree. Please refer to rips_persistence to see
// how to do the same thing with the Rips_complex wrapper for less detailed
// steps.
// ----------------------------------------------------------------------------

// Types definition
using Simplex_tree = Gudhi::Simplex_tree<>;
using Simplex_handle = Simplex_tree::Simplex_handle;
using Filtration_value = Simplex_tree::Filtration_value;
using Point = std::vector<double>;
using Points_off_reader = Gudhi::Points_off_reader<Point>;
using Proximity_graph = Gudhi::Proximity_graph<Simplex_tree>;

class Cech_blocker {
 private:
  using Point_cloud = std::vector<Point>;
  using Point_iterator = Point_cloud::const_iterator;
  using Coordinate_iterator = Point::const_iterator;
  using Min_sphere = Gudhi::Miniball::Miniball<Gudhi::Miniball::CoordAccessor<Point_iterator, Coordinate_iterator>>;

 public:
  bool operator()(Simplex_handle sh) {
    std::vector<Point> points;
    for (auto vertex : simplex_tree_.simplex_vertex_range(sh)) {
      points.push_back(point_cloud_[vertex]);
#ifdef DEBUG_TRACES
      std::clog << "#(" << vertex << ")#";
#endif  // DEBUG_TRACES
    }
    Filtration_value radius = Gudhi::Minimal_enclosing_ball_radius()(points);
#ifdef DEBUG_TRACES
    std::clog << "radius = " << radius << " - " << (radius > max_radius_) << std::endl;
#endif  // DEBUG_TRACES
    simplex_tree_.assign_filtration(sh, radius);
    return (radius > max_radius_);
  }
  Cech_blocker(Simplex_tree& simplex_tree, Filtration_value max_radius, const std::vector<Point>& point_cloud)
      : simplex_tree_(simplex_tree), max_radius_(max_radius), point_cloud_(point_cloud) {
    dimension_ = point_cloud_[0].size();
  }

 private:
  Simplex_tree simplex_tree_;
  Filtration_value max_radius_;
  std::vector<Point> point_cloud_;
  int dimension_;
};

void program_options(int argc, char* argv[], std::string& off_file_points, Filtration_value& max_radius, int& dim_max);

int main(int argc, char* argv[]) {
  std::string off_file_points;
  Filtration_value max_radius;
  int dim_max;

  program_options(argc, argv, off_file_points, max_radius, dim_max);

  // Extract the points from the file filepoints
  Points_off_reader off_reader(off_file_points);

  // Compute the proximity graph of the points
  Proximity_graph prox_graph = Gudhi::compute_proximity_graph<Simplex_tree>(off_reader.get_point_cloud(), max_radius,
                                                                            Gudhi::Minimal_enclosing_ball_radius());

  // Construct the Rips complex in a Simplex Tree
  Simplex_tree st;
  // insert the proximity graph in the simplex tree
  st.insert_graph(prox_graph);
  // expand the graph until dimension dim_max
  st.expansion_with_blockers(dim_max, Cech_blocker(st, max_radius, off_reader.get_point_cloud()));

  std::clog << "The complex contains " << st.num_simplices() << " simplices \n";
  std::clog << "   and has dimension " << st.dimension() << " \n";

  // Sort the simplices in the order of the filtration
  st.initialize_filtration();

#if DEBUG_TRACES
  std::clog << "********************************************************************\n";
  std::clog << "* The complex contains " << st.num_simplices() << " simplices - dimension=" << st.dimension() << "\n";
  std::clog << "* Iterator on Simplices in the filtration, with [filtration value]:\n";
  for (auto f_simplex : st.filtration_simplex_range()) {
    std::clog << "   "
              << "[" << st.filtration(f_simplex) << "] ";
    for (auto vertex : st.simplex_vertex_range(f_simplex)) {
      std::clog << static_cast<int>(vertex) << " ";
    }
    std::clog << std::endl;
  }
#endif  // DEBUG_TRACES

  return 0;
}

void program_options(int argc, char* argv[], std::string& off_file_points, Filtration_value& max_radius, int& dim_max) {
  namespace po = boost::program_options;
  po::options_description hidden("Hidden options");
  hidden.add_options()("input-file", po::value<std::string>(&off_file_points),
                       "Name of an OFF file containing a point set.\n");

  po::options_description visible("Allowed options", 100);
  visible.add_options()("help,h", "produce help message")(
      "max-radius,r",
      po::value<Filtration_value>(&max_radius)->default_value(std::numeric_limits<Filtration_value>::infinity()),
      "Maximal length of an edge for the Rips complex construction.")(
      "cpx-dimension,d", po::value<int>(&dim_max)->default_value(1),
      "Maximal dimension of the Rips complex we want to compute.");

  po::positional_options_description pos;
  pos.add("input-file", 1);

  po::options_description all;
  all.add(visible).add(hidden);

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).options(all).positional(pos).run(), vm);
  po::notify(vm);

  if (vm.count("help") || !vm.count("input-file")) {
    std::clog << std::endl;
    std::clog << "Construct a Cech complex defined on a set of input points.\n \n";

    std::clog << "Usage: " << argv[0] << " [options] input-file" << std::endl << std::endl;
    std::clog << visible << std::endl;
    exit(-1);
  }
}


//#define DEBUG 1

#include <iostream>
#include <iomanip>
//#include <cmath>
#include "Hubbard2D.hpp"
#ifdef USE_SPECTRA
#include <MatOp/SparseGenMatProd.h>
#include <SymEigsSolver.h>
#endif

#include <boost/random.hpp>
#include <boost/limits.hpp>
#include <ietl/interface/eigen3.h>
#include <ietl/vectorspace.h>
#include <ietl/lanczos.h>

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

  std::ofstream outfile;
  outfile.open("dope_energies.dat");
  outfile << std::setprecision(9);
  int Lx=4,Ly=4; //dimensions of lattice
  for(int ne=1;ne<6;ne++){//number of electrons
    for(int U=1;U<=8;U++){
      //int ne=2; //# of electrons
      //int U=4;
      HubbardModel2D H(ne,ne,1,U);
      std::cout << "2D "<<Lx<<"x"<<Ly<<" Hubbard model with "
      << ne <<" up/down electrons and U/t="<<U<<std::endl;
      H.makeBasis();
      std::cout <<"Made Basis: " << H.getBasis()->size()<< std::endl;

      H.buildHubbard2D();
      std::cout <<"Built Matrix" << std::endl;
      SpMat Hmat = *(H.getH());
      std::cout << "# of non-zero elements:"<<(H.getH())->nonZeros() << std::endl;
      H.getH()->makeCompressed();

      //Exact Diag with specrta
#ifdef USE_SPECTRA
      // Construct matrix operation object using the wrapper class SparseGenMatProd
      using wrapper_t =Spectra::SparseGenMatProd<double,Eigen::ColMajor,long>;
      wrapper_t op(Hmat);

      // Construct eigen solver object, requesting the largest three eigenvalues
      Spectra::SymEigsSolver< double, Spectra::SMALLEST_ALGE , wrapper_t > eigs(&op, 1, 6);
      // Initialize and compute
      eigs.init();
      int nconv = eigs.compute();

#ifdef DEBUG
      if(eigs.info() != Spectra::SUCCESSFUL)
          std::cout <<"Warning something failed " <<nconv << std::endl;
#endif

      std::cout<< "E0="<<eigs.eigenvalues()[0]<<std::endl;
#else
      typedef boost::lagged_fibonacci607 Gen;
      Gen mygen;
      mygen.seed(0);

      typedef Eigen::SparseMatrix<double,Eigen::RowMajor,long> Matrix;
      typedef Eigen::VectorXd Vector;
      typedef ietl::vectorspace<Vector> Vecspace;

      // Creation of an iteration object:
      int max_iter = 10*H.getBasis()->size();
      double rel_tol = 500*std::numeric_limits<double>::epsilon();
      double abs_tol = std::pow(std::numeric_limits<double>::epsilon(),2./3);
      int n_lowest_eigenval = 1;
      std::vector<double> eigen;
      std::vector<double> err;
      std::vector<int> multiplicity;

      //std::vector<double> groundStates;

      Vecspace vec(Hmat.cols());
      ietl::lanczos<Matrix,Vecspace> lanczos(Hmat,vec);
      ietl::lanczos_iteration_nlowest<double>
      iter(max_iter, n_lowest_eigenval, rel_tol, abs_tol);
      std::cout << "lanczos" << std::endl;
      try{
        lanczos.calculate_eigenvalues(iter,mygen);
        eigen = lanczos.eigenvalues();
        err = lanczos.errors();
        multiplicity = lanczos.multiplicities();
        std::cout<<"number of iterations: "<<iter.iterations()<<"\n";
        //groundStates.push_back(eigen[0]);
        outfile<<ne <<" " <<U<<" " <<eigen[0]<<std::endl;
      }
      catch (std::runtime_error& e) {
        std::cout << e.what() << "\n";
      }
      std::cout << "#        eigenvalue            error         multiplicity\n";
      for (int i=0;i<10;++i)
        std::cout << i << "\t" << eigen[i] << "\t" << err[i] << "\t"
        << multiplicity[i] << "\n";

      /*
      // call of eigenvectors function follows:
      std::cout << "\nEigen vectors computations for the lowest eigenvalue:\n\n";
      std::vector<double>::iterator start = eigen.begin();
      std::vector<double>::iterator end = eigen.begin()+1;
      std::vector<Vector> eigenvectors; // for storing the eigen vectors.
      ietl::Info<double> info; // (m1, m2, ma, eigenvalue, residualm, status).

      try {
        lanczos.eigenvectors(start,end,std::back_inserter(eigenvectors),info,mygen,max_iter);
      }
      catch (std::runtime_error& e) {
        std::cout << e.what() << "\n";
      }

      for(int i=0;i<10;i++){
        std::cout << eigenvectors[0](i)<<std::endl;
      }
      std::cout << " Information about the eigenvector computations:\n\n";
      for(int i = 0; i < info.size(); i++) {
        std::cout << " m1(" << i+1 << "): " << info.m1(i) << ", m2(" << i+1 << "): "
        << info.m2(i) << ", ma(" << i+1 << "): " << info.ma(i) << " eigenvalue("
        << i+1 << "): " << info.eigenvalue(i) << " residual(" << i+1 << "): "
        << info.residual(i) << " error_info(" << i+1 << "): "
        << info.error_info(i) << "\n\n";
      }
      */
#endif
    }//U
  }//ne
  outfile.close();
  return 0;
}


#include <igl/directed_edge_orientations.h>
#include <igl/directed_edge_parents.h>
#include <igl/forward_kinematics.h>
#include <igl/PI.h>
#include <igl/lbs_matrix.h>
#include <igl/deform_skeleton.h>
#include <igl/dqs.h>
#include <igl/readDMAT.h>
#include <igl/readOBJ.h>
#include <igl/readTGF.h>
#include <igl/opengl/glfw/Viewer.h>

#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include <vector>
#include <algorithm>
#include <iostream>

#include "tutorial_shared_path.h"

typedef
  std::vector<Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> >
  RotationList;

const Eigen::RowVector3d sea_green(70./255.,252./255.,167./255.);
Eigen::MatrixXd V,W,C,U,M;
Eigen::MatrixXi F,BE;
Eigen::VectorXi P;
std::vector<RotationList > poses;
double anim_t = 0.0;
double anim_t_dir = 0.015;
bool use_dqs = false;
bool recompute = true;

bool pre_draw(igl::opengl::glfw::Viewer & viewer)
{
  using namespace Eigen;
  using namespace std;
  if(recompute)
  {
    // Find pose interval
    const int begin = (int)floor(anim_t)%poses.size();
    const int end = (int)(floor(anim_t)+1)%poses.size();
    const double t = anim_t - floor(anim_t);

    // Interpolate pose and identity
    RotationList anim_pose(poses[begin].size());
    for(int e = 0;e<poses[begin].size();e++)
    {
      anim_pose[e] = poses[begin][e].slerp(t,poses[end][e]);
    }
    // Propagate relative rotations via FK to retrieve absolute transformations
    RotationList vQ;
    vector<Vector3d> vT;
    igl::forward_kinematics(C,BE,P,anim_pose,vQ,vT);
    const int dim = C.cols();
    MatrixXd T(BE.rows()*(dim+1),dim);
    for(int e = 0;e<BE.rows();e++)
    {
      Affine3d a = Affine3d::Identity();
      a.translate(vT[e]);
      a.rotate(vQ[e]);
      T.block(e*(dim+1),0,dim+1,dim) =
        a.matrix().transpose().block(0,0,dim+1,dim);
    }
    // Compute deformation via LBS as matrix multiplication
    if(use_dqs)
    {
      igl::dqs(V,W,vQ,vT,U);
    }else
    {
      U = M*T;
    }

    // Also deform skeleton edges
    MatrixXd CT;
    MatrixXi BET;
    igl::deform_skeleton(C,BE,T,CT,BET);

    viewer.data().set_vertices(U);
    viewer.data().set_edges(CT,BET,sea_green);
    viewer.data().compute_normals();
    if(viewer.core().is_animating)
    {
      anim_t += anim_t_dir;
    }
    else
    {
      recompute=false;
    }
  }
  return false;
}

bool key_down(igl::opengl::glfw::Viewer &viewer, unsigned char key, int mods)
{
  recompute = true;
  switch(key)
  {
    case 'D':
    case 'd':
      use_dqs = !use_dqs;
      return true;
    case ' ':
      viewer.core().is_animating = !viewer.core().is_animating;
      return true;
  }
  return false;
}

int main(int argc, char *argv[])
{
  using namespace Eigen;
  using namespace std;
  igl::readOBJ(TUTORIAL_SHARED_PATH "/arm.obj",V,F);
  U=V;
  igl::readTGF(TUTORIAL_SHARED_PATH "/arm.tgf",C,BE);
  // retrieve parents for forward kinematics
  igl::directed_edge_parents(BE,P);
  RotationList rest_pose;
  igl::directed_edge_orientations(C,BE,rest_pose);
  poses.resize(4,RotationList(4,Quaterniond::Identity()));
  // poses[1] // twist
  const Quaterniond twist(AngleAxisd(igl::PI,Vector3d(1,0,0)));
  poses[1][2] = rest_pose[2]*twist*rest_pose[2].conjugate();
  const Quaterniond bend(AngleAxisd(-igl::PI*0.7,Vector3d(0,0,1)));
  poses[3][2] = rest_pose[2]*bend*rest_pose[2].conjugate();

  igl::readDMAT(TUTORIAL_SHARED_PATH "/arm-weights.dmat",W);
  igl::lbs_matrix(V,W,M);

  // Plot the mesh with pseudocolors
  igl::opengl::glfw::Viewer viewer;
  viewer.data().set_mesh(U, F);
  viewer.data().set_edges(C,BE,sea_green);
  viewer.data().show_lines = false;
  viewer.data().show_overlay_depth = false;
  viewer.data().line_width = 1;
  viewer.core().trackball_angle.normalize();
  viewer.callback_pre_draw = &pre_draw;
  viewer.callback_key_down = &key_down;
  viewer.core().is_animating = false;
  viewer.core().camera_zoom = 2.5;
  viewer.core().animation_max_fps = 30.;
  cout<<"Press [d] to toggle between LBS and DQS"<<endl<<
    "Press [space] to toggle animation"<<endl;
  viewer.launch();
}


//
// Created by Ujjwal Chadha on 11/2/19.
//

#include "Mesh.h"
#include <iostream>
#include <fstream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <vector>
#include "Utils.h"
#include <unordered_map>

using namespace std;
using namespace Eigen;

Mesh::Mesh() {

}

Mesh::Mesh(const MatrixXf& vertices, const MatrixXf& faces):
        Mesh(vertices, faces, Eigen::Vector3f(0.0, 1.1, 2.2), WIREFRAME) {
}

Mesh::Mesh(const MatrixXf &vertices, const MatrixXf &faces, const Vector3f& color, const RenderType& renderType) {
    this->vertices = vertices;
    this->faces = faces;
    this->model = MatrixXf(4, 4);
    this->model <<  1,    0.,   0.,   0.,
            0.,   1,    0.,   0.,
            0.,   0.,   1,    0.,
            0.,   0.,   0.,   1.;

    this->color = color;
    this->renderType = renderType;

    this->triangleVertices = calculateTriangleVertices(faces, vertices);
    this->faceNormals = calculateFaceNormals(faces, vertices, this->triangleVertices);
    this->vertexNormals = calculateVertexNormals(faces, vertices, this->triangleVertices, this->faceNormals);
}

Mesh Mesh::fromOffFile(const string &filePath) {
    return fromOffFile(filePath, Eigen::Vector3f(0.0, 1.1, 2.2), WIREFRAME);
}

Mesh Mesh::fromOffFile(const string& filepath, const Vector3f& color, const RenderType& renderType) {
    ifstream inputFile;
    inputFile.open(filepath);

    if (!inputFile) {
        throw std::runtime_error("Error opening data file: " + filepath);
    } else {
        string line;
        getline(inputFile, line);
        getline(inputFile, line);
        vector<string> meta = Utils::splitString(line, " ");
        int numVertices = stoi(meta.at(0));
        int numFaces = stoi(meta.at(1));

        MatrixXf vertices = MatrixXf::Zero(3, numVertices);
        for (int i = 0; i < numVertices; i++) {
            getline(inputFile, line);
            vector<string> vertexString = Utils::splitString(line, " ");

            vertices(0, i) = (stof(vertexString[0]));
            vertices(1, i) = (stof(vertexString[1]));
            vertices(2, i) = stof(vertexString[2]);
        }

        MatrixXf faces = MatrixXf::Zero(3, numFaces);
        for (int i = 0; i < numFaces; i++) {
            getline(inputFile, line);
            vector<string> faceString = Utils::splitString(line, " ");

            faces(0, i) = stoi(faceString[1]);
            faces(1, i) = stoi(faceString[2]);
            faces(2, i) = stoi(faceString[3]);
        }
        inputFile.close();

        Vector3f baryCenter = calculateBarycenter(faces, vertices);
        for (long i = 0; i < vertices.cols(); i++) {
            vertices.col(i) << vertices.col(i) - baryCenter;
        }
        return Mesh(vertices, faces, color, renderType);
    }
}

Vector3f Mesh::calculateBarycenter(const MatrixXf &faces, const MatrixXf &vertices) {
    Vector3f centroid(0., 0., 0.);
    for (long faceNumber = 0; faceNumber < faces.cols(); faceNumber++) {
        const Vector3f& a = vertices.col(faces(0, faceNumber));
        const Vector3f& b = vertices.col(faces(1, faceNumber));
        const Vector3f& c = vertices.col(faces(2, faceNumber));

        const Vector3f& center = (a + b + c) / 3;
        centroid = centroid + center;
    }
    return centroid / faces.cols();
}

MatrixXf Mesh::calculateTriangleVertices(const MatrixXf &faces, const MatrixXf &vertices) {
    MatrixXf triangleVertices = MatrixXf::Zero(3, faces.cols() * faces.rows());
    for (long faceNumber = 0; faceNumber < faces.cols(); faceNumber++) {
        for (long faceVertexNumber = 0; faceVertexNumber < faces.rows(); faceVertexNumber++) {
            int vertexNumber = faces(faceVertexNumber, faceNumber);
            triangleVertices.col((faces.rows()*faceNumber) + faceVertexNumber) << vertices.col(vertexNumber);
        }
    }
    return triangleVertices;
}

MatrixXf Mesh::calculateFaceNormals(const MatrixXf& faces, const MatrixXf& vertices, MatrixXf triangleVertices) {
    MatrixXf normals = MatrixXf::Zero(3, triangleVertices.cols());
    for (long i = 0; i < triangleVertices.cols(); i += 3) {
        Vector3f a = triangleVertices.col(i);
        Vector3f b = triangleVertices.col(i + 1);
        Vector3f c = triangleVertices.col(i + 2);

        Vector3f normal = ((b - a).cross(c - a)).normalized();
        normals.col(i) << normal;
        normals.col(i + 1) << normal;
        normals.col(i + 2) << normal;
    }
    return normals;
}

MatrixXf Mesh::calculateVertexNormals(const MatrixXf& faces, const MatrixXf& vertices, const MatrixXf& triangleVertices,
        const MatrixXf& faceNormals) {

    unordered_map<int, vector<int>> vertexNumberToFaceNumbersMap;
    for (long faceNumber = 0; faceNumber < faces.cols(); faceNumber++) {
        for (long faceVertexNumber = 0; faceVertexNumber < faces.rows(); faceVertexNumber++) {
            int vertexNumber = faces(faceVertexNumber, faceNumber);
            if (vertexNumberToFaceNumbersMap.find(vertexNumber) == vertexNumberToFaceNumbersMap.end()) {
                vertexNumberToFaceNumbersMap[vertexNumber] = std::vector<int>();
            }
            vertexNumberToFaceNumbersMap[vertexNumber].push_back(faceNumber);
        }
    }

    MatrixXf normals(3, triangleVertices.cols());
    for (long faceNumber = 0; faceNumber < faces.cols(); faceNumber++) {
        for (long faceVertexNumber = 0; faceVertexNumber < faces.rows(); faceVertexNumber++) {
            int vertexNumber = faces(faceVertexNumber, faceNumber);
            vector<int> facesAdjascentToVertex =  vertexNumberToFaceNumbersMap[vertexNumber];
            Vector3f vertexNormal(0.0, 0.0, 0.0);
            for (int face: facesAdjascentToVertex) {
                Vector3f faceNormal = faceNormals.col(face*3);
                vertexNormal += faceNormal;
            }
            normals.col((3*faceNumber) + faceVertexNumber) = vertexNormal.normalized();
        }
    }
    return normals;
}

void Mesh::scaleToUnitCube() {
    float xmax = -999999, xmin = 99999;
    float ymax = -999999, ymin = 99999;
    float zmax = -999999, zmin = 99999;
    for (long i = 0; i < getVertices().cols(); i++) {
        Vector3f vertex = getVertices().col(i);
        if (vertex(0) < xmin) {
            xmin = vertex(0);
        }
        if (vertex(0) > xmax) {
            xmax = vertex(0);
        }
        if (vertex(1) < ymin) {
            ymin = vertex(1);
        }
        if (vertex(1) > ymax) {
            ymax = vertex(1);
        }
        if (vertex(2) < zmin) {
            zmin = vertex(2);
        }
        if (vertex(2) > zmax) {
            zmax = vertex(2);
        }
    }
    scale(1 / max(max(xmax-xmin, ymax-ymin), zmax-zmin));
}

RenderType Mesh::getRenderType() {
    return renderType;
}

void Mesh::setRenderType(const RenderType& renderType) {
    this->renderType = renderType;
}

Vector3f Mesh::getColor() {
    return this->color;
}

void Mesh::setColor(const Vector3f &color) {
    this->color = color;
}

void Mesh::translate(const Vector3f& translateBy) {
    this->model << Utils::generateTranslationMatrix(translateBy) * this->model;
}

void Mesh::scale(float factor) {
    this->model << Utils::generateScaleAboutPointMatrix(getTranslation(), factor) * this->model;
}

void Mesh::rotate(int axis, float radians) {
    this->model << Utils::generateRotateAboutPointMatrix(axis, radians, getTranslation()) * this->model;
}

Vector3f Mesh::getTranslation() {
    auto res = Vector3f(this->model.block(0, 3, 3, 1));
    return res;
}

MatrixXf Mesh::getVertices() {
    return this->vertices;
}

MatrixXf Mesh::getFaces() {
    return this->faces;
}

MatrixXf Mesh::getModel() {
    return this->model;
}

MatrixXf Mesh::getTriangleVertices() {
    return this->triangleVertices;
}

MatrixXf Mesh::getFaceNormals() {
    return this->faceNormals;
}

MatrixXf Mesh::getVertexNormals() {
    return this->vertexNormals;
}

Mesh::~Mesh() {

}


#include <iostream>
#include <algorithm>
#include <iterator>
#include <cmath>
#include <fstream>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/math/constants/constants.hpp>

#include "miMaS/field.h"
#include "miMaS/weno.h"
#include "miMaS/fft.h"
#include "miMaS/array_view.h"
#include "miMaS/poisson.h"

namespace math = boost::math::constants;

int main(int,char**)
{
  const int Nx = 9;
  double l=2.*math::pi<double>();
  double dx = l/Nx, dt = 0.5*dx;
  ublas::vector<double> v(Nx),kx(Nx);
  double vk=-0.5;

	for ( auto i=0 ; i<Nx/2+1 ; ++i )   { kx[i] = 2.*math::pi<double>()*i/l; }
	for ( auto i=0 ; i<((Nx/2)) ; ++i ) { kx[i+Nx/2+1] = -kx[Nx/2-i]; }

  for ( auto i=0 ; i<Nx ; ++i ) {
    v[i] = std::cos((double)i*dx);
  }

  std::ofstream f0("init.dat");
  for ( auto i=0 ; i<Nx ; ++i ) { f0 << i*dx << " " << v[i] << "\n"; }
  f0 << std::endl;
  f0.close();

  fft::spectrum s(Nx);

std::cout << std::endl;
  int Nb_iter = 100;
  for ( auto t=0 ; t<Nb_iter ; ++t ) {
    std::cout << t*dt << "----\n";
    s.fft(&v[0]);
    for ( auto i=0 ; i<Nx ; ++i ) {
      std::cout << kx[i] << " " << s[i][fft::re] << " " << s[i][fft::im] <<  "\n";
    }
    std::cout << "\n----\n" ;

    for ( auto i=0 ; i<Nx ; ++i ) {
      double re = s[i][fft::re], im = s[i][fft::im];
      s[i][fft::re] = std::cos(kx[i]*dt*vk)*re + std::sin(kx[i]*dt*vk)*im;
      s[i][fft::im] = std::cos(kx[i]*dt*vk)*im - std::sin(kx[i]*dt*vk)*re;
    }
    s.ifft(&v[0]);
  }
std::cout << std::endl;

  std::ofstream f1("vp.dat");
  for ( auto i=0 ; i<Nx ; ++i ) {
    f1 << i*dx << " " << v[i] << " " << std::cos(i*dx-vk*(Nb_iter)*dt) << "\n";
  }
  f1 << std::endl;
  f1.close();

  return 0;
}


/**
 * @file Math.cpp
 * @author Giulio Romualdi
 * @copyright Released under the terms of the MIT License.
 * @date 2021
 */

#include <utility>

#include <Eigen/Cholesky>
#include <Eigen/Dense>

#include <ScsEigen/Logger.h>
#include <ScsEigen/Math.h>

std::pair<bool, Eigen::MatrixXd>
ScsEigen::choleskyDecomposition(const Eigen::Ref<const Eigen::MatrixXd>& A)
{
    if (A.rows() != A.cols())
    {
        log()->error("[ScsEigen::choleskyDecomposition] A is not square.");
        assert(false);
        return std::pair(false, Eigen::MatrixXd());
    }

    // In most case the cholesky decomposition should be fine.
    // if eigen is not able to find a feasible solution we try to use a robust cholesky
    // decomposition
    Eigen::LLT<Eigen::MatrixXd> lltA(A);
    if (lltA.info() == Eigen::Success)
    {
        return std::pair(true, lltA.matrixU());
    } else
    {
        Eigen::LDLT<Eigen::MatrixXd> ldltA(A);
        if (ldltA.info() == Eigen::Success)
        {
            return std::pair(true, ldltA.matrixU());
        }
    }

    log()->error("[ScsEigen::choleskyDecomposition] Unable to compute the Cholesky decomposition.");
    return std::pair(false, Eigen::MatrixXd());
}


#ifndef __PROBABILITY_DISTRIBUTIONS__DISCRETE_IMPL_HPP__
#define __PROBABILITY_DISTRIBUTIONS__DISCRETE_IMPL_HPP__

#include "discrete.hpp"

#include "const_slice.hpp"
#include "slice.hpp"

#include <boost/random/discrete_distribution.hpp>
#include <cmath>

namespace ProbabilityDistributions {
  template <unsigned int K, class D, class W, class T>
  Discrete<K,D,W,T>::Discrete():
    p_(K, 1./K) { }

  template <unsigned int K, class D, class W, class T>
  Discrete<K,D,W,T>::Discrete(std::vector<T> const& p):
    p_(p) {
      assert(p.size() == K);
    }

  template <unsigned int K, class D, class W, class T>
  template <class RNG>
  void Discrete<K,D,W,T>::sample(MA::Array<D>& samples, size_t n_samples,
      RNG& rng) const {
    MA::Size::SizeType size(2);
    size[0] = n_samples;
    size[1] = K;
    samples.resize(size);

    boost::random::discrete_distribution<unsigned int, T> dist(p_.begin(),
        p_.end());

    MA::Slice<T> slice(samples, 0);
    for (size_t j = 0; j < slice.total_left_size(); j++) {
      MA::Array<T> s = slice.get_element(j);

      unsigned int val = dist(rng);
      for (unsigned int i = 0; i < K; i++) {
        if (i == val)
          s(i) = 1;
        else
          s(i) = 0;
      }
    }
  }

  template <unsigned int K, class D, class W, class T>
  T Discrete<K,D,W,T>::log_likelihood(MA::ConstArray<D> const& data,
          MA::ConstArray<W> const& weight) const {
    check_data_and_weight(data, weight);

    T ll = 0;
    std::vector<T> log_p(p_);
    for (unsigned int i = 0; i < K; i++)
      log_p[i] = log(log_p[i]);

    MA::ConstSlice<T> slice(data, 0);
    for (size_t j = 0; j < slice.total_left_size(); j++) {
      MA::ConstArray<T> const& sample = slice.get_element(j);
      T w = weight(j);
      for (unsigned int i = 0; i < K; i++) {
        T s = sample(i);
        if (s != 0)
          ll += w * s * log_p[i];
      }
    }

    return ll;
  }

  template <unsigned int K, class D, class W, class T>
  void Discrete<K,D,W,T>::MLE(MA::ConstArray<D> const& data,
      MA::ConstArray<W> const& weight, std::vector<size_t> const& indexes) {
    check_data_and_weight(data, weight);

    for (unsigned int i = 0; i < K; i++)
      p_[i] = 0;

    MA::ConstSlice<T> slice(data, 0);
    for (size_t j = 0; j < slice.total_left_size(); j++) {
      MA::ConstArray<T> const& sample = slice.get_element(j);
      T w = weight(j);
      for (unsigned int i = 0; i < K; i++)
        p_[i] += w * sample(i);
    }

    normalize();
  }

  template <unsigned int K, class D, class W, class T>
  void Discrete<K,D,W,T>::sample_to_index(MA::Array<unsigned int>& indexes,
          MA::ConstArray<D> const& samples) const {
    assert(samples.size().size() == 2);
    assert(samples.size()[0] > 0);
    assert(samples.size()[1] == K);

    MA::Size::SizeType size(1);
    size[0] = samples.size()[0];
    indexes.resize(size);

    MA::ConstSlice<T> samples_slice(samples, 0);
    for (size_t j = 0; j < samples_slice.total_left_size(); j++) {
      MA::ConstArray<T> const& s = samples_slice.get_element(j);
      size_t i;
      for (i = 0; i < K; i++)
        if (s(i) == 1) {
          indexes(j) = i;
          break;
        }
      assert(i < K);
    }
  }

  template <unsigned int K, class D, class W, class T>
  void Discrete<K,D,W,T>::index_to_sample(MA::Array<D>& samples,
          MA::ConstArray<unsigned int> const& indexes) const {
    assert(indexes.size().size() == 1);
    assert(indexes.size()[0] > 0);

    MA::Size::SizeType size = indexes.size();
    size.push_back(K);
    samples.resize(size);

    MA::Slice<T> samples_slice(samples, 0);
    for (size_t j = 0; j < samples_slice.total_left_size(); j++) {
      MA::Array<T>& s = samples_slice.get_element(j);
      unsigned int index = indexes(j);
      assert(index < K);
      for (size_t i = 0; i < K; i++) {
        if (i == index)
          s(i) = 1;
        else
          s(i) = 0;
      }
    }
  }

  template <unsigned int K, class D, class W, class T>
  void Discrete<K,D,W,T>::check_data_and_weight(MA::ConstArray<D> const& data,
          MA::ConstArray<W> const& weight) const {
    assert(data.size().size() == 2);
    assert(data.size()[0] > 0);
    assert(data.size()[1] == K);
    assert(weight.size().size() == 1);
    assert(weight.size()[0] == data.size()[0]);
  }

  template <unsigned int K, class D, class W, class T>
  void Discrete<K,D,W,T>::normalize() {
    T sum = 0;
    for (unsigned int i = 0; i < K; i++)
      sum += p_[i];

    for (unsigned int i = 0; i < K; i++)
      p_[i] /= sum;
  }

  template <unsigned int K, class D, class W, class T>
  void Discrete<K,D,W,T>::anneal(T temp) {
    for (unsigned int i = 0; i < K; i++)
      p_[i] = std::exp(std::log(p_[i])/temp);
    normalize();
  }
};

#endif


#include <Eigen/Dense>

#include "NumpySaver.hpp"
#include "Timer.hpp"
#include "fft.hpp"
#include "image_fft.hpp"
#include "progressbar.hpp"

using namespace Eigen;
std::tuple<double, double> measure_fft_time(Index input_size) {
  VectorXd vec = VectorXd::Random(input_size);
  VectorXcd dest(input_size);

  return Timer::measure_time(
      [&]() { fft(vec.data(), dest.data(), input_size); }, .01);
}

int main(int argc, char const* argv[]) {
#ifndef NDEBUG
  FFT::tests();
  // image_test();
  compress_test();
#endif

  VectorXd input_sizes_lin = VectorXd::LinSpaced(1030 - 500 + 1, 500, 1030);
  VectorXd input_sizes_p2(14);
  // dont judge me
  input_sizes_p2 << 2, 4, 8, 16, 32, 64, 128, 256, 512, 1024, 2048, 4096, 8192,
      16384;

  progressbar bar(input_sizes_lin.size() + input_sizes_p2.size());

  VectorXd mean_lin(input_sizes_lin.size()), std_lin(input_sizes_lin.size());
  for (Index k = 0; k < input_sizes_lin.size(); k++) {
    bar.update();
    auto [mean, std] = measure_fft_time(input_sizes_lin[k] + 1e-5);
    mean_lin[k] = mean;
    std_lin[k] = std;
  }

  VectorXd mean_p2(input_sizes_p2.size()), std_p2(input_sizes_p2.size());
  for (Index k = 0; k < input_sizes_p2.size(); k++) {
    bar.update();
    auto [mean, std] = measure_fft_time(input_sizes_p2[k]);
    mean_p2[k] = mean;
    std_p2[k] = std;
  }

  NumpySaver("build/output/times_lin.npy")
      << input_sizes_lin << mean_lin << std_lin;
  NumpySaver("build/output/times_p2.npy")
      << input_sizes_p2 << mean_p2 << std_p2;

  VectorXd lost(6);
  VectorXcd lost_fft(lost.size());
  VectorXd lost_dct(lost.size());
  lost << 4, 8, 15, 16, 23, 42;
  fft(lost.data(), lost_fft.data(), lost.size());
  dct(lost.data(), lost_dct.data(), lost.size());
  NumpySaver("build/output/lost.npy")
      << lost << lost_fft.real() << lost_fft.imag() << lost_dct;

  VectorXd plot(1000);
  VectorXcd plot_fft(plot.size());
  for (Index k = 0; k < plot.size() / 3; k++) {
    plot[k] = 0;
  }
  for (Index k = plot.size() / 3; k < plot.size() / 3 * 2; k++) {
    plot[k] = 1;
  }
  for (Index k = plot.size() / 3 * 2; k < plot.size(); k++) {
    plot[k] = 0;
  }
  fft(plot.data(), plot_fft.data(), plot.size());
  NumpySaver("build/output/plot.npy")
      << plot << plot_fft.real() << plot_fft.imag();

  compress_image("images/A.png", "build/output/A9.ldw", 0.9);
  compress_image("images/A.png", "build/output/A5.ldw", 0.5);
  compress_image("images/A.png", "build/output/A2.ldw", 0.2);
  compress_image("images/A.png", "build/output/A1.ldw", 0.1);
  decompress_image("build/output/A9.ldw", "build/plots/A9.png");
  decompress_image("build/output/A5.ldw", "build/plots/A5.png");
  decompress_image("build/output/A2.ldw", "build/plots/A2.png");
  decompress_image("build/output/A1.ldw", "build/plots/A1.png");

  blur("images/dune.png", "build/output/dune_blur1.png",
       "build/output/dune_blur1_mask.png", .1);
  blur("images/dune.png", "build/output/dune_blur2.png",
       "build/output/dune_blur2_mask.png", .2);
  blur("images/dune.png", "build/output/dune_blur4.png",
       "build/output/dune_blur4_mask.png", .4);
  blur("images/dune.png", "build/output/dune_blur3.png",
       "build/output/dune_blur3_mask.png", .3);

  blur_smooth("images/dune.png", "build/output/dune_blur_smooth.png",
              "build/output/dune_blur_smooth_mask.png", .001);

  rect_filter("images/dune.png", "build/output/dune_blur_rect.png",
              "build/output/dune_rect_mask.png", .1, .1);

  sharpen("images/blackhole.png", "build/output/blackhole_sharp9.png",
          "build/output/blackhole_sharp9_mask.png", .09);
  sharpen("images/blackhole.png", "build/output/blackhole_sharp6.png",
          "build/output/blackhole_sharp6_mask.png", .06);
  sharpen("images/blackhole.png", "build/output/blackhole_sharp3.png",
          "build/output/blackhole_sharp3_mask.png", .03);
  sharpen("images/blackhole.png", "build/output/blackhole_sharp1.png",
          "build/output/blackhole_sharp1_mask.png", .01);

  sharpen_smooth("images/blackhole.png",
                 "build/output/blackhole_sharp_smooth_g.png",
                 "build/output/blackhole_sharp_smooth_mask_g.png", .01, true);
  sharpen_smooth("images/blackhole.png",
                 "build/output/blackhole_sharp_smooth.png",
                 "build/output/blackhole_sharp_smooth_mask.png", .01, false);

  // compress_image("images/dune.png", "build/output/dune8.ldw", 0.9);
  // compress_image("images/dune.png", "build/output/dune5.ldw", 0.5);
  // compress_image("images/dune.png", "build/output/dune1.ldw", 0.1);
  // decompress_image("build/output/dune5.ldw", "build/plots/dune9.png");
  // decompress_image("build/output/dune5.ldw", "build/plots/dune5.png");
  // decompress_image("build/output/dune1.ldw", "build/plots/dune1.png");

  //   sharpen("images/blackhole.png", "build/plots/blackhole001_bw.png",
  //           "build/plots/blackhole001_bw_mask.png", .001, true);
  //   sharpen("images/blackhole.png", "build/plots/blackhole05_bw.png",
  //           "build/plots/blackhole01_bw_mask.png", .01, true);
  //   sharpen("images/blackhole.png", "build/plots/blackhole01_bw.png",
  //           "build/plots/blackhole01_bw_mask.png", .01, true);
  //   sharpen("images/blackhole.png", "build/plots/blackhole0_bw.png",
  //           "build/plots/blackhole0_bw_mask.png", .0, true);

  //   sharpen("images/dune.png", "build/plots/dune_bw_sharp.png",
  //           "build/plots/dune_bw_sharp_mask.png", .005, true);
  //   blur("images/dune.png", "build/plots/dune_bw_blur.png",
  //        "build/plots/dune_bw_blur_mask.png", .95, true);
  //   sharpen("images/dune.png", "build/plots/dune_bw.png",
  //           "build/plots/dune_bw_mask.png", 0, true);

  //   rect_filter("images/dune.png", "build/plots/dune_bw_rect.png",
  //               "build/plots/dune_bw_rect_mask.png", .5, .5, true);
  //   anti_rect_filter("images/dune.png", "build/plots/dune_bw_arect.png",
  //                    "build/plots/dune_bw_arect_mask.png", .05, .05, true);

  //   blur("images/A.png", "build/plots/A_blur95.png",
  //        "build/plots/A_blur95_mask.png", .95);
  //   blur("images/A.png", "build/plots/A_blur9.png",
  //        "build/plots/A_blur9_mask.png", .9);
  //   blur("images/A.png", "build/plots/A_blur8.png",
  //        "build/plots/A_blur8_mask.png", .8);
  //   blur("images/A.png", "build/plots/A_blur7.png",
  //        "build/plots/A_blur7_mask.png", .7);
  //   blur("images/A.png", "build/plots/A_blur6.png",
  //        "build/plots/A_blur6_mask.png", .6);
  //   blur("images/A.png", "build/plots/A_blur5.png",
  //        "build/plots/A_blur5_mask.png", .5);
  //   blur("images/A.png", "build/plots/A_blur4.png",
  //        "build/plots/A_blur4_mask.png", .4);
  //   blur("images/A.png", "build/plots/A_blur3.png",
  //        "build/plots/A_blur3_mask.png", .3);
  //   blur("images/A.png", "build/plots/A_blur2.png",
  //        "build/plots/A_blur2_mask.png", .2);
  //   blur("images/A.png", "build/plots/A_blur1.png",
  //        "build/plots/A_blur1_mask.png", .1);

  sharpen("images/A.png", "build/output/A_sharpen95.png",
          "build/output/A_sharpen95_mask.png", .95);
  //   sharpen("images/A.png", "build/plots/A_sharpen9.png",
  //           "build/plots/A_sharpen9_mask.png", .9);
  //   sharpen("images/A.png", "build/plots/A_sharpen8.png",
  //           "build/plots/A_sharpen8_mask.png", .8);
  //   sharpen("images/A.png", "build/plots/A_sharpen7.png",
  //           "build/plots/A_sharpen7_mask.png", .7);
  //   sharpen("images/A.png", "build/plots/A_sharpen6.png",
  //           "build/plots/A_sharpen6_mask.png", .6);
  //   sharpen("images/A.png", "build/plots/A_sharpen5.png",
  //           "build/plots/A_sharpen5_mask.png", .5);
  //   sharpen("images/A.png", "build/plots/A_sharpen4.png",
  //           "build/plots/A_sharpen4_mask.png", .4);
  //   sharpen("images/A.png", "build/plots/A_sharpen3.png",
  //           "build/plots/A_sharpen3_mask.png", .3);
  //   sharpen("images/A.png", "build/plots/A_sharpen2.png",
  //           "build/plots/A_sharpen2_mask.png", .2);
  sharpen("images/A.png", "build/output/A_sharpen1.png",
          "build/output/A_sharpen1_mask.png", .1);

  sharpen_smooth("images/A.png", "build/output/A_sharp_smooth.png",
                 "build/output/A_sharp_smooth_mask.png", .001, false);

  //   rect_filter("images/A.png", "build/plots/A_blur_rect95.png",
  //               "build/plots/A_blur_rect95_mask.png", .95, .95);
  //   rect_filter("images/A.png", "build/plots/A_blur_rect9.png",
  //               "build/plots/A_blur_rect9_mask.png", .9, .9);
  //   rect_filter("images/A.png", "build/plots/A_blur_rect8.png",
  //               "build/plots/A_blur_rect8_mask.png", .8, .8);
  //   rect_filter("images/A.png", "build/plots/A_blur_rect7.png",
  //               "build/plots/A_blur_rect7_mask.png", .7, .7);
  //   rect_filter("images/A.png", "build/plots/A_blur_rect6.png",
  //               "build/plots/A_blur_rect6_mask.png", .6, .6);
  //   rect_filter("images/A.png", "build/plots/A_blur_rect5.png",
  //               "build/plots/A_blur_rect5_mask.png", .5, .5);
  //   rect_filter("images/A.png", "build/plots/A_blur_rect4.png",
  //               "build/plots/A_blur_rect4_mask.png", .4, .4);
  //   rect_filter("images/A.png", "build/plots/A_blur_rect3.png",
  //               "build/plots/A_blur_rect3_mask.png", .3, .3);
  //   rect_filter("images/A.png", "build/plots/A_blur_rect2.png",
  //               "build/plots/A_blur_rect2_mask.png", .2, .2);
  //   rect_filter("images/A.png", "build/plots/A_blur_rect1.png",
  //               "build/plots/A_blur_rect1_mask.png", .1, .1);

  //   anti_rect_filter("images/A.png", "build/plots/A_blur_anti_rect95.png",
  //                    "build/plots/A_blur_anti_rect95_mask.png", .95, .95);
  //   anti_rect_filter("images/A.png", "build/plots/A_blur_anti_rect9.png",
  //                    "build/plots/A_blur_anti_rect9_mask.png", .9, .9);
  //   anti_rect_filter("images/A.png", "build/plots/A_blur_anti_rect8.png",
  //                    "build/plots/A_blur_anti_rect8_mask.png", .8, .8);
  //   anti_rect_filter("images/A.png", "build/plots/A_blur_anti_rect7.png",
  //                    "build/plots/A_blur_anti_rect7_mask.png", .7, .7);
  //   anti_rect_filter("images/A.png", "build/plots/A_blur_anti_rect6.png",
  //                    "build/plots/A_blur_anti_rect6_mask.png", .6, .6);
  //   anti_rect_filter("images/A.png", "build/plots/A_blur_anti_rect5.png",
  //                    "build/plots/A_blur_anti_rect5_mask.png", .5, .5);
  //   anti_rect_filter("images/A.png", "build/plots/A_blur_anti_rect4.png",
  //                    "build/plots/A_blur_anti_rect4_mask.png", .4, .4);
  //   anti_rect_filter("images/A.png", "build/plots/A_blur_anti_rect3.png",
  //                    "build/plots/A_blur_anti_rect3_mask.png", .3, .3);
  //   anti_rect_filter("images/A.png", "build/plots/A_blur_anti_rect2.png",
  //                    "build/plots/A_blur_anti_rect2_mask.png", .2, .2);
  //   anti_rect_filter("images/A.png", "build/plots/A_blur_anti_rect1.png",
  //                    "build/plots/A_blur_anti_rect1_mask.png", .1, .1);

  //   sharpen("images/boy.png", "build/plots/boy.png",
  //           "build/plots/boy_control.png", .05);

  //   sharpen_smooth("images/boy.png", "build/plots/boy_smooth.png",
  //                  "build/plots/boy_control_smooth.png", .001);
}


#include <Discregrid/All>
#include <Eigen/Dense>
#include <cxxopts/cxxopts.hpp>

#include <string>
#include <iostream>
#include <array>

#include "sph_kernel.hpp"
#include "gauss_quadrature.hpp"

using namespace Eigen;

std::istream& operator>>(std::istream& is, std::array<unsigned int, 3>& data)
{
	is >> data[0] >> data[1] >> data[2];
	return is;
}

std::istream& operator>>(std::istream& is, AlignedBox3d& data)
{
	is	>> data.min()[0] >> data.min()[1] >> data.min()[2]
		>> data.max()[0] >> data.max()[1] >> data.max()[2];
	return is;
}

int main(int argc, char* argv[])
{
	cxxopts::Options options(argv[0], "Generates a signed distance field from a closed two-manifold triangle mesh.");
	options.positional_help("[input OBJ file]");

	options.add_options()
	("h,help", "Prints this help text")
	("r,rest_density", "Rest density rho0 of the fluid", cxxopts::value<double>()->default_value("1000.0"))
	("i,invert", "Invert field")
	("s,smoothing_length", "Kernel smoothing length", cxxopts::value<double>()->default_value("0.1"))
	("o,output", "Ouput file in cdf format", cxxopts::value<std::string>()->default_value(""))
	("no-reduction", "Disables discarding of cells for sparse layout.")
	("input", "Discrete grid file containing input SDF in field 0", cxxopts::value<std::vector<std::string>>())
	;

	try
	{
		options.parse_positional("input");
		options.parse(argc, argv);
	}
	catch (cxxopts::OptionException const& e)
	{
		std::cout << "error parsing options: " << e.what() << std::endl;
		exit(1);
	}
	if (options.count("help"))
	{
		std::cout << options.help() << std::endl;
		std::cout << std::endl << std::endl << "Example: GenerateSDF -r \"50 50 50\" dragon.obj" << std::endl;
		exit(0);
	}
	if (!options.count("input"))
	{
		std::cout << "ERROR: No input SDF given." << std::endl;
		std::cout << options.help() << std::endl;
		std::cout << std::endl << std::endl << "Example: GenerateDensityMap -r \"50 50 50\" field.cdf" << std::endl;
		exit(1);
	}
	auto filename = options["input"].as<std::vector<std::string>>().front();

	if (!std::ifstream(filename).good())
	{
		std::cerr << "ERROR: Input file does not exist!" << std::endl;
		exit(1);
	}

	auto sdf = std::unique_ptr<Discregrid::DiscreteGrid>{};

	auto lastindex = filename.find_last_of(".");
	auto extension = filename.substr(lastindex+1, filename.length() - lastindex);

	std::cout << "Load SDF...";
	if (extension == "cdf")
	{
		sdf = std::make_unique<Discregrid::CubicLagrangeDiscreteGrid>(filename);
	}
	std::cout << "DONE" << std::endl;

	auto h = options["s"].as<double>();
	auto sph_kernel = CubicKernel{};
	sph_kernel.setRadius(h);
	auto gamma = [&](Vector3d const& x)
	{
		auto ar = sph_kernel.getRadius();
		auto dist = sdf->interpolate(0u, x);
		if (dist > ar)
			return 0.0;
		return 1.0 - dist / ar;
	};
	auto int_domain = AlignedBox3d(Vector3d::Constant(-h), Vector3d::Constant(h));
	auto rho0 = options["r"].as<double>();
	auto density_func = [&](Vector3d const& x)
	{
		auto dist = sdf->interpolate(0u, x);
		if (dist > 2.0 * sph_kernel.getRadius())
		{
			return 0.0;
		}

		auto integrand = [&sph_kernel, &gamma, &x](Vector3d const& xi)
		{
			auto res = gamma(x + xi) * sph_kernel.W(xi);
			return res;
		};

		auto res = GaussQuadrature::integrate(integrand, int_domain, 30);
		return rho0 * res;
	};

	auto no_reduction = options["no-reduction"].count() > 0u;

	auto cell_diag = sdf->cellSize().norm();
	std::cout << "Generate density map..." << std::endl;
	sdf->addFunction(density_func, true, [&](Vector3d const& x_)
	{
		if (no_reduction)
		{
			return true;
		}
		auto x = x_.cwiseMax(sdf->domain().min()).cwiseMin(sdf->domain().max());
		auto dist = sdf->interpolate(0u, x);
		if (dist == std::numeric_limits<double>::max())
		{
			return false;
		}

		return -6.0 * h < dist + cell_diag && dist -cell_diag < 2.0 * h;
	});

	if (options["no-reduction"].count() == 0u)
	{
		std::cout << "Reduce discrete fields...";
		sdf->reduceField(0u, [&](auto const&, double v)
		{
			return -6.0 * h < v + cell_diag && v -cell_diag  < 2.0 * h;
		});
		sdf->reduceField(1u, [&](auto const&, double v)
		{
			return 0.0 <= v && v <= 3.0 * rho0;
		});
		std::cout << "DONE" << std::endl;
	}

	std::cout << "Serialize discretization...";
	auto output_file = options["o"].as<std::string>();
	if (output_file == "")
	{
		output_file = filename;
		if (output_file.find(".") != std::string::npos)
		{
			auto lastindex = output_file.find_last_of(".");
			output_file = output_file.substr(0, lastindex);
		}
		output_file += ".cdm";
	}
	sdf->save(output_file);
	std::cout << "DONE" << std::endl;

	return 0;
}


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

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

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
// intentionally remove indentation of snippet
{
Matrix4d A = Matrix4d::Random(4,4);
cout << "Here is a random 4x4 matrix:" << endl << A << endl;
HessenbergDecomposition<Matrix4d> hessOfA(A);
Matrix4d pm = hessOfA.packedMatrix();
cout << "The packed matrix M is:" << endl << pm << endl;
cout << "The upper Hessenberg part corresponds to the matrix H, which is:"
     << endl << hessOfA.matrixH() << endl;
Vector3d hc = hessOfA.householderCoefficients();
cout << "The vector of Householder coefficients is:" << endl << hc << endl;

}
  return 0;
}


//  (C) Copyright Matt Borland 2021.
//  Use, modification and distribution are 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)

#ifndef BOOST_MATH_CCMATH_ILOGB_HPP
#define BOOST_MATH_CCMATH_ILOGB_HPP

#include <cmath>
#include <type_traits>
#include <boost/math/tools/is_constant_evaluated.hpp>
#include <boost/math/ccmath/logb.hpp>
#include <boost/math/ccmath/isinf.hpp>
#include <boost/math/ccmath/isnan.hpp>
#include <boost/math/ccmath/abs.hpp>

namespace boost::math::ccmath {

// If arg is not zero, infinite, or NaN, the value returned is exactly equivalent to static_cast<int>(std::logb(arg))
template <typename Real, std::enable_if_t<!std::is_integral_v<Real>, bool> = true>
inline constexpr int ilogb(Real arg) noexcept
{
    if(BOOST_MATH_IS_CONSTANT_EVALUATED(arg))
    {
        return boost::math::ccmath::abs(arg) == Real(0) ? FP_ILOGB0 :
               boost::math::ccmath::isinf(arg) ? INT_MAX :
               boost::math::ccmath::isnan(arg) ? FP_ILOGBNAN :
               static_cast<int>(boost::math::ccmath::logb(arg));
    }
    else
    {
        using std::ilogb;
        return ilogb(arg);
    }
}

template <typename Z, std::enable_if_t<std::is_integral_v<Z>, bool> = true>
inline constexpr int ilogb(Z arg) noexcept
{
    return boost::math::ccmath::ilogb(static_cast<double>(arg));
}

inline constexpr int ilogbf(float arg) noexcept
{
    return boost::math::ccmath::ilogb(arg);
}

#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
inline constexpr int ilogbl(long double arg) noexcept
{
    return boost::math::ccmath::ilogb(arg);
}
#endif

} // Namespaces

#endif // BOOST_MATH_CCMATH_ILOGB_HPP


#include "laplace.hpp"

#include <gtest/gtest.h>

#include <boost/random/mersenne_twister.hpp>

using namespace MultidimensionalArray;
using namespace ProbabilityDistributions;

TEST(LaplaceTest, Likelihood) {
  boost::random::mt19937 rng;
  const unsigned int n_samples = 100;
  Laplace<double> dist(0, 1);
  Array<double> samples;
  dist.sample(samples, n_samples, rng);

  auto indexes = Distribution<double>::sort_data(samples);

  double likelihood1 = dist.log_likelihood(samples);

  dist.MLE(samples, indexes);

  double likelihood2 = dist.log_likelihood(samples);

  EXPECT_GE(likelihood2, likelihood1);

  EXPECT_LT(0, dist.get_lambda());
}

TEST(LaplaceTest, MLE) {
  boost::random::mt19937 rng;
  const unsigned int n_samples = 100;
  Laplace<double> dist(0, 1);
  Array<double> samples;
  dist.sample(samples, n_samples, rng);
  auto indexes = Distribution<double>::sort_data(samples);
  dist.MLE(samples, indexes);

  double mu = dist.get_mu(), lambda = dist.get_lambda();
  double eps = 1e-2;
  double ll = dist.log_likelihood(samples);

  dist.set_mu(mu + eps);
  EXPECT_GE(ll, dist.log_likelihood(samples));
  dist.set_mu(mu - eps);
  EXPECT_GE(ll, dist.log_likelihood(samples));
  dist.set_mu(mu);

  dist.set_lambda(lambda + eps);
  EXPECT_GE(ll, dist.log_likelihood(samples));
  dist.set_lambda(lambda - eps);
  EXPECT_GE(ll, dist.log_likelihood(samples));
  dist.set_lambda(lambda);
}

TEST(LaplaceTest, Samples) {
  boost::random::mt19937 rng;
  const unsigned int n_samples = 100;
  Laplace<double> dist(0, 1);
  Array<double> samples;
  dist.sample(samples, n_samples, rng);
  for (size_t i = 0; i < n_samples; i++) {
    EXPECT_LT(-10, samples(i,0));
    EXPECT_GT(10, samples(i,0));
  }
}

TEST(LaplaceTest, ExtremeSamples) {
  Laplace<double> dist(0, 1);
  Array<double> samples({3,1});
  samples(0,0) = -1;
  samples(1,0) = 0;
  samples(2,0) = 1;

  double w1[] = {1, 1, 10}, w2[] = {10, 1, 1};
  auto indexes = Distribution<double>::sort_data(samples);

  {
    MA::ConstArray<double> weight({3}, w1);
    dist.MLE(samples, weight, indexes);
    EXPECT_LT(dist.get_mu(), samples(2,0));
    EXPECT_GT(dist.get_mu(), samples(1,0));
  }

  {
    MA::ConstArray<double> weight({3}, w2);
    dist.MLE(samples, weight, indexes);
    EXPECT_LT(dist.get_mu(), samples(1,0));
    EXPECT_GT(dist.get_mu(), samples(0,0));
  }
}


#include "util.hpp"
#include <armadillo>

namespace util{
  /*
    For a discrete probability distribution Pi = p(i), returns the index
    of an element randomly selected by sampling the distribution.

    Examples:
    * sample_discrete({0.5, 0.5}) returns 0 or 1 with probability 0.5
    * sample_discrete({0.0, 0.0, 1.0}) returns 2 with probability 1.0

    Note:
    While there does exist std::discrete_distribution, I think that the
    below implementation is superior in that it does not require
    instantiating a new distribution for every round.
  */
  arma::uword sample_discrete(const arma::vec &p){
    /*
      Sanity checks; require:
      * Pi >= 0 forall i
      * Sum(Pi) == 1
      */
    std::string die = "";
    const double eps = 1e-12;
    if (p.has_nan()){die = "P is malformed; it contains NaN!";}
    if (arma::any(p < -1 * eps)){die = "P cannot have negative elements!";}
    if (std::abs(arma::sum(p) - 1) > eps){ die = "P is not normed!";}

    if (die != ""){
      p.t().print("P");
      throw std::logic_error(die);
    }

    const double zeta = arma::randu();
    return as_scalar(arma::find(arma::cumsum(p) > zeta, 1));
  }

  // return a uvec with filled with the indicies [a,b)
  arma::uvec range(arma::uword a, arma::uword b){
    if (a > b){
      throw std::logic_error("cannot compute range with a > b");
    }

    arma::uword N = b - a;
    arma::uvec R(N, arma::fill::zeros);
    for (arma::uword i = 0; i < N; i++){
      R(i) = i + a;
    }

    return R;
  }

  // variant for [0, n)
  arma::uvec range(arma::uword n){ return range(0, n); }

  /*
    std::hypot() for complex<double>

    returns sqrt(|a|^2 + |b|^2)

    I'm not sure what is the "best" sequence for the real() operations.
    The present choice---to call real() twice, once after each
    multiplication---was made because that's the first time the
    opperands mathematically lie on the real line. Waiting longer seemed
    to invite the accumulation of small numerical deviations.
  */
  double hypot(std::complex<double> a, std::complex<double> b){
    return std::sqrt(std::real(std::conj(a)*a) + std::real(std::conj(b)*b) );
  }

  arma::vec center_of_mass(const arma::mat &R, const arma::vec &m){
    // R.B.: sum(A,1) gives the sum of the elements of each row of A
    return arma::sum(R.each_row() % m.t(), 1) / arma::sum(m);
  }

  /*
    For the (3xN) configuration space vectors A, B, compute the sum of
    the cross products of the vectors in each column: Sum(Ai X Bi)
  */
  arma::vec sum_cross(const arma::mat A, const arma::mat B){
    if ((A.n_cols != B.n_cols) || (3 != A.n_rows) || (3 != B.n_rows)){
      throw std::logic_error("Invalid dimension!");
    }

    arma::vec sum(3, arma::fill::zeros);

    for (arma::uword i = 0; i < A.n_cols; i++){
      sum += arma::cross(A.col(i), B.col(i));
    }

    return sum;
  }
}