///////////////////////////////////////////////////////////////////////////////
// weighted_median.hpp
//
//  Copyright 2006 Eric Niebler, Olivier Gygi. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_MEDIAN_HPP_EAN_28_10_2005
#define BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_MEDIAN_HPP_EAN_28_10_2005

#include <boost/mpl/placeholders.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/median.hpp>
#include <boost/accumulators/statistics/weighted_p_square_quantile.hpp>
#include <boost/accumulators/statistics/weighted_density.hpp>
#include <boost/accumulators/statistics/weighted_p_square_cumulative_distribution.hpp>

namespace boost { namespace accumulators
{

namespace impl
{
    ///////////////////////////////////////////////////////////////////////////////
    // weighted_median_impl
    //
    /**
        @brief Median estimation for weighted samples based on the \f$P^2\f$ quantile estimator

        The \f$P^2\f$ algorithm for weighted samples is invoked with a quantile probability of 0.5.
    */
    template<typename Sample>
    struct weighted_median_impl
      : accumulator_base
    {
        // for boost::result_of
        typedef typename numeric::functional::average<Sample, std::size_t>::result_type result_type;

        weighted_median_impl(dont_care) {}

        template<typename Args>
        result_type result(Args const &args) const
        {
            return weighted_p_square_quantile_for_median(args);
        }
    };

    ///////////////////////////////////////////////////////////////////////////////
    // with_density_weighted_median_impl
    //
    /**
        @brief Median estimation for weighted samples based on the density estimator

        The algorithm determines the bin in which the \f$0.5*cnt\f$-th sample lies, \f$cnt\f$ being
        the total number of samples. It returns the approximate horizontal position of this sample,
        based on a linear interpolation inside the bin.
    */
    template<typename Sample>
    struct with_density_weighted_median_impl
      : accumulator_base
    {
        typedef typename numeric::functional::average<Sample, std::size_t>::result_type float_type;
        typedef std::vector<std::pair<float_type, float_type> > histogram_type;
        typedef iterator_range<typename histogram_type::iterator> range_type;
        // for boost::result_of
        typedef float_type result_type;

        template<typename Args>
        with_density_weighted_median_impl(Args const &args)
          : sum(numeric::average(args[sample | Sample()], (std::size_t)1))
          , is_dirty(true)
        {
        }

        void operator ()(dont_care)
        {
            this->is_dirty = true;
        }

        template<typename Args>
        result_type result(Args const &args) const
        {
            if (this->is_dirty)
            {
                this->is_dirty = false;

                std::size_t cnt = count(args);
                range_type histogram = weighted_density(args);
                typename range_type::iterator it = histogram.begin();
                while (this->sum < 0.5 * cnt)
                {
                    this->sum += it->second * cnt;
                    ++it;
                }
                --it;
                float_type over = numeric::average(this->sum - 0.5 * cnt, it->second * cnt);
                this->median = it->first * over + (it + 1)->first * ( 1. - over );
            }

            return this->median;
        }

    private:
        mutable float_type sum;
        mutable bool is_dirty;
        mutable float_type median;
    };

    ///////////////////////////////////////////////////////////////////////////////
    // with_p_square_cumulative_distribution_weighted_median_impl
    //
    /**
        @brief Median estimation for weighted samples based on the \f$P^2\f$ cumulative distribution estimator

        The algorithm determines the first (leftmost) bin with a height exceeding 0.5. It
        returns the approximate horizontal position of where the cumulative distribution
        equals 0.5, based on a linear interpolation inside the bin.
    */
    template<typename Sample, typename Weight>
    struct with_p_square_cumulative_distribution_weighted_median_impl
      : accumulator_base
    {
        typedef typename numeric::functional::multiplies<Sample, Weight>::result_type weighted_sample;
        typedef typename numeric::functional::average<weighted_sample, std::size_t>::result_type float_type;
        typedef std::vector<std::pair<float_type, float_type> > histogram_type;
        typedef iterator_range<typename histogram_type::iterator> range_type;
        // for boost::result_of
        typedef float_type result_type;

        with_p_square_cumulative_distribution_weighted_median_impl(dont_care)
          : is_dirty(true)
        {
        }

        void operator ()(dont_care)
        {
            this->is_dirty = true;
        }

        template<typename Args>
        result_type result(Args const &args) const
        {
            if (this->is_dirty)
            {
                this->is_dirty = false;

                range_type histogram = weighted_p_square_cumulative_distribution(args);
                typename range_type::iterator it = histogram.begin();
                while (it->second < 0.5)
                {
                    ++it;
                }
                float_type over = numeric::average(it->second - 0.5, it->second - (it - 1)->second);
                this->median = it->first * over + (it + 1)->first * ( 1. - over );
            }

            return this->median;
        }
    private:
        mutable bool is_dirty;
        mutable float_type median;
    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::weighted_median
// tag::with_density_weighted_median
// tag::with_p_square_cumulative_distribution_weighted_median
//
namespace tag
{
    struct weighted_median
      : depends_on<weighted_p_square_quantile_for_median>
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::weighted_median_impl<mpl::_1> impl;
    };
    struct with_density_weighted_median
      : depends_on<count, weighted_density>
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::with_density_weighted_median_impl<mpl::_1> impl;
    };
    struct with_p_square_cumulative_distribution_weighted_median
      : depends_on<weighted_p_square_cumulative_distribution>
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::with_p_square_cumulative_distribution_weighted_median_impl<mpl::_1, mpl::_2> impl;
    };

}

///////////////////////////////////////////////////////////////////////////////
// extract::weighted_median
//
namespace extract
{
    extractor<tag::median> const weighted_median = {};
}

using extract::weighted_median;
// weighted_median(with_p_square_quantile) -> weighted_median
template<>
struct as_feature<tag::weighted_median(with_p_square_quantile)>
{
    typedef tag::weighted_median type;
};

// weighted_median(with_density) -> with_density_weighted_median
template<>
struct as_feature<tag::weighted_median(with_density)>
{
    typedef tag::with_density_weighted_median type;
};

// weighted_median(with_p_square_cumulative_distribution) -> with_p_square_cumulative_distribution_weighted_median
template<>
struct as_feature<tag::weighted_median(with_p_square_cumulative_distribution)>
{
    typedef tag::with_p_square_cumulative_distribution_weighted_median type;
};

}} // namespace boost::accumulators

#endif


#include <iostream>
#include <Eigen/Dense>
#include "../Optimizer/optimizer"
using namespace std;
using namespace Eigen;

double func (double x) {
    return pow(x + 10, 2);
}

int main () {
    cout << "Using Function: (x + 10)^2 for single variable algorithms testing." << endl;
    cout << "Test Bounding Phase:" << endl;
    double ipt = 5.4;
    Vector2d range = boundingPhase(func, ipt);
    cout << "Range from bounding Phase for initial point :" << ipt << endl;
    cout << range << endl;

    cout << "Derivatives at " << ipt << endl;
    cout << derivative(func, ipt) << endl;

    cout << "Finding optimal point using above range for Newton Rapshon Method." << endl;
    cout << "Optimal Point is: ";
    cout << newtonRapshon (func, range) << endl;;
}


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * 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.
 ********************************************************************************/

#ifndef RW_MATH_LINEARALGEBRA_HPP
#define RW_MATH_LINEARALGEBRA_HPP

/**
 * @file LinearAlgebra.hpp
 */
#if !defined(SWIG)
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <limits>
#endif
namespace rw { namespace math {

    /** @addtogroup math */
    /* @{*/

    /**
     * @brief Collection of Linear Algebra functions
     */
    class LinearAlgebra
    {
      public:
        //! @brief Type for Eigen matrices used to reduce namespace cluttering.
        template< class T = double > struct EigenMatrix
        {
            //! type of this matrix
            typedef Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > type;
        };

        //! @brief Type for Eigen vectors, used to reduce namespace cluttering.
        template< class T = double > struct EigenVector
        {
            //! type of this Vector
            typedef Eigen::Matrix< T, Eigen::Dynamic, 1 > type;
        };

        /**
         * @brief Performs a singular value decomposition (SVD)
         *
         * The SVD computes the decomposition
         * \f$ \mathbf{M}=\mathbf{U}*\mathbf{DiagonalMatrix(\sigma)}*\mathbf{V}^T \f$ .
         *
         * @param M [in] the matrix to decomposite
         * @param U [out] Result matrix \f$\mathbf{U}\f$
         * @param sigma [out] The \f$\mathbf{sigma}\f$ vector with diagonal elements
         * @param V [out] Result matrix \f$\mathbf{V}\f$
         */
        static void svd (const Eigen::Matrix<double,-1,-1>& M, Eigen::Matrix<double,-1,-1>& U, Eigen::Matrix<double,-1,1>& sigma,
                         Eigen::Matrix<double,-1,-1>& V);

        /**
         * \brief Calculates the moore-penrose (pseudo) inverse of a matrix
         * @f$ \mathbf{M}^+@f$
         *
         * \param am [in] the matrix @f$ \mathbf{M} @f$ to be inverted
         *
         * \param precision [in] the precision to use, values below this
         * treshold are considered singular
         *
         * \return the pseudo-inverse @f$ \mathbf{M}^+@f$ of @f$ \mathbf{M} @f$
         *
         * \f$ \mathbf{M}^+=\mathbf{V}\mathbf{\Sigma} ^+\mathbf{U}^T \f$ where
         * \f$ \mathbf{V} \f$, \f$ \mathbf{\Sigma} \f$ and \f$ \mathbf{U} \f$
         * are optained using Singular Value Decomposition (SVD)
         *
         *
         */
        static Eigen::Matrix<double,-1,-1> pseudoInverse (const Eigen::Matrix<double,-1,-1>& am, double precision = 1e-6);

        /**
         * @brief Checks the penrose conditions
         * @param A [in] a matrix
         * @param X [in] a pseudoinverse of A
         * @param prec [in] the tolerance
         *
         * @return true if the pseudoinverse X of A fullfills the penrose
         * conditions, false otherwise
         *
         * Checks the penrose conditions:
         *
         * @f$
         * AXA = A
         * @f$
         *
         * @f$
         * XAX = X
         * @f$
         *
         * @f$
         * (AX)^T = AX
         * @f$
         *
         * @f$
         * (XA)^T = XA
         * @f$
         */
        static bool checkPenroseConditions (const Eigen::Matrix<double,-1,-1>& A, const Eigen::Matrix<double,-1,-1>& X,
                                            double prec);

        /**
         * \brief Calculates matrix determinant
         * \param m [in] a square matrix
         * \return the matrix determinant
         */
        template< class R > static inline double det (const Eigen::MatrixBase< R >& m)
        {
            return m.determinant ();
        }

        /**
         * @brief Calculates matrix inverse.
         * @param M [in] input matrix @f$ \mathbf{M} @f$ to invert
         * @return output matrix @f$ \mathbf{M}^{-1} @f$
         **/
        template< class T > static T inverse (const Eigen::MatrixBase< T >& M)
        {
            return M.inverse ();
        }

        /**
         * @brief Checks if a given matrix is in SO(n) (special orthogonal)
         * @param M [in] \f$ \mathbf{M} \f$
         * @return true if \f$ M\in SO(n) \f$
         *
         * \f$ SO(n) = {\mathbf{R}\in \mathbb{R}^{n\times n} :
         * \mathbf{R}\mathbf{R}^T=\mathbf{I}, det \mathbf{R}=+1} \f$
         *
         */
        template< class R > static inline bool isSO (const Eigen::MatrixBase< R >& M)
        {
            return M.cols () == M.rows () && isProperOrthonormal (M);
        }

        /**
         * @brief Checks if a given matrix is in SO(n) (special orthogonal)
         * @param M [in] \f$ \mathbf{M} \f$
         * @param precision [in] the precision to use for floating point comparison
         * @return true if \f$ M\in SO(n) \f$
         *
         * \f$ SO(n) = {\mathbf{R}\in \mathbb{R}^{n\times n} :
         * \mathbf{R}\mathbf{R}^T=\mathbf{I}, det \mathbf{R}=+1} \f$
         *
         */
        template< class R >
        static inline bool isSO (const Eigen::MatrixBase< R >& M, typename R::Scalar precision)
        {
            return M.cols () == M.rows () && isProperOrthonormal (M, precision);
        }

        /**
         * @brief Checks if a given matrix is skew-symmetrical
         * @param M [in] \f$ \mathbf{M} \f$ the matrix to check
         *
         * @return true if the property
         * \f$ \mathbf{M}=-\mathbf{M}^T \f$ holds,
         * false otherwise.
         */
        template< class R > static inline bool isSkewSymmetric (const Eigen::MatrixBase< R >& M)
        {
            return (M + M.transpose ()).template lpNorm< Eigen::Infinity > () == 0.0;
        }

        /**
         * @brief Checks if a given matrix is proper orthonormal
         * @return true if the matrix is proper orthonormal, false otherwise
         *
         * A matrix is proper orthonormal if it is orthonormal and its determinant
         * is equal to \f$ +1 \f$
         */
        template< class R >
        static inline bool isProperOrthonormal (
            const Eigen::MatrixBase< R >& r,
            typename R::Scalar precision = std::numeric_limits< typename R::Scalar >::epsilon ())
        {
            return isOrthonormal (r, precision) && fabs (r.determinant () - 1.0) <= precision;
        }

        /**
         * @brief Checks if a given matrix is orthonormal
         * @return true if the matrix is orthonormal, false otherwise
         *
         * A matrix is orthonormal if all of it's column's are mutually orthogonal
         * and all of it's column's has unit length.
         *
         * that is for any \f$ i, j \f$ the following holds
         * \f$ col_i . col_j = 0 \f$ and \f$ ||col_i|| = 1 \f$
         *
         * Another nessesary and sufficient condition of orthonormal matrices is that
         * \f$ \mathbf{M}\mathbf{M}^T=I \f$
         */
        template< class R >
        static inline bool isOrthonormal (
            const Eigen::MatrixBase< R >& r,
            typename R::Scalar precision = std::numeric_limits< typename R::Scalar >::epsilon ())
        {
            return (r * r.transpose ()).isIdentity (precision);
            // const Eigen::MatrixBase<R> m = r*r.transpose() ;
            // return m.isIdentity(1e-15);
            // double scale = m.norm();//m.lpNorm<Eigen::Infinity>();
            // return scale == 0.0;
        }

        /**
         * @brief Decomposition for a symmetric matrix.
         * @param Am1 [in] a symmetric matrix.
         * @return the decomposition as a pair with eigenvectors and eigenvalues.
         */
        template< class T >
        // static std::pair<typename EigenMatrix<T>::type, typename EigenVector<T>::type >
        // eigenDecompositionSymmetric(const typename EigenMatrix<T>::type& Am1)
        static std::pair< typename EigenMatrix< T >::type, typename EigenVector< T >::type >
        eigenDecompositionSymmetric (const Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic >& Am1)
        {
            Eigen::SelfAdjointEigenSolver< Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > >
                eigenSolver;
            eigenSolver.compute (Am1);
            return std::make_pair (eigenSolver.eigenvectors (), eigenSolver.eigenvalues ());
        }

        /**
         * @brief Eigen decomposition of a matrix.
         * @param Am1 [in] the matrix.
         * @return the decomposition as a pair with eigenvectors and eigenvalues.
         */
        template< class T >
        static std::pair< typename EigenMatrix< std::complex< T > >::type,
                          typename EigenVector< std::complex< T > >::type >
        eigenDecomposition (const typename Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic >& Am1)
        {
            Eigen::EigenSolver< Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > > eigenSolver;
            eigenSolver.compute (Am1);

            Eigen::Matrix< std::complex< T >, Eigen::Dynamic, Eigen::Dynamic > vectors =
                eigenSolver.eigenvectors ();
            Eigen::Matrix< std::complex< T >, Eigen::Dynamic, 1 > values =
                eigenSolver.eigenvalues ();
            return std::make_pair (vectors, values);
        }

      private:
    };

    template<>
    std::pair< typename LinearAlgebra::EigenMatrix< double >::type,
               typename LinearAlgebra::EigenVector< double >::type >
    LinearAlgebra::eigenDecompositionSymmetric< double > (const Eigen::MatrixXd& Am1);

    template<>
    std::pair< typename LinearAlgebra::EigenMatrix< std::complex< double > >::type,
               typename LinearAlgebra::EigenVector< std::complex< double > >::type >
    LinearAlgebra::eigenDecomposition< double > (const Eigen::MatrixXd& Am1);

    /*@}*/
}}    // namespace rw::math

#endif    // end include guard


#include <iostream>
#include <cstdint>
#include <cmath>
#include <utility>
#include <fstream>
#include <vector>

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

#include "assemblyop.hpp"
#include "preconditioner.cpp"

using namespace Eigen;
using namespace std;

enum Preconditioner { NONE, DIAGONAL, BACKWARD, FORWARD };

pair<MatrixXd,MatrixXd> getRates(const string& fname) {
	vector<double> rates;
	double r;

	ifstream file(fname, ios::in);
	if (!file.is_open()) {
		cout << "Error: No such file " << fname << endl;
		exit(1);
	}

	while (file >> r)
		rates.push_back(r);
	file.close();

	unsigned int k = (unsigned int) (sqrt(0.25 + (double) rates.size()) - 0.5);

	if (k*(k+1) != rates.size()) {
		cout << "Error: Entries in " << fname << " are not properly formatted." << endl;
	}

	MatrixXd A(k,k);
	MatrixXd B(k,k);

	unsigned int ind = 0, i, j;
	for (i = 0; i < k; i++) {
		for (j = 0; j < i; j++)
			A(i,j) = A(j,i) = rates[ind++];
		A(i,i) = rates[ind++];
	}
	for (i = 0; i < k; i++) {
		for (j = 0; j < i; j++)
			B(i,j) = B(j,i) = rates[ind++];
		B(i,i) = rates[ind++];
	}

	return make_pair(A,B);
}

int main(int argc, char** argv) {
	if (argc < 4) {
		cout << "Usage:\t" << argv[0] << " mass rates moments [rescale [diagnostic [precond]]]" << endl;
		cout << "mass:\tthe total number of units in the assembly system" << endl;
		cout << "rates:\tthe filename containing the rate specification for the assembly system" << endl;
		cout << "moments:\tthe number of uncentered moments to compute" << endl;
		cout << "rescale:\t(optional) scale A(i,j) by this quantity. Default=1.0" << endl;
		cout << "diagnostic:\t(optional) if present, output only the all-monomer value and the number of iterations." << endl;
		cout << "precond:\t(optional) if precond < 0, performs forward SOR(-precond);" << endl << "        \tif precond = 0 (or absent), no preconditioning;" << endl << "        \tif precond > 0, backward SOR(precond);" << endl;
		return 0;
	}

	uintmax_t mass = atoi(argv[1]);

	string file(argv[2]);
	pair<MatrixXd,MatrixXd> pr = getRates(file);

	size_t nmoments = atoi(argv[3]);

	double sigma = 1.0;
	if (argc > 4)
		sigma = atof(argv[4]);
	bool diagnostic = argc > 5;
	Preconditioner precond = NONE;
	double omega = 0.0;
	if (argc > 6) {
		precond = DIAGONAL;
		omega = atof(argv[6]);
		if (omega < 0.0) {
			precond = FORWARD;
			omega *= -1.0;
		}
		else if (omega > 0.0) {
			precond = BACKWARD;
		}
	}

	MatrixXd B = get<1>(pr);
	size_t n0 = B.rows()+1;

	PartitionList<uintmax_t> states(n0-1, mass);

	MatrixXd A = sigma * get<0>(pr);
	AssemblyOp<uintmax_t> Lambda(states, A, B);
	VectorXd b, c = VectorXd::Constant(states.size(), 1.0);

	if (precond == NONE) {
		BiCGSTAB< AssemblyOp<uintmax_t>, IdentityPreconditioner > solver(Lambda);
		for (size_t i = 1; i <= nmoments; i++) {
			b = ((double) i) * c;
			c = solver.solve(b);
			if (diagnostic)
				cout << c(0) << '\t' << solver.iterations() << endl;
			else
				cout << c.transpose() << endl;
		}
	}
	else if (precond == DIAGONAL) {
		BiCGSTAB< AssemblyOp<uintmax_t>, AssemblyDiagonalPreconditioner<uintmax_t> > solver(Lambda);
		for (size_t i = 1; i <= nmoments; i++) {
			b = ((double) i) * c;
			c = solver.solve(b);
			if (diagnostic)
				cout << c(0) << '\t' << solver.iterations() << endl;
			else
				cout << c.transpose() << endl;
		}
	}
	else if (precond == FORWARD) {
		BiCGSTAB< AssemblyOp<uintmax_t>, AssemblyLowerPreconditioner<uintmax_t> > solver(Lambda);
		solver.preconditioner().setRelaxation(omega);
		for (size_t i = 1; i <= nmoments; i++) {
			b = ((double) i) * c;
			c = solver.solve(b);
			if (diagnostic)
				cout << c(0) << '\t' << solver.iterations() << endl;
			else
				cout << c.transpose() << endl;
		}
	}
	else if (precond == BACKWARD) {
		BiCGSTAB< AssemblyOp<uintmax_t>, AssemblyUpperPreconditioner<uintmax_t> > solver(Lambda);
		solver.preconditioner().setRelaxation(omega);
		for (size_t i = 1; i <= nmoments; i++) {
			b = ((double) i) * c;
			c = solver.solve(b);
			if (diagnostic)
				cout << c(0) << '\t' << solver.iterations() << endl;
			else
				cout << c.transpose() << endl;
		}
	}

	return 0;

}


/*
 * Matrix-vector multiply
 *
 * Information on Intel vector pragmas is available in the following link:
 * https://software.intel.com/en-us/cpp-compiler-developer-guide-and-reference-vector-1#58209E46-70EA-4C47-BED6-E69236C6680C
 */

// https://en.cppreference.com/w/cpp/memory/c/aligned_alloc
#include <cstdlib>  // aligned_alloc (C++11), std::aligned_alloc (C++17)
#include <ctime>
#include <iostream>
#include <vector>
#include <boost/align/aligned_allocator.hpp>
#include <cmath>
// #include "papi.h"
#include "simd.h"
#include "environ.h"
#include "utils.h"
#include "vutils.h"

///////////////////////////////////////////////////////////////////////////////
// USER CONFIGURATION
///////////////////////////////////////////////////////////////////////////////
// Square matrix dimension
const size_t N = 6;

// Precision for floating-point operations
// Valid values are: 4, 8
#define REAL_TYPE 4

///////////////////////////////////////////////////////////////////////////////
// FP and SIMD
///////////////////////////////////////////////////////////////////////////////
// Precision for floating-point operations
#if REAL_TYPE == 4
typedef float real;
typedef SIMD_FLT vreal;
#elif REAL_TYPE == 8
typedef double real;
typedef SIMD_DBL vreal;
#endif

// Number of floating-point values that fit into a SIMD register
const int SIMD_STREAMS = SIMD_WIDTH_BYTES / sizeof(real);
const int LOG2STREAMS = std::log2(SIMD_STREAMS);

///////////////////////////////////////////////////////////////////////////////
// PROGRAM
///////////////////////////////////////////////////////////////////////////////
template <typename T>
using aligned_vector = std::vector<T, boost::alignment::aligned_allocator<T, SIMD_WIDTH_BYTES>>;

void gemv(
    const size_t n,
    const size_t lda,
    const aligned_vector<real> v1,
    const aligned_vector<real> v2,
    aligned_vector<real> &dp)
{
    for (size_t row = 0; row < n; row++) {
        for (size_t col = 0; col < n; col++) {
            dp[row] += v1[row * lda + col] * v2[col];
        }
    }
}

void gemv_simd_tree_sum(
    const size_t n,
    const size_t lda,
    const real *v1,
    const real *v2,
    real *dp)
{
#if defined(__INTEL_COMPILER)
    __assume_aligned(v1, SIMD_WIDTH_BYTES);
    __assume_aligned(v2, SIMD_WIDTH_BYTES);
    const real *_v1 = v1;
    const real *_v2 = v2;
#elif defined(__GNUC__)
    const real *_v1 = (real *)__builtin_assume_aligned(v1, SIMD_WIDTH_BYTES);
    const real *_v2 = (real *)__builtin_assume_aligned(v2, SIMD_WIDTH_BYTES);
#endif

    for (size_t row = 0; row < n; row++) {
        vreal vdp = simd_zero();
        for (size_t col = 0; col < lda; col+=SIMD_STREAMS) {
            vreal vv1 = simd_load(&_v1[row * lda + col]);
            vreal vv2 = simd_load(&_v2[col]);
#if defined(__FMA__)
            vdp = simd_fmadd(vv1, vv2, vdp);
#else
            vv1 = simd_mul(vv1, vv2);
            vdp = simd_add(vv1, vdp);
#endif
        }

        // Binary tree sum reduction
        for (size_t i = 0; i < LOG2STREAMS - 1; i++) {
            vdp = simd_hadd(vdp, vdp);
        }

        real tdp[SIMD_STREAMS] __attribute__((aligned(SIMD_WIDTH_BYTES)));
        simd_store(tdp, vdp);
        // NOTE: 'dp' does need to be aligned because it is used to store a scalar value.
#if SIMD_WIDTH_BITS == 128
        // HADD from SSE3 does not interleave horizontal sums.
        dp[row] = tdp[0] + tdp[1];
#else
        dp[row] = tdp[0] + tdp[SIMD_STREAMS / 2];
#endif
    }
}

void print_matrix(
    const size_t n,
    const size_t m,
    const size_t lda,
    const vector<real> v)
{
    for (size_t row = 0; row < n; row++) {
        for (size_t col = 0; col < m; col++) {
            std::cout << v[row * lda + col] << ", ";
        }
        std::cout << std::endl;
    }

}

int main(int argc, char *argv[])
{
    int result = 0;

    size_t num_matrices = 1;
    if (argc > 1) {
        num_matrices = std::atoi(argv[1]);
    }

    detectCPU();
    detectSIMD();

    std::cout << "Alignment: " << SIMD_WIDTH_BYTES << std::endl;
    std::cout << "Num. elems: " << SIMD_STREAMS << std::endl;

    // Number of elements in padded matrix column to conform with SIMD alignment
    const size_t lda = (((N * sizeof(real)) / SIMD_WIDTH_BYTES) * SIMD_WIDTH_BYTES + SIMD_WIDTH_BYTES) / sizeof(real);
    // For unaligned rows, set LDA to N
    // const size_t lda = N;

    // Create a vector of given size
    aligned_vector<real> v1(N * lda);      // matrix
    aligned_vector<real> v2(1 * lda, 1.);  // column vector, set to 1 --> add rows of matrix
    aligned_vector<real> dp(N, 0);  // resulting column vector (dot products)

    // Zero out extra rows used for padding, to prevent floating-point exception during vector multiplication.
    // This memory elements are never modified, so set once.
    memset(v2.data() + N, 0, (lda - N) * sizeof(real));

    real *arr_A = NULL, *arr_B = NULL, *arr_C = NULL;
    // scalar_malloc(&arr_A, SIMD_WIDTH_BYTES, SIMD_STREAMS);
    // scalar_malloc(&arr_B, SIMD_WIDTH_BYTES, SIMD_STREAMS);
    // scalar_malloc(&arr_C, SIMD_WIDTH_BYTES, SIMD_STREAMS);

    for (int i = 0; i < SIMD_STREAMS; ++i) {
        arr_A[i] = 1.;
        arr_B[i] = (real)i;
    }

    vreal va = simd_load(arr_A);
    vreal vb = simd_load(arr_B);
    vreal vc = simd_add(va, vb);
    simd_store(arr_C, vc);

    for (int i = 0; i < SIMD_STREAMS; ++i) {
        if (arr_C[i] != (arr_A[i] + arr_B[i]))
            result += 1;
        else
            std::cout << arr_C[i] << std::endl;
    }

    scalar_free(&arr_A);
    scalar_free(&arr_B);
    scalar_free(&arr_C);

    return result;
}


/// \file
/// \brief A file for detecting circle using a regression algorithm. This file is a library file
///

#include "sensor_msgs/LaserScan.h"
#include "nuslam/circle_detection.hpp"
#include <ros/ros.h>
#include <ros/console.h>
#include <Eigen/SVD>
#include <Eigen/Cholesky>
#include <cmath>
#include "rigid2d/rigid2d.hpp"

using std::cout;
using Eigen::Matrix;
using Eigen::Dynamic;
using Eigen::BDCSVD;
using Eigen::ComputeFullV;
using Eigen::ComputeFullU;

using Eigen::Vector4d;
using rigid2d::Vector2D;

typedef Matrix<double, Dynamic, 4> Matrix4dn;
typedef Matrix<double, 4, 4> Matrix4d;

namespace circleDetection
{

/// \brief A constructor for Poin2d data type
/// \param x1 - x co-ordinate
/// \param y1 - y co-ordinate
Point2d::Point2d(double x1, double y1)
{

  x = x1;
  y = y1;
}

/// \brief A function that fits circle to the list of points given
/// \param CirclePoints -  A list containing points for which a circle needs to be fit.
Vector3d fitCircle(vector<Vector2D> circlePoints)
{

  Matrix4dn circleMatrix;
  Matrix4d H, Hinv;
  circleMatrix.resize(circlePoints.size(), 4);
  Vector4d equCoeff;
  size_t i;
  double sum_x=0, sum_y=0, sum_z=0;
  int pointsCount = circlePoints.size();

  for(auto point: circlePoints)
  {
    sum_x += point.x;
    sum_y += point.y;
  }
  double mean_x = sum_x / (double) circlePoints.size();
  double mean_y = sum_y / (double) circlePoints.size();

  for (auto& point:circlePoints)
  {
    point.x = point.x - mean_x;
    point.y = point.y - mean_y;
  }

  std::cout << "Here is the matrix m:\n" << circlePoints[0].x <<circlePoints[0].y << std::endl;

  for(i=0; i < circlePoints.size(); i++)
  {
    circleMatrix(i, 0) = pow(circlePoints[i].x, 2) + pow(circlePoints[i].y, 2);
    circleMatrix(i, 1) = circlePoints[i].x;
    circleMatrix(i, 2) = circlePoints[i].y;
    circleMatrix(i, 3) = 1.0;
    sum_z += circleMatrix(i, 0);
  }

  double mean_z = sum_z / (double) pointsCount;

  auto M = circleMatrix.transpose() * circleMatrix / pointsCount;

  // Initialise H matrix
  H(0,0) = 8 * mean_z;
  H(0,1) = 0;
  H(0,2) = 0;
  H(0,3) = 2;
  H(1,0) = 0;
  H(1,1) = 1;
  H(1,2) = 0;
  H(1,3) = 0;
  H(2,0) = 0;
  H(2,1) = 0;
  H(2,2) = 1;
  H(2,3) = 0;
  H(3,0) = 2;
  H(3,1) = 0;
  H(3,2) = 0;
  H(3,3) = 0;

  // Initialise H inverse matrix
  Hinv(0,0) = 0;
  Hinv(0,1) = 0;
  Hinv(0,2) = 0;
  Hinv(0,3) = 0.5;
  Hinv(1,0) = 0;
  Hinv(1,1) = 1;
  Hinv(1,2) = 0;
  Hinv(1,3) = 0;
  Hinv(2,0) = 0;
  Hinv(2,1) = 0;
  Hinv(2,2) = 1;
  Hinv(2,3) = 0;
  Hinv(3,0) = 0.5;
  Hinv(3,1) = 0;
  Hinv(3,2) = 0;
  Hinv(3,3) = -2 * mean_z;

  BDCSVD<Matrix4dn> svdCircle( circleMatrix, ComputeFullV | ComputeFullU  );
  auto sigmaValues = svdCircle.singularValues();
  auto V = svdCircle.matrixV();

  if(sigmaValues.size() ==4 && sigmaValues[3] > 1e-12)
  {

    auto sigmaMatrix = sigmaValues.asDiagonal();
    auto Y = V * sigmaMatrix * V.transpose();
    std::cout<< "Y cols" << Y.cols() <<" Y rows"<<Y.rows();
    std::cout<< "H cols" << Hinv.cols() <<" H rows" << Hinv.rows();

    std::cout << "Here is the matrix m:\n" << Y << std::endl;
    auto Q = Y * Hinv * Y;
    Eigen::SelfAdjointEigenSolver<Matrix4dn> eig(Q);

    auto eigVecQ = eig.eigenvectors();
    auto eigValues = eig.eigenvalues();

    double minEigValue = std::numeric_limits<double>::infinity();
    int minEigIndex;

    for(int i=0; i< eigValues.size(); i++)
    {
      if(eigValues[i] < minEigValue && eigValues[i] > 0)
      {
        minEigValue = eigValues[i];
        minEigIndex = i;
      }
    }

    Vector4d smallEigVec = {eigVecQ(0,minEigIndex), eigVecQ(1,minEigIndex), eigVecQ(2,minEigIndex), eigVecQ(3,minEigIndex)};

    std::cout << "Here is the matrix m:\n" << eigVecQ << std::endl;
    std::cout << "Here is the matrix m:\n" << smallEigVec << std::endl;
    std::cout << "Q eigen values" << eigValues;

    equCoeff = Y.colPivHouseholderQr().solve(smallEigVec);
  }
  else
  {

    equCoeff = {V(0,3), V(1,3), V(2,3), V(3,3)};
  }

  double a = -equCoeff[1] / equCoeff[0] / 2.0 + mean_x;
  double b = -equCoeff[2] / equCoeff[0] / 2.0 + mean_y;
  double r2 = (pow(equCoeff[1], 2) + pow(equCoeff[2], 2) - 4 * equCoeff[0] * equCoeff[3]) / (4.0 * pow(equCoeff[0], 2));
  double r = sqrt(r2);
  Vector3d circleCoeff = {a, b, r};

 std::cout << "Here is the matrix m:\n" << circleMatrix << std::endl;
 std::cout << "Here is the matrix m:\n" << M << std::endl;
 std::cout<< "Sigma values\n"<<sigmaValues << std::endl;
 std::cout<< "Circle coefficients" << circleCoeff;
 std::cout<< "x center" << mean_x;
 std::cout<< "y center" << mean_y;
 return circleCoeff;

}

/// \brief A function that calculates the least squares error for the estimated circle
///        fitting parameters
/// \param observedPoints - All the points for which a circular fit needs to be done
/// \param circleCoeff - Circle parameters estimated

double calculateError(const vector<Vector2D>& observedPoints, Vector3d circleCoeff)
{
  double totalError=0;
  double error = 0;

  for(auto point:observedPoints)
  {
    error = pow(point.x - circleCoeff[0], 2) + pow(point.y - circleCoeff[1], 2) - pow(circleCoeff[2], 2);
    error = pow(error, 2);
    totalError += error;
  }
  totalError = totalError / (double) observedPoints.size();
  totalError = sqrt(totalError);
  return totalError;

}

}


//
// $Id: LinearSolver.hpp 1195 2009-08-14 22:12:04Z chambm $
//
//
// Original author: Darren Kessner <darren@proteowizard.org>
//
// Copyright 2007 Spielberg Family Center for Applied Proteomics
//   Cedars Sinai Medical Center, Los Angeles, California  90048
//
// 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.
//

#ifndef _LINEARSOLVER_HPP_
#define _LINEARSOLVER_HPP_

#include "pwiz/utility/misc/Export.hpp"
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <stdexcept>

#include <iostream>
#include <iomanip>
#include <math.h>

#include "qr.hpp"

namespace pwiz {
namespace math {

enum PWIZ_API_DECL LinearSolverType {LinearSolverType_LU, LinearSolverType_QR};

template <LinearSolverType solver_type = LinearSolverType_LU>
class LinearSolver;

template<>
class LinearSolver<LinearSolverType_LU>
{
public:

    /// solve system of linear equations Ax = y using boost::ublas;
    /// note: extra copying inefficiencies for ease of client use
    template<typename matrix_type, typename vector_type>
    vector_type solve(const matrix_type& A,
                      const vector_type& y)
    {
        namespace ublas = boost::numeric::ublas;

        matrix_type A_factorized = A;
        ublas::permutation_matrix<size_t> pm(y.size());

        int singular = lu_factorize(A_factorized, pm);
        if (singular) throw std::runtime_error("[LinearSolver<LU>::solve()] A is singular.");

        vector_type result(y);
        lu_substitute(A_factorized, pm, result);

        return result;
    }
};

template<>
class LinearSolver<LinearSolverType_QR>
{
public:

    /// solve system of linear equations Ax = y using boost::ublas;
    /// note: extra copying inefficiencies for ease of client use
    template<typename matrix_type, typename vector_type>
    vector_type solve(const matrix_type& A, const vector_type& y)
    {
        typedef typename matrix_type::size_type size_type;
        typedef typename matrix_type::value_type value_type;

        namespace ublas = boost::numeric::ublas;

        matrix_type Q(A.size1(), A.size2()), R(A.size1(), A.size2());

        qr (A, Q, R);

        vector_type b = prod(trans(Q), y);

        vector_type result;
        if (R.size1() > R.size2())
        {
            size_type min = (R.size1() < R.size2() ? R.size1() : R.size2());

            result = ublas::solve(subrange(R, 0, min, 0, min),
                                  subrange(b, 0, min),
                                  ublas::upper_tag());
        }
        else
        {
            result = ublas::solve(R, b, ublas::upper_tag());
        }
        return result;
    }
};

} // namespace math
} // namespace pwiz

#endif // _LINEARSOLVER_HPP_


//Libraries
#include <Eigen/Dense>
#include "MathFuncs.h"
#include "AttitudeControl.h"
#include "QuatRotEuler.h"
#include <stdio.h>		/* printf */
#include <stdlib.h>     /* system, NULL, EXIT_FAILURE */
#include <math.h>		/* pow, sqrt */
using Eigen::Matrix;
// Constant definitions
#define PI 3.14159265
#define K_ROLL  5			//Control gain for roll motion
#define K_PITCH 5			//Control gain for pitch motion
#define K_YAW   0			//Control gain for yaw motion
#define K_WX    2			//Control gain for angular velocity about x
#define K_WY    2			//Control gain for angular velocity about y
#define K_WZ    1			//Control gain for angular velocity about z
#define KT_a	1.09862e-6	//Thrust coefficient for the quadrotor's propellers in function of PWM squared
#define KT_b	1.05000e-3	//Thrust coefficient for the quadrotor's propellers in function of PWM
#define KT_bSQR pow(KT_b,2)
#define RAD     0.12		//Radius of quadcopter (distance between center and farther tip of propeller)
#define SIN_45  0.7071067
#define COS_45  0.7071067
#define Ctm     9.22e-3

//Functions

//Attitude error: e_r = 0.5*invSkew(Rref'*Rbw - Rbw'*Rref)
Matrix<float, 3, 1> AttitudeErrorVector(Matrix<float, 3, 3> Rbw, Matrix<float, 3, 3> Rdes){
	Matrix<float, 3, 3> M_aux1 = Rdes.transpose()*Rbw; //M_aux1 = Rdes'*Rbw
	Matrix<float, 3, 3> M_aux2 = Rbw.transpose()*Rdes; //M_aux2 = Rbw'*Rdes
	Matrix<float, 3, 1> V_aux = invSkew(M_aux1-M_aux2);		//V_aux = invSkew(M_aux1 - M_aux2)

	return V_aux*-0.5; //0.5*V_aux
}

//Attitude control inputs: u_att = -Kr*e_r - Kw*e_w
Matrix<float, 3, 1> AttitudeControlInputs(Matrix<float, 3, 3> Kr, Matrix<float, 3, 3> Kw, Matrix<float, 3, 1> e_r, Matrix<float, 3, 1> e_w){
	Matrix<float, 3, 1> V_aux1 = Kr*e_r*(-1); //V_aux1 = -Kr*e_r
	Matrix<float, 3, 1> V_aux2 = Kw*e_w*(-1); //V_aux2 = -Kw*e_w
	//PrintVec3(V_aux1, "Proportional");
	//PrintVec3(V_aux2, "Derivative");

	//return V_aux2;

	return V_aux1+V_aux2; //V_aux1 + V_aux2
}

/*Return inputs to quadrotor for attitude control
q[4];		//Attitude quaternion
Rdes;		//Desired rotation matrix
w_bw[3];	//Angular velocity of the system
wDes[3];	//Desired angular velocity
u1;			//Thrust
This algorithm was extracted from Mellinger, 2011: Minimum Snap Trajectory Generation and Control for Quadrotors*/
Matrix<float, 4, 1> attitudeControl(Matrix<float, 4, 1> q, Matrix<float, 3, 3> Rdes, Matrix<float, 3, 1> w_bw, Matrix<float, 3, 1> wDes, float u1){

	//Controller gain matrices
	Matrix<float, 3, 1> K_eVec, K_wVec, e_r, e_w, u_att;
	Matrix<float, 4, 1> u;
	Matrix<float, 3, 3> K_r, K_w, Rbw;

	K_eVec << K_ROLL,
			  K_PITCH,
			  K_YAW;	//Attitude error gains
	K_wVec << K_WX,
			  K_WY,
			  K_WZ;			//Angular velocity gains

	K_r = K_eVec.asDiagonal();
	K_w = K_wVec.asDiagonal();

	//Get rotation matrix from quaternion
	Rbw = Quat2rot(q); //Matrix obtained in the NED parameterization
        //PrintMat3x3(Rbw);
	//PrintMat3x3(Rdes);

	//Attitude error: e_r = 0.5*invSkew(Rref'*Rbw - Rbw'*Rref)
	e_r = AttitudeErrorVector(Rbw, Rdes);
	//PrintVec3(e_r, "ErrorAtt");

	//Angular velocity error: e_w = w_bw - wDes
	e_w = w_bw-wDes;

	//Attitude control input: u_att = -K_r*e_r - K_w*e_w
	u_att = AttitudeControlInputs(K_r, K_w, e_r, e_w);

	//Saturate the attitude torques to the system so that they are not too high
	u_att << saturate(u_att(0), -20, 20),
			 saturate(u_att(1), -20, 20),
			 saturate(u_att(2), -20, 20);

	//Output variable (the negatives below are due to the fact of using NED coordinates instead of NWU)

	u <<       u1,
		 u_att(0),
		 u_att(1),
		 u_att(2);

	return u;
}

/* Convert inputs u = [Thrust, torque_roll, torque_pitch, torque_yaw] into pwm values (T coordinates)
T coordinates assumes: u = [KT    KT    KT   KT   ] [pwm1^2]
                            0     -KT*L 0    KT*L ] [pwm2^2]
                            -KT*L 0     KT*L 0    ] [pwm3^2]
                            KM    -KM   KM   -KM  ] [pwm4^2]
KT = thrust coefficient for the propellers
KM = moment coefficient for the propellers */
// Matrix<float, 4, 1> u2pwmTshape(Matrix<float, 4, 1> u){
// 	//u = M.pwm^2 ==> pwm^2 = inv(M)*u

// 	Matrix<float, 4, 4> inv_M;
// 	Matrix<float, 4, 1> pwm_squared, pwm;

// 	inv_M << 1 / (4 * KT), 0, -1 / (2 * KT * RAD), 1 / (4 * KM),
// 			 1 / (4 * KT), -1 / (2 * KT * RAD), 0, -1 / (4 * KM),
// 			 1 / (4 * KT), 0, 1 / (2 * KT * RAD), 1 / (4 * KM),
// 			 1 / (4 * KT), 1 / (2 * KT * RAD), 0, -1 / (4 * KM);

// 	pwm_squared = inv_M*u;

// 	//Saturate pwm values before taking square root to avoid square root of negative numbers
// 	float mean_pwmSq = u(0) / (4 * KT);
// 	pwm_squared << saturate(pwm_squared(0), 0, 2 * mean_pwmSq),
// 				   saturate(pwm_squared(1), 0, 2 * mean_pwmSq),
// 				   saturate(pwm_squared(2), 0, 2 * mean_pwmSq),
// 				   saturate(pwm_squared(3), 0, 2 * mean_pwmSq);

// 	//Assign outputs (note that Mikicopter assign minimum value at 1000
// 	pwm << sqrt(pwm_squared(0))/1000,
// 		   sqrt(pwm_squared(1))/1000,
// 		   sqrt(pwm_squared(2))/1000,
// 		   sqrt(pwm_squared(3))/1000;
// // PrintVec4(pwm,"pwm");
// 	return pwm;
// }

Matrix<float, 4, 1> Thrusts2PWM(Matrix<float, 4, 1> thrusts){

	Matrix<float, 4, 1> pwm;
	float val;
	// float KT_bSqr = pow(KT_b,2);

	//Solution to quadratic equation KT_a.x^2 + KT_b.x - thrust = 0
	for (int i = 0; i < 4; i++){
		val = (-KT_b+sqrt(KT_bSQR + 4*thrusts(i)*KT_a))/(2*KT_a);
		pwm(i) = saturate(val,0,1000);
	}

	// pwm << sqrt(thrusts(0) / KT_a)/1000,
	// 	   sqrt(thrusts(1) / KT_a)/1000,
	// 	   sqrt(thrusts(2) / KT_a)/1000,
	// 	   sqrt(thrusts(3) / KT_a)/1000;

	//Assign outputs in range 0-1 (we calibrated KTs using 0-1000 range)
	return pwm*0.001;
}

/* Convert inputs u = [Thrust, torque_roll, torque_pitch, torque_yaw] into pwm values (T coordinates)
T coordinates assumes: u = [1    1   1    1  ] [PWM1(T1)]
						   [0   -L   0    L  ] [PWM2(T2)]
						   [-L   0   L    0  ] [PWM3(T3)]
						   [Ctm -Ctm Ctm -Ctm] [PWM4(T4)]
Ctm = ratio between Moment and Thrust per propeller */
Matrix<float, 4, 1> u2pwmTshape(Matrix<float, 4, 1> u){
	//u = M.pwm^2 ==> pwm^2 = inv(M)*u

	Matrix<float, 4, 4> inv_M;
	Matrix<float, 4, 1> thrusts;

	inv_M << 1 / 4.0, 0,             -1 / (2 * RAD), 1 / (4 * Ctm),
			 1 / 4.0, -1 / (2 * RAD), 0,            -1 / (4 * Ctm),
			 1 / 4.0, 0,              1 / (2 * RAD), 1 / (4 * Ctm),
			 1 / 4.0, 1 / (2 * RAD),  0,            -1 / (4 * Ctm);

	thrusts = inv_M*u;

	//Saturate pwm values before taking square root to avoid square root of negative numbers
	float mean_thrusts = u(0) / 4.0;
	thrusts <<  saturate(thrusts(0), 0, 2 * mean_thrusts),
			    saturate(thrusts(1), 0, 2 * mean_thrusts),
				saturate(thrusts(2), 0, 2 * mean_thrusts),
				saturate(thrusts(3), 0, 2 * mean_thrusts);

	return Thrusts2PWM(thrusts);
}

/* Convert inputs u = [Thrust, torque_roll, torque_pitch, torque_yaw] into pwm values (X coordinates)
X coordinates assumes: u =[1    0      0       0 ] [1    1   1    1  ] [PWM1(T1)]
						  [0    cos(o) -sin(o) 0 ] [0   -L   0    L  ] [PWM2(T2)]
						  [0    sin(o) cos(o)  0 ] [-L   0   L    0  ] [PWM3(T3)]
						  [0    0      0       1 ] [Ctm -Ctm Ctm -Ctm] [PWM4(T4)]
Ctm = ratio between Moment and Thrust per propeller */
Matrix<float, 4, 1> u2pwmXshape(Matrix<float, 4, 1> u){
	// double angle = PI / 4;	//Angle between T coordinates and X coordinates
	Matrix<float, 4, 4> Rot;				//Rotation matrix (note that the inverse of a rotation matrix is its transpose)
	//u.v[3] = 0;
	// Rot(.M[0][0]) = 1; Rot.M[0][1] = 0;           Rot.M[0][2] = 0;          Rot.M[0][3] = 0;
	// Rot.M[1][0] = 0; Rot.M[1][1] = cos(angle);  Rot.M[1][2] = sin(angle); Rot.M[1][3] = 0;
	// Rot.M[2][0] = 0; Rot.M[2][1] = -sin(angle); Rot.M[2][2] = cos(angle); Rot.M[2][3] = 0;
	// Rot.M[3][0] = 0; Rot.M[3][1] = 0;           Rot.M[3][2] = 0;          Rot.M[3][3] = 1;

	Rot << 1,       0,      0, 0,
		   0,  COS_45, SIN_45, 0,
		   0, -SIN_45, COS_45, 0,
		   0,       0,      0, 1;
	//PrintVec4(u,"u");

	//Multiply inverse of rotation matrix by input u
	Matrix<float, 4, 1> uX = Rot*u;
	//PrintMat4x4(Rot);
	//PrintVec4(uX,"uX");
	// u2pwmTshape2(uX);
	return u2pwmTshape(uX);
}


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <sophus/se3.hpp>

using namespace std;
using namespace cv;

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

// Functions to Change the pixel points into camera normalization points
Point2d pixel2cam(const Point2d &p, const Mat &K);

// using SVD for getting the ICP problem
void solveIcp(const vector<Point3f> & pts1,const vector<Point3f> & pts2, Mat & R, Mat &t );

int main(int argc, char const *argv[])
{
 if (argc != 5) {
    cout << "usage: pose_estimation_3d3d img1 img2 depth1 depth2" << endl;
    return 1;
  }
 // ORB-BRIEF
  Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
  Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);
// Keypoints and corresponding pairs
  std::vector<KeyPoint> keypoints_01;
  std::vector<KeyPoint> keypoints_02;
  std::vector<DMatch> matches;

  find_feature_matches(img_1,img_2, keypoints_01,keypoints_02,matches);
  cout<<"There is totally "<< matches.size()<<" matches found"<<endl;

    // 建立2D点对应的3D点
  Mat depth1 = imread(argv[3], CV_LOAD_IMAGE_UNCHANGED);       // 深度图为16位无符号数，单通道图像
  Mat depth2 = imread(argv[4], CV_LOAD_IMAGE_UNCHANGED);       // 深度图为16位无符号数，单通道图像
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1); // Camera 内参
  vector<Point3f> pts1, pts2;

 for (DMatch m:matches) {
     //找到 Pairs对应的3D点坐标 depth
    ushort d1 = depth1.ptr<unsigned short>(int(keypoints_01[m.queryIdx].pt.y))[int(keypoints_01[m.queryIdx].pt.x)];
    ushort d2 = depth2.ptr<unsigned short>(int(keypoints_02[m.trainIdx].pt.y))[int(keypoints_02[m.trainIdx].pt.x)];
    if (d1 == 0 || d2 == 0)   // bad depth
      continue;
    Point2d p1 = pixel2cam(keypoints_01[m.queryIdx].pt, K);   // Camera X'/Z', Y'/Z'
    Point2d p2 = pixel2cam(keypoints_02[m.trainIdx].pt, K);
    float dd1 = float(d1) / 5000.0;  // 定义一个单位长度 就是定义一个Z'
    float dd2 = float(d2) / 5000.0;
    pts1.push_back(Point3f(p1.x * dd1, p1.y * dd1, dd1));  // 定义一个单位长度 就是定义一个Z'
    pts2.push_back(Point3f(p2.x * dd2, p2.y * dd2, dd2));
  }

  cout << "3d-3d pairs: " << pts1.size() << endl;
   // Solve ICP by SVD
   Mat R; Mat t;
   solveIcp(pts1,pts2,R,t);
   cout<< "R is "<<"\n"<<R<<endl;
   cout<<"t is "<<"\n"<<t<<endl;

// You can Also Using G2O for Bundle Adjustment

    return 0;
}

void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          std::vector<KeyPoint> &keypoints_1,
                          std::vector<KeyPoint> &keypoints_2,
                          std::vector<DMatch> &matches) {
  //-- 初始化
  Mat descriptors_1, descriptors_2;
  // used in OpenCV3
  Ptr<FeatureDetector> detector = ORB::create();
  Ptr<DescriptorExtractor> descriptor = ORB::create();
  // use this if you are in OpenCV2
  // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
  // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
  Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
  //-- 第一步:检测 Oriented FAST 角点位置
  detector->detect(img_1, keypoints_1);
  detector->detect(img_2, keypoints_2);

  //-- 第二步:根据角点位置计算 BRIEF 描述子
  descriptor->compute(img_1, keypoints_1, descriptors_1);
  descriptor->compute(img_2, keypoints_2, descriptors_2);

  //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
  vector<DMatch> match;
  //BFMatcher matcher ( NORM_HAMMING );
  matcher->match(descriptors_1, descriptors_2, match);

  //-- 第四步:匹配点对筛选
  double min_dist = 10000, max_dist = 0;

  //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
  for (int i = 0; i < descriptors_1.rows; i++) {
    double dist = match[i].distance;
    if (dist < min_dist) min_dist = dist;
    if (dist > max_dist) max_dist = dist;
  }
  //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
  for (int i = 0; i < descriptors_1.rows; i++) {
    if (match[i].distance <= max(2 * min_dist, 30.0)) {
      matches.push_back(match[i]);
    }
  }
}

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

void solveIcp(const vector<Point3f> & pts1,const vector<Point3f> & pts2, Mat & R, Mat &t ){
 // 计算质心
 Point3f p1_mean, p2_mean;  // p1 and p2 are pairs
for(int i =0; i<pts1.size();i++){
    p1_mean+= pts1[i];
    p2_mean+= pts2[i];
    }
int nums_points=pts1.size();
p1_mean =  p1_mean / nums_points;
p2_mean = p2_mean / nums_points;
// 得到pts1 和 pts2 的去掉质心的坐标
vector <Point3f> pts1_after(nums_points); // 动态分配内存
vector <Point3f> pts2_after(nums_points); // 动态分配内存

for(int i =0;i<nums_points;i++){
pts1_after[i] = pts1[i] - p1_mean;
pts2_after[i] = pts2[i] - p2_mean;
    }
// Get W matrix
Eigen::Matrix3d W = Eigen::Matrix3d::Zero(); // init with 0
for(int i =0;i<pts1.size();i++){
    W += Eigen::Vector3d(pts1_after[i].x, pts1_after[i].y,pts1_after[i].z) * Eigen::Vector3d(pts2_after[i].x, pts2_after[i].y, pts2_after[i].z).transpose();
 }
 cout<< " W matirx for SVD to Solving the R is "<<W<<endl;

 // 对 W 进行 SVD 分解
  // SVD on W
  Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix3d U = svd.matrixU();
  Eigen::Matrix3d V = svd.matrixV();
  Eigen:: Matrix3d matrix_R = U *(V.transpose());
  Eigen:: Vector3d t_ = Eigen::Vector3d(p1_mean.x,p1_mean.y,p1_mean.z) - matrix_R * Eigen::Vector3d(p2_mean.x,p2_mean.y,p2_mean.z) ;

  R = (Mat_<double>(3,3)<<matrix_R(0,0),matrix_R(0,1),matrix_R(0,2),
  matrix_R(1,0),matrix_R(1,1),matrix_R(1,2),
  matrix_R(2,0),matrix_R(2,1),matrix_R(2,2));
  t =(Mat_<double>(3,1)<<t_(0,0),t_(1,0),t_(2,0));

}


//
// Created by Xiang on 2017/12/19.
//

#include <opencv2/opencv.hpp>
#include <string>
#include <chrono>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace cv;

string file_1 = "../LK1.png";  // first image
string file_2 = "../LK2.png";  // second image

inline double get(const cv::Mat &img, float x, float y) {
    // boundary check
    if (x < 0) x = 0;
    if (y < 0) y = 0;
    if (x >= img.cols - 1) x = img.cols - 2;
    if (y >= img.rows - 1) y = img.rows - 2;

    double xx = x - floor(x);
    double yy = y - floor(y);
    int x_a1 = std::min(img.cols - 1, int(x) + 1);
    int y_a1 = std::min(img.rows - 1, int(y) + 1);

    return (1 - xx) * (1 - yy) * img.at<uchar>(y, x)
    + xx * (1 - yy) * img.at<uchar>(y, x_a1)
    + (1 - xx) * yy * img.at<uchar>(y_a1, x)
    + xx * yy * img.at<uchar>(y_a1, x_a1);
}

void opticalFlowLK_linear(const cv::Mat& img1, const cv::Mat& img2,
                          const std::vector<cv::Point2f>& kps1, std::vector<cv::Point2f>& kps2,
                          std::vector<bool>& success)
{
    kps2 = kps1;
    success.resize(kps1.size(), true);
    for (int iter = 0; iter < 10; ++iter)
    {
        for (int i = 0; i < kps1.size(); ++i)
        {
            auto& kp = kps1[i];
            int half_w_size = 4;

            int idx = 0;
            Eigen::Matrix<double, Eigen::Dynamic, 2> A(static_cast<int>(std::pow(half_w_size*2+1, 2)), 2);
            Eigen::Matrix<double, Eigen::Dynamic, 1> b(static_cast<int>(std::pow(half_w_size*2+1, 2)), 1);

            for (int xx = -half_w_size; xx <= half_w_size; ++xx)
            {
                for (int yy = -half_w_size; yy <= half_w_size; ++yy)
                {
                    double grad_x = 0.5 * (get(img1, kp.x+xx+1, kp.y+yy) - get(img1, kp.x+xx-1, kp.y+yy));
                    double grad_y = 0.5 * (get(img1, kp.x+xx, kp.y+yy+1) - get(img1, kp.x+xx, kp.y+yy-1));
                    double grad_t = get(img2, kps2[i].x+xx, kps2[i].y+yy) - get(img1, kp.x+xx, kp.y+yy);
                    A(idx, 0) = grad_x;
                    A(idx, 1) = grad_y;
                    b(idx, 0) = grad_t;
                    ++idx;
                }
            }
            Eigen::Vector2d uv = -(A.transpose() * A).inverse() * A.transpose() * b;

            if (std::isnan(uv.x()) || std::isnan(uv.y()))
            {
                success[i] = false;
            }

            kps2[i].x += uv.x();
            kps2[i].y += uv.y();
        }
    }
}

void opticalFlowLK_gauss_newton(const cv::Mat& img1, const cv::Mat& img2,
                                const std::vector<cv::Point2f>& kps1, std::vector<cv::Point2f>& kps2,
                                std::vector<bool>& success, bool inverse=false, bool reinit_kps2=true)
{
    if (reinit_kps2)
    {
        std::cout << "re-init pts in img2 with points in img1" << std::endl;
        kps2 = kps1;
    }
    int max_iters = 10;
    success.resize(kps1.size(), true);
    for (int i = 0; i < kps1.size(); ++i)
    {
        Eigen::Matrix2d H = Eigen::Matrix2d::Zero();
        Eigen::Vector2d g = Eigen::Vector2d::Zero();
        double prev_cost = 0.0;
        double cost = 0.0;
        int half_w_size = 4;

        for (int iter = 0; iter < max_iters; ++iter)
        {
            prev_cost = cost;
            cost = 0.0;
            if (!inverse)
            {
                H = Eigen::Matrix2d::Zero();
                g = Eigen::Vector2d::Zero();
            }
            else
            {
                g = Eigen::Vector2d::Zero();
            }

            for (int xx = -half_w_size; xx <= half_w_size; ++xx)
            {
                for (int yy = -half_w_size; yy <= half_w_size; ++yy)
                {
                    double err = get(img1, kps1[i].x+xx, kps1[i].y+yy) - get(img2, kps2[i].x+xx, kps2[i].y+yy);
                    double grad_x, grad_y;
                    if (!inverse)
                    {
                        grad_x = 0.5 * (get(img2, kps2[i].x+xx+1, kps2[i].y+yy) -   get(img2, kps2[i].x+xx-1, kps2[i].y+yy));
                        grad_y = 0.5 * (get(img2, kps2[i].x+xx,   kps2[i].y+yy+1) - get(img2, kps2[i].x+xx,   kps2[i].y+yy-1));
                    }
                    else if (iter == 0) // if inverse mode, j and H are only computed once
                    {
                        grad_x = 0.5 * (get(img1, kps1[i].x+xx+1, kps1[i].y+yy) -   get(img1, kps1[i].x+xx-1, kps1[i].y+yy));
                        grad_y = 0.5 * (get(img1, kps1[i].x+xx,   kps1[i].y+yy+1) - get(img1, kps1[i].x+xx,   kps1[i].y+yy-1));
                    }
                    Eigen::Vector2d J(-grad_x, -grad_y);
                    if (inverse == false || iter == 0)
                    {
                        H += J * J.transpose();
                    }
                    g += -J * err;
                    cost += err * err;
                }
            }

            Eigen::Vector2d uv = H.ldlt().solve(g);
            if (std::isnan(uv.x()) || std::isnan(uv.y()))
            {
                success[i] = false;
                break;
            }

            if (iter > 0 && prev_cost < cost)
                break;

            kps2[i].x += uv.x();
            kps2[i].y += uv.y();

            if (uv.norm() < 1e-2)
                break; // converged
        }
    }

}

void opticalFlowLK_gauss_newton_pyramid(const cv::Mat& img1, const cv::Mat& img2,
                                        const std::vector<cv::Point2f>& kps1, std::vector<cv::Point2f>& kps2,
                                        std::vector<bool>& success, int n_layers)
{
    int factor = 2;
    double scale = 1.0 / std::pow(factor, n_layers-1);
    std::vector<cv::Point2f> kps2_s;
    for (int si = 0; si < n_layers; ++si)
    {
        std::cout << "scale = " << scale << "\n";
        cv::Mat img1_s;
        cv::resize(img1, img1_s, cv::Size(), scale, scale);
        cv::Mat img2_s;
        cv::resize(img2, img2_s, cv::Size(), scale, scale);
        std::vector<cv::Point2f> kps1_s = kps1;
        for (auto& p : kps1_s)
        {
            p.x *= scale;
            p.y *= scale;
        }
        std::cout << "img size = " << img1_s.size() << "\n";
        opticalFlowLK_gauss_newton(img1_s, img2_s, kps1_s, kps2_s, success, true, si==0);

        if (si < n_layers-1)
        {
            for (auto& p : kps2_s)
            {
                p.x *= factor;
                p.y *= factor;
            }
        }

        scale *= factor;
    }
    kps2 = kps2_s;
}

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

    // images, note they are CV_8UC1, not CV_8UC3
    Mat img1 = imread(file_1, 0);
    Mat img2 = imread(file_2, 0);

    // key points, using GFTT here.
    vector<KeyPoint> kp1;
    Ptr<GFTTDetector> detector = GFTTDetector::create(500, 0.01, 20); // maximum 500 keypoints
    detector->detect(img1, kp1);

    // // use opencv's flow for validation
    // vector<Point2f> pt1, pt2;
    // for (auto &kp: kp1) pt1.push_back(kp.pt);
    // vector<uchar> status;
    // vector<float> error;
    // auto t1 = chrono::steady_clock::now();
    // cv::calcOpticalFlowPyrLK(img1, img2, pt1, pt2, status, error);
    // auto t2 = chrono::steady_clock::now();
    // auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    // cout << "optical flow by opencv: " << time_used.count() << endl;

    std::vector<cv::Point2f> pts1(kp1.size()), pts2;
    for (int i = 0; i < kp1.size(); ++i)
        pts1[i] = kp1[i].pt;
    std::vector<uchar> status;
    std::vector<float> errors;
    auto t1 = chrono::steady_clock::now();
    cv::calcOpticalFlowPyrLK(img1, img2, pts1, pts2, status, errors);
    auto t2 = chrono::steady_clock::now();
    auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "optical flow by opencv: " << time_used.count() << endl;

    Mat img2_CV;
    cv::cvtColor(img2, img2_CV, CV_GRAY2BGR);
    for (int i = 0; i < pts2.size(); i++) {
        if (status[i]) {
            cv::circle(img2_CV, pts2[i], 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img2_CV, pts1[i], pts2[i], cv::Scalar(0, 250, 0));
        }
    }
    // cv::imshow("tracked by opencv", img2_CV);
    // cv::waitKey(0);
    cv::imwrite("lk_opencv.png", img2_CV);

    // --- LK with linear solution
    std::vector<cv::Point2f> pts3;
    std::vector<bool> success;
    t1 = chrono::steady_clock::now();
    opticalFlowLK_linear(img1, img2, pts1, pts3, success);
    t2 = chrono::steady_clock::now();
    time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "optical flow by LK linear: " << time_used.count() << endl;

    Mat img2_LK;
    cv::cvtColor(img2, img2_LK, CV_GRAY2BGR);
    for (int i = 0; i < pts3.size(); i++) {
        if (status[i]) {
            cv::circle(img2_LK, pts3[i], 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img2_LK, pts1[i], pts3[i], cv::Scalar(0, 250, 0));
        }
    }
    cv::imwrite("lk_linear.png", img2_LK);

    // --- LK with Gauss-Newton
    std::vector<cv::Point2f> pts4;
    t1 = chrono::steady_clock::now();
    opticalFlowLK_gauss_newton(img1, img2, pts1, pts4, success, false);
    t2 = chrono::steady_clock::now();
    time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "optical flow by LK Gauss-Newton: " << time_used.count() << endl;
    Mat img2_GN;
    cv::cvtColor(img2, img2_GN, CV_GRAY2BGR);
    for (int i = 0; i < pts4.size(); i++) {
        if (status[i]) {
            cv::circle(img2_GN, pts4[i], 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img2_GN, pts1[i], pts4[i], cv::Scalar(0, 250, 0));
        }
    }
    cv::imwrite("lk_gauss-newton.png", img2_GN);

    // --- LK with Gauss-Newton (inverse)
    std::vector<cv::Point2f> pts5;
    t1 = chrono::steady_clock::now();
    opticalFlowLK_gauss_newton(img1, img2, pts1, pts5, success, true);
    t2 = chrono::steady_clock::now();
    time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "optical flow by LK Gauss-Newton inverse: " << time_used.count() << endl;

    Mat img2_GN_inv;
    cv::cvtColor(img2, img2_GN_inv, CV_GRAY2BGR);
    for (int i = 0; i < pts5.size(); i++) {
        if (status[i]) {
            cv::circle(img2_GN_inv, pts5[i], 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img2_GN_inv, pts1[i], pts5[i], cv::Scalar(0, 250, 0));
        }
    }
    cv::imwrite("lk_gauss-newton_inverse.png", img2_GN_inv);

    // --- LK with Pyramidal Gauss-Newton (multi-layers)
    std::vector<cv::Point2f> pts6;
    t1 = chrono::steady_clock::now();
    opticalFlowLK_gauss_newton_pyramid(img1, img2, pts1, pts6, success, 4);
    t2 = chrono::steady_clock::now();
    time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "optical flow by LK Pyramidal Gauss-Newton: " << time_used.count() << endl;

    Mat img2_GN_Py;
    cv::cvtColor(img2, img2_GN_Py, CV_GRAY2BGR);
    for (int i = 0; i < pts6.size(); i++) {
        if (status[i]) {
            cv::circle(img2_GN_Py, pts6[i], 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img2_GN_Py, pts1[i], pts6[i], cv::Scalar(0, 250, 0));
        }
    }
    cv::imwrite("lk_gauss-newton_pyramidal.png", img2_GN_Py);

    return 0;
}


#ifndef CPPMATH_MATRIX_PSEUDOINVERSESVD_IMPL_HPP_
#define CPPMATH_MATRIX_PSEUDOINVERSESVD_IMPL_HPP_

#include <Eigen/Core>

#include "PseudoInverseSVD.hpp"

namespace cppmath
{
    template< typename T >
    PseudoInverseSVD< T >::PseudoInverseSVD( const T& matrix, float threshold ) :
                    Eigen::JacobiSVD< T >( matrix, Eigen::ComputeThinU | Eigen::ComputeThinV ), m_threshold( threshold )
    {
        m_hasInverse = false;
    }

    template< typename T >
    PseudoInverseSVD< T >::~PseudoInverseSVD< T >()
    {
    }

    template< typename T >
    const T& PseudoInverseSVD< T >::compute()
    {
        if( !m_hasInverse )
        {
            compute( &m_inverse );
            m_hasInverse = true;
        }
        return m_inverse;
    }

    template< typename T >
    void PseudoInverseSVD< T >::compute( T* const pinvmat ) const
    {
        if( m_hasInverse )
        {
            *pinvmat = m_inverse;
        }
        else
        {

            eigen_assert( Eigen::JacobiSVD< T >::m_isInitialized && "SVD is not initialized." );
            typename Eigen::JacobiSVD< T >::SingularValuesType singularValues_inv = Eigen::JacobiSVD< T >::m_singularValues;
            for( long i = 0; i < Eigen::JacobiSVD< T >::m_workMatrix.cols(); ++i )
            {
                if( Eigen::JacobiSVD< T >::m_singularValues( i ) > m_threshold )
                    singularValues_inv( i ) = 1.0 / Eigen::JacobiSVD< T >::m_singularValues( i );
                else
                    singularValues_inv( i ) = 0;
            }
            *pinvmat = ( Eigen::JacobiSVD< T >::m_matrixV * singularValues_inv.asDiagonal()
                            * Eigen::JacobiSVD< T >::m_matrixU.transpose() );
        }
    }

    template< typename T >
    T PseudoInverseSVD< T >::operator*( const T& m )
    {
        if( !m_hasInverse )
        {
            compute( &m_inverse );
            m_hasInverse = true;
        }
        return m_inverse * m;
    }

    template< typename T >
    T PseudoInverseSVD< T >::operator*( const T& m ) const
    {
        if( m_hasInverse )
        {
            m_inverse * m;
        }
        else
        {
            T inv;
            compute( &inv );
            return inv * m;
        }
    }
} /* namespace cppmath */

#endif  // CPPMATH_MATRIX_PSEUDOINVERSESVD_IMPL_HPP_


//
// Copyright 2019 Olzhas Zhumabek <anonymous.from.applecity@gmail.com>
//
// 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_GIL_IMAGE_PROCESSING_HARRIS_HPP
#define BOOST_GIL_IMAGE_PROCESSING_HARRIS_HPP

#include <boost/gil/image_view.hpp>
#include <boost/gil/typedefs.hpp>
#include <boost/gil/extension/numeric/kernel.hpp>

namespace boost { namespace gil {
/// \defgroup CornerDetectionAlgorithms
/// \brief Algorithms that are used to find corners in an image
///
/// These algorithms are used to find spots from which
/// sliding the window will produce large intensity change

/// \brief function to record Harris responses
/// \ingroup CornerDetectionAlgorithms
///
/// This algorithm computes Harris responses
/// for structure tensor represented by m11, m12_21, m22 views.
/// Note that m12_21 represents both entries (1, 2) and (2, 1).
/// Window length represents size of a window which is slided around
/// to compute sum of corresponding entries. k is a discrimination
/// constant against edges (usually in range 0.04 to 0.06).
/// harris_response is an out parameter that will contain the Harris responses.
template <typename T, typename Allocator>
void compute_harris_responses(
    boost::gil::gray32f_view_t m11,
    boost::gil::gray32f_view_t m12_21,
    boost::gil::gray32f_view_t m22,
    boost::gil::detail::kernel_2d<T, Allocator> weights,
    float k,
    boost::gil::gray32f_view_t harris_response)
{
    if (m11.dimensions() != m12_21.dimensions() || m12_21.dimensions() != m22.dimensions()) {
        throw std::invalid_argument("m prefixed arguments must represent"
            " tensor from the same image");
    }

    std::ptrdiff_t const window_length = weights.size();
    auto const width = m11.width();
    auto const height = m11.height();
    auto const half_length = window_length / 2;

    for (auto y = half_length; y < height - half_length; ++y)
    {
        for (auto x = half_length; x < width - half_length; ++x)
        {
            float ddxx = 0;
            float dxdy = 0;
            float ddyy = 0;
            for (gil::gray32f_view_t::coord_t y_kernel = 0;
                y_kernel < window_length;
                ++y_kernel) {
                for (gil::gray32f_view_t::coord_t x_kernel = 0;
                    x_kernel < window_length;
                    ++x_kernel) {
                    ddxx += m11(x + x_kernel - half_length, y + y_kernel - half_length)
                        .at(std::integral_constant<int, 0>{}) * weights.at(x_kernel, y_kernel);
                    dxdy += m12_21(x + x_kernel - half_length, y + y_kernel - half_length)
                        .at(std::integral_constant<int, 0>{}) * weights.at(x_kernel, y_kernel);
                    ddyy += m22(x + x_kernel - half_length, y + y_kernel - half_length)
                        .at(std::integral_constant<int, 0>{}) * weights.at(x_kernel, y_kernel);
                }
            }
            auto det = (ddxx * ddyy) - dxdy * dxdy;
            auto trace = ddxx + ddyy;
            auto harris_value = det - k * trace * trace;
            harris_response(x, y).at(std::integral_constant<int, 0>{}) = harris_value;
        }
    }
}

}} //namespace boost::gil
#endif


/*
 * $Revision: 565 $ $Date: 2011-02-15 16:00:43 -0800 (Tue, 15 Feb 2011) $
 *
 * Copyright by Astos Solutions GmbH, Germany
 *
 * this file is published under the Astos Solutions Free Public License
 * For details on copyright and terms of use see
 * http://www.astos.de/Astos_Solutions_Free_Public_License.html
 */

#include "GeneralEllipse.h"
#include <Eigen/QR>
#include <cmath>

using namespace vesta;
using namespace Eigen;
using namespace std;

/** Compute the principal semi-axes of the ellipsoid. The axes
  * are the rows of the returned matrix. Note that there is no
  * ordering of semi-major or semi-minor axes.
  */
Matrix<double, 3, 2>
GeneralEllipse::principalSemiAxes() const
{
    Matrix2d S;
    double s00 = m_generatingVectors.col(0).dot(m_generatingVectors.col(0));
    double s01 = m_generatingVectors.col(0).dot(m_generatingVectors.col(1));
    double s11 = m_generatingVectors.col(1).dot(m_generatingVectors.col(1));
    double s10 = s01;
    S << s00, s01, s10, s11;

    SelfAdjointEigenSolver<Matrix2d> solver(S, true);
    Vector2d e = solver.eigenvalues();
    Matrix2d ev = solver.eigenvectors();

    Matrix<double, 3, 2> result;
    result.col(0) = ev(0, 0) * m_generatingVectors.col(0) + ev(1, 0) * m_generatingVectors.col(1);
    result.col(1) = ev(0, 1) * m_generatingVectors.col(0) + ev(1, 1) * m_generatingVectors.col(1);

    return result;
}


#include "luminosityHadronic.h"
#include <gsl/gsl_sf_bessel.h>
#include <fparameters/parameters.h>
#include <fmath/RungeKutta.h>
#include <nrMath/integrators.h>
#include <fmath/interpolation.h>
#include <flosses/crossSectionInel.h>
#include <fmath/physics.h>
#include <algorithm>

#include <gsl/gsl_math.h>

#include <boost/math/special_functions/bessel.hpp>

double fntHadron(double x, const Particle& p, const double density, const SpaceCoord& psc) //funcion a integrar   x=Ecreator; L=L(Ega)
{
	double Kpi = 0.17;
	double eval = p.mass*cLight2+x/Kpi;

	//double Ekin = Ep/Kpi;

	double distCreator = 0.0;
	if (eval < p.emax() && eval > p.emin()) {
		distCreator = p.distribution.interpolate({ { 0, eval } }, &psc);
	}

	//double thr = 0.0016; //1GeV
	//double sigma = 30e-27*(0.95+0.06*log(Ekin/thr));

	double Eth = 1.22e9 * EV_TO_ERG;
	double l = log10((protonMass*cLight2+x/Kpi)/1.6); //evaluada en eval
	double sigma = 1.e-27 * (34.3+1.88*l+0.25*l*l) * P2(1.0 - pow(Eth/eval,4));
	double pionEmiss = cLight*density*sigma*distCreator/Kpi;  //sigma = crossSectionHadronicDelta(Ekin)
															  //lo saco asi pongo la condicion Ekin > Ethr en el limite de la int

	double result = pionEmiss/sqrt(P2(x)-P2(chargedPionMass*cLight2));
	return (x > chargedPionMass*cLight2 ? result : 0.0);
}

double luminosityNTHadronic(double E, const Particle& creator,
	const double density, const SpaceCoord& psc)
{
	double Kpi = 0.17;
	double thr = 0.0016; //1GeV

	double Max  = creator.emax();   //esto es un infinito
	double Min  = std::max(E+P2(chargedPionMass*cLight2)/(4*E),thr*Kpi); //== Ekin > Ethr
	Min = E+0.25*P2(chargedPionMass*cLight2)/E;
	double integral = integSimpsonLog(Min, Max,[&](double x)
				{
					return fntHadron(x,creator,density,psc);
				},100);
	//double integral = RungeKuttaSimple(Min, Max,
	//	[&](double x) {return fntHadron(x, creator, density, psc); }
	//);    //integra entre Emin y Emax

	double luminosity = 2.0*integral*P2(E); // [erg s^-1 cm^-3 ]

	return luminosity;
}

double fntHadronTh(double x, const double temp, const double density, const SpaceCoord& psc) //funcion a integrar   x=Ecreator; L=L(Ega)
{
	double Kpi = 0.17;
	double eval = protonMass*cLight2+x/Kpi;

	//double Ekin = Ep/Kpi;
	double g = eval / (protonMass*cLight2);
	double beta = sqrt(1.0-1.0/(g*g));
	double theta = boltzmann*temp/(protonMass*cLight2);
	double bessel = gsl_sf_bessel_Kn(2, 1.0/theta);
	double distCreator = (bessel > 0.0 ? density * g*g*beta / (theta*bessel) * exp(-g/theta) / (protonMass*cLight2) : 0.0);

	//double thr = 0.0016; //1GeV
	//double sigma = 30e-27*(0.95+0.06*log(Ekin/thr));

	double Eth = 1.22e9 * EV_TO_ERG;
	double l = log10((protonMass*cLight2+x/Kpi)/1.6); //evaluada en eval
	double sigma = 1.e-27 * (34.3+1.88*l+0.25*l*l) * P2(1.0 - pow(Eth/eval,4));
	double pionEmiss = cLight*density*sigma*distCreator/Kpi;  //sigma = crossSectionHadronicDelta(Ekin)
															  //lo saco asi pongo la condicion Ekin > Ethr en el limite de la int

	double result = pionEmiss/sqrt(P2(x)-P2(chargedPionMass*cLight2));
	return (x > chargedPionMass*cLight2 ? result : 0.0);
}

double luminosityThHadronic(double E, const double temp,
	const double density, const SpaceCoord& psc)
{
	double Kpi = 0.17;
	double thr = 0.0016; //1GeV

	double Max  = pow(10,1.5)*protonMass*cLight2;   //esto es un infinito
	double Min  = std::max(E+P2(chargedPionMass*cLight2)/(4*E),thr*Kpi); //== Ekin > Ethr
	Min = E+0.25*P2(chargedPionMass*cLight2)/E;
	double integral = (Min < Max ? integSimpsonLog(Min, Max,[&](double x)
				{
					return fntHadronTh(x,temp, density,psc);
				},100) : 0.0);
	//double integral = RungeKuttaSimple(Min, Max,
	//	[&](double x) {return fntHadron(x, creator, density, psc); }
	//);    //integra entre Emin y Emax

	double luminosity = 2.0*integral*P2(E); // [erg s^-1 cm^-3 ]

	return 2.0*luminosity;
}

/*
class luminosityHadronic2;

double heaviside(double x,double a,double b)
{
	return (a <= x && x <= b ? 1.0 : 0.0);
}

double auxf3(double dGeV, void *params)
{
	struct four_d_params *p = (struct four_d_params *) params;
	double sGeV = p->p1;
	double gx = p->p2;
	double GammaGeV = p->p3;
	double isoGeV = p->p4;

	double pGeV = protonMass*cLight2/1.6e-3;
	double piGeV = neutralPionMass*cLight2/1.6e-3;
	double gd = (sGeV + dGeV*dGeV - pGeV*pGeV)/(2.0*dGeV*sqrt(sGeV));
	double betad = sqrt(1.0-1.0/(gd*gd));
	double epi = (dGeV*dGeV+piGeV*piGeV-pGeV*pGeV)/(2.0*dGeV);
	double ppi = sqrt(epi*epi-piGeV*piGeV);
	double aux1 = 0.5/(betad*gd*ppi);   // REVISAR ESTE p_pi
	double aux2 = P2(dGeV-isoGeV)+GammaGeV*GammaGeV;
	double h = heaviside(gx*piGeV,gd*(epi-betad*ppi),gd*(epi+betad*ppi));

	return aux1*h/aux2;    // [GeV^-3]

}

double inclusiveSigma(double sGeV)
{
	double pGeV = protonMass*cLight2/1.6e-3;
	double piGeV = neutralPionMass*cLight2/1.6e-3;
	double eta=sqrt(P2(sGeV-P2(piGeV)-P2(2.0*pGeV))-4.0*P2(piGeV*2.0*pGeV))/(2.0*piGeV*sqrt(sGeV));
	double pthr = 0.78; //[GeV]
	double sigma=0.0;;

	double p = sqrt(P2(0.5*sGeV/pGeV-pGeV)-P2(pGeV));

	if (p >= pthr && p <= 0.96) {
		sigma = 0.032*eta*eta+0.04*pow(eta,6)+0.047*pow(eta,8);
	} else if (p > 0.96 && p <= 1.27) {
		sigma = 32.6*pow(p-0.8,3.21);
	} else if (p > 1.27 && p <= 8.0) {
		sigma = 5.4*pow(p-0.8,0.81);
	} else if (p > 8.0) {
		sigma = 32.0 * log(p) + 48.5 / sqrt(p) - 59.5;
	}
	return 1.0e-27 * sigma;     // [cm^2]
}

double dsigma(double gx, double gr, double sGeV)
{
	double error;
	int status;

	double GammaGeV = 0.0575;
	double pGeV = protonMass*cLight2/1.6e-3;
	double piGeV = neutralPionMass * cLight2 / 1.6e-3;
	double isoGeV = 1.236;
	double atan1 = atan((sqrt(sGeV)-pGeV-isoGeV)/GammaGeV);
	double atan2 = atan((pGeV+piGeV-isoGeV)/GammaGeV);
	double aux1 = GammaGeV/(atan1-atan2);

	double Min = pGeV+piGeV;
	double Max = sqrt(sGeV)-pGeV;

	struct four_d_params auxf3_params = {sGeV,gx,GammaGeV,isoGeV};
	gsl_function gsl_auxf3;
		gsl_auxf3.function = &auxf3;
		gsl_auxf3.params = &auxf3_params;

	double integ = integrator_qags(&gsl_auxf3,Min,Max,0,1.0e-2,100,&error,&status);

	return inclusiveSigma(sGeV)*aux1*integ*piGeV;  // [cm^2]
}

double auxf2(double gx, void *params)
{
	struct four_d_params *p = (struct four_d_params *) params;
	double gr = p->p1;
	double epi = p->p2;
	double normtemp = p->p3;
	double sGeV = p->p4;

	double piGeV = neutralPionMass*cLight2/1.6e-3;
	double g = epi/piGeV;
	double beta = sqrt(1.0-1.0/(g*g));
	double betax = sqrt(1.0-1.0/(gx*gx));
	double q = sqrt(2.0*(gr+1.0))/normtemp;

	double f1 = exp(-q*g*gx*(1.0-beta*betax))-exp(-q*g*gx*(1.0+beta*betax));

	return f1/(betax*gx) * dsigma(gx,gr,sGeV);
}

double auxf(double gr, void *params)
{
	double error;
	int status;

	struct two_d_params *p = (struct two_d_params *) params;
	double epi = p->p1;
	double normtemp = p->p2;

	double piGeV = neutralPionMass*cLight2 / 1.6e-3;
	double pGeV = protonMass*cLight2/1.6e-3;
	double sGeV = 2.0*P2(pGeV)*(gr+1.0);
	double ji = (sGeV-4.0*P2(pGeV)+P2(piGeV))/(2.0*sqrt(sGeV));
	double Max = ji/piGeV;

	struct four_d_params auxf2_params = {gr,epi,normtemp,sGeV};
	gsl_function gsl_auxf2;
		gsl_auxf2.function = &auxf2;
		gsl_auxf2.params = &auxf2_params;

	double integral = integrator_qags(&gsl_auxf2,1.0,Max,0,1.0e-2,100,&error,&status);
	double result = (gr*gr-1.0) / sqrt(2.0*(gr+1.0)) * integral;
}

double fPion(double epi, double density, double temp)
{
	double error;
	int status;

	double normtemp = boltzmann* temp / (protonMass*cLight2);
	struct two_d_params auxf_params = {epi,normtemp};
	gsl_function gsl_auxf;
		gsl_auxf.function = &auxf;
		gsl_auxf.params = &auxf_params;

	double k2 = boost::math::cyl_bessel_k(2, 1.0/normtemp);
	double piGeV = neutralPionMass*cLight2/1.6e-3;
	double constant = cLight*density*density/(4.0*piGeV*normtemp*k2*k2);

	return constant * integrator_qags(&gsl_auxf,1.0,1.0e3,0,1.0e-2,100,&error,&status);
}

double fHadron(double epi, void *params)
{
	struct two_d_params *p = (struct two_d_params *) params;
	double density = p->p1;
	double temp= p->p2;

	double piGeV = neutralPionMass*cLight2/1.6e-3;
	double qpi = fPion(epi,density,temp);
	return qpi / sqrt(P2(epi)-P2(piGeV));   // [cm-3 s-1 GeV-2]
}

double luminosityHadronic2(double E, const double density, double temp)
{
	double error;
	int status;

	struct two_d_params fHadron_params = {density,temp};
	gsl_function gsl_fHadron;
		gsl_fHadron.function = &fHadron;
		gsl_fHadron.params = &fHadron_params;

	double Kpi = 0.17;
	double thr = 0.0016; //1GeV

	double Min  = cHadron(E);
	Min = Min / 1.6e-3;
	double Max = 1.0e3;  // [en GeV]

	double integral = 2.0 * integrator_qags(&gsl_fHadron,Min,Max,0,1.0e-2,100,&error,&status);

	double jpp = integral * E*planck * 0.25/pi; // [erg s^-1 Hz^-1 cm^-3]
	jpp = jpp / (1.6e-3);
	return jpp;
}*/


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_CSCD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_CSCD_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object returns the cosecant in degree: \f$1/\sin(180/(\pi x))\f$.

    @par Header <boost/simd/function/cscd.hpp>

    @par Note

     As most other trigonometric function cscd can be called with a
     second optional parameter  which is a tag on speed and accuracy
     (see @ref cos for further details)

    @see csc, cscpi,

    @par Example:

      @snippet cscd.cpp cscd

    @par Possible output:

      @snippet cscd.txt cscd

  **/
  IEEEValue cscd(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/cscd.hpp>
#include <boost/simd/function/simd/cscd.hpp>

#endif