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

/*
 Copyright (C) 2015 Andres Hernandez

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

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

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file levyflightdistribution.hpp
    \brief Levy Flight, aka Pareto Type I, distribution as needed by Boost Random
*/

#ifndef quantlib_levy_flight_distribution_hpp
#define quantlib_levy_flight_distribution_hpp

#include <ql/types.hpp>
#include <ql/errors.hpp>
#include <boost/config/no_tr1/cmath.hpp>
#include <boost/random/detail/config.hpp>
#include <boost/random/detail/operators.hpp>
#include <boost/random/uniform_01.hpp>
#include <iosfwd>

namespace QuantLib {

    //! Levy Flight distribution as needed by Boost Random
    /*! The levy flight distribution is a random distribution with
        the following form:
        \f[
        p(x) = \frac{\alpha x_m^{\alpha}}{x^{\alpha+1}}
        \f]
        with support over \f$ x \in [x_m, \infty) \f$
        and the parameter \f$ \alpha > 0 \f$.

        Levy Flight is normally defined as \f$ x_m = 1 \f$ and \f$ 0 <
        \alpha < 2 \f$, which is where \f$ p(x) \f$ has an infinite
        variance. However, the more general version, known as Pareto
        Type I, is well defined for \f$ \alpha > 2 \f$, so the current
        implementation does not restrict \f$ \alpha \f$ to be smaller
        than 2.
    */
    class LevyFlightDistribution
    {
      public:
        typedef Real input_type;
        typedef Real result_type;

        class param_type
        {
          public:

            typedef LevyFlightDistribution distribution_type;

            /*!    Constructs parameters with a given xm and alpha
                Requires: alpha > 0
            */
            param_type(Real xm = 1.0, Real alpha = 1.0)
              : xm_(xm), alpha_(alpha) { QL_REQUIRE(alpha_ > 0.0, "alpha must be larger than 0"); }

            //! Returns the xm parameter of the distribution
            Real xm() const { return xm_; }

            //! Returns the alpha parameter of the distribution
            Real alpha() const { return alpha_; }

            //! Writes the parameters to a @c std::ostream
            BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, param_type, parm)
            {
                os << parm.xm_ << " " << parm.alpha_;
                return os;
            }

            //! Reads the parameters from a @c std::istream
            BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, param_type, parm)
            {
                is >> parm.xm_ >> std::ws >> parm.alpha_;
                return is;
            }

            //! Returns true if the two sets of parameters are equal
            BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(param_type, lhs, rhs)
            { return lhs.xm_ == rhs.xm_ && lhs.alpha_ == rhs.alpha_; }

            //! Returns true if the two sets of parameters are different
            BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(param_type)

        private:
            Real xm_;
            Real alpha_;
        };

        //! \name Constructors
        //@{
        /*! Constructs a LevyFlightDistribution with a given xm and alpha
            Requires: alpha > 0
        */
        explicit LevyFlightDistribution(Real xm = 1.0, Real alpha = 1.0)
          : xm_(xm), alpha_(alpha) { QL_REQUIRE(alpha_ > 0.0, "alpha must be larger than 0"); }

        //!Constructs a LevyFlightDistribution from its parameters
        explicit LevyFlightDistribution(const param_type& parm)
          : xm_(parm.xm()), alpha_(parm.alpha()) {}

        // compiler-generated copy ctor and assignment operator are fine
        //@}

        //! \name Inspectors
        //@{
        //! Returns the xm parameter of the distribution
        Real xm() const { return xm_; }

        //! Returns the alpha parameter of the distribution
        Real alpha() const { return alpha_; }

        //! Returns the smallest value that the distribution can produce
        Real min BOOST_PREVENT_MACRO_SUBSTITUTION () const
        { return xm_; }
        //! Returns the largest value that the distribution can produce
        Real max BOOST_PREVENT_MACRO_SUBSTITUTION () const
        { return QL_MAX_REAL; }

        //! Returns the parameters of the distribution
        param_type param() const { return {xm_, alpha_}; }
        //@}

        //! Sets the parameters of the distribution
        void param(const param_type& parm) {
            xm_ = parm.xm();
            alpha_ = parm.alpha();
        }

        /*! Effects: Subsequent uses of the distribution do not depend
            on values produced by any engine prior to invoking reset.
        */
        void reset() { }

        //! Returns the value of the pdf for x
        Real operator()(Real x) const{
            using std::pow;
            if(x < xm_) return 0.0;
            return alpha_*pow(xm_/x, alpha_)/x;
        }

        /*!    Returns a random variate distributed according to the
            levy flight distribution.
        */
        template<class Engine>
        result_type operator()(Engine& eng) const{
            using std::pow;
            return xm_*pow(boost::random::uniform_01<Real>()(eng), -1.0/alpha_);
        }

        /*!    Returns a random variate distributed according to the
            levy flight with parameters specified by parm
        */
        template<class Engine>
        result_type operator()(Engine& eng, const param_type& parm) const{
            return LevyFlightDistribution (parm)(eng);
        }

        //! Writes the distribution to a std::ostream
        BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, LevyFlightDistribution, ed)
        {
            os << ed.xm_ << " " << ed.alpha_;
            return os;
        }

        //! Reads the distribution from a std::istream
        BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, LevyFlightDistribution, ed)
        {
            is >> ed.xm_ >> std::ws >> ed.alpha_;
            return is;
        }

        /*! Returns true iff the two distributions will produce identical
            sequences of values given equal generators.
        */
        BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(LevyFlightDistribution, lhs, rhs)
        { return lhs.xm_ == rhs.xm_ && lhs.alpha_ == rhs.alpha_; }

        /*!    Returns true iff the two distributions will produce different
            sequences of values given equal generators.
        */
        BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(LevyFlightDistribution)

    private:
        result_type xm_;
        result_type alpha_;
    };

}

#endif


#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include <manifold/SO3.h>
#include <manifold/gradientDescentSE3.h>

class GDSE3gmm : public GDSE3<double> {
 public:
  GDSE3gmm(const Eigen::Vector3d& muA, const
      Eigen::Matrix3d& covA, const Eigen::Vector3d& muB, const
      Eigen::Matrix3d& covB)
    : piA_(1.), piB_(1.), muA_(muA), muB_(muB), covA_(covA), covB_(covB)
  {
    std::cout << "-A-"
      << muA.transpose() << std::endl
      << covA << std::endl;
    std::cout << "-B-"
      << muB.transpose() << std::endl
      << covB << std::endl;
  };

  virtual void ComputeJacobian(const SE3d& theta, Eigen::Matrix<double,6,1>* J, double* f) {
    SE3d T = theta;
    Eigen::Matrix3d R = T.matrix().topLeftCorner(3,3);
    Eigen::Vector3d t = T.matrix().topRightCorner(3,1);
    // TODO: maybe this has to be negated?
    Eigen::Vector3d m = (R*muB_ - muA_);
    Eigen::Matrix3d SB = R*covB_*R.transpose();
    Eigen::Matrix3d S = covA_+SB;
    double logCA = -0.5*log(2.*M_PI)*3-0.5*covA_.determinant();
    double logCB = -0.5*log(2.*M_PI)*3-0.5*SB.determinant();
    double logD = log(piA_) + log(piB_) + logCA +logCB;
    double z = -0.5*(t-m).dot(S.ldlt().solve(t-m));
//    std::cout << "logCA=" << logCA << " logCB=" << logCB << std::endl;
//    std::cout << "logD=" << logD << " z=" << z
//      << " exp(logD+z)=" << exp(logD + z) << std::endl;
    if (J) {
      J->fill(exp(logD + z));
      J->topRows(3) = J->topRows(3).array()*(-S.ldlt().solve(t-m)).array();
      for (uint32_t j=0; j<3; ++j) {
        Eigen::Matrix3d G = SO3d::G(j);
        Eigen::Matrix3d C = G*SB+SB*G.transpose();
        Eigen::Matrix3d SinvC = S.ldlt().solve(C);
        Eigen::Matrix3d SinvG = S.ldlt().solve(G);
        Eigen::Matrix3d Sinv = S.inverse();

//        std::cout << "G = "<<G<<std::endl
//         << "C = "         <<C<<std::endl
//         << "SinvC ="<<  SinvC<<std::endl
//         << "SinvG ="<<  SinvG<<std::endl
//         << "Sinv = "<<  Sinv<<std::endl;
//         std::cout << G.transpose()*Sinv << std::endl
//           << SinvC*Sinv << std::endl
//           << SinvG << std::endl;
//            << (G.transpose()*Sinv-SinvC*Sinv+SinvG) << std::endl;
//           << C << std::endl;

        std::cout << "@G" << j
           << "\t" << SinvC.trace()
           << "\t" << - t.dot(SinvC*S.ldlt().solve(t))
           << "\t" << -2.*t.dot(SinvC*S.ldlt().solve(muA_))
           << "\t" << -2.*t.dot((-SinvC*S.ldlt().solve(R)+SinvG*R)*muB_)
           << "\t" << (R*muB_).dot((G.transpose()*Sinv-SinvC*Sinv+SinvG)*R*muB_)
           << "\t" << +2.*(R*muB_).dot((SinvG*(Eigen::Matrix3d::Identity()-SB*Sinv))*R*muB_)
           << std::endl;

        (*J)(3+j) *= -0.5*(
            SinvC.trace()
            - t.dot(SinvC*S.ldlt().solve(t))
            -2.*t.dot(SinvC*S.ldlt().solve(muA_))
            -2.*t.dot((-SinvC*S.ldlt().solve(R)+SinvG*R)*muB_)
            +(R*muB_).dot((G.transpose()*Sinv-SinvC*Sinv+SinvG)*R*muB_));
      }
      (*J) *= -1.;
//      J->bottomRows(3).fill(0.);
    }
    if (f) {
      *f = -exp(logD + z);
    }
  };
 protected:
  SO3d Rmu_;
  double piA_;
  double piB_;
  Eigen::Vector3d muA_;
  Eigen::Vector3d muB_;
  Eigen::Matrix3d covA_;
  Eigen::Matrix3d covB_;
};

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

  double theta = 15.*M_PI/180.;
  Eigen::Matrix3d R;
  R << 1, 0, 0,
         0, cos(theta), sin(theta),
         0, -sin(theta), cos(theta);
//  R = Eigen::Matrix3d::Identity();
  Eigen::Vector3d t = Eigen::Vector3d::Ones();
  Eigen::Matrix3d covA =   Eigen::Vector3d(1.,.1,1.).asDiagonal();
  Eigen::Matrix3d covB = R*covA*R.transpose();
  Eigen::Vector3d muA = Eigen::Vector3d::Zero();
  Eigen::Vector3d muB = R*muA+t;

  SE3d T;

  GDSE3gmm gd(muA, covA, muB, covB);
  gd.Compute(T, 1e-6, 200);
//  gd.Compute(T, 0, 200);
  T = gd.GetMinimum();
  Eigen::Vector3d tEst = T.matrix().topRightCorner(3,1);
  Eigen::Matrix3d REst = T.matrix().topLeftCorner(3,3);
  std::cout << T << std::endl;
  std::cout << R << std::endl;
  std::cout << REst.transpose() << std::endl;
  std::cout << (-REst.transpose()*tEst).transpose() << std::endl;
  std::cout << t.transpose() << std::endl;

}


#include <cstdio>
#include <cstdlib>
#include <MNIST.h>
#include <Matrix.h>
#include <Function.h>
#include <Eigen/Dense>

int main() {
    MNIST mnist;
    if (mnist.load("./../dataset/t10k-images-idx3-ubyte", "./../dataset/t10k-labels-idx1-ubyte") != 0) {
        fprintf(stderr, "Failed to load MNIST.\n");
        return -1;
    }

    MatrixXd W1(784, 50);
    if (initMatrix(W1, "./data/W1.csv") != 0) {
        fprintf(stderr, "Failed to init W1\n");
        return -1;
    }

    MatrixXd W2(50, 100);
    if (initMatrix(W2, "./data/W2.csv") != 0) {
        fprintf(stderr, "Failed to init W2\n");
        return -1;
    }

    MatrixXd W3(100, 10);
    if (initMatrix(W3, "./data/W3.csv") != 0) {
        fprintf(stderr, "Failed to init W3\n");
        return -1;
    }

    RowVectorXd b1(50);
    if (initRowVector(b1, "./data/b1.csv") != 0) {
        fprintf(stderr, "Failed to init b1\n");
        return -1;
    }

    RowVectorXd b2(100);
    if (initRowVector(b2, "./data/b2.csv") != 0) {
        fprintf(stderr, "Failed to init b2\n");
        return -1;
    }

    RowVectorXd b3(10);
    if (initRowVector(b3, "./data/b3.csv") != 0) {
        fprintf(stderr, "Failed to init b3\n");
        return -1;
    }

    int accuracyCnt = 0;
    const MatrixXd& images = mnist.getImages();
    const VectorXi& labels = mnist.getLabels();
    for (int i = 0; i < images.rows(); ++i) {
        const MatrixXd& M = mnist.getImages();
        const RowVectorXd& x = M.row(i);
        const RowVectorXd a1 = x * W1 + b1;
        const RowVectorXd z1 = sigmoid(a1);
        const RowVectorXd a2 = z1 * W2 + b2;
        const RowVectorXd z2 = sigmoid(a2);
        const RowVectorXd a3 = z2 * W3 + b3;
        const RowVectorXd y = softmax(a3);

        const int p = argmax(y);
        if (p == labels(i)) {
            ++accuracyCnt;
        }
    }

    std::printf("Accuracy:%lf\n", (double)(accuracyCnt) / images.rows());

    return 0;
}


#include "VPD.h"
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

#include <lsd.h>
#include <iostream>
#include <cmath>
#include <boost/numeric/ublas/lu.hpp>

#include "VPCluster.h"
#include "VPSample.h"

std::vector< std::pair<float, float> > 	extractVP 		(image_double &image, const cv::Mat &img);
cv::Mat 								makeFeatureMap	(std::vector< std::pair<float, float> > &vp, const cv::Mat &img, double *reliability = NULL);

template<typename T>
std::vector<T> polyfit( const std::vector<T>& oX, const std::vector<T>& oY, int nDegree ) {
	using namespace boost::numeric::ublas;

	if ( oX.size() != oY.size() )
		throw std::invalid_argument( "X and Y vector sizes do not match" );

	// more intuative this way
	nDegree++;

	size_t nCount =  oX.size();
	matrix<T> oXMatrix( nCount, nDegree );
	matrix<T> oYMatrix( nCount, 1 );

	// copy y matrix
	for ( size_t i = 0; i < nCount; i++ )
	{
		oYMatrix(i, 0) = oY[i];
	}

	// create the X matrix
	for ( size_t nRow = 0; nRow < nCount; nRow++ )
	{
		T nVal = 1.0f;
		for ( int nCol = 0; nCol < nDegree; nCol++ )
		{
			oXMatrix(nRow, nCol) = nVal;
			nVal *= oX[nRow];
		}
	}

	// transpose X matrix
	matrix<T> oXtMatrix( trans(oXMatrix) );
	// multiply transposed X matrix with X matrix
	matrix<T> oXtXMatrix( prec_prod(oXtMatrix, oXMatrix) );
	// multiply transposed X matrix with Y matrix
	matrix<T> oXtYMatrix( prec_prod(oXtMatrix, oYMatrix) );

	// lu decomposition
	permutation_matrix<T> pert(oXtXMatrix.size1());
	const std::size_t singular = lu_factorize(oXtXMatrix, pert);
	// must be singular
	if( singular != 0 ) {
		// then the regression cannot be done... return an empty vector
		return std::vector<T>();
	}

	// backsubstitution
	lu_substitute(oXtXMatrix, pert, oXtYMatrix);

	// copy the result to coeff
	return std::vector<T>( oXtYMatrix.data().begin(), oXtYMatrix.data().end() );
}

template<typename T>
std::vector<T> polyval( const std::vector<T>& oCoeff,  const std::vector<T>& oX ) {
	size_t nCount =  oX.size();
	size_t nDegree = oCoeff.size();
	std::vector<T>	oY( nCount );

	for ( size_t i = 0; i < nCount; i++ )
	{
		T nY = 0;
		T nXT = 1;
		T nX = oX[i];
		for ( size_t j = 0; j < nDegree; j++ )
		{
			// multiply current x by a coefficient
			nY += oCoeff[j] * nXT;
			// power up the X
			nXT *= nX;
		}
		oY[i] = nY;
	}

	return oY;
}

struct Segment {
	float x;
	float y;
	float xend;
	float yend;
	float angle;
};

float  horizonLine(cv::Mat &image) {
	if(image.empty()) return 0;

	std::vector<float> x, y;

	cv::Mat gray;
	cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY);

	cv::Mat edges;
	cv::Canny(gray, edges, 110, 220);

	std::vector<cv::Vec4i> lines;
    cv::HoughLinesP( edges, lines, 1, CV_PI/2, 0, 30, 1 );
    float meanY = 0; int nbLines = 0;
    float meanY2 = 0;
    for( size_t i = 0; i < lines.size(); i++ )
    {
    	if(lines[i][0] == lines[i][2]) continue;

		if(lines[i][1] > (.7 * image.rows)) continue;
		if(lines[i][1] < (.3 * image.rows)) continue;

        cv::line( image, cv::Point(lines[i][0], lines[i][1]),
            cv::Point(lines[i][2], lines[i][3]), cv::Scalar(0,0,255), 3, 8 );

        x.push_back(lines[i][0]);
		x.push_back(lines[i][2]);
		y.push_back(lines[i][1]);
		y.push_back(lines[i][3]);

		meanY += lines[i][1]; ++nbLines;
		meanY += lines[i][3]; ++nbLines;

		meanY2 += lines[i][1] * lines[i][1];
		meanY2 += lines[i][3] * lines[i][3];

    }
    meanY /= nbLines;
    meanY2 /= nbLines;

    float stdev = std::sqrt(meanY2 - meanY*meanY);
    float meanValue = meanY;

    meanY = 0; nbLines = 0;
    for( size_t i = 0; i < lines.size(); i++ )
    {
    	if(lines[i][0] == lines[i][2]) continue;

		if(lines[i][1] > (.7 * image.rows)) continue;
		if(lines[i][1] < (.3 * image.rows)) continue;

        if(std::abs(lines[i][1] - meanValue) > 1.3 * stdev) continue;

		meanY += lines[i][1]; ++nbLines;
		meanY += lines[i][3]; ++nbLines;

    }
    meanY /= nbLines;

    if(nbLines == 0) {
    	meanY = image.rows/2;
    } else {
    	std::vector<float> model = polyfit<float>(x, y, 1);

	    if(std::abs(model[0]- image.rows/2) < std::abs(meanY - image.rows/2))
	    	meanY = model[0];
    }

	cv::line(image, cv::Point_<int>(0, meanY), cv::Point_<int>(image.cols, meanY), cv::Scalar( 0, 255, 0 ), 2);

return meanY;

}

std::vector< std::pair<float, float> >  extractVP(const cv::Mat &image) {

	// ------------------------------------------------------------------------------------
	// convert image input

	if(image.empty()) return std::vector< std::pair<float, float> >();

	cv::Mat gray;
	cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY);
	cv::Mat grayF;
	gray.convertTo(grayF, CV_32FC1);

	image_double image_d = new_image_double(gray.cols, gray.rows);
	for(int i = 0 ; i < gray.cols*gray.rows ; ++i) {
		image_d->data[i] = reinterpret_cast<float*>(grayF.data)[i];
	}

	return extractVP(image_d, image);

}

std::vector< std::pair<float, float> > extractVP(image_double &image_d, const cv::Mat &) {

	// cv::Mat colorImg = image.clone();

	// ------------------------------------------------------------------------------------
	// Run LSD line segment detector

	ntuple_list ntuple = LineSegmentDetection( image_d, 0.7,
                                  0.6, 2.6,
                                  22.5, 0.0, 0.6,
                                  1024, 1.0,
                                  NULL );

	// ntuple_list ntuple = LineSegmentDetection( image_d, 0.7,
    //                               0.6, 2.6,
    //                               22.5, 0.0, 0.6,
    //                               1024, 1.0,
    //                               NULL );

	std::vector< std::vector<float> *> pts;

	std::vector<float> stroke_length(ntuple->size);
	for(size_t i = 0 ; i < ntuple->size ; ++i) {
		stroke_length[i] = static_cast<float>( (ntuple->values[ i * ntuple->dim + 0 ] - ntuple->values[ i * ntuple->dim + 2 ])*(ntuple->values[ i * ntuple->dim + 0 ] - ntuple->values[ i * ntuple->dim + 2 ]) + (ntuple->values[ i * ntuple->dim + 1 ] - ntuple->values[ i * ntuple->dim + 3 ])*(ntuple->values[ i * ntuple->dim + 1 ] - ntuple->values[ i * ntuple->dim + 3 ]));
	}

	std::sort(stroke_length.begin(), stroke_length.end());

	if(stroke_length.empty()) {
		free_image_double(image_d);
		free_ntuple_list(ntuple);
		return std::vector< std::pair<float, float> >();
	}

	// std::cout << stroke_length[static_cast<int>(stroke_length.size()*.7)] << std::endl;

	 // std::cout << ntuple->size << "line segments found:\n";
	for(size_t i = 0 ; i < ntuple->size ; ++i) {
		float len = static_cast<float>( (ntuple->values[ i * ntuple->dim + 0 ] - ntuple->values[ i * ntuple->dim + 2 ])*(ntuple->values[ i * ntuple->dim + 0 ] - ntuple->values[ i * ntuple->dim + 2 ]) + (ntuple->values[ i * ntuple->dim + 1 ] - ntuple->values[ i * ntuple->dim + 3 ])*(ntuple->values[ i * ntuple->dim + 1 ] - ntuple->values[ i * ntuple->dim + 3 ]) );
		if(len < stroke_length[static_cast<int>(stroke_length.size()*.7)]) continue;
		if(len < 827.748f) continue;

		std::vector<float>* p = new std::vector<float>(4);
		pts.push_back(p);
		for(size_t j = 0 ; j < ntuple->dim ; ++j) {
			if(j < 4) (*p)[j] = static_cast<float>(ntuple->values[ i * ntuple->dim + j ]);
			// std::cout << ntuple->values[ i * ntuple->dim + j ] << " ";
		}
		 // std::cout << "\n";
	}

	free_image_double(image_d);
	free_ntuple_list(ntuple);

	// for(size_t i = 0 ; i < pts.size() ; ++i) {
	// 	cv::line(colorImg, cv::Point((*pts[i])[0], (*pts[i])[1]),  cv::Point((*pts[i])[2], (*pts[i])[3]), cv::Scalar(255,0,0),2);
	// }

	// ------------------------------------------------------------------------------------
	// JLinkage for cluster detection

	std::vector<unsigned int> Lables;
	std::vector<unsigned int> LableCount;

	if(pts.size() < 20) {
		for(size_t i = 0 ; i < pts.size() ; ++i) {
			delete pts[i];
		}

		return std::vector< std::pair<float, float> >();
	}

	std::vector<std::vector<float> *> *mModels =
		VPSample::run(&pts, 5000, 2, 0, 3); // VPSample::run(&pts, 5000, 2, 0, 3);
	int classNum = VPCluster::run(Lables, LableCount, &pts, mModels, 2, 2);
	// std::cout<<"vpdetection found "<<classNum<<" classes!"<<std::endl;

	//2.1. release other resource
	for(unsigned int i=0; i < mModels->size(); ++i)
		delete (*mModels)[i];
	delete mModels;

	// ------------------------------------------------------------------------------------
	// Compute the vanishing point positions

	std::vector< std::vector<float> > lengths(Lables.size());
	std::vector<float> dydx(pts.size());
	std::vector<float> b(pts.size());
	std::vector<bool>  disabled(pts.size(), false);

	for(size_t i = 0 ; i < pts.size() ; ++i) {
		lengths[Lables[i]].push_back(std::sqrt(((*pts[i])[1]-(*pts[i])[3])*((*pts[i])[1]-(*pts[i])[3]) +  ((*pts[i])[0]-(*pts[i])[2])*((*pts[i])[0]-(*pts[i])[2])));
		if(std::abs(((*pts[i])[0] - (*pts[i])[2])) < 0.00001) {
			disabled[i] = true;
			continue;
		}

		dydx[i] = ((*pts[i])[1] - (*pts[i])[3]) / ((*pts[i])[0] - (*pts[i])[2]);
		b[i] =  (*pts[i])[1] - dydx[i] * (*pts[i])[0];

		if(atan(std::abs(dydx[i])) < (20*3.141592653/180)) {
			disabled[i] = true;
		}

	}

	for(int i = 0 ; i < classNum ; ++i) {
		std::sort(lengths[i].begin(), lengths[i].end());
	}

	std::vector<float> numPoints(classNum, 0);
	std::vector<float> vp_x(classNum, 0);
	std::vector<float> vp_y(classNum, 0);

	for(size_t ii = 0 ; ii < pts.size() ; ++ii) {
		for(size_t jj = ii+1 ; jj < pts.size() ; ++jj) {

			float l1 = std::sqrt(((*pts[ii])[1]-(*pts[ii])[3])*((*pts[ii])[1]-(*pts[ii])[3]) +  ((*pts[ii])[0]-(*pts[ii])[2])*((*pts[ii])[0]-(*pts[ii])[2]));
			float l2 = std::sqrt(((*pts[jj])[1]-(*pts[jj])[3])*((*pts[jj])[1]-(*pts[jj])[3]) +  ((*pts[jj])[0]-(*pts[jj])[2])*((*pts[jj])[0]-(*pts[jj])[2]));

			if(Lables[ii] != Lables[jj]) continue;
			if(disabled[ii] || disabled[jj]) continue;
			if(lengths[Lables[ii]].size() == 0) continue;
			if(lengths[Lables[jj]].size() == 0) continue;

			if(l1 < lengths[Lables[ii]][static_cast<int>(lengths[Lables[ii]].size()*.7)]) continue;
			if(l2 < lengths[Lables[jj]][static_cast<int>(lengths[Lables[jj]].size()*.7)]) continue;

			if(std::abs((dydx[jj] - dydx[ii])) < 0.000001) continue;

			float x = (b[ii] - b[jj]) / (dydx[jj] - dydx[ii]);
			float y = dydx[ii] * x + b[ii];

			// cv::line(colorImg, cv::Point((*pts[ii])[0], (*pts[ii])[1]),  cv::Point((*pts[ii])[2], (*pts[ii])[3]), cv::Scalar(0,255,0),2);
			// cv::line(colorImg, cv::Point((*pts[jj])[0], (*pts[jj])[1]),  cv::Point((*pts[jj])[2], (*pts[jj])[3]), cv::Scalar(0,0,255),2);

			vp_x[Lables[ii]] += x;
			vp_y[Lables[ii]] += y;
			++numPoints[Lables[ii]];
		}
	}

	std::vector< std::pair<float, float> > vp;
	int considered_idx = 0;
	for(int i = 0 ; i < classNum ; ++i) {
		if(numPoints[i] == 0)
			continue;

		vp_x[i] /= numPoints[i];
		vp_y[i] /= numPoints[i];

		// if(numPoints[i] < 1)
		// 	continue;

		considered_idx = i;
		vp.push_back(std::make_pair(vp_x[i], vp_y[i]));

		// cv::circle(colorImg, cv::Point(vp_x[i], vp_y[i]), 3, cv::Scalar(0,0,255),2);
	}

	//cv::imshow("colorImg", colorImg);
	// cv::waitKey();

	// ------------------------------------------------------------------------------------
	// Done. free memory

	for(size_t i = 0 ; i < pts.size() ; ++i) {
		delete pts[i];
	}

	return vp;

}

cv::Mat makeFeatureMap	(std::vector< std::pair<float, float> > &vp, const cv::Mat &img, double *reliability) {
	cv::Mat featureMap(img.rows, img.cols, CV_64FC1, cv::Scalar(0.f));

	if(reliability != NULL)  *reliability = 0.f;

	std::vector<float> dists;
	for(size_t i = 0 ; i < vp.size() ; ++i) {
        float normx = vp[i].first/img.cols;
        float normy = vp[i].second/img.rows;

		if(normx > 1.f || normx < 0.f) continue;
		if(normy > 1.f || normy < 0.f) continue;

        // std::cout << normx << " " << normy << std::endl;

        float d = (normx-.5f)*(normx-.5f) + (normy-.5f)*(normy-.5f);

		dists.push_back(d);
	}

	std::sort(dists.begin(), dists.end());

    for(size_t i = 0 ; i < vp.size() ; ++i) {
        float normx = vp[i].first/img.cols;
        float normy = vp[i].second/img.rows;

		if(normx > 1.f || normx < 0.f) continue;
		if(normy > 1.f || normy < 0.f) continue;

        // std::cout << normx << " " << normy << std::endl;

        float d = (normx-.5f)*(normx-.5f) + (normy-.5f)*(normy-.5f);

		if(dists.size() > 7 && dists[dists.size()-7] > d) {
			continue;
		}

		if(std::abs(normy-.5f) > 0.25) continue;

		d =  (normy-.5f)*(normy-.5f);

        float sigma = 0.00005f / (d + 0.01f);
		if(reliability != NULL) {
			*reliability = std::max(static_cast<float>(*reliability), sigma);
		}

        for(int i = 0 ; i < img.rows ; ++i) {
            for(int j = 0 ; j < img.cols ; ++j) {
                float fi = static_cast<float>(i) / img.rows;
                float fj = static_cast<float>(j) / img.cols;
                featureMap.at<double>(i,j) = std::max(featureMap.at<double>(i,j), static_cast<double>(std::exp(-((fj-normx)*(fj-normx)*2 + (fi-normy)*(fi-normy))/sigma)));

            }
        }
    }

    return featureMap;
}

cv::Mat vanishingLineFeatureMapF64	(const cv::Mat &img, double *reliability) {

	image_double image_d = new_image_double(img.cols, img.rows);
	for(int i = 0 ; i < img.cols*img.rows ; ++i) {
		image_d->data[i] = reinterpret_cast<double*>(img.data)[i];
	}

	std::vector< std::pair<float, float> > vp = extractVP(image_d, img);

    return makeFeatureMap(vp, img, reliability);
}

cv::Mat vanishingLineFeatureMapF64C3(const cv::Mat &img, double *reliability) {

	cv::Mat imgu(img.rows, img.cols, CV_8UC3, cv::Scalar(0.f, 0.f, 0.f));
    for(int i = 0 ; i < img.rows ; ++i) {
        for(int j = 0 ; j < img.cols ; ++j) {
            cv::Point3_<unsigned char> &o = imgu.at< cv::Point3_<unsigned char> >(i,j);
            const cv::Point3_<double> &in = img.at< cv::Point3_<double> >(i,j);

            o.x = static_cast<unsigned char>(in.x*255);
            o.y = static_cast<unsigned char>(in.y*255);
            o.z = static_cast<unsigned char>(in.z*255);
        }
    }

	std::vector< std::pair<float, float> > vp = extractVP(imgu);

    return makeFeatureMap(vp, img, reliability);
}

cv::Mat vanishingLineFeatureMap (const cv::Mat &img, double *reliability) {

	if(img.empty()) return cv::Mat();

    std::vector< std::pair<float, float> > vp = extractVP(img);

    return makeFeatureMap(vp, img, reliability);
}


#include <iostream>
#include <vector>
#include <random>
#include <chrono>
#include <cstdint>
#include <boost/math/special_functions/prime.hpp>

static const std::size_t kNbItems( 1000000 );

namespace method1
{

void removeDuplicates( std::vector<uint32_t> & items )
{
    using namespace boost::math;
    uint64_t checker = 1u; // Be careful of integer overflow here

    std::size_t n = 0;
    for( const uint32_t x : items )
    {
        const uint32_t p = prime( x );
        if ( checker % p )
        {
            checker *= p;
            items[n++] = x;
        }
    }

    items.resize( n );
}

}

namespace method2
{

void removeDuplicates( std::vector<uint32_t> & items )
{
    std::sort( items.begin(), items.end() );
    items.erase( std::unique( items.begin(), items.end() ), items.end() );
}

}

/**
 * @brief main goes here
 */
int main( int argc, char **argv )
{
    std::mt19937 rng;
    rng.seed( time( 0 ) );

    std::uniform_int_distribution<uint32_t> rand( 0, 10 );
    std::vector<uint32_t> v;
    v.reserve( kNbItems );
    for( std::size_t i = 0; i < kNbItems; ++i )
    {
        v.push_back( rand( rng ) );
    }

    // Benchmark
    typedef std::chrono::high_resolution_clock Clock;
    using std::chrono::microseconds;
    using std::chrono::duration_cast;

    std::vector<uint32_t> uniqueItems1( v );
    {
        auto t1 = Clock::now();
        method1::removeDuplicates( uniqueItems1 );
        auto t2 = Clock::now();
        std::cout << "Method1 took : " << duration_cast<microseconds>(t2-t1).count() << std::endl;
    }

    std::vector<uint32_t> uniqueItems2( v );
    {
        auto t1 = Clock::now();
        method2::removeDuplicates( uniqueItems2 );
        auto t2 = Clock::now();
        std::cout << "Method2 took : " << duration_cast<microseconds>(t2-t1).count() << std::endl;
    }

    std::sort( uniqueItems1.begin(), uniqueItems1.end() );
    std::sort( uniqueItems2.begin(), uniqueItems2.end() );
    if ( uniqueItems1 == uniqueItems2 )
    {
        std::cout << "RESULTS ARE CONFORM" << std::endl;
    }
    else
    {
        std::cerr << "OUPS... Algorithm outputs different resutls!" << std::endl;
    }
    std::cout << "uniqueItems1.size(): " << uniqueItems1.size() << std::endl;
    std::cout << "uniqueItems2.size(): " << uniqueItems2.size() << std::endl;
}


#include "conex/jordan_matrix_algebra.h"

#include "gtest/gtest.h"
#include <Eigen/Dense>

#include "conex/debug_macros.h"
#include "conex/test/test_util.h"

namespace conex {

using Eigen::MatrixXd;
using Eigen::VectorXd;

using JordanTypes = testing::Types<Real, Complex, Quaternions, Octonions>;

VectorXd sort(const VectorXd& x) {
  auto y = x;
  std::sort(y.data(), y.data() + x.rows());
  return y;
}

template <typename T>
class TestCases : public testing::Test {
 public:
  void DoMultiplyByIdentity() {
    auto X = T::Random(3, 3);
    auto I = T::Identity(3);
    EXPECT_TRUE(T::IsEqual(X, T::Multiply(X, I)));
  }
  void VerifyJordanIdentity(int n) {
    using Matrix = typename T::Matrix;
    Matrix A = T::Random(n, n);
    A = T::Add(A, T::ConjugateTranspose(A));
    Matrix B = T::Random(n, n);
    B = T::Add(B, T::ConjugateTranspose(B));
    auto W = T::JordanMultiply(A, B);

    EXPECT_TRUE(T::IsHermitian(B));
    EXPECT_TRUE(T::IsHermitian(A));
    EXPECT_TRUE(T::IsHermitian(W));

    // Test Jordan identity.
    auto Asqr = T::JordanMultiply(A, A);
    auto BA = T::JordanMultiply(A, B);
    auto P1 = T::JordanMultiply(Asqr, BA);

    auto BAsqr = T::JordanMultiply(B, Asqr);
    auto P2 = T::JordanMultiply(A, BAsqr);

    EXPECT_TRUE(T::IsEqual(P1, P2));
  }

  void DoQuadraticRepresentationAssociativeTest(int d) {
    using Matrix = typename T::Matrix;
    Matrix A = T::Random(d, d);
    A = T::Add(A, T::ConjugateTranspose(A));
    Matrix B = T::Random(d, d);
    B = T::Add(B, T::ConjugateTranspose(B));
    Matrix Yref = T::QuadraticRepresentation(B, A);

    Matrix Y1 = T::Multiply(T::Multiply(B, A), B);
    Matrix Y2 = T::Multiply(B, T::Multiply(A, B));
    Y1 = T::ScalarMultiply(Y1, .5);
    Y2 = T::ScalarMultiply(Y2, .5);
    Y1 = T::Add(Y1, Y2);

    if (std::is_same<T, Octonions>::value) {
      EXPECT_FALSE(T::IsEqual(Y1, Yref));
    } else {
      EXPECT_TRUE(T::IsEqual(Y1, Yref));
    }
  }

  void DoEigenvalueTests() {
    double eps = 1e-9;
    using Matrix = typename T::Matrix;
    int n = 3;
    Matrix Q = T::Random(n, n);
    Q = T::Add(Q, T::ConjugateTranspose(Q));
    Q = T::JordanMultiply(Q, Q);

    auto I = T::Identity(n);
    double normsqr = T::TraceInnerProduct(Q, Q);
    double trace = T::TraceInnerProduct(I, Q);
    auto eigvals = T::Eigenvalues(Q);

    EXPECT_TRUE(eigvals.minCoeff() > eps);
    EXPECT_NEAR(eigvals.squaredNorm(), normsqr, eps);
    EXPECT_NEAR(eigvals.sum(), trace, eps);

    // eigvals = T::Eigenvalues(I);
    // for (int i = 0; i < eigvals.rows(); i++) {
    //   EXPECT_NEAR(eigvals(i), 1, 1e-8);
    // }
  }

  void DoTestOrthogonal(int d) {
    if (std::is_same<T, Octonions>::value) {
      return;
    }
    double eps = 1e-8;
    auto Q = T::Random(d, d);

    Q = T::Orthogonalize(Q);

    auto I = T::ScalarMultiply(T::Identity(d), -1);
    auto res = T::Add(I, T::Multiply(Q, T::ConjugateTranspose(Q)));
    EXPECT_TRUE(T::TraceInnerProduct(res, res) < eps);
  }

  void DoEigenvaluesFromSpectralDecomp(int d) {
    if (std::is_same<T, Octonions>::value) {
      return;
    }
    double eps = 1e-8;
    auto Q = T::Random(d, d);

    Q = T::Orthogonalize(Q);
    HyperComplexMatrix D = T::Zero(d, d);
    for (int i = 0; i < d; i++) {
      D.at(0)(i, i) = -d / 2 + i;
    }
    auto X = T::Multiply(T::Multiply(Q, D), T::ConjugateTranspose(Q));

    VectorXd calc = sort(T::Eigenvalues(X));
    for (int i = 0; i < d; i++) {
      EXPECT_NEAR(calc(i), D.at(0)(i, i), eps);
    }

    auto r = T::Random(d, 1);
    calc = sort(T::ApproximateEigenvalues(X, r, d));
    for (int i = 0; i < d; i++) {
      EXPECT_NEAR(calc(i), D.at(0)(i, i), eps);
    }
  }

  void DoAsymmetricEigenvaluesTest(int d) {
    if (std::is_same<T, Octonions>::value) {
      return;
    }
    double eps = 1e-8;
    auto Wsqrt = T::Random(d, d);
    Wsqrt = T::Add(Wsqrt, T::ConjugateTranspose(Wsqrt));
    auto S = T::Random(d, d);
    S = T::Add(S, T::ConjugateTranspose(S));

    auto W = T::Multiply(Wsqrt, T::ConjugateTranspose(Wsqrt));

    VectorXd ref = sort(T::Eigenvalues(T::QuadraticRepresentation(Wsqrt, S)));

    VectorXd calc;
    calc = sort(T::EigenvaluesOfJacobiMatrix(T::Multiply(W, S), W, d));
    auto calc2 = sort(
        T::ApproximateEigenvalues(T::Multiply(W, S), W, T::Random(d, 1), d));
    for (int i = 0; i < d; i++) {
      EXPECT_NEAR(calc(i), calc2(i), eps);
    }

    for (int i = 0; i < d; i++) {
      EXPECT_NEAR(calc(i), ref(i), eps);
    }
  }

  void RankOneTest(int d) {
    assert(d == 3);
    double eps = 1e-5;
    auto Wsqrt = T::Random(d, 1);

    if (std::is_same<T, Octonions>::value) {
      // Lemma 14.90 Spinors and Calibrations By F. Reese Harvey shows
      // that primitive idempotents of the Albert algebra
      // are of the form w w^*, with associator [w_1 w_2 w_3] = 0.
      // This implies w_i are contained in a quaternion subalgebra.
      auto WsqrtQ = Quaternions::Random(d, 1);
      Wsqrt = T::Zero(d, 1);
      for (int i = 0; i < 4; i++) {
        Wsqrt.at(i) = WsqrtQ.at(i);
      }
    }

    Wsqrt = T::ScalarMultiply(Wsqrt, 1.0 / Wsqrt.norm());
    auto W = T::Multiply(Wsqrt, T::ConjugateTranspose(Wsqrt));

    // Add noise to make eigenvalues distinct.
    W.at(0)(0, 0) += 0.00003 * eps;
    W.at(0)(1, 1) += 0.00001 * eps;
    W.at(0)(2, 2) += 0.00002 * eps;

    VectorXd ref(d);
    ref.setZero();
    ref(0) = 1;
    ref = sort(ref);
    VectorXd calc = sort(T::Eigenvalues(W));
    for (int i = 0; i < d; i++) {
      EXPECT_NEAR(calc(i), ref(i), eps);
    }
  }
};

TYPED_TEST_CASE(TestCases, JordanTypes);
TYPED_TEST(TestCases, MultiplyByIdentity) {
  TestFixture::DoMultiplyByIdentity();
}

TYPED_TEST(TestCases, JordanIdentity) { TestFixture::VerifyJordanIdentity(3); }

TYPED_TEST(TestCases, QuadraticRepresentationAssociativeTest) {
  TestFixture::DoQuadraticRepresentationAssociativeTest(3);
}

TYPED_TEST(TestCases, EigenvalueTests) { TestFixture::DoEigenvalueTests(); }

TYPED_TEST(TestCases, DoTestOrthogonal) { TestFixture::DoTestOrthogonal(3); }

TYPED_TEST(TestCases, DoAsymmetricEigenvaluesTest) {
  TestFixture::DoAsymmetricEigenvaluesTest(3);
}

TYPED_TEST(TestCases, DoEigenvaluesFromSpectralDecomp) {
  TestFixture::DoEigenvaluesFromSpectralDecomp(3);
}

TYPED_TEST(TestCases, RankOneTest) { TestFixture::RankOneTest(3); }

TEST(JordanMatrixAlgebra, HermitianRealMatchesEigen) {
  using T = Real;
  int n = 3;
  auto Q = T::Random(n, n);
  Q = T::JordanMultiply(Q, Q);

  EXPECT_TRUE(
      (sort(T::Eigenvalues(Q)) - sort(eig(Q.at(0)).eigenvalues)).norm() < 1e-8);
}

}  // namespace conex


#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <boost/multiprecision/cpp_int.hpp>

namespace mp = boost::multiprecision;

int main( int argc, char **argv )
{
    if ( argc < 2 ){
        std::cout << argv[ 0 ] << " number" << std::endl;

    }else{
        mp::cpp_int number( argv[ 1 ] );

        mp::cpp_int prev_00(1);
        mp::cpp_int prev_01(0);
        mp::cpp_int current(1);
        mp::cpp_int result(0);
        mp::cpp_int i(0);

        while( i < number )
        {
            result = current;
            current = prev_00 + prev_01;
            prev_01 = prev_00;
            prev_00 = current;
            i += 1;
        }
        std::cout << result << std::endl;
    }
}


#include <Eigen/Core>
#include <Eigen/Dense>
#include <iostream>
#include <vector>
#include "./include/prints.hpp"

std::vector<int> baseIndex;

void triangularize(Eigen::MatrixXd& B){
    int _m = B.rows();
    int _n = B.cols();
    int start_row = 0;                                  // 每次从start_row开始判定
    for (int col = 0; col < _n; col ++){
        int row = start_row;
        bool push_flag = false;
        if (B(row, col) == 0.0){
            for (; row < _m; row ++){
                if (B(row, col) != 0.0){
                    B.row(col).swap(B.row(row));        // 为0和不为0行进行交换
                    push_flag = true;
                    row ++;
                    break;
                }
            }
        }
        else{
            row ++;
            push_flag = true;
        }
        for (; row < _m; row ++){
            double head = B(row, col);
            if (head != 0.0){
                B.row(row) -= head / B(col, col) * B.row(col);  // 消除头部0
            }
        }
        if (push_flag == true){
            baseIndex.emplace_back(col);
            start_row ++;
            if (start_row >= _m){
                return;
            }
        }
    }
}

int main(){
    std::cout << "==================== test ladderize ==================\n";
    int row = 3, col = 6;
    Eigen::MatrixXd mat(row, col);
    mat << 1, -2, 1, 0, 0, 2,
        0, 1, -3, 1, 0, 1,
        0, 1, -1, 0, 1, 2;
    std::cout << "Before ladderize:\n";
    printMat(mat);
    triangularize(mat);
    std::cout << "After ladderize:\n";
    printMat(mat);
    std::cout << "Base index:\n";
    for (int ind: baseIndex){
        std::cout << ind << ", ";
    }
    std::cout << std::endl;
    Eigen::MatrixXd B(row, row);
    for (int i = 0; i < row; i++){
        B.col(i) = mat.col(baseIndex[i]);
    }
    std::cout << "B:\n";
    printMat(B);
    Eigen::MatrixXd Binv = B.colPivHouseholderQr().solve(Eigen::MatrixXd::Identity(row, row));
    std::cout << "Binv:\n";
    printMat(Binv);
    Eigen::MatrixXd res = Binv * mat;
    std::cout << "Result:\n";
    printMat(res);
    return 0;
}


//
// Copyright 2020 Debabrata Mandal <mandaldebabrata123@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_HISTOGRAM_EQUALIZATION_HPP
#define BOOST_GIL_IMAGE_PROCESSING_HISTOGRAM_EQUALIZATION_HPP

#include <boost/gil/histogram.hpp>
#include <boost/gil/image.hpp>

#include <cmath>
#include <map>
#include <vector>

namespace boost { namespace gil {

/////////////////////////////////////////
/// Histogram Equalization(HE)
/////////////////////////////////////////
/// \defgroup HE HE
/// \brief Contains implementation and description of the algorithm used to compute
///        global histogram equalization of input images.
///
///        Algorithm :-
///        1. If histogram A is to be equalized compute the cumulative histogram of A.
///        2. Let CFD(A) refer to the cumulative histogram of A
///        3. For a uniform histogram A', CDF(A') = A'
///        4. We need to transfrom A to A' such that
///        5. CDF(A') = CDF(A) => A' = CDF(A)
///        6. Hence the pixel transform , px => histogram_of_ith_channel[px].
///

/// \fn histogram_equalization
/// \ingroup HE
/// \tparam SrcKeyType Key Type of input histogram
/// @param src_hist INPUT Input source histogram
/// \brief Overload for histogram equalization algorithm, takes in a single source histogram
///        and returns the color map used for histogram equalization.
///
template <typename SrcKeyType>
std::map<SrcKeyType, SrcKeyType> histogram_equalization(histogram<SrcKeyType> const& src_hist)
{
    histogram<SrcKeyType> dst_hist;
    return histogram_equalization(src_hist, dst_hist);
}

/// \overload histogram_equalization
/// \ingroup HE
/// \tparam SrcKeyType Key Type of input histogram
/// \tparam DstKeyType Key Type of output histogram
/// @param src_hist INPUT source histogram
/// @param dst_hist OUTPUT Output histogram
/// \brief Overload for histogram equalization algorithm, takes in both source histogram &
///        destination histogram and returns the color map used for histogram equalization
///        as well as transforming the destination histogram.
///
template <typename SrcKeyType, typename DstKeyType>
std::map<SrcKeyType, DstKeyType>
    histogram_equalization(histogram<SrcKeyType> const& src_hist, histogram<DstKeyType>& dst_hist)
{
    static_assert(
        std::is_integral<SrcKeyType>::value &&
        std::is_integral<DstKeyType>::value,
        "Source and destination histogram types are not appropriate");

    using value_t = typename histogram<SrcKeyType>::value_type;
    dst_hist.clear();
    double sum          = src_hist.sum();
    SrcKeyType min_key  = std::numeric_limits<DstKeyType>::min();
    SrcKeyType max_key  = std::numeric_limits<DstKeyType>::max();
    auto cumltv_srchist = cumulative_histogram(src_hist);
    std::map<SrcKeyType, DstKeyType> color_map;
    std::for_each(cumltv_srchist.begin(), cumltv_srchist.end(), [&](value_t const& v) {
        DstKeyType trnsfrmd_key =
            static_cast<DstKeyType>((v.second * (max_key - min_key)) / sum + min_key);
        color_map[std::get<0>(v.first)] = trnsfrmd_key;
    });
    std::for_each(src_hist.begin(), src_hist.end(), [&](value_t const& v) {
        dst_hist[color_map[std::get<0>(v.first)]] += v.second;
    });
    return color_map;
}

/// \overload histogram_equalization
/// \ingroup HE
/// @param src_view  INPUT source image view
/// @param dst_view  OUTPUT Output image view
/// @param bin_width INPUT Histogram bin width
/// @param mask      INPUT Specify is mask is to be used
/// @param src_mask  INPUT Mask vector over input image
/// \brief Overload for histogram equalization algorithm, takes in both source & destination
///        image views and histogram equalizes the input image.
///
template <typename SrcView, typename DstView>
void histogram_equalization(
    SrcView const& src_view,
    DstView const& dst_view,
    std::size_t bin_width = 1,
    bool mask = false,
    std::vector<std::vector<bool>> src_mask = {})
{
    gil_function_requires<ImageViewConcept<SrcView>>();
    gil_function_requires<MutableImageViewConcept<DstView>>();

    static_assert(
        color_spaces_are_compatible<
            typename color_space_type<SrcView>::type,
            typename color_space_type<DstView>::type>::value,
        "Source and destination views must have same color space");

    // Defining channel type
    using source_channel_t = typename channel_type<SrcView>::type;
    using dst_channel_t    = typename channel_type<DstView>::type;
    using coord_t          = typename SrcView::x_coord_t;

    std::size_t const channels = num_channels<SrcView>::value;
    coord_t const width        = src_view.width();
    coord_t const height       = src_view.height();
    std::size_t pixel_max      = std::numeric_limits<dst_channel_t>::max();
    std::size_t pixel_min      = std::numeric_limits<dst_channel_t>::min();

    for (std::size_t i = 0; i < channels; i++)
    {
        histogram<source_channel_t> h;
        fill_histogram(nth_channel_view(src_view, i), h, bin_width, false, false, mask, src_mask);
        h.normalize();
        auto h2 = cumulative_histogram(h);
        for (std::ptrdiff_t src_y = 0; src_y < height; ++src_y)
        {
            auto src_it = nth_channel_view(src_view, i).row_begin(src_y);
            auto dst_it = nth_channel_view(dst_view, i).row_begin(src_y);
            for (std::ptrdiff_t src_x = 0; src_x < width; ++src_x)
            {
                if (mask && !src_mask[src_y][src_x])
                    dst_it[src_x][0] = channel_convert<dst_channel_t>(src_it[src_x][0]);
                else
                    dst_it[src_x][0] = static_cast<dst_channel_t>(
                        h2[src_it[src_x][0]] * (pixel_max - pixel_min) + pixel_min);
            }
        }
    }
}

}}  //namespace boost::gil

#endif


// https://www.boost.org/doc/libs/1_71_0/more/getting_started/windows.html

#include <boost/lambda/lambda.hpp>
#include <iostream>
#include <iterator>
#include <algorithm>

using namespace std;
using namespace boost::lambda;

int main()
{
	typedef istream_iterator<int> in;

	cout << "Usage: Enter some numbers in the console with space between and press enter" << endl;

	for_each(
		in(cin), in(), cout << (_1 * 3) << " ");
}


#include "layer.h"

#include <random>
#include <cmath>
#include <boost/range/algorithm/fill.hpp>
#include <boost/range/algorithm/for_each.hpp>

namespace ann {

Layer::Layer(size_t inputRowCount, size_t outputRowCount, layerFx forwardFunc, layerFx derivateFunc)
    : mForwardFunc(forwardFunc)
    , mDerivateFunc(derivateFunc)
{
    mWeights = math::make_matrix<float>(outputRowCount, inputRowCount);
    mBias = math::make_matrix<float>(outputRowCount, 1);

    std::random_device rd{};
    std::mt19937 generator{rd()};
    std::normal_distribution<float> distribution{0.0f, 1.0f};

    const auto div = std::sqrtf(static_cast<float>(inputRowCount / 2));
    boost::for_each(mWeights, [&generator, &distribution, &div](float &value) {
        value = distribution(generator) / div;
    });
    boost::fill(mBias, 0.01f);
}

math::MatrixF Layer::feedForward(const math::MatrixF &inputData)
{
    mInput = inputData;
    mZ = (mWeights * mInput).addColumnVector(mBias);
    mActivation = mForwardFunc(mZ);
    return mActivation;
}

math::MatrixF Layer::beginBackPropagation(const math::MatrixF &expectedOutput)
{
    const auto m = static_cast<float>(mInput.getColumnCount());

    auto lossd = mActivation - expectedOutput;
    mWeightDelta = (1.0f / m) * (lossd * mInput.transpose());
    mBiasDelta =  (1.0f / m) * math::rowSum(lossd);

    return mWeights.transpose() * lossd;
}

math::MatrixF Layer::calculateGradients(const math::MatrixF& prevDz)
{
    const auto m = static_cast<float>(mInput.getColumnCount());

    const auto dz = prevDz.hadamardProduct(mDerivateFunc(mActivation));
    mWeightDelta = (1.0f / m) * (dz * mInput.transpose());
    mBiasDelta = (1.0f / m) * math::rowSum(dz);

    return mWeights.transpose() * dz;
}

void Layer::applyGradients(float learnSpeed)
{
    mWeights = mWeights - (learnSpeed * mWeightDelta);
    mBias = mBias - (learnSpeed * mBiasDelta);
}

}


#include <Eigen/StdVector>
#include <unsupported/Eigen/BVH>
#include <iostream>

using namespace Eigen;
typedef AlignedBox<double, 2> Box2d;

Box2d ei_bounding_box(const Vector2d &v) { return Box2d(v, v); } //compute the bounding box of a single point

struct PointPointMinimizer //how to compute squared distances between points and rectangles
{
  PointPointMinimizer() : calls(0) {}
  typedef double Scalar;

  double minimumOnVolumeVolume(const Box2d &r1, const Box2d &r2) { ++calls; return r1.squaredExteriorDistance(r2); }
  double minimumOnVolumeObject(const Box2d &r, const Vector2d &v) { ++calls; return r.squaredExteriorDistance(v); }
  double minimumOnObjectVolume(const Vector2d &v, const Box2d &r) { ++calls; return r.squaredExteriorDistance(v); }
  double minimumOnObjectObject(const Vector2d &v1, const Vector2d &v2) { ++calls; return (v1 - v2).squaredNorm(); }

  int calls;
};

int main()
{
  typedef std::vector<Vector2d, aligned_allocator<Vector2d> > StdVectorOfVector2d;
  StdVectorOfVector2d redPoints, bluePoints;
  for(int i = 0; i < 100; ++i) { //initialize random set of red points and blue points
    redPoints.push_back(Vector2d::Random());
    bluePoints.push_back(Vector2d::Random());
  }

  PointPointMinimizer minimizer;
  double minDistSq = std::numeric_limits<double>::max();

  //brute force to find closest red-blue pair
  for(int i = 0; i < (int)redPoints.size(); ++i)
    for(int j = 0; j < (int)bluePoints.size(); ++j)
      minDistSq = std::min(minDistSq, minimizer.minimumOnObjectObject(redPoints[i], bluePoints[j]));
  std::cout << "Brute force distance = " << sqrt(minDistSq) << ", calls = " << minimizer.calls << std::endl;

  //using BVH to find closest red-blue pair
  minimizer.calls = 0;
  KdBVH<double, 2, Vector2d> redTree(redPoints.begin(), redPoints.end()), blueTree(bluePoints.begin(), bluePoints.end()); //construct the trees
  minDistSq = BVMinimize(redTree, blueTree, minimizer); //actual BVH minimization call
  std::cout << "BVH distance         = " << sqrt(minDistSq) << ", calls = " << minimizer.calls << std::endl;

  return 0;
}


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, Indiana University,
// Bloomington, IN 47405.
//
// Permission to modify the code and to distribute the code is
// granted, provided the text of this NOTICE is retained, a notice if
// the code was modified is included with the above COPYRIGHT NOTICE
// and with the COPYRIGHT NOTICE in the LICENSE file, and that the
// LICENSE file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
#include <iostream>
#include <boost/graph/edge_list.hpp>
#include <boost/graph/bellman_ford_shortest_paths.hpp>

int
main()
{
  using namespace boost;
  // ID numbers for the routers (vertices).
  enum
  { A, B, C, D, E, F, G, H, n_vertices };
  const int n_edges = 11;
  typedef std::pair < int, int >Edge;

  // The list of connections between routers stored in an array.
  Edge edges[] = {
  Edge(A, B), Edge(A, C),
        Edge(B, D), Edge(B, E), Edge(C, E), Edge(C, F), Edge(D, H),
        Edge(D, E), Edge(E, H), Edge(F, G), Edge(G, H)
  };

  // Specify the graph type and declare a graph object
  typedef edge_list < Edge*, Edge, std::ptrdiff_t, std::random_access_iterator_tag> Graph;
  Graph g(edges, edges + n_edges);

  // The transmission delay values for each edge.
  float delay[] =
    {5.0, 1.0, 1.3, 3.0, 10.0, 2.0, 6.3, 0.4, 1.3, 1.2, 0.5};

  // Declare some storage for some "external" vertex properties.
  char name[] = "ABCDEFGH";
  int parent[n_vertices];
  for (int i = 0; i < n_vertices; ++i)
    parent[i] = i;
  float distance[n_vertices];
  std::fill(distance, distance + n_vertices, std::numeric_limits < float >::max());
  // Specify A as the source vertex
  distance[A] = 0;

  bool r = bellman_ford_shortest_paths(g, int (n_vertices),
                                       weight_map(make_iterator_property_map
                                                  (&delay[0],
                                                   get(edge_index, g),
                                                   delay[0])).
                                       distance_map(&distance[0]).
                                       predecessor_map(&parent[0]));

  if (r)
    for (int i = 0; i < n_vertices; ++i)
      std::cout << name[i] << ": " << distance[i]
        << " " << name[parent[i]] << std::endl;
  else
    std::cout << "negative cycle" << std::endl;

  return EXIT_SUCCESS;
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2015.
//  Copyright Nikhar Agrawal 2015.
//  Copyright Paul Bristow 2015.
//  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)
//

//! \file
//!\brief Perform tests of frexp and ldexp for fixed_point.

#define BOOST_TEST_MODULE test_negatable_func_frexp_ldexp
#define BOOST_LIB_DIAGNOSTIC

#include <cmath>
#include <iomanip>
#include <iostream>

#include <boost/fixed_point/fixed_point.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/test/included/unit_test.hpp>

namespace local
{
  template<typename FixedPointType>
  const FixedPointType& tolerance_maker(const int fuzzy_bits)
  {
    static const FixedPointType the_tolerance = ldexp(FixedPointType(1), FixedPointType::resolution + fuzzy_bits);

    return the_tolerance;
  }

  template<typename FixedPointType,
           typename FloatPointType = typename FixedPointType::float_type>
  void test_frexp_ldexp(const int fuzzy_bits)
  {
    // Use at least 17 resolution bits.
    // Use at least  6 range bits.

    BOOST_STATIC_ASSERT(-FixedPointType::resolution >= 17);
    BOOST_STATIC_ASSERT( FixedPointType::range      >=  6);

    using std::ldexp;
    using std::frexp;

    const FixedPointType a1(boost::math::constants::pi        <FixedPointType>());      const FloatPointType b1(boost::math::constants::pi        <FloatPointType>());
    const FixedPointType a2(boost::math::constants::e         <FixedPointType>());      const FloatPointType b2(boost::math::constants::e         <FloatPointType>());
    const FixedPointType a3(boost::math::constants::ln_two    <FixedPointType>());      const FloatPointType b3(boost::math::constants::ln_two    <FloatPointType>());
    const FixedPointType a4(boost::math::constants::zeta_three<FixedPointType>());      const FloatPointType b4(boost::math::constants::zeta_three<FloatPointType>());
    const FixedPointType a5(boost::math::constants::pi        <FixedPointType>() * 11); const FloatPointType b5(boost::math::constants::pi        <FloatPointType>() * 11);
    const FixedPointType a6(boost::math::constants::pi        <FixedPointType>() / 11); const FloatPointType b6(boost::math::constants::pi        <FloatPointType>() / 11);
    const FixedPointType a7(boost::math::constants::ln_two    <FixedPointType>() /  5); const FloatPointType b7(boost::math::constants::ln_two    <FloatPointType>() /  5);
    const FixedPointType a8(boost::math::constants::phi       <FixedPointType>() / 11); const FloatPointType b8(boost::math::constants::phi       <FloatPointType>() / 11);

    FixedPointType c1; FixedPointType c2; FixedPointType c3; FixedPointType c4;
    FixedPointType c5; FixedPointType c6; FixedPointType c7; FixedPointType c8;

    int exp2a1; int exp2a2; int exp2a3; int exp2a4;
    int exp2a5; int exp2a6; int exp2a7; int exp2a8;

    int exp2b1; int exp2b2; int exp2b3; int exp2b4;
    int exp2b5; int exp2b6; int exp2b7; int exp2b8;

    BOOST_CHECK_CLOSE_FRACTION(c1 = frexp(a1, &exp2a1), FixedPointType(frexp(b1, &exp2b1)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(c2 = frexp(a2, &exp2a2), FixedPointType(frexp(b2, &exp2b2)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(c3 = frexp(a3, &exp2a3), FixedPointType(frexp(b3, &exp2b3)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(c4 = frexp(a4, &exp2a4), FixedPointType(frexp(b4, &exp2b4)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(c5 = frexp(a5, &exp2a5), FixedPointType(frexp(b5, &exp2b5)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(c6 = frexp(a6, &exp2a6), FixedPointType(frexp(b6, &exp2b6)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(c7 = frexp(a7, &exp2a7), FixedPointType(frexp(b7, &exp2b7)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(c8 = frexp(a8, &exp2a8), FixedPointType(frexp(b8, &exp2b8)), tolerance_maker<FixedPointType>(fuzzy_bits));

    BOOST_CHECK_EQUAL(exp2a1, exp2b1);
    BOOST_CHECK_EQUAL(exp2a2, exp2b2);
    BOOST_CHECK_EQUAL(exp2a3, exp2b3);
    BOOST_CHECK_EQUAL(exp2a4, exp2b4);
    BOOST_CHECK_EQUAL(exp2a5, exp2b5);
    BOOST_CHECK_EQUAL(exp2a6, exp2b6);
    BOOST_CHECK_EQUAL(exp2a7, exp2b7);
    BOOST_CHECK_EQUAL(exp2a8, exp2b8);

    BOOST_CHECK_CLOSE_FRACTION(ldexp(c1, exp2a1), a1, tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(ldexp(c2, exp2a2), a2, tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(ldexp(c3, exp2a3), a3, tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(ldexp(c4, exp2a4), a4, tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(ldexp(c5, exp2a5), a5, tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(ldexp(c6, exp2a6), a6, tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(ldexp(c7, exp2a7), a7, tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(ldexp(c8, exp2a8), a8, tolerance_maker<FixedPointType>(fuzzy_bits));

    BOOST_CHECK_CLOSE_FRACTION(ldexp(c1, exp2a1), FixedPointType(b1), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(ldexp(c2, exp2a2), FixedPointType(b2), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(ldexp(c3, exp2a3), FixedPointType(b3), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(ldexp(c4, exp2a4), FixedPointType(b4), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(ldexp(c5, exp2a5), FixedPointType(b5), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(ldexp(c6, exp2a6), FixedPointType(b6), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(ldexp(c7, exp2a7), FixedPointType(b7), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(ldexp(c8, exp2a8), FixedPointType(b8), tolerance_maker<FixedPointType>(fuzzy_bits));
  }
}

BOOST_AUTO_TEST_CASE(test_negatable_func_frexp_ldexp)
{
  { typedef boost::fixed_point::negatable<  6,  -17> fixed_point_type; static_cast<void>(local::test_frexp_ldexp<fixed_point_type>(5)); }
  { typedef boost::fixed_point::negatable<  6,  -25> fixed_point_type; static_cast<void>(local::test_frexp_ldexp<fixed_point_type>(5)); }
  { typedef boost::fixed_point::negatable<  6,  -46> fixed_point_type; static_cast<void>(local::test_frexp_ldexp<fixed_point_type>(5)); }
  { typedef boost::fixed_point::negatable<  6,  -57> fixed_point_type; static_cast<void>(local::test_frexp_ldexp<fixed_point_type>(5)); }
  { typedef boost::fixed_point::negatable<  6, -121> fixed_point_type; static_cast<void>(local::test_frexp_ldexp<fixed_point_type>(5)); }
  { typedef boost::fixed_point::negatable<  6, -233> fixed_point_type; static_cast<void>(local::test_frexp_ldexp<fixed_point_type>(5)); }
  { typedef boost::fixed_point::negatable< 39, -200> fixed_point_type; static_cast<void>(local::test_frexp_ldexp<fixed_point_type>(5)); }
  { typedef boost::fixed_point::negatable<100, -139> fixed_point_type; static_cast<void>(local::test_frexp_ldexp<fixed_point_type>(5)); }
}


// Copyright (c) 2018 Graphcore Ltd. All rights reserved.
// Simple tests for popsolver.
//
#include <popsolver/Model.hpp>
#define BOOST_TEST_MODULE Div
#include "poplibs_support/Algorithm.hpp"
#include <boost/test/unit_test.hpp>

using namespace popsolver;

BOOST_AUTO_TEST_CASE(CeilDiv) {
  Model m;
  auto a = m.addConstant(10);
  auto b = m.addConstant(4);
  auto c = m.ceildiv(a, b);
  auto s = m.minimize(c);
  BOOST_CHECK_EQUAL(s[c], DataType{3});
}

BOOST_AUTO_TEST_CASE(CeilDiv2) {
  Model m;
  auto a = m.addVariable();
  auto b = m.addConstant(4);
  auto c = m.ceildiv(a, b);
  m.lessOrEqual(DataType{3}, c);
  auto s = m.minimize(a);
  BOOST_CHECK_EQUAL(s[a], DataType{9});
  BOOST_CHECK_EQUAL(s[c], DataType{3});
}

BOOST_AUTO_TEST_CASE(FloorDiv) {
  Model m;
  auto a = m.addVariable();
  auto b = m.addConstant(4);
  auto c = m.floordiv(a, b);
  m.lessOrEqual(DataType{3}, c);
  auto s = m.minimize(a);
  BOOST_CHECK_EQUAL(s[a], DataType{12});
  BOOST_CHECK_EQUAL(s[c], DataType{3});
}

BOOST_AUTO_TEST_CASE(CeilDivZero) {
  Model m;
  auto a = m.addVariable();
  auto b = m.addConstant(0);
  auto c = m.ceildiv(a, b);
  BOOST_CHECK_EQUAL(m.minimize(c).validSolution(), false);
}

BOOST_AUTO_TEST_CASE(CeilDivConstrainDivisor) {
  using poplibs_support::ceildiv;
  const unsigned maxDivisor = 20;
  for (unsigned divisor = 1; divisor != maxDivisor; ++divisor) {
    Model m;
    const unsigned dividend = 49;
    auto a = m.addConstant(dividend, "a");
    auto b = m.addConstant(divisor, "b");
    auto c = m.ceildivConstrainDivisor(a, b, "c");

    auto s = m.minimize(c);
    // The constrained div result is only valid when a smaller divisor would
    // give a different result
    auto expectSolution = divisor < 2 || ceildiv(dividend, divisor) !=
                                             ceildiv(dividend, divisor - 1);
    BOOST_CHECK_EQUAL(s.validSolution(), expectSolution);
    // division correct?
    if (expectSolution && s.validSolution()) {
      BOOST_CHECK_EQUAL(s[c], DataType{poplibs_support::ceildiv(
                                  dividend, s[b].getAs<unsigned>())});
    }
  }
}


#ifndef MLA_SOLVERS_CONJUGATE_GRADIENT_HPP
#define MLA_SOLVERS_CONJUGATE_GRADIENT_HPP

#include <string>

#include <boost/lexical_cast.hpp>

#include <mla/matrix/all.h++>
#include <mla/vector/all.h++>

#include <mla/operations/level1/dot.h++>
#include <mla/operations/level1/axpy.h++>
#include <mla/operations/level1/scale.h++>
#include <mla/operations/level2/gemv.h++>

#include <mla/solvers/SolverReturnCodes.h++>

#include <mla/output.h++>

namespace mla
{

/**
 * Conjugate gradient method algorithm
 *
 *@param A	Matrix
 *@param x	unknown vector
 *@param b	vector
 *@param delta	error tolerance
 *@param max_iterations	maximum number of iterations allowed
 *
 *@return
 */
template<typename Scalar, template<typename> class MatrixStoragePolicy, template<typename> class VectorStoragePolicyX, template<typename> class VectorStoragePolicyB >
ReturnCode
cg(MatrixStoragePolicy<Scalar> &A, VectorStoragePolicyX<Scalar> &x, VectorStoragePolicyB<Scalar> &b, const double delta, int max_iterations)
{
	if( !A.isSquare() )
	{
		throw LAException("cg: A must be a square matrix");
	}

	if(A.columns() != b.size())
	{
		throw LAException("cg: A.columns() != b.size()");
	}

	x.resize( b.size() );

	VectorStoragePolicyB<Scalar> r, p(A.columns());
	VectorStoragePolicyB<Scalar> Ap(A.columns());
	Scalar dotrr, dotrrnew, alpha;

	//r = b - A*x;
	r = b;
	mla::gemv( (Scalar)(-1.0f), A, x, (Scalar)(1.0f), r );
	p = r;

	dotrr = dot(r,r);

	for (int iter = 0; iter < max_iterations; iter++)
	{
		//Ap = A*p;
		mla::gemv( (Scalar)1.0f, A, p, (Scalar)1.0f, Ap);

		alpha = dotrr/dot(p,Ap);
		//x = x + alpha*p;
		mla::axpy(alpha, p, x);

		// r_{k+1] = r_{k} - a*A*p
		mla::axpy( (Scalar)-1.0f*alpha, Ap, r);

		dotrrnew = dot(r,r);

		if(dotrrnew < delta)
		{
			return OK;
		}

		//p = r + (dotrrnew/dotrr)*p;
		mla::scale( dotrrnew/dotrr, p);
		mla::axpy( (Scalar)1.0f, r, p);

		dotrr = dotrrnew;
	}

	return ERR_EXCESSIVE_ITERATIONS;
}

}

#endif


/*
 * Copyright 2009-2020 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE cubicspline_test

// Standard includes
#include <iostream>

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

// Local VOTCA includes
#include "votca/tools/cubicspline.h"

using namespace votca::tools;

BOOST_AUTO_TEST_SUITE(cubicspline_test)

BOOST_AUTO_TEST_CASE(spline_grid_high) {
  CubicSpline cspline;
  cspline.setBCInt(0);
  cspline.GenerateGrid(0, 9, 2);
  Eigen::VectorXd values_ref = Eigen::VectorXd::Zero(5);
  values_ref << 0, 2, 4, 6, 9;

  bool equal_val = values_ref.isApprox(cspline.getX(), 1e-5);
  BOOST_CHECK_EQUAL(equal_val, true);
  if (!equal_val) {
    std::cout << "result value" << std::endl;
    std::cout << cspline.getX().transpose() << std::endl;
    std::cout << "ref value" << std::endl;
    std::cout << values_ref.transpose() << std::endl;
  }
}

BOOST_AUTO_TEST_CASE(spline_grid_low) {
  CubicSpline cspline;
  cspline.setBCInt(0);
  cspline.GenerateGrid(0, 7, 2);
  Eigen::VectorXd values_ref = Eigen::VectorXd::Zero(4);
  values_ref << 0, 2, 4, 7;

  bool equal_val = values_ref.isApprox(cspline.getX(), 1e-5);
  BOOST_CHECK_EQUAL(equal_val, true);
  if (!equal_val) {
    std::cout << "result value" << std::endl;
    std::cout << cspline.getX().transpose() << std::endl;
    std::cout << "ref value" << std::endl;
    std::cout << values_ref.transpose() << std::endl;
  }
}

BOOST_AUTO_TEST_CASE(cubicspline_fit_test) {

  votca::Index size = 80;
  Eigen::VectorXd x = Eigen::VectorXd::Zero(size);
  Eigen::VectorXd y = Eigen::VectorXd::Zero(size);
  for (votca::Index i = 0; i < size; ++i) {
    x(i) = 0.25 * double(i);
    y(i) = std::sin(x(i));
  }
  CubicSpline cspline;
  cspline.setBCInt(0);
  cspline.GenerateGrid(0.4, 0.6, 0.1);
  Eigen::VectorXd gridpoints_ref = Eigen::VectorXd::Zero(3);
  gridpoints_ref << 0.4, 0.5, 0.6;
  bool grid_check = cspline.getX().isApprox(gridpoints_ref, 1e-5);
  BOOST_CHECK_EQUAL(grid_check, true);
  if (!grid_check) {
    std::cout << "result value" << std::endl;
    std::cout << cspline.getX().transpose() << std::endl;
    std::cout << "ref value" << std::endl;
    std::cout << gridpoints_ref.transpose() << std::endl;
  }
  cspline.Fit(x, y);

  Eigen::VectorXd rs = Eigen::VectorXd::Zero(10);
  rs << 0.45, 0.47, 0.8, 0.75, 0.6, 0.4, 0.9, 0.55, 0, 0;
  Eigen::VectorXd values_ref = Eigen::VectorXd::Zero(10);
  values_ref << 0.311213, 0.310352, 0.296153, 0.298304, 0.304759, 0.313364,
      0.291851, 0.30691, 0.33058, 0.33058;
  Eigen::VectorXd derivatives_ref = Eigen::VectorXd::Zero(10);
  derivatives_ref << -0.0430277, -0.0430282, -0.0430231, -0.0430267, -0.0430313,
      -0.0430272, -0.0430128, -0.0430308, -0.04306, -0.04306;
  Eigen::VectorXd values = cspline.Calculate(rs);
  Eigen::VectorXd derivatives = cspline.CalculateDerivative(rs);

  bool equal_val = values_ref.isApprox(values, 1e-5);

  if (!equal_val) {
    std::cout << "result value" << std::endl;
    std::cout << values.transpose() << std::endl;
    std::cout << "ref value" << std::endl;
    std::cout << values_ref.transpose() << std::endl;
  }
  BOOST_CHECK_EQUAL(equal_val, true);

  bool equal_derivative = derivatives_ref.isApprox(derivatives, 1e-5);

  if (!equal_derivative) {
    std::cout << "result value" << std::endl;
    std::cout << derivatives.transpose() << std::endl;
    std::cout << "ref value" << std::endl;
    std::cout << derivatives_ref.transpose() << std::endl;
  }
  BOOST_CHECK_EQUAL(equal_derivative, true);
}

BOOST_AUTO_TEST_CASE(cubicspline_interpolate_test) {

  int size = 80;
  Eigen::VectorXd x = Eigen::VectorXd::Zero(size);
  Eigen::VectorXd y = Eigen::VectorXd::Zero(size);
  for (int i = 0; i < size; ++i) {
    x(i) = 0.25 * i;
    y(i) = std::sin(x(i));
  }
  CubicSpline cspline;
  cspline.setBCInt(0);
  cspline.Interpolate(x, y);

  Eigen::VectorXd rs = Eigen::VectorXd::Zero(10);
  rs << 0.45, 0.47, 0.8, 0.75, 0.6, 0.4, 0.9, 0.55, 0, 0;
  Eigen::VectorXd values_ref = Eigen::VectorXd::Zero(10);
  values_ref << 0.434964, 0.452886, 0.717353, 0.681639, 0.564637, 0.389415,
      0.78332, 0.522684, 0, 0;
  Eigen::VectorXd derivatives_ref = Eigen::VectorXd::Zero(10);
  derivatives_ref << 0.900494, 0.891601, 0.69661, 0.731673, 0.825305, 0.921091,
      0.621663, 0.852451, 0.999978, 0.999978;
  Eigen::VectorXd values = cspline.Calculate(rs);
  Eigen::VectorXd derivatives = cspline.CalculateDerivative(rs);

  bool equal_val = values_ref.isApprox(values, 1e-5);

  if (!equal_val) {
    std::cout << "result value" << std::endl;
    std::cout << values.transpose() << std::endl;
    std::cout << "ref value" << std::endl;
    std::cout << values_ref.transpose() << std::endl;
  }
  BOOST_CHECK_EQUAL(equal_val, true);

  bool equal_derivative = derivatives_ref.isApprox(derivatives, 1e-5);

  if (!equal_derivative) {
    std::cout << "result value" << std::endl;
    std::cout << derivatives.transpose() << std::endl;
    std::cout << "ref value" << std::endl;
    std::cout << derivatives_ref.transpose() << std::endl;
  }
  BOOST_CHECK_EQUAL(equal_derivative, true);
}

BOOST_AUTO_TEST_CASE(cubicspline_matrix_test) {

  CubicSpline cspline;
  cspline.setBCInt(0);
  cspline.GenerateGrid(0.4, 0.6, 0.1);
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(1, 6);
  Eigen::MatrixXd Aref = Eigen::MatrixXd::Zero(1, 6);

  Aref << 0.0, -9.0, 10.0, 0.0, -0.03333333, -0.01666667;

  cspline.AddToFitMatrix(A, 0.5, 0, 0, 1.0, 1.0);

  bool equalMatrix = Aref.isApprox(A, 1e-5);
  if (!equalMatrix) {
    std::cout << "result A" << std::endl;
    std::cout << A << std::endl;
    std::cout << "ref A" << std::endl;
    std::cout << Aref << std::endl;
  }
  BOOST_CHECK_EQUAL(equalMatrix, true);
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * Copyright Evan Miller, 2020
 * 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)
 */

#include "mp_t.hpp"
#include <boost/math/tools/test_data.hpp>
#include <boost/test/included/prg_exec_monitor.hpp>
#include <boost/math/special_functions/jacobi_theta.hpp>
#include <fstream>
#include <boost/math/tools/test_data.hpp>

using namespace boost::math::tools;
using namespace boost::math;
using namespace std;

struct jacobi_theta_data_generator
{
   boost::math::tuple<mp_t, mp_t, mp_t, mp_t> operator()(mp_t z, mp_t tau)
   {
      return boost::math::make_tuple(
              jacobi_theta1tau(z, tau),
              jacobi_theta2tau(z, tau),
              jacobi_theta3tau(z, tau),
              jacobi_theta4tau(z, tau));
   }
};

int cpp_main(int argc, char*argv [])
{
   parameter_info<mp_t> arg1, arg2;
   test_data<mp_t> data;

   bool cont;
   std::string line;

   if(argc < 1)
      return 1;

   std::cout << "Welcome.\n"
      "This program will generate spot tests for the Jacobi Theta functions.\n"
      ;

   do{
      if(0 == get_user_parameter_info(arg1, "z"))
         return 1;
      if(0 == get_user_parameter_info(arg2, "tau"))
         return 1;

      data.insert(jacobi_theta_data_generator(), arg1, arg2);

      std::cout << "Any more data [y/n]?";
      std::getline(std::cin, line);
      boost::algorithm::trim(line);
      cont = (line == "y");
   } while(cont);

   std::cout << "Generating " << data.size() << " test points.";

   std::cout << "Enter name of test data file [default=jacobi_theta.ipp]";
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "jacobi_theta.ipp";
   std::ofstream ofs(line.c_str());
   ofs << std::scientific << std::setprecision(40);
   write_code(ofs, data, "jacobi_theta_data");

   return 0;
}


#include <memory>
#include <algorithm>
#include <vector>
#include "lib.h"
#include "expression.h"
#include "phase_expression.h"
#include "phase_solver.h"
#include <thread>
#include <iterator>
#include "exprtk.hpp"

#include <boost/numeric/odeint.hpp>
#include <cmath>

using Variables = exprtk::symbol_table<double>;
using Formula = exprtk::expression<double>;
using Parser = exprtk::parser<double>;

using state_type = std::vector<double>;

class CauchyProblem {

public:
    CauchyProblem(double init_x, double init_y):
        _init_x(init_x), _init_y(init_y) { };

    void operator()(const state_type & x, state_type & dxdt, double t) {
        dxdt[0] = 2 * std::exp(pow(t, 2)) - x[0] / t;
    };

private:
    double _init_x;
    double _init_y;
};

double ode(double x, double t) {
    return 2 * std::exp(pow(t, 2)) - x / t;;
}

double runge_kutta_step(double x, double t, double step) {
    auto k1 = ode(x, t);
    auto k2 = ode(x + step / 2 * k1, t + step / 2);
    auto k3 = ode(x + step / 2 * k2, t + step / 2);
    auto k4 = ode(x + step * k3, t + step);
    return x + step / 6 * (k1 + 2 * k2 + 2 * k3 + k4);
}

auto expression = std::make_shared<PhaseExpression>(
        1,
        "2*exp(t^2)-x/t",
        std::make_pair(0.0, 0.0));

double runge_kutta_step_expr(double x, double t, double step) {

    auto k1 = expression->evaluate(x, t);
    auto k2 = expression->evaluate(x + step / 2 * k1, t + step / 2);
    auto k3 = expression->evaluate(x + step / 2 * k2, t + step / 2);
    auto k4 = expression->evaluate(x + step * k3, t + step);
    return x + step / 6 * (k1 + 2 * k2 + 2 * k3 + k4);
}

void cauchy_problem(const state_type & x, state_type & dxdt, double t) {
    dxdt[0] = 2 * std::exp(pow(t, 2)) - x[0] / t;
};

int main (int argc, char * argv[])
{
    const double t = 1;
    const double m_x = std::exp(1);
    const double step = 0.0000001;

    // auto system = CauchyProblem(1, std::exp(1));
    // auto stepper = boost::numeric::odeint::runge_kutta4<state_type>();

    // state_type x { std::exp(1) };
    //     // state_type x { 27.2991 };

    // boost::numeric::odeint::integrate_const(stepper, system, x, 1.0, 2.0, step);

    // std::cout<< "The solution at 2.0 is " << x[x.size() - 1] << std::endl;

    // auto expression = std::make_shared<PhaseExpression>(
    //     1,
    //     "2*exp(t^2)-x/t",
    //     std::make_pair(m_x, t));
    // auto solver = PhaseSolver(expression, std::make_pair(m_x, t));
    // auto result = solver.solve(2.0, step);
    // std::cout<< "The solution with CustomSolver at 2.0 is " << result << std::endl;

    auto correctColution = std::exp(pow(2, 2)) / 2;

    auto cur_x = m_x;
    for (double cur_t = t; cur_t <= 2; cur_t += step) {
        cur_x = runge_kutta_step(cur_x, cur_t, step);
    }

    std::cout << "Manual Runge-Kutta at point 2 is " << cur_x << std::endl;
    std::cout << "The right solution is " << correctColution << std::endl;

    return 0;
}


//  (C) Copyright Raffi Enficiaud 2014.
//  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)

//  See http://www.boost.org/libs/test for the library home page.

//[example_code
#define BOOST_TEST_MODULE example67
#include <boost/test/included/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>
#include <sstream>

namespace bdata = boost::unit_test::data;

// Generates a Fibonacci sequence
std::vector<float> fibonacci() {
  std::vector<float> ret(8);
  ret[0] = 0;
  ret[1] = 1;

  for(std::size_t s(2); s < ret.size(); s++)
  {
    ret[s] = ret[s-1] + ret[s-2];
  }
  return ret;
}

BOOST_DATA_TEST_CASE(
  test1,
  bdata::make(fibonacci()),
  array_element)
{
  std::cout << "test 1: "
    << array_element
    << std::endl;
  BOOST_TEST(array_element <= 13);
}

// Generates a map from a vector
std::map<std::string, float> vect_2_str(std::vector<float> v)
{
  std::map<std::string, float> out;
  for(std::size_t s(0); s < v.size(); s++)
  {
    std::ostringstream o;
    o << v[s];
    out[o.str()] = v[s];
  }
  return out;
}

typedef std::pair<const std::string, float> pair_map_t;
BOOST_TEST_DONT_PRINT_LOG_VALUE( pair_map_t )

BOOST_DATA_TEST_CASE(
  test2,
  bdata::make(vect_2_str(fibonacci())),
  array_element)
{
  std::cout << "test 2: \""
    << array_element.first << "\", "
    << array_element.second
    << std::endl;
  BOOST_TEST(array_element.second <= 13);
}
//]


//
// Copyright (c) 2009, Markus Rickert
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//

#include <fstream>
#include <iostream>
#include <Eigen/Eigenvalues>
#include <rl/math/Matrix.h>
#include <rl/math/Vector.h>

int
main(int argc, char** argv)
{
	if (argc < 2)
	{
		std::cout << "Usage: rlPcaDemo FILE" << std::endl;
		return EXIT_FAILURE;
	}

	std::ifstream data;
	data.open(argv[1]);

	std::size_t rows;
	data >> rows;
	std::size_t cols;
	data >> cols;

	rl::math::Matrix src(rows, cols);

	for (std::size_t i = 0; i < rows; ++i)
	{
		for (std::size_t j = 0; j < cols; ++j)
		{
			data >> src(i, j);
		}
	}

	data.close();

	std::cout << "src = " << std::endl << src << std::endl;

	rl::math::Vector mean(cols);
	mean.setZero();

	for (std::size_t i = 0; i < rows; ++i)
	{
		mean += src.row(i);
	}

	mean /= static_cast<rl::math::Real>(rows);

	std::cout << "mean = " << mean.transpose() << std::endl;

	rl::math::Matrix covariance(cols, cols);
	covariance.setZero();

	for (std::size_t i = 0; i < rows; ++i)
	{
		rl::math::Vector delta = src.row(i).transpose() - mean;
		covariance += delta * delta.transpose();
	}

	covariance /= static_cast<rl::math::Real>(rows) - 1;

	std::cout << "covariance = " << std::endl << covariance << std::endl;

	Eigen::EigenSolver<rl::math::Matrix> eigen(covariance);

	std::cout << "eigenvectors = " << std::endl << eigen.eigenvectors() << std::endl;
	std::cout << "eigenvalues = " << std::endl << eigen.eigenvalues() << std::endl;

	return EXIT_SUCCESS;
}


// SPDX-License-Identifier: MIT
// Copyright (c) 2019-2021 Thomas Vanderbruggen <th.vanderbruggen@gmail.com>

#ifndef SCICPP_CORE_STATS
#define SCICPP_CORE_STATS

#include "scicpp/core/functional.hpp"
#include "scicpp/core/numeric.hpp"
#include "scicpp/core/units/quantity.hpp"

#include <Eigen/Dense>
#include <algorithm>
#include <cmath>
#include <limits>
#include <numeric>
#include <tuple>

namespace scicpp::stats {

namespace detail {

template <class Array>
auto quiet_nan() {
    return std::numeric_limits<typename Array::value_type>::quiet_NaN();
}

} // namespace detail

//---------------------------------------------------------------------------------
// amax
//---------------------------------------------------------------------------------

template <class Array>
constexpr auto amax(const Array &f) {
    if (f.empty()) {
        return detail::quiet_nan<Array>();
    }

    return *std::max_element(f.cbegin(), f.cend());
}

//---------------------------------------------------------------------------------
// amin
//---------------------------------------------------------------------------------

template <class Array>
constexpr auto amin(const Array &f) {
    if (f.empty()) {
        return detail::quiet_nan<Array>();
    }

    return *std::min_element(f.cbegin(), f.cend());
}

//---------------------------------------------------------------------------------
// ptp
//---------------------------------------------------------------------------------

template <class Array>
constexpr auto ptp(const Array &f) {
    if (f.empty()) {
        return detail::quiet_nan<Array>();
    }

    const auto [it_min, it_max] = std::minmax_element(f.cbegin(), f.cend());
    return *it_max - *it_min;
}

//---------------------------------------------------------------------------------
// average
//---------------------------------------------------------------------------------

template <class Array1, class Array2>
constexpr auto average(const Array1 &f, const Array2 &weights) {
    if (f.empty() || (f.size() != weights.size())) {
        return detail::quiet_nan<Array1>();
    }

    return inner(f, weights) / sum(weights);
}

//---------------------------------------------------------------------------------
// median
//---------------------------------------------------------------------------------

namespace detail {

// https://stackoverflow.com/questions/1719070/what-is-the-right-approach-when-using-stl-container-for-median-calculation
template <class InputIt>
auto median_inplace(InputIt first, InputIt last) {
    using T = typename std::iterator_traits<InputIt>::value_type;
    using raw_t = units::representation_t<T>;
    const auto size = std::distance(first, last);

    if (size == 0) {
        return std::numeric_limits<T>::quiet_NaN();
    }

    const signed_size_t half = size / 2;
    const auto target = first + half;
    std::nth_element(first, target, last);

    if (size % 2 != 0) { // vector size is odd
        return *target;
    } else {
        const auto max_it = std::max_element(first, first + half);
        return (*max_it + *target) / raw_t{2}; // cf. std::midpoint (C++20)
    }
}

} // namespace detail

template <class InputIt, class Predicate>
auto median(InputIt first, InputIt last, Predicate p) {
    auto v = filter(std::vector(first, last), p);
    return detail::median_inplace(v.begin(), v.end());
}

template <class Array, class Predicate>
auto median(const Array &f, Predicate filter) {
    return median(f.cbegin(), f.cend(), filter);
}

template <class Array>
auto median(Array &&f) {
    if constexpr (std::is_lvalue_reference_v<Array>) {
        auto tmp = f;
        return detail::median_inplace(tmp.begin(), tmp.end());
    } else {
        return detail::median_inplace(f.begin(), f.end());
    }
}

template <class Array>
auto nanmedian(const Array &f) {
    return median(f, filters::not_nan);
}

//---------------------------------------------------------------------------------
// mean
//---------------------------------------------------------------------------------

template <class InputIt, class Predicate>
constexpr auto mean(InputIt first, InputIt last, Predicate filter) {
    using T = typename std::iterator_traits<InputIt>::value_type;

    if (std::distance(first, last) == 0) {
        return std::numeric_limits<T>::quiet_NaN();
    }

    const auto [res, cnt] = sum(first, last, filter);
    return res / units::representation_t<T>(cnt);
}

template <class Array, class Predicate>
constexpr auto mean(const Array &f, Predicate filter) {
    return mean(f.cbegin(), f.cend(), filter);
}

template <class Array>
constexpr auto mean(const Array &f) {
    return mean(f, filters::all);
}

template <class Array>
auto nanmean(const Array &f) {
    return mean(f, filters::not_nan);
}

template <class Array, typename T = typename Array::value_type>
constexpr auto tmean(const Array &f,
                     const std::array<T, 2> &limits,
                     const std::array<bool, 2> &inclusive = {true, true}) {
    return mean(f, filters::Trim<T>(limits, inclusive));
}

//---------------------------------------------------------------------------------
// gmean
//---------------------------------------------------------------------------------

template <class Array>
auto gmean(Array &&f) {
    using T = typename std::decay_t<Array>::value_type;

    if (f.empty()) {
        return std::numeric_limits<T>::quiet_NaN();
    }

    if constexpr (units::is_quantity_v<T>) {
        using namespace operators;
        return T(std::exp(mean(log(std::forward<Array>(f) / T(1)))));
    } else {
        return std::exp(mean(log(std::forward<Array>(f))));
    }
}

template <class Array, class Predicate>
auto gmean(Array &&f, Predicate p) {
    return gmean(filter(std::forward<Array>(f), p));
}

template <class Array>
auto nangmean(Array &&f) {
    return gmean(std::forward<Array>(f), filters::not_nan);
}

//---------------------------------------------------------------------------------
// covariance
//---------------------------------------------------------------------------------

template <int ddof = 0, class InputIt1, class InputIt2, class Predicate>
constexpr auto covariance(InputIt1 first1,
                          InputIt1 last1,
                          InputIt2 first2,
                          InputIt2 last2,
                          Predicate filter) {
    using T1 = typename std::iterator_traits<InputIt1>::value_type;
    using T2 = typename std::iterator_traits<InputIt2>::value_type;
    using raw_t1 = units::representation_t<T1>;
    using raw_t2 = units::representation_t<T2>;
    using raw_t = std::common_type_t<raw_t1, raw_t2>;
    using prod_t = decltype(std::declval<T1>() * std::declval<T2>());

    static_assert(meta::is_predicate<Predicate, T1>);
    static_assert(meta::is_predicate<Predicate, T2>);

    scicpp_require(std::distance(first1, last1) ==
                   std::distance(first2, last2));

    if (std::distance(first1, last1) == 0) {
        return std::make_tuple(std::numeric_limits<prod_t>::quiet_NaN(),
                               signed_size_t(0));
    }

    // Pairwise recursive implementation of covariance summation
    const auto [m1_, m2_, cov_, c_] = pairwise_accumulate<64>(
        first1,
        last1,
        first2,
        last2,
        [&](auto f1, auto l1, auto f2, auto l2) {
            const auto m1 = mean(f1, l1, filter);
            const auto m2 = mean(f2, l2, filter);

            auto res = utils::set_zero<prod_t>();
            signed_size_t cnt = 0;

            for (; f1 != l1; ++f1, ++f2) {
                if (filter(*f1) && filter(*f2)) {
                    if constexpr (meta::is_complex_v<T2>) {
                        res += (*f1 - m1) * std::conj(*f2 - m2);
                    } else {
                        res += (*f1 - m1) * (*f2 - m2);
                    }
                    cnt++;
                }
            }

            return std::make_tuple(m1, m2, res, cnt);
        },
        [&](const auto res1, const auto res2) {
            // Combine covariances
            // https://stackoverflow.com/questions/45773857/merging-covariance-from-two-sets-to-create-new-covariance
            const auto [m11, m12, covar1, n1] = res1;
            const auto [m21, m22, covar2, n2] = res2;

            const auto n_c = n1 + n2;
            const auto m1_c = (raw_t1{1} / raw_t1(n_c)) *
                              (raw_t1(n1) * m11 + raw_t1(n2) * m21);
            const auto m2_c = (raw_t2{1} / raw_t2(n_c)) *
                              (raw_t2(n1) * m12 + raw_t2(n2) * m22);
            const auto covar_c = covar1 + covar2 +
                                 (raw_t(n1) * raw_t(n2) / raw_t(n_c)) *
                                     conj(m12 - m22) * (m11 - m21);
            return std::make_tuple(m1_c, m2_c, covar_c, n_c);
        });

    if (unlikely(c_ - ddof <= 0)) {
        return std::make_tuple(std::numeric_limits<decltype(cov_)>::infinity(),
                               c_);
    } else {
        return std::make_tuple(cov_ / raw_t(c_ - ddof), c_);
    }
}

template <int ddof = 0, class Array1, class Array2, class Predicate>
constexpr auto
covariance(const Array1 &f1, const Array2 &f2, Predicate filter) {
    return std::get<0>(covariance<ddof>(
        f1.cbegin(), f1.cend(), f2.cbegin(), f2.cend(), filter));
}

template <int ddof = 0, class Array1, class Array2>
constexpr auto covariance(const Array1 &f1, const Array2 &f2) {
    return covariance<ddof>(f1, f2, filters::all);
}

template <int ddof = 0, class Array1, class Array2>
auto nancovariance(const Array1 &f1, const Array2 &f2) {
    return covariance<ddof>(f1, f2, filters::not_nan);
}

//---------------------------------------------------------------------------------
// var
//---------------------------------------------------------------------------------

template <int ddof = 0, class InputIt, class Predicate>
constexpr auto var(InputIt first, InputIt last, Predicate filter) {
    const auto [v, n] = covariance<ddof>(first, last, first, last, filter);
    using T = std::decay_t<decltype(v)>;

    if constexpr (meta::is_complex_v<T>) {
        // The variance is always a nonnegative real number
        return std::make_tuple(std::real(v), n);
    } else {
        return std::make_tuple(v, n);
    }
}

template <int ddof = 0, class Array, class Predicate>
constexpr auto var(const Array &f, Predicate filter) {
    return std::get<0>(var<ddof>(f.cbegin(), f.cend(), filter));
}

template <int ddof = 0, class Array>
constexpr auto var(const Array &f) {
    return var<ddof>(f, filters::all);
}

template <int ddof = 0, class Array>
auto nanvar(const Array &f) {
    return var<ddof>(f, filters::not_nan);
}

template <int ddof = 1, class Array, typename T = typename Array::value_type>
constexpr auto tvar(const Array &f,
                    const std::array<T, 2> &limits,
                    const std::array<bool, 2> &inclusive = {true, true}) {
    return var<ddof>(f, filters::Trim<T>(limits, inclusive));
}

//---------------------------------------------------------------------------------
// std
//---------------------------------------------------------------------------------

template <int ddof = 0, class Array, class Predicate>
auto std(const Array &a, Predicate filter) {
    return units::sqrt(var<ddof>(a, filter));
}

template <int ddof = 0, class Array>
auto std(const Array &a) {
    return units::sqrt(var<ddof>(a));
}

template <int ddof = 0, class Array>
auto nanstd(const Array &a) {
    return units::sqrt(nanvar<ddof>(a));
}

template <int ddof = 1, class Array, typename T = typename Array::value_type>
auto tstd(const Array &a,
          const std::array<T, 2> &limits,
          const std::array<bool, 2> &inclusive = {true, true}) {
    return units::sqrt(tvar<ddof>(a, limits, inclusive));
}

//---------------------------------------------------------------------------------
// sem
//---------------------------------------------------------------------------------

template <int ddof = 1, class Array, class Predicate>
auto sem(const Array &a, Predicate filter) {
    const auto [v, n] = var<ddof>(a.cbegin(), a.cend(), filter);
    using T = std::decay_t<decltype(v)>;
    using raw_t = units::representation_t<T>;
    return units::sqrt(v / raw_t(n));
}

template <int ddof = 1, class Array>
auto sem(const Array &a) {
    const auto v = var<ddof>(a);
    using T = std::decay_t<decltype(v)>;
    using raw_t = units::representation_t<T>;
    return units::sqrt(v / raw_t(a.size()));
}

template <int ddof = 1, class Array>
auto nansem(const Array &a) {
    return sem<ddof>(a, filters::not_nan);
}

template <int ddof = 1, class Array, typename T = typename Array::value_type>
constexpr auto tsem(const Array &f,
                    const std::array<T, 2> &limits,
                    const std::array<bool, 2> &inclusive = {true, true}) {
    return sem<ddof>(f, filters::Trim<T>(limits, inclusive));
}

//---------------------------------------------------------------------------------
// moment
//---------------------------------------------------------------------------------

template <intmax_t n, class Array, class Predicate>
auto moment(const Array &f, [[maybe_unused]] Predicate filter) {
    using namespace operators;
    using T = typename Array::value_type;

    if constexpr (n == 0) {
        return T{1};
    } else if constexpr (n == 1) {
        return T{0};
    } else if constexpr (n == 2) {
        return var(f, filter);
    } else {
        // This allocates an extra array,
        // but preserves pairwise recursion precision
        return mean(pow<n>(f - mean(f, filter)), filter);
        // Combination of moments
        // http://prod.sandia.gov/techlib/access-control.cgi/2008/086212.pdf
    }
}

template <intmax_t n, class Array>
auto moment(const Array &f) {
    return moment<n>(f, filters::all);
}

template <intmax_t n, class Array>
auto nanmoment(const Array &f) {
    return moment<n>(f, filters::not_nan);
}

//---------------------------------------------------------------------------------
// kurtosis
//---------------------------------------------------------------------------------

enum KurtosisDef { Fisher = 0, Pearson = 1 };

template <KurtosisDef def = KurtosisDef::Fisher, class Array, class Predicate>
auto kurtosis(const Array &f, Predicate filter) {
    const auto m2 = moment<2>(f, filter);
    const auto m4 = moment<4>(f, filter);
    const auto k = m4 / (m2 * m2);

    if constexpr (def == KurtosisDef::Fisher) {
        using T = decltype(k);
        return k - T(3);
    } else {
        return k;
    }
}

template <KurtosisDef def = KurtosisDef::Fisher, class Array>
auto kurtosis(const Array &f) {
    return kurtosis<def>(f, filters::all);
}

template <KurtosisDef def = KurtosisDef::Fisher, class Array>
auto nankurtosis(const Array &f) {
    return kurtosis<def>(f, filters::not_nan);
}

//---------------------------------------------------------------------------------
// skew
//---------------------------------------------------------------------------------

template <class Array, class Predicate>
auto skew(const Array &f, Predicate filter) {
    const auto m2 = moment<2>(f, filter);
    const auto m3 = moment<3>(f, filter);
    return m3 / units::sqrt(m2 * m2 * m2);
}

template <class Array>
auto skew(const Array &f) {
    return skew(f, filters::all);
}

template <class Array>
auto nanskew(const Array &f) {
    return skew(f, filters::not_nan);
}

//---------------------------------------------------------------------------------
// covariance matrix
//---------------------------------------------------------------------------------

template <int ddof = 1, class Array1, class Array2, class Predicate>
auto cov(const Array1 &f1, const Array2 &f2, Predicate filter) {
    const auto covar = covariance<ddof>(f1, f2, filter);
    using T = std::decay_t<decltype(covar)>;

    Eigen::Matrix<T, 2, 2> res;
    res(0, 0) = T(var<ddof>(f1, filter));
    res(0, 1) = covar;
    res(1, 0) = conj(covar);
    res(1, 1) = T(var<ddof>(f2, filter));
    return res;
}

template <int ddof = 1, class Array1, class Array2>
auto cov(const Array1 &f1, const Array2 &f2) {
    return cov<ddof>(f1, f2, filters::all);
}

template <int ddof = 1, class Array1, class Array2>
auto nancov(const Array1 &f1, const Array2 &f2) {
    return cov<ddof>(f1, f2, filters::not_nan);
}

} // namespace scicpp::stats

#endif // SCICPP_CORE_STATS