#include <Eigen/LU>
#include <cmath>
#include <iostream>
#include <mathtoolbox/constants.hpp>
#include <mathtoolbox/log-determinant.hpp>
#include <mathtoolbox/probability-distributions.hpp>
#include <nlopt-util.hpp>
#include <sequential-line-search/gaussian-process-regressor.hpp>
#include <sequential-line-search/utils.hpp>

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

namespace
{
    using namespace sequential_line_search;

    const bool   use_log_normal_prior  = true;
    const double a_prior_mu            = std::log(0.500);
    const double a_prior_sigma_squared = 0.50;
    const double b_prior_mu            = std::log(1e-04);
    const double b_prior_sigma_squared = 0.50;
    const double r_prior_mu            = std::log(0.500);
    const double r_prior_sigma_squared = 0.50;

    inline VectorXd Concat(const double scalar, const VectorXd& vector)
    {
        VectorXd result(vector.size() + 1);

        result(0)                        = scalar;
        result.segment(1, vector.size()) = vector;

        return result;
    }

    double calc_grad_a_prior(const double a)
    {
        return mathtoolbox::GetLogOfLogNormalDistDerivative(a, a_prior_mu, a_prior_sigma_squared);
    }

    double calc_grad_b_prior(const double b)
    {
        return mathtoolbox::GetLogOfLogNormalDistDerivative(b, b_prior_mu, b_prior_sigma_squared);
    }

    double calc_grad_r_i_prior(const Eigen::VectorXd& r, const int index)
    {
        return mathtoolbox::GetLogOfLogNormalDistDerivative(r(index), r_prior_mu, r_prior_sigma_squared);
    }

    double calc_a_prior(const double a)
    {
        return mathtoolbox::GetLogOfLogNormalDist(a, a_prior_mu, a_prior_sigma_squared);
    }

    double calc_b_prior(const double b)
    {
        return mathtoolbox::GetLogOfLogNormalDist(b, b_prior_mu, b_prior_sigma_squared);
    }

    double calc_r_i_prior(const Eigen::VectorXd& r, const int index)
    {
        return mathtoolbox::GetLogOfLogNormalDist(r(index), r_prior_mu, r_prior_sigma_squared);
    }

    double calc_grad_b(const MatrixXd& X,
                       const MatrixXd& K_y_inv,
                       const VectorXd& y,
                       const double    a,
                       const double    b,
                       const VectorXd& r)
    {
        const MatrixXd K_y_grad_b = CalcLargeKYNoiseLevelDerivative(X, Concat(a, r), b);
        const double   term1      = +0.5 * y.transpose() * K_y_inv * K_y_grad_b * K_y_inv * y;
        const double   term2      = -0.5 * (K_y_inv * K_y_grad_b).trace();
        return term1 + term2 + (use_log_normal_prior ? calc_grad_b_prior(b) : 0.0);
    }

    VectorXd calc_grad_theta(const MatrixXd&             X,
                             const MatrixXd&             K_y_inv,
                             const VectorXd&             y,
                             const VectorXd&             kernel_hyperparams,
                             const KernelThetaDerivative kernel_theta_derivative)
    {
        const std::vector<MatrixXd> tensor = CalcLargeKYThetaDerivative(X, kernel_hyperparams, kernel_theta_derivative);

        VectorXd grad(kernel_hyperparams.size());
        for (unsigned i = 0; i < kernel_hyperparams.size(); ++i)
        {
            const MatrixXd& K_y_grad_r_i = tensor[i];

            const double term1 = +0.5 * y.transpose() * K_y_inv * K_y_grad_r_i * K_y_inv * y;
            const double term2 = -0.5 * (K_y_inv * K_y_grad_r_i).trace();

            const double prior =
                use_log_normal_prior
                    ? (i == 0
                           ? calc_grad_a_prior(kernel_hyperparams(i))
                           : calc_grad_r_i_prior(kernel_hyperparams.segment(1, kernel_hyperparams.size() - 1), i - 1))
                    : 0.0;

            grad(i) = term1 + term2 + prior;
        }

        return grad;
    }

    VectorXd calc_grad(const MatrixXd&             X,
                       const MatrixXd&             K_y_inv,
                       const VectorXd&             y,
                       const double                a,
                       const double                b,
                       const VectorXd&             r,
                       const KernelThetaDerivative kernel_theta_derivative)
    {
        const unsigned D = X.rows();

        VectorXd grad(D + 2);

        const VectorXd grad_theta = calc_grad_theta(X, K_y_inv, y, Concat(a, r), kernel_theta_derivative);

        grad(0)            = grad_theta(0);
        grad(1)            = calc_grad_b(X, K_y_inv, y, a, b, r);
        grad.segment(2, D) = grad_theta.segment(1, D);

        return grad;
    }

    struct Data
    {
        const MatrixXd              X;
        const VectorXd              y;
        const Kernel                kernel;
        const KernelThetaDerivative kernel_theta_derivative;
    };

    // For counting the number of function evaluations
    unsigned count;

    // Log likelihood that will be maximized
    double objective(const std::vector<double>& x, std::vector<double>& grad, void* data)
    {
        // For counting the number of function evaluations
        ++count;

        const MatrixXd& X = static_cast<const Data*>(data)->X;
        const VectorXd& y = static_cast<const Data*>(data)->y;

        const auto kernel                  = static_cast<const Data*>(data)->kernel;
        const auto kernel_theta_derivative = static_cast<const Data*>(data)->kernel_theta_derivative;

        const unsigned N = X.cols();

        const double   a = x[0];
        const double   b = x[1];
        const VectorXd r = Eigen::Map<const VectorXd>(&x[2], x.size() - 2);

        const MatrixXd K_y     = CalcLargeKY(X, Concat(a, r), b, kernel);
        const MatrixXd K_y_inv = K_y.inverse();

        // When the algorithm is gradient-based, compute the gradient vector
        if (grad.size() == x.size())
        {
            const VectorXd g = calc_grad(X, K_y_inv, y, a, b, r, kernel_theta_derivative);
            for (unsigned i = 0; i < g.rows(); ++i)
            {
                grad[i] = g(i);
            }
        }

        // Constant
        constexpr double prod_of_two_and_pi = 2.0 * mathtoolbox::constants::pi;

        const double term1 = -0.5 * y.transpose() * K_y_inv * y;
        const double term2 = -0.5 * mathtoolbox::CalcLogDetOfSymmetricPositiveDefiniteMatrix(K_y);
        const double term3 = -0.5 * N * std::log(prod_of_two_and_pi);

        // Computing the regularization terms from a prior assumptions
        const double a_prior = calc_a_prior(a);
        const double b_prior = calc_b_prior(b);
        const double r_prior = [&r]()
        {
            double sum = 0.0;
            for (unsigned i = 0; i < r.rows(); ++i)
            {
                sum += calc_r_i_prior(r, i);
            }
            return sum;
        }();
        const double regularization = use_log_normal_prior ? (a_prior + b_prior + r_prior) : 0.0;

        return term1 + term2 + term3 + regularization;
    }
} // namespace

namespace sequential_line_search
{
    GaussianProcessRegressor::GaussianProcessRegressor(const MatrixXd&  X,
                                                       const VectorXd&  y,
                                                       const KernelType kernel_type)
        : Regressor(kernel_type), m_X(X), m_y(y)
    {
        if (X.rows() == 0)
        {
            return;
        }

        PerformMapEstimation();

        m_K_y     = CalcLargeKY(X, m_kernel_hyperparams, m_noise_hyperparam, m_kernel);
        m_K_y_inv = m_K_y.inverse();
    }

    GaussianProcessRegressor::GaussianProcessRegressor(const Eigen::MatrixXd& X,
                                                       const Eigen::VectorXd& y,
                                                       const Eigen::VectorXd& kernel_hyperparams,
                                                       double                 noise_hyperparam,
                                                       const KernelType       kernel_type)
        : Regressor(kernel_type),
          m_X(X),
          m_y(y),
          m_kernel_hyperparams(kernel_hyperparams),
          m_noise_hyperparam(noise_hyperparam)
    {
        if (X.rows() == 0)
        {
            return;
        }

        m_K_y     = CalcLargeKY(X, m_kernel_hyperparams, m_noise_hyperparam, m_kernel);
        m_K_y_inv = m_K_y.inverse();
    }

    double GaussianProcessRegressor::PredictMu(const VectorXd& x) const
    {
        // TODO: Incorporate a mean function
        const VectorXd k = CalcSmallK(x, m_X, m_kernel_hyperparams, m_kernel);
        return k.transpose() * m_K_y_inv * m_y;
    }

    double GaussianProcessRegressor::PredictSigma(const VectorXd& x) const
    {
        const VectorXd k = CalcSmallK(x, m_X, m_kernel_hyperparams, m_kernel);

        // This code assumes that the kernel is either ARD squared exponential or ARD Matern and the first
        // hyperparameter represents the intensity of the kernel.
        assert(m_kernel_hyperparams.size() == x.size() + 1);
        const double intensity = m_kernel_hyperparams[0];

        return std::sqrt(intensity - k.transpose() * m_K_y_inv * k);
    }

    Eigen::VectorXd GaussianProcessRegressor::PredictMuDerivative(const Eigen::VectorXd& x) const
    {
        // TODO: Incorporate a mean function
        const MatrixXd k_x_derivative =
            CalcSmallKSmallXDerivative(x, m_X, m_kernel_hyperparams, m_kernel_first_arg_derivative);
        return k_x_derivative * m_K_y_inv * m_y;
    }

    Eigen::VectorXd GaussianProcessRegressor::PredictSigmaDerivative(const Eigen::VectorXd& x) const
    {
        const MatrixXd k_x_derivative =
            CalcSmallKSmallXDerivative(x, m_X, m_kernel_hyperparams, m_kernel_first_arg_derivative);
        const VectorXd k     = CalcSmallK(x, m_X, m_kernel_hyperparams, m_kernel);
        const double   sigma = PredictSigma(x);
        return -(1.0 / sigma) * k_x_derivative * m_K_y_inv * k;
    }

    void GaussianProcessRegressor::PerformMapEstimation()
    {
        const unsigned D = m_X.rows();

        Data data{m_X, m_y, m_kernel, m_kernel_theta_derivative};

        const VectorXd x_ini = [&]()
        {
            VectorXd x(D + 2);

            x(0)            = std::exp(a_prior_mu);
            x(1)            = std::exp(b_prior_mu);
            x.segment(2, D) = VectorXd::Constant(D, std::exp(r_prior_mu));

            return x;
        }();

        const VectorXd upper = VectorXd::Constant(D + 2, 5e+01);
        const VectorXd lower = VectorXd::Constant(D + 2, 1e-08);

        const VectorXd x_glo = nloptutil::solve(x_ini, upper, lower, objective, nlopt::GN_DIRECT, &data, true, 300);
        const VectorXd x_loc = nloptutil::solve(x_glo, upper, lower, objective, nlopt::LD_TNEWTON, &data, true, 1000);

        m_kernel_hyperparams = Concat(x_loc(0), x_loc.segment(2, D));
        m_noise_hyperparam   = x_loc(1);
    }
} // namespace sequential_line_search


#pragma once
#include "math.h"
#include "math.h"
#include <tuple>
#include "integration.hpp"
//#include "datatypes.hpp"
#include <boost/python.hpp>
#include <boost/math/special_functions.hpp>
#include "gsl/gsl_sf_bessel.h"

namespace gd {
using namespace std;

void py_export_profile();

class Density3d {
public:
	virtual double density(double x, double y, double z) = 0;
};

class Profile3d : public Density3d {
public:
	virtual double density(double x, double y, double z) = 0;
	virtual double potential(double x, double y, double z) = 0;
	virtual double dphidx(double x, double y, double z) = 0;
	virtual double dphidy(double x, double y, double z) = 0;
	virtual double dphidz(double x, double y, double z) = 0;

	/*double enclosed_mass(double r) {
		auto dmass = [this](double rp){ return this->densityr(rp) * rp*rp * 4 * M_PI; };
		IntegratorGSL<> integratorGSL(dmass); // the integrator
		double integral = integratorGSL.integrate(0,r);
		return integral;
	}
	double total_mass() {
		auto dmass = [this](double rp){ return this->densityr(rp) * rp*rp * 4 * M_PI; };
		IntegratorGSL<> integratorGSL(dmass); // the integrator
		double integral = integratorGSL.integrate_to_inf(0);
		return integral;
	}*/
};

class ProfileModel3d {
public:
	virtual double density(double x, double y, double z) = 0;
	virtual double potential(double x, double y, double z) = 0;
	virtual double dphidx(double x, double y, double z) = 0;
	virtual double dphidy(double x, double y, double z) = 0;
	virtual double dphidz(double x, double y, double z) = 0;
};

class ProfileModel3d1C : public ProfileModel3d {
public:
	ProfileModel3d1C(Profile3d* p) : p(p) {}
	virtual double density(double x, double y, double z) { return p->density(x, y, z); }
	virtual double potential(double x, double y, double z) { return p->potential(x, y, z); }
	virtual double dphidx(double x, double y, double z) { return p->dphidx(x, y, z); }
	virtual double dphidy(double x, double y, double z) { return p->dphidy(x, y, z); }
	virtual double dphidz(double x, double y, double z) { return p->dphidz(x, y, z); }
	Profile3d* p;
};

class ProfileModel3d2C : public ProfileModel3d {
public:
	ProfileModel3d2C(Profile3d* p1, Profile3d* p2) : p1(p1), p2(p2) {}
	virtual double density(double x, double y, double z) { return p1->density(x, y, z) +  p2->density(x, y, z); }
	virtual double potential(double x, double y, double z) { return p1->potential(x, y, z) + p2->potential(x, y, z); }
	virtual double dphidx(double x, double y, double z) { return p1->dphidx(x, y, z) + p2->dphidx(x, y, z); }
	virtual double dphidy(double x, double y, double z) { return p1->dphidy(x, y, z) + p2->dphidy(x, y, z); }
	virtual double dphidz(double x, double y, double z) { return p1->dphidz(x, y, z) + p2->dphidz(x, y, z); }
	Profile3d* p1;
	Profile3d* p2;
};

class NFW3d : public Profile3d {
public:
	NFW3d(double mass200, double rs, double G, double rho_crit) : mass200(mass200), rs(rs), G(G) {
		r200 = pow(mass200/200*3/(4*M_PI)/rho_crit, 1./3);
		c = r200/rs;
		double x = r200 / rs;
		rho0 = mass200 / (4*M_PI*pow(rs,3)*(log(1+x)-x/(1+x)));
	}
	virtual double density(double x, double y, double z) {
		return this->densityr(sqrt(x*x + y*y + z*z));
	}
	virtual double potential(double x, double y, double z) {
		return this->potentialr(sqrt(x*x + y*y + z*z));
	}
	virtual double dphidx(double x, double y, double z) {
		double r = sqrt(x*x + y*y + z*z);
		double dphidr = this->dphidr(r);
		return -dphidr*x/r;
	}
	virtual double dphidy(double x, double y, double z) {
		double r = sqrt(x*x + y*y + z*z);
		double dphidr = this->dphidr(r);
		return -dphidr*y/r;
	}
	virtual double dphidz(double x, double y, double z) {
		double r = sqrt(x*x + y*y + z*z);
		double dphidr = this->dphidr(r);
		return -dphidr*z/r;
	}
	double potentialr(double r) {
		double x = r/rs;
		return - 4 * M_PI * G * rho0 * rs*rs * log(1+x)/x;
	}
	double densityr(double r) {
		double x = r/rs;
		return rho0 / (x * pow(1+x, 2));
	}
	double dphidr(double r) {
		double x = r/rs;
		double f = -4.0*M_PI*G*rho0*pow(rs, 3)/pow(r,2);
		double t1 = log(1.0 + x);
		double t2 = x/(1.0+x);
		//cout << x << ", " << f << ", " << t1 << ", " << t2 << endl;
		return -f*(t1 - t2);
	}
	double mass200, rho0, rs, r200, c, G;
};

class NFW3dTriax : public Profile3d {
public:
	NFW3dTriax(double mass200, double rs, double G, double rho_crit, double a, double b, double c, double ra) : mass200(mass200), rs(rs), G(G), a(a), b(b), c(c), ra(ra)  {
		r200 = pow(mass200/200*3/(4*M_PI)/rho_crit, 1./3);
		concentration = r200/rs;
		double x = r200 / rs;
		rho0 = mass200 / (4*M_PI*pow(rs,3)*(log(1+x)-x/(1+x)));
	}
	virtual double density(double x, double y, double z) {
		return 0 ;//this->densityr(sqrt(x*x + y*y + z*z));
	}
	virtual double potential(double x, double y, double z) {
		return this->potentialr( this->rtilde(x, y, z));
	}
	double rtilde(double x, double y, double z) {
		double r = sqrt(x*x + y*y + z*z);
		double re = sqrt(x*x / (a*a) + y*y/ (b*b) + z*z/(c*c) );
		return (ra+r)*re / (ra+re);
	}
	virtual double dphidx(double x, double y, double z) {
		double r = sqrt(x*x + y*y + z*z);
		double re = sqrt(x*x / (a*a) + y*y/ (b*b) + z*z/(c*c) );
		double rtilde = this->rtilde(x, y, z);
		double dphidrtilde = this->dphidr(rtilde);
		double drtildedx = x * (ra * ra * r + ra * r*r + a*a*pow(re,3)  + ra*a*a*re*re) / (a*a*r*re*pow(ra+re, 2));
		return -dphidrtilde*drtildedx;
	}
	virtual double dphidy(double x, double y, double z) {
		double r = sqrt(x*x + y*y + z*z);
		double re = sqrt(x*x / (a*a) + y*y/ (b*b) + z*z/(c*c) );
		double rtilde = this->rtilde(x, y, z);
		double dphidrtilde = this->dphidr(rtilde);
		double drtildedy = y * (ra * ra * r + ra * r*r + b*b*pow(re,3)  + ra*b*b*re*re) / (b*b*r*re*pow(ra+re, 2));
		return -dphidrtilde*drtildedy;
	}
	virtual double dphidz(double x, double y, double z) {
		double r = sqrt(x*x + y*y + z*z);
		double re = sqrt(x*x / (a*a) + y*y/ (b*b) + z*z/(c*c) );
		double rtilde = this->rtilde(x, y, z);
		double dphidrtilde = this->dphidr(rtilde);
		double drtildedz = z * (ra * ra * r + ra * r*r + c*c*pow(re,3)  + ra*c*c*re*re) / (c*c*r*re*pow(ra+re, 2));
		return -dphidrtilde*drtildedz;
	}
	double potentialr(double r) {
		double x = r/rs;
		return - 4 * M_PI * G * rho0 * rs*rs * log(1+x)/x;
	}
	double densityr(double r) {
		double x = r/rs;
		return rho0 / (x * pow(1+x, 2));
	}
	double dphidr(double r) {
		double x = r/rs;
		double f = -4.0*M_PI*G*rho0*pow(rs, 3)/pow(r,2);
		double t1 = log(1.0 + x);
		double t2 = x/(1.0+x);
		//cout << x << ", " << f << ", " << t1 << ", " << t2 << endl;
		return -f*(t1 - t2);
	}
	double mass200, rho0, rs, r200, concentration, G, a, b, c, ra;
};

class  MiyamotoNagai3d : public Profile3d {
public:
	MiyamotoNagai3d(double mass, double G, double a, double b) : mass(mass), G(G), a(a), b(b)  {
	}
	virtual double density(double x, double y, double z) {
		return 0 ;//this->densityr(sqrt(x*x + y*y + z*z));
	}
	virtual double potential(double x, double y, double z) {
		return -G*mass*pow(x*x + y*y +pow(a + pow(z*z+b*b, .5), 2.), -.5);
		//return this->potentialr( this->rtilde(x, y, z));
	}
	virtual double dphidx(double x, double y, double z) {
		double part = G*mass*pow( x*x + y*y + pow(a+ pow(z*z+b*b,.5) , 2.), -1.5);
		return -x*part;
	}
	virtual double dphidy(double x, double y, double z) {
		double part = G*mass*pow( x*x + y*y + pow(a+ pow(z*z+b*b,.5) , 2.), -1.5);
		return -y*part;
	}
	virtual double dphidz(double x, double y, double z) {
		double part = G*mass*pow( x*x + y*y + pow(a+ pow(z*z+b*b,.5) , 2.), -1.5);
		return -z*part*(a/sqrt(b*b+z*z) + 1);
	}
	double mass, G, a, b;
};

}


#include "assembly.h"
#include <armadillo>
#include <iostream>

// Uncomment out the following line to disable assertions
//#define NDEBUG 1
#include <cassert>

using namespace arma;
using namespace std;

/*TODO work with linked lists instead of C-style arrays
 *       this will be easier for memory management */

/**
 * Assembles the global stiffness matrix given an array of elements, mutator
 *
 * @param [mat] kg Global stiffness matrix (by reference)
 * @param [MechElem*] elems Array of pointers to elements to assemble
 * @param [unsigned int] numElements Number of elements in array
 * @param [unsigned int] dofPerElem Total degrees of freedom per element
 * @return void
 */
void mglobalStiffness(mat &kg, MechElem **elems, unsigned int numElements,
        unsigned int dofPerElem, int pState)
{
    // TODO Can we find a way to relate this to global dof??
    //assert(kg.n_cols == dofPerElem);
    //assert(kg.n_rows == dofPerElem);
    /*
     * TODO reach a consistency where matrices passed by reference are either
     *      assumed to be zeroed before being passed or are zeroed on pass
     */
    kg.zeros();
    int *gdofs;
    unsigned int elemNum, i, j;
    MechElem *pElem;
    mat *ke = new mat(dofPerElem, dofPerElem);
    for (elemNum = 0; elemNum < numElements; elemNum++)
    {
        pElem = elems[elemNum];
        gdofs = pElem->getGdofs();
        pElem->mstiffness(*ke, pState);
        for (i = 0; i < dofPerElem; i++)
        {
            for (j = 0; j < dofPerElem; j++)
            {
                // TODO consider renumbering DOF so that we no longer require
                // this silly minus one business
                kg(gdofs[i]-1, gdofs[j]-1) += (*ke)(i, j);
            }
        }
    }
    // free dynamic memory
    delete ke;
}

/**
 * Assembles the global body force vector given an array of elements, mutator
 *
 * @param [mat] bg Global body force vector (by reference)
 * @param [MechElem*] elems Array of pointers to elements to assemble
 * @param [unsigned int] numElements Number of elements in array
 * @param [unsigned int] dofPerElem Total degrees of freedom per element
 * @return void
 */
void mglobalBodyForce(vec &bg, MechElem **elems, unsigned int numElements,
        unsigned int dofPerElem)
{
    // TODO Can we write this for global degrees of freedom?
    //assert(bg.n_cols == dofPerElem);

    bg.zeros();
    int *gdofs;
    unsigned int elemNum, i;
    MechElem *pElem;
    vec *be = new vec(dofPerElem);
    for (elemNum = 0; elemNum < numElements; elemNum++)
    {
        pElem = elems[elemNum];
        gdofs = pElem->getGdofs();
        pElem->mbodyForce(*be);
        for (i = 0; i < dofPerElem; i++)
            bg(gdofs[i]-1) += (*be)(i);
    }
    // free dynamic memory
    delete be;
}


#include "cpu/image_proc.h"

#include <Eigen/Dense>
#include <map>
#include <string>
#include <cmath>
#include <math.h>

#include "procrustes.h"

namespace image_proc {

    using namespace flow_data_gen;

    using Vec2d = Eigen::Vector2d;
    using Vec2f = Eigen::Vector2f;
    using Vec2i = Eigen::Vector2i;
    using Vec3d = Eigen::Vector3d;
    using Vec3f = Eigen::Vector3f;
    using Vec3i = Eigen::Vector3i;
	using Vec4d = Eigen::Vector4d;
	using Vec4f = Eigen::Vector4f;
	using Vec4i = Eigen::Vector4i;

	using Mat2d = Eigen::Matrix2d;
	using Mat2f = Eigen::Matrix2f;
	using Mat2i = Eigen::Matrix2i;
	using Mat3d = Eigen::Matrix3d;
	using Mat3f = Eigen::Matrix3f;
	using Mat3i = Eigen::Matrix3i;
	using Mat4d = Eigen::Matrix4d;
	using Mat4f = Eigen::Matrix4f;
	using Mat4i = Eigen::Matrix4i;

    void backproject_depth_ushort(py::array_t<unsigned short>& in, py::array_t<float>& out, float fx, float fy, float cx, float cy, float normalizer) {
        assert(in.ndim() == 2);
        assert(out.ndim() == 3);

        int width = in.shape(1);
        int height = in.shape(0);
        assert(out.shape(0) == 3);
        assert(out.shape(1) == height);
        assert(out.shape(2) == width);

        for (int y = 0; y < height; y++) {
            for (int x = 0; x < width; x++) {
                float depth = float(*in.data(y, x)) / normalizer;

                if (depth > 0) {
                    float pos_x = depth * (x - cx) / fx;
                    float pos_y = depth * (y - cy) / fy;
                    float pos_z = depth;

                    *out.mutable_data(0, y, x) = pos_x;
                    *out.mutable_data(1, y, x) = pos_y;
                    *out.mutable_data(2, y, x) = pos_z;
                }
            }
        }
    }

    void backproject_depth_float(py::array_t<float>& in, py::array_t<float>& out, float fx, float fy, float cx, float cy) {
        assert(in.ndim() == 2);
        assert(out.ndim() == 3);

        int width = in.shape(1);
        int height = in.shape(0);
        assert(out.shape(0) == 3);
        assert(out.shape(1) == height);
        assert(out.shape(2) == width);

        for (int y = 0; y < height; y++) {
            for (int x = 0; x < width; x++) {
                float depth = *in.data(y, x);

                if (depth > 0) {
                    float pos_x = depth * (x - cx) / fx;
                    float pos_y = depth * (y - cy) / fy;
                    float pos_z = depth;

                    *out.mutable_data(0, y, x) = pos_x;
                    *out.mutable_data(1, y, x) = pos_y;
                    *out.mutable_data(2, y, x) = pos_z;
                }
            }
        }
    }

    void compute_normals(const py::array_t<float>& point_image, py::array_t<float>& normals, float epsilon) {

        int height = point_image.shape(0);
        int width  = point_image.shape(1);

        for (int y = 1; y < (height - 1); y++) {
            for (int x = 1; x < (width - 1); x++) {

                Vec3f xp(*point_image.data(y, x + 1, 0), *point_image.data(y, x + 1, 1), *point_image.data(y, x + 1, 2));
                Vec3f xn(*point_image.data(y, x - 1, 0), *point_image.data(y, x - 1, 1), *point_image.data(y, x - 1, 2));

                Vec3f yp(*point_image.data(y + 1, x, 0), *point_image.data(y + 1, x, 1), *point_image.data(y + 1, x, 2));
                Vec3f yn(*point_image.data(y - 1, x, 0), *point_image.data(y - 1, x, 1), *point_image.data(y - 1, x, 2));

                if (!xp.allFinite() || !xn.allFinite() || !yp.allFinite() || !yn.allFinite()) {
                    continue;
                }

                Vec3f a = xp - xn;
                Vec3f b = yp - yn;

                Vec3f cross = b.cross(a);

                if (!cross.allFinite()) {
                    continue;
                }

                float norm = cross.norm();

                if (norm < epsilon) {
                    continue;
                }

                cross = cross / norm;

                *normals.mutable_data(y, x, 0) = cross.x();
                *normals.mutable_data(y, x, 1) = cross.y();
                *normals.mutable_data(y, x, 2) = cross.z();
            }
        }
    }

    void compute_normals_via_pca(const py::array_t<float>& point_image, py::array_t<float>& normals, const int kernel_size, const float max_distance) {

        int height = point_image.shape(0);
        int width  = point_image.shape(1);

        int ex = kernel_size;
        int ey = kernel_size;

        for (int y = 0; y < height; y++) {
            for (int x = 0; x < width; x++) {

                Vec3f point(*point_image.data(y, x, 0), *point_image.data(y, x, 1), *point_image.data(y, x, 2));

                if (!point.allFinite()) {
                    continue;
                }

                int counter = 0;
                Vec3f center(0, 0, 0);
                std::vector<Vec3f> valid_points;

                for (int yi = y - ey / 2; yi <= y + ey / 2; ++yi) {
                    for (int xi = x - ex / 2; xi <= x + ex / 2; ++xi) {
                        if (xi >= 0 && xi < width && yi >= 0 && yi < height) {
                            Vec3f point_neighbor(*point_image.data(yi, xi, 0), *point_image.data(yi, xi, 1), *point_image.data(yi, xi, 2));

                            if (point_neighbor.allFinite() && (point - point_neighbor).norm() <= max_distance) {
                                center += point_neighbor;
                                valid_points.push_back(point_neighbor);
                                ++counter;
                            }
                        }
                    }
                }
                center /= (float)counter;

                Eigen::MatrixXf A(3, counter);
                for (int idx = 0; idx < counter; ++idx) {
                    A.col(idx) = valid_points[idx] - center;
                }

                Eigen::Matrix3f cov = A * A.transpose();
                Eigen::EigenSolver<Eigen::Matrix3f> eigen_solver(cov, true);
                const Vec3f& eigen_value = eigen_solver.eigenvalues().real();

                int min_idx = 0;
                eigen_value.minCoeff(&min_idx);
                Vec3f min_eigen_vector = eigen_solver.eigenvectors().real().col(min_idx);

                Vec3f normal;
                // Use eigen vector facing camera as normal.
                if (min_eigen_vector.dot(point) < 0) {
                    normal = min_eigen_vector.normalized();
                }
                else {
                    normal = -min_eigen_vector.normalized();
                }

                *normals.mutable_data(y, x, 0) = normal.x();
                *normals.mutable_data(y, x, 1) = normal.y();
                *normals.mutable_data(y, x, 2) = normal.z();
            }
        }
    }

    float estimate_rigid_pose(
         const py::array_t<float>& sourcePointsArray, const py::array_t<float>& targetPointsArray,
         int numIterationsRANSAC, int numSamplesRANSAC, int minPointsRANSAC, float inlierDistanceRANSAC, float inlierDistanceFinal,
         py::array_t<float>& estimatedPose
    ) {
        if (numSamplesRANSAC > minPointsRANSAC) {
            std::cout << "The minimum number of points should be at least as big as the sample size." << std::endl;
            exit(1);
        }

        // Compute valid 3D matches.
        int nMatches = sourcePointsArray.shape(0);

        std::vector<Vec3f> sourcePoints, targetPoints;
        for (int i = 0; i < nMatches; i++) {
            Vec3f sourcePoint(
                *sourcePointsArray.data(i, 0),
                *sourcePointsArray.data(i, 1),
                *sourcePointsArray.data(i, 2)
            );
            Vec3f targetPoint(
                *targetPointsArray.data(i, 0),
                *targetPointsArray.data(i, 1),
                *targetPointsArray.data(i, 2)
            );

            sourcePoints.push_back(sourcePoint);
            targetPoints.push_back(targetPoint);
        }

        int nMatches3D = sourcePoints.size();
        if (nMatches3D < minPointsRANSAC) {
            return 0;
        }

        // Execute Procrustes with RANSAC.
        Mat4f globalRigidPose = ProcrustesRANSAC::estimatePose(
            sourcePoints, targetPoints, numIterationsRANSAC, numSamplesRANSAC, inlierDistanceRANSAC
        );

        // Compute the number of inliers.
        int nInliers = 0;
        for (int i = 0; i < nMatches3D; i++) {
            Eigen::Vector3f transformedSourcePoint = globalRigidPose.block(0, 0, 3, 3) * sourcePoints[i] + globalRigidPose.block(0, 3, 3, 1);
            float distance = (transformedSourcePoint - targetPoints[i]).norm();

            if (distance <= inlierDistanceFinal) {
                nInliers++;
            }
        }

        float inlierRatio = float(nInliers) / nMatches3D;

        // Store the estimated pose.
        estimatedPose.resize({ 4, 4 }, false);

        for (int i = 0; i < 4; i++) {
            for (int j = 0; j < 4; j++) {
                *estimatedPose.mutable_data(i, j) = globalRigidPose(i, j);
            }
        }

        return inlierRatio;
    }

    void compute_mesh_from_depth(
        const py::array_t<float>& pointImage, float maxTriangleEdgeDistance,
        py::array_t<float>& vertexPositions, py::array_t<int>& faceIndices
    ) {
        int width = pointImage.shape(2);
        int height = pointImage.shape(1);

        // Compute valid pixel vertices and faces.
        // We also need to compute the pixel -> vertex index mapping for
        // computation of faces.
        // We connect neighboring pixels on the square into two triangles.
        // We only select valid triangles, i.e. with all valid vertices and
        // not too far apart.
        // Important: The triangle orientation is set such that the normals
        // point towards the camera.
        std::vector<Eigen::Vector3f> vertices;
        std::vector<Eigen::Vector3i> faces;

        int vertexIdx = 0;
        std::vector<int> mapPixelToVertexIdx(width * height, -1);

        for (int y = 0; y < height - 1; y++) {
            for (int x = 0; x < width - 1; x++) {
                Eigen::Vector3f obs00(*pointImage.data(0, y, x), *pointImage.data(1, y, x), *pointImage.data(2, y, x));
                Eigen::Vector3f obs01(*pointImage.data(0, y + 1, x), *pointImage.data(1, y + 1, x), *pointImage.data(2, y + 1, x));
                Eigen::Vector3f obs10(*pointImage.data(0, y, x + 1), *pointImage.data(1, y, x + 1), *pointImage.data(2, y, x + 1));
                Eigen::Vector3f obs11(*pointImage.data(0, y + 1, x + 1), *pointImage.data(1, y + 1, x + 1), *pointImage.data(2, y + 1, x + 1));

                int idx00 = y * width + x;
                int idx01 = (y + 1) * width + x;
                int idx10 = y * width + (x + 1);
                int idx11 = (y + 1) * width + (x + 1);

                bool valid00 = obs00.z() > 0;
                bool valid01 = obs01.z() > 0;
                bool valid10 = obs10.z() > 0;
                bool valid11 = obs11.z() > 0;

                if (valid00 && valid01 && valid10) {
                    float d0 = (obs00 - obs01).norm();
                    float d1 = (obs00 - obs10).norm();
                    float d2 = (obs01 - obs10).norm();

                    if (d0 <= maxTriangleEdgeDistance && d1 <= maxTriangleEdgeDistance && d2 <= maxTriangleEdgeDistance) {
                        int vIdx0 = mapPixelToVertexIdx[idx00];
                        int vIdx1 = mapPixelToVertexIdx[idx01];
                        int vIdx2 = mapPixelToVertexIdx[idx10];

                        if (vIdx0 == -1) {
                            vIdx0 = vertexIdx;
                            mapPixelToVertexIdx[idx00] = vertexIdx;
                            vertices.push_back(obs00);
                            vertexIdx++;
                        }
                        if (vIdx1 == -1) {
                            vIdx1 = vertexIdx;
                            mapPixelToVertexIdx[idx01] = vertexIdx;
                            vertices.push_back(obs01);
                            vertexIdx++;
                        }
                        if (vIdx2 == -1) {
                            vIdx2 = vertexIdx;
                            mapPixelToVertexIdx[idx10] = vertexIdx;
                            vertices.push_back(obs10);
                            vertexIdx++;
                        }

                        faces.push_back(Eigen::Vector3i(vIdx0, vIdx1, vIdx2));
                    }
                }

                if (valid01 && valid10 && valid11) {
                    float d0 = (obs10 - obs01).norm();
                    float d1 = (obs10 - obs11).norm();
                    float d2 = (obs01 - obs11).norm();

                    if (d0 <= maxTriangleEdgeDistance && d1 <= maxTriangleEdgeDistance && d2 <= maxTriangleEdgeDistance) {
                        int vIdx0 = mapPixelToVertexIdx[idx11];
                        int vIdx1 = mapPixelToVertexIdx[idx10];
                        int vIdx2 = mapPixelToVertexIdx[idx01];

                        if (vIdx0 == -1) {
                            vIdx0 = vertexIdx;
                            mapPixelToVertexIdx[idx11] = vertexIdx;
                            vertices.push_back(obs11);
                            vertexIdx++;
                        }
                        if (vIdx1 == -1) {
                            vIdx1 = vertexIdx;
                            mapPixelToVertexIdx[idx10] = vertexIdx;
                            vertices.push_back(obs10);
                            vertexIdx++;
                        }
                        if (vIdx2 == -1) {
                            vIdx2 = vertexIdx;
                            mapPixelToVertexIdx[idx01] = vertexIdx;
                            vertices.push_back(obs01);
                            vertexIdx++;
                        }

                        faces.push_back(Eigen::Vector3i(vIdx0, vIdx1, vIdx2));
                    }
                }
            }
        }

        // Convert to numpy array.
        int nVertices = vertices.size();
        int nFaces = faces.size();

        if (nVertices > 0 && nFaces > 0) {
            // Reference check should be set to false otherwise there is a runtime
            // error. Check why that is the case.
            vertexPositions.resize({ nVertices, 3 }, false);
            faceIndices.resize({ nFaces, 3 }, false);

            for (int i = 0; i < nVertices; i++) {
                *vertexPositions.mutable_data(i, 0) = vertices[i].x();
                *vertexPositions.mutable_data(i, 1) = vertices[i].y();
                *vertexPositions.mutable_data(i, 2) = vertices[i].z();
            }

            for (int i = 0; i < nFaces; i++) {
                *faceIndices.mutable_data(i, 0) = faces[i].x();
                *faceIndices.mutable_data(i, 1) = faces[i].y();
                *faceIndices.mutable_data(i, 2) = faces[i].z();
            }
        }
    }

    void compute_validity_and_sign_mask(
        const py::array_t<float>& grid_points_cam,
        const py::array_t<bool>& valid_depth_mask_image,
        const py::array_t<float>& depth_image,
        float fx, float fy, float cx, float cy, int w, int h,
        float truncation,
        py::array_t<bool>& validity_grid_mask,
        py::array_t<float>& sign_grid_mask
    ) {

        int num_grid_points = grid_points_cam.shape(0);

        for (int pt_id = 0; pt_id < num_grid_points; pt_id++) {

            float x = *grid_points_cam.data(pt_id, 0);
            float y = *grid_points_cam.data(pt_id, 1);
            float z = *grid_points_cam.data(pt_id, 2);

            int px = int(round(fx * x / z + cx));
            int py = int(round(fy * y / z + cy));

            if (px < 0 || px >= w || py < 0 || py >= h) {
                continue;
            }

            bool is_valid_pixel = *valid_depth_mask_image.data(py, px);

            if (!is_valid_pixel) {
                continue;
            }

            float d = *depth_image.data(py, px);

            // If the z coordinate of the grid point is further away from the camera
            // than the surface + some truncation, then we invalidate the grid point
            if (z > (d + truncation)) {
                *validity_grid_mask.mutable_data(pt_id) = false;
            }

            if (z > d) {
                *sign_grid_mask.mutable_data(pt_id) = -1.0;
            }

        }
    }

} //namespace image_proc


/*

Copyright (c) 2015, Project OSRM contributors
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 "coordinate_calculation.hpp"

#include "../util/mercator.hpp"
#include "../util/string_util.hpp"

#include <boost/assert.hpp>

#include <osrm/coordinate.hpp>

#include <cmath>

#include <limits>

namespace
{
constexpr static const float RAD = 0.017453292519943295769236907684886f;
// earth radius varies between 6,356.750-6,378.135 km (3,949.901-3,963.189mi)
// The IUGG value for the equatorial radius is 6378.137 km (3963.19 miles)
constexpr static const float earth_radius = 6372797.560856f;
}

namespace coordinate_calculation
{

double great_circle_distance(const int lat1,
                                                     const int lon1,
                                                     const int lat2,
                                                     const int lon2)
{
    BOOST_ASSERT(lat1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lat2 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon2 != std::numeric_limits<int>::min());
    const double lt1 = lat1 / COORDINATE_PRECISION;
    const double ln1 = lon1 / COORDINATE_PRECISION;
    const double lt2 = lat2 / COORDINATE_PRECISION;
    const double ln2 = lon2 / COORDINATE_PRECISION;
    const double dlat1 = lt1 * (RAD);

    const double dlong1 = ln1 * (RAD);
    const double dlat2 = lt2 * (RAD);
    const double dlong2 = ln2 * (RAD);

    const double dLong = dlong1 - dlong2;
    const double dLat = dlat1 - dlat2;

    const double aHarv = std::pow(std::sin(dLat / 2.0), 2.0) +
                         std::cos(dlat1) * std::cos(dlat2) * std::pow(std::sin(dLong / 2.), 2);
    const double cHarv = 2. * std::atan2(std::sqrt(aHarv), std::sqrt(1.0 - aHarv));
    return earth_radius * cHarv;
}

double great_circle_distance(const FixedPointCoordinate &coordinate_1,
                                                     const FixedPointCoordinate &coordinate_2)
{
    return great_circle_distance(coordinate_1.lat, coordinate_1.lon, coordinate_2.lat,
                                 coordinate_2.lon);
}

float euclidean_distance(const FixedPointCoordinate &coordinate_1,
                                                 const FixedPointCoordinate &coordinate_2)
{
    return euclidean_distance(coordinate_1.lat, coordinate_1.lon, coordinate_2.lat,
                              coordinate_2.lon);
}

float euclidean_distance(const int lat1,
                                                 const int lon1,
                                                 const int lat2,
                                                 const int lon2)
{
    BOOST_ASSERT(lat1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lat2 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon2 != std::numeric_limits<int>::min());

    const float float_lat1 = (lat1 / COORDINATE_PRECISION) * RAD;
    const float float_lon1 = (lon1 / COORDINATE_PRECISION) * RAD;
    const float float_lat2 = (lat2 / COORDINATE_PRECISION) * RAD;
    const float float_lon2 = (lon2 / COORDINATE_PRECISION) * RAD;

    const float x_value = (float_lon2 - float_lon1) * std::cos((float_lat1 + float_lat2) / 2.f);
    const float y_value = float_lat2 - float_lat1;
    return std::hypot(x_value, y_value) * earth_radius;
}

float perpendicular_distance(const FixedPointCoordinate &source_coordinate,
                                                     const FixedPointCoordinate &target_coordinate,
                                                     const FixedPointCoordinate &query_location)
{
    float ratio;
    FixedPointCoordinate nearest_location;

    return perpendicular_distance(source_coordinate, target_coordinate, query_location,
                                  nearest_location, ratio);
}

float perpendicular_distance(const FixedPointCoordinate &segment_source,
                                                     const FixedPointCoordinate &segment_target,
                                                     const FixedPointCoordinate &query_location,
                                                     FixedPointCoordinate &nearest_location,
                                                     float &ratio)
{
    return perpendicular_distance_from_projected_coordinate(
        segment_source, segment_target, query_location,
        {mercator::lat2y(query_location.lat / COORDINATE_PRECISION),
         query_location.lon / COORDINATE_PRECISION},
        nearest_location, ratio);
}

float perpendicular_distance_from_projected_coordinate(
    const FixedPointCoordinate &source_coordinate,
    const FixedPointCoordinate &target_coordinate,
    const FixedPointCoordinate &query_location,
    const std::pair<double, double> &projected_coordinate)
{
    float ratio;
    FixedPointCoordinate nearest_location;

    return perpendicular_distance_from_projected_coordinate(source_coordinate, target_coordinate,
                                                            query_location, projected_coordinate,
                                                            nearest_location, ratio);
}

float perpendicular_distance_from_projected_coordinate(
    const FixedPointCoordinate &segment_source,
    const FixedPointCoordinate &segment_target,
    const FixedPointCoordinate &query_location,
    const std::pair<double, double> &projected_coordinate,
    FixedPointCoordinate &nearest_location,
    float &ratio)
{
    BOOST_ASSERT(query_location.is_valid());

    // initialize values
    const double x = projected_coordinate.first;
    const double y = projected_coordinate.second;
    const double a = mercator::lat2y(segment_source.lat / COORDINATE_PRECISION);
    const double b = segment_source.lon / COORDINATE_PRECISION;
    const double c = mercator::lat2y(segment_target.lat / COORDINATE_PRECISION);
    const double d = segment_target.lon / COORDINATE_PRECISION;
    double p, q /*,mX*/, nY;
    if (std::abs(a - c) > std::numeric_limits<double>::epsilon())
    {
        const double m = (d - b) / (c - a); // slope
        // Projection of (x,y) on line joining (a,b) and (c,d)
        p = ((x + (m * y)) + (m * m * a - m * b)) / (1.f + m * m);
        q = b + m * (p - a);
    }
    else
    {
        p = c;
        q = y;
    }
    nY = (d * p - c * q) / (a * d - b * c);

    // discretize the result to coordinate precision. it's a hack!
    if (std::abs(nY) < (1.f / COORDINATE_PRECISION))
    {
        nY = 0.f;
    }

    // compute ratio
    ratio =
        static_cast<float>((p - nY * a) / c); // These values are actually n/m+n and m/m+n , we need
    // not calculate the explicit values of m an n as we
    // are just interested in the ratio
    if (std::isnan(ratio))
    {
        ratio = (segment_target == query_location ? 1.f : 0.f);
    }
    else if (std::abs(ratio) <= std::numeric_limits<float>::epsilon())
    {
        ratio = 0.f;
    }
    else if (std::abs(ratio - 1.f) <= std::numeric_limits<float>::epsilon())
    {
        ratio = 1.f;
    }

    // compute nearest location
    BOOST_ASSERT(!std::isnan(ratio));
    if (ratio <= 0.f)
    {
        nearest_location = segment_source;
    }
    else if (ratio >= 1.f)
    {
        nearest_location = segment_target;
    }
    else
    {
        // point lies in between
        nearest_location.lat = static_cast<int>(mercator::y2lat(p) * COORDINATE_PRECISION);
        nearest_location.lon = static_cast<int>(q * COORDINATE_PRECISION);
    }
    BOOST_ASSERT(nearest_location.is_valid());

    const float approximate_distance =
        euclidean_distance(query_location, nearest_location);
    BOOST_ASSERT(0.f <= approximate_distance);
    return approximate_distance;
}

void lat_or_lon_to_string(const int value, std::string &output)
{
    char buffer[12];
    buffer[11] = 0; // zero termination
    output = printInt<11, 6>(buffer, value);
}

float deg_to_rad(const float degree)
{
    return degree * (static_cast<float>(M_PI) / 180.f);
}

float rad_to_deg(const float radian)
{
    return radian * (180.f * static_cast<float>(M_1_PI));
}

float bearing(const FixedPointCoordinate &first_coordinate,
                                      const FixedPointCoordinate &second_coordinate)
{
    const float lon_diff =
        second_coordinate.lon / COORDINATE_PRECISION - first_coordinate.lon / COORDINATE_PRECISION;
    const float lon_delta = deg_to_rad(lon_diff);
    const float lat1 = deg_to_rad(first_coordinate.lat / COORDINATE_PRECISION);
    const float lat2 = deg_to_rad(second_coordinate.lat / COORDINATE_PRECISION);
    const float y = std::sin(lon_delta) * std::cos(lat2);
    const float x =
        std::cos(lat1) * std::sin(lat2) - std::sin(lat1) * std::cos(lat2) * std::cos(lon_delta);
    float result = rad_to_deg(std::atan2(y, x));
    while (result < 0.f)
    {
        result += 360.f;
    }

    while (result >= 360.f)
    {
        result -= 360.f;
    }
    return result;
}

}


#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <quadrotor_msgs/QuadData.h>
#include <quadrotor_msgs/TrajectoryData.h>
#include <math.h>
#include <std_msgs/Int8.h>

#define USE_JOYSTICK 0

struct TrajectoryInfo
{
    double xcoef[8];
    double ycoef[8];
    double zcoef[8];
    double duration;
};

TrajectoryInfo cur_traj, next_traj;
bool receive_traj = false, buffer = false, init_b = false;
int count = 0;
int mode = 0;
double des_height = 1, addtf = 0;
double des_pos[2] = {0, 0};
double t_takeoff = 3;
double t_goto = 2, t_wait = 20;
Eigen::MatrixXd A_takeoff(6,6);
Eigen::MatrixXd A_goto(6,6);
bool initial_vicon=false, re_init = false, done_init = false, start = false;
double x_init=0,y_init=0,z_init=0;
double tscale = 1;

void startprogram (const std_msgs::Int8::ConstPtr & msg)
{
  if(msg->data == 0){
    start = true;
    ROS_INFO("GET MESSAGE START");
  }
}

void initialize_var()
{
A_takeoff << 1.0,0.0,0.0,0.0,0.0,0.0,
0.0,1.0,0.0,0.0,0.0,0.0,
0.0,0.0,2.0,0.0,0.0,0.0,
1,t_takeoff,pow(t_takeoff,2),pow(t_takeoff,3),pow(t_takeoff,4),pow(t_takeoff,5),
0,1,2*t_takeoff,3*pow(t_takeoff,2),4*pow(t_takeoff,3),5*pow(t_takeoff,4),
0.0,0.0,2.0,6.0*t_takeoff,12.0*pow(t_takeoff,2),20.0*pow(t_takeoff,3);
A_goto << 1.0,0.0,0.0,0.0,0.0,0.0,
0.0,1.0,0.0,0.0,0.0,0.0,
0.0,0.0,2.0,0.0,0.0,0.0,
1,t_goto,pow(t_goto,2),pow(t_goto,3),pow(t_goto,4),pow(t_goto,5),
0,1,2*t_goto,3*pow(t_goto,2),4*pow(t_goto,3),5*pow(t_goto,4),
0.0,0.0,2.0,6.0*t_goto,12.0*pow(t_goto,2),20.0*pow(t_goto,3);
}

void cal_takeoff()
{
    Eigen::VectorXd b(6), sol(6);
    b<< z_init,0.0,0.0,des_height+addtf,0.0,0.0;
    sol = A_takeoff.colPivHouseholderQr().solve(b);
    cur_traj.duration = t_takeoff;
    cur_traj.zcoef[0] = 0;
    cur_traj.zcoef[1] = 0;
    for(int i = 0; i < 6; i++){
        cur_traj.zcoef[7-i] = sol(i);
    }
    done_init = true;
    ROS_INFO("solve takeoff with duration = %f and coef (%f,%f,%f,%f,%f,%f)", cur_traj.duration,sol(0),sol(1),sol(2),sol(3),sol(4),sol(5));
}

void cal_goto()
{
    Eigen::VectorXd bx(6),by(6), bz(6), solx(6), soly(6), solz(6);
    bx<< x_init,0.0,0.0,des_pos[0],0.0,0.0;
    solx = A_goto.colPivHouseholderQr().solve(bx);
    by<< y_init,0.0,0.0,des_pos[1],0.0,0.0;
    soly = A_goto.colPivHouseholderQr().solve(by);
    bz<< z_init,0.0,0.0,des_height,0.0,0.0;
    solz = A_goto.colPivHouseholderQr().solve(bz);
    cur_traj.duration = t_goto;
    cur_traj.xcoef[0] = 0;
    cur_traj.xcoef[1] = 0;
    cur_traj.ycoef[0] = 0;
    cur_traj.ycoef[1] = 0;
    cur_traj.zcoef[0] = 0;
    cur_traj.zcoef[1] = 0;
    for(int i = 0; i < 6; i++){
      cur_traj.xcoef[7-i] = solx(i);
      cur_traj.ycoef[7-i] = soly(i);
      cur_traj.zcoef[7-i] = solz(i);
    }
    done_init = true;
    ROS_INFO("solve goto");
}

void viconCallback(const nav_msgs::Odometry::ConstPtr& vicon)
{
if(!initial_vicon)
{
  x_init = vicon->pose.pose.position.x;
  y_init = vicon->pose.pose.position.y;
  z_init = vicon->pose.pose.position.z;
  initial_vicon = true;
  cal_takeoff();
}
else{
  if(re_init){
  x_init = vicon->pose.pose.position.x;
  y_init = vicon->pose.pose.position.y;
  z_init = vicon->pose.pose.position.z;
  re_init = false;
  cal_goto();
  }
}
}

void updateTrajCallback(const quadrotor_msgs::TrajectoryData::ConstPtr & msg){
    next_traj.xcoef[0] = msg->xcoef[0];
    next_traj.ycoef[0] = msg->ycoef[0];
    next_traj.zcoef[0] = msg->zcoef[0];
    next_traj.xcoef[1] = msg->xcoef[1];
    next_traj.ycoef[1] = msg->ycoef[1];
    next_traj.zcoef[1] = msg->zcoef[1];
    next_traj.xcoef[2] = msg->xcoef[2];
    next_traj.ycoef[2] = msg->ycoef[2];
    next_traj.zcoef[2] = msg->zcoef[2];
    next_traj.xcoef[3] = msg->xcoef[3];
    next_traj.ycoef[3] = msg->ycoef[3];
    next_traj.zcoef[3] = msg->zcoef[3];
    next_traj.xcoef[4] = msg->xcoef[4];
    next_traj.ycoef[4] = msg->ycoef[4];
    next_traj.zcoef[4] = msg->zcoef[4];
    next_traj.xcoef[5] = msg->xcoef[5];
    next_traj.ycoef[5] = msg->ycoef[5];
    next_traj.zcoef[5] = msg->zcoef[5];
    next_traj.xcoef[6] = msg->xcoef[6];
    next_traj.ycoef[6] = msg->ycoef[6];
    next_traj.zcoef[6] = msg->zcoef[6];
    next_traj.xcoef[7] = msg->xcoef[7];
    next_traj.ycoef[7] = msg->ycoef[7];
    next_traj.zcoef[7] = msg->zcoef[7];
    next_traj.duration = msg->duration;
    buffer = true;
    ROS_INFO("seg receive: (%f %f %f)", next_traj.xcoef[7],next_traj.ycoef[7],next_traj.zcoef[7]);
}

double cal_pos(double c[],double t)
{
    return c[0]*pow(t,7.0) +c[1]*pow(t,6.0) + c[2]*pow(t,5.0) + c[3]*pow(t,4.0) + c[4]*pow(t,3.0) + c[5]*pow(t,2.0) + c[6]*t + c[7];
}

double cal_vel(double c[],double t)
{
    return (c[0]*7*pow(t,6.0) +c[1]*6*pow(t,5.0) + c[2]*5*pow(t,4.0) + c[3]*4*pow(t,3.0) + c[4]*3*pow(t,2.0) + c[5]*2*t + c[6])/tscale;
}

double cal_acc(double c[],double t)
{
    return (c[0]*42*pow(t,5.0) +c[1]*30*pow(t,4.0) + c[2]*20*pow(t,3.0) + c[3]*12*pow(t,2.0) + c[4]*6*t + c[5]*2)/(tscale*tscale);
}

int main(int argc, char **argv)
{
    ros::init(argc,argv,"quad_trajectory");
    ros::NodeHandle nh("~");
    ros::Publisher pub = nh.advertise<quadrotor_msgs::QuadData>("trajectory", 10, true);
    ros::Subscriber trajectory_sub = nh.subscribe("trajectoryinfo",10,updateTrajCallback, ros::TransportHints().tcpNoDelay());
    ros::Subscriber vicon_sub = nh.subscribe("odom",10,viconCallback, ros::TransportHints().tcpNoDelay());
    if(USE_JOYSTICK)
        ros::Subscriber switch_sub = nh.subscribe("startsignal",10,startprogram, ros::TransportHints().tcpNoDelay());
    ros::Rate loop_rate(100);
    quadrotor_msgs::QuadData traj;
    nh.param("des_pos/x", des_pos[0], 0.0);
    nh.param("des_pos/y", des_pos[1], 0.0);
    nh.param("des_pos/z", des_height, 0.8);
    nh.param("add_tf", addtf, 0.0);
    nh.param("time/scale", tscale, 1.0);
    nh.param("time/takeoff", t_takeoff, 4.0);
    nh.param("time/goto", t_goto, 4.0);
    nh.param("time/land", t_wait, 20.0);
    initialize_var();
    double t_start = -1;
    double t_end = 0;
    double cur_t;
    double cur_off[3] = {0,0,des_height};
    double t_command = 0, t_hover = 0;
    bool follow_path = false;
    if(USE_JOYSTICK){
     while(!start)
        ros::spinOnce();
    }
    else{
        sleep(3);
        ros::spinOnce();
    }
    // }

    t_command = ros::Time::now().toSec();

    while(nh.ok())
    {
        traj.write = 0;
       if(mode == 0 && done_init){
            cur_t = ros::Time::now().toSec();
            t_command = cur_t;
            t_start = cur_t;
            t_end = cur_t + cur_traj.duration*tscale;
            traj.mode = 0;
            while(cur_t <= t_end){
               double t = (cur_t - t_start)/tscale;
               traj.z = cal_pos(cur_traj.zcoef,t)-0.1;
               traj.vz = cal_vel(cur_traj.zcoef,t);
               traj.acc_z = cal_acc(cur_traj.zcoef,t);
               cur_t = ros::Time::now().toSec();
               pub.publish(traj);
               ros::spinOnce();
               loop_rate.sleep();
            }
            mode = 1;
            re_init = true;
            done_init = false;
            t_command = cur_t;
            follow_path = false;
        }

        if(mode == 1 && done_init){
            cur_t = ros::Time::now().toSec();
            t_command = cur_t;
            t_start = cur_t;
            t_end = cur_t + cur_traj.duration*tscale;
            traj.mode = 2;
            while(cur_t <= t_end){
                double t = (cur_t - t_start)/tscale;
                traj.x = cal_pos(cur_traj.xcoef,t);
                traj.vx = cal_vel(cur_traj.xcoef,t);
                traj.acc_x = cal_acc(cur_traj.xcoef,t);
                traj.y = cal_pos(cur_traj.ycoef,t);
                traj.vy = cal_vel(cur_traj.ycoef,t);
                traj.acc_y = cal_acc(cur_traj.ycoef,t);
                traj.z = cal_pos(cur_traj.zcoef,t);
                traj.vz = cal_vel(cur_traj.zcoef,t);
                traj.acc_z = cal_acc(cur_traj.zcoef,t);
                pub.publish(traj);
                ros::spinOnce();
                loop_rate.sleep();
                cur_t = ros::Time::now().toSec();
            }
            mode = 2;
            t_command = cur_t;
            follow_path = false;
        }

        if(mode == 2 && buffer){
            count++;
            cur_traj.duration = next_traj.duration;
            for(int ii = 0; ii < 8; ii++){
                cur_traj.xcoef[ii] = next_traj.xcoef[ii];
                cur_traj.ycoef[ii] = next_traj.ycoef[ii];
                cur_traj.zcoef[ii] = next_traj.zcoef[ii];
            }
            cur_t = ros::Time::now().toSec();
            t_start = cur_t;
            t_end = cur_t + cur_traj.duration*tscale;
            ROS_INFO("Traj: %d from %f %f %f",count,cur_traj.xcoef[7],cur_traj.ycoef[7],cur_traj.zcoef[7]);
            buffer = false;
            while(cur_t < t_end)
            {
                traj.mode = 2;
                traj.write = 1;
                double t = (cur_t - t_start)/tscale;
                traj.x = cal_pos(cur_traj.xcoef,t);
                traj.y = cal_pos(cur_traj.ycoef,t);
                traj.z = cal_pos(cur_traj.zcoef,t);
                traj.vx = cal_vel(cur_traj.xcoef,t);
                traj.vy = cal_vel(cur_traj.ycoef,t);
                traj.vz = cal_vel(cur_traj.zcoef,t);
                traj.acc_x = cal_acc(cur_traj.xcoef,t);
                traj.acc_y = cal_acc(cur_traj.ycoef,t);
                traj.acc_z = cal_acc(cur_traj.zcoef,t);
                traj.yaw = 0;
                pub.publish(traj);
                // ros::spinOnce();
                loop_rate.sleep();
                cur_t = ros::Time::now().toSec();
            }
            t_command = cur_t;
            printf("end\n");
        } else {
            t_hover = ros::Time::now().toSec() - t_command;
            if(t_hover > t_wait) {
                traj.mode = 3;
            } else {
                traj.mode = 1;
            }
        }
        pub.publish(traj);
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}


/* vim: set sw=4 sts=4 et foldmethod=syntax : */

#include <gcs/constraints/comparison.hh>
#include <gcs/constraints/arithmetic.hh>
#include <gcs/problem.hh>
#include <gcs/solve.hh>
#include <util/for_each.hh>

#include <cstdlib>
#include <iostream>
#include <optional>
#include <vector>

#include <boost/program_options.hpp>

using namespace gcs;

using std::cerr;
using std::cout;
using std::endl;
using std::nullopt;
using std::optional;
using std::pair;
using std::string;
using std::to_string;
using std::vector;

namespace po = boost::program_options;

auto main(int argc, char * argv[]) -> int
{
    po::options_description display_options{ "Program options" };
    display_options.add_options()
        ("help", "Display help information")
        ("prove", "Create a proof");

    po::options_description all_options{ "All options" };
    all_options.add_options()
        ("size", po::value<int>()->default_value(88), "Size of the problem to solve")
        ("all", "Find all solutions")
        ;

    all_options.add(display_options);

    po::positional_options_description positional_options;
    positional_options
        .add("size", -1);

    po::variables_map options_vars;

    try {
        po::store(po::command_line_parser(argc, argv)
                .options(all_options)
                .positional(positional_options)
                .run(), options_vars);
        po::notify(options_vars);
    }
    catch (const po::error & e) {
        cerr << "Error: " << e.what() << endl;
        cerr << "Try " << argv[0] << " --help" << endl;
        return EXIT_FAILURE;
    }

    if (options_vars.count("help")) {
        cout << "Usage: " << argv[0] << " [options] [size]" << endl;
        cout << endl;
        cout << display_options << endl;
        return EXIT_SUCCESS;
    }

    cout << "Replicating the n-Queens benchmark." << endl;
    cout << "See Laurent D. Michel, Pierre Schaus, Pascal Van Hentenryck:" << endl;
    cout << "\"MiniCP: a lightweight solver for constraint programming.\"" << endl;
    cout << "Math. Program. Comput. 13(1): 133-184 (2021)." << endl;
    cout << "This should take 49339390 recursions with default options." << endl;
    cout << endl;

    int size = options_vars["size"].as<int>();
    Problem p = options_vars.count("prove") ? Problem{ Proof{ "n_queens.opb", "n_queens.veripb" } } : Problem{ };

    vector<SimpleIntegerVariableID> queens;
    for (int v = 0 ; v != size ; ++v)
        queens.push_back(p.create_integer_variable(0_i, Integer{ size - 1 }, "queen" + to_string(v)));

    for (int i = 0 ; i < size ; ++i) {
        for (int j = i + 1 ; j < size ; ++j) {
            p.post(NotEquals{ queens[i], queens[j] });
            p.post(NotEquals{ queens[i] + Integer{ j - i }, queens[j] });
            p.post(NotEquals{ queens[i] + -Integer{ j - i }, queens[j] });
        }
    }

    auto stats = solve_with(p, SolveCallbacks{
            .solution = [&] (const State & s) -> bool {
                cout << "solution:";
                for (auto & v : queens)
                    cout << " " << s(v);
                cout << endl;

                return options_vars.count("all");
                },
            .guess = [&] (const State & state, IntegerVariableID var) -> vector<Literal> {
                return vector<Literal>{ var == state.lower_bound(var), var != state.lower_bound(var) };
            }
            });

    cout << stats;

    return EXIT_SUCCESS;
}


/*!
 * *****************************************************************************
 *   \file depthMapCreationHSBS_main.cpp
 *   \author moennen
 *   \brief
 *   \date 2018-02-19
 *   *****************************************************************************/

#include "utils/imgFileLst.h"
#include "utils/cv_utils.h"

#include <Eigen/Dense>

#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>

#include <boost/filesystem.hpp>

#include <iostream>

using namespace std;
using namespace cv;
using namespace glm;
using namespace boost;
using namespace Eigen;

namespace
{
//------------------------------------------------------------------------------
//
void processImg( Mat& img )
{
   img = img( Rect( 150, img.rows / 3 + 20, img.cols - 300, 2 * img.rows / 3 - 20 ) ).clone();
}

//------------------------------------------------------------------------------
//
Mat processDepthMask( Mat& depth )
{
   depth =
       depth( Rect( 150, depth.rows / 3 + 20, depth.cols - 300, 2 * depth.rows / 3 - 20 ) ).clone();

   Mat mask = Mat( depth.rows, depth.cols, CV_8UC1 );

#pragma omp parallel for
   for ( unsigned y = 0; y < depth.rows; y++ )
   {
      const unsigned short* depthPtr = depth.ptr<unsigned short>( y );
      unsigned char* maskPtr = mask.ptr<unsigned char>( y );
      for ( unsigned x = 0; x < depth.cols; x++ )
      {
         const unsigned short& d = depthPtr[x];
         maskPtr[x] = d <= 0 ? 0 : 255;
      }
   }

   return mask;
}

//------------------------------------------------------------------------------
//
void processDepth(
    Mat& depth,
    const Mat& img,
    const float undef,
    const float sp_z,
    const float col_z,
    const float filter )
{
   depth =
       depth( Rect( 150, depth.rows / 3 + 20, depth.cols - 300, 2 * depth.rows / 3 - 20 ) ).clone();

   vector<Mat> dPyr;
   dPyr.emplace_back( depth.rows, depth.cols, CV_32FC4 );

   // fill the first level
#pragma omp parallel for
   for ( unsigned y = 0; y < depth.rows; y++ )
   {
      const float* depthPtr = depth.ptr<float>( y );
      vec4* dPtr = dPyr.back().ptr<vec4>( y );
      for ( unsigned x = 0; x < depth.cols; x++ )
      {
         const float d = depthPtr[x];
         dPtr[x] = d == undef ? vec4( 0.0 )
                              : vec4( d, 1.0f, static_cast<float>( x ), static_cast<float>( y ) );
      }
   }

   vector<Mat> iPyr;
   iPyr.push_back( img.clone() );

   unsigned currPyrSz = std::min( depth.rows, depth.cols );

   // downscale : integrate the depth value using position
   while ( currPyrSz >= 2 )
   {
      Mat iCurr;
      resize( iPyr.back(), iCurr, Size( 0, 0 ), 0.5, 0.5, INTER_AREA );
      Mat dCurr( iCurr.rows, iCurr.cols, CV_32FC4 );

#pragma omp parallel for
      for ( unsigned y = 0; y < dCurr.rows; y++ )
      {
         vec4* depthPtr = dCurr.ptr<vec4>( y );

         for ( unsigned x = 0; x < dCurr.cols; x++ )
         {
            vec4 val( 0.0 );
            float w = 0.0;

            const vec2 pos( x * 2.f, y * 2.f );

            for ( int dy = -1; dy <= 1; dy++ )
            {
               for ( int dx = -1; dx <= 1; dx++ )
               {
                  const vec2 dpos( x * 2.f + dx, y * 2.f + dy );
                  const vec4 dH = cv_utils::imsample32F<vec4>( dPyr.back(), dpos );
                  if ( dH.y > 0.0 )
                  {
                     const float dist = distance( pos, dpos );
                     const float z = exp( -dist * dist / sp_z );
                     w += z;
                     val += z * dH;
                  }
               }
            }

            depthPtr[x] = val / ( w > 0.0 ? w : 1.0f );
            assert( !isnan( depthPtr[x].y ) );
            assert( depthPtr[x].x <= 1.0 );
         }
      }

      dPyr.push_back( dCurr.clone() );
      iPyr.push_back( iCurr.clone() );
      currPyrSz = std::min( iCurr.rows, iCurr.cols );
   }

   // upscale
   for ( size_t i = 2; i <= dPyr.size(); ++i )
   {
      const size_t cl = dPyr.size() - i;
      const size_t pl = cl + 1;

      const Mat& iCurr = iPyr[cl];
      const Mat& iPrev = iPyr[pl];

      Mat& dCurr = dPyr[cl];
      const Mat& dPrev = dPyr[pl];

#pragma omp parallel for
      for ( unsigned y = 0; y < dCurr.rows; y++ )
      {
         vec4* depthPtr = dCurr.ptr<vec4>( y );
         const vec3* imgPtr = iCurr.ptr<vec3>( y );

         for ( unsigned x = 0; x < dCurr.cols; x++ )
         {
            vec4& dH = depthPtr[x];

            if ( ( dH.y == 0.0 ) || ( filter > 0.0 ) )
            {
               vec4 val( 0.0 );
               float w = 0.0;

               const vec3& cH = imgPtr[x];
               const vec2 pos( x, y );

               for ( int dy = -1; dy <= 1; dy++ )
               {
                  for ( int dx = -1; dx <= 1; dx++ )
                  {
                     const vec2 dpos( (float)x + (float)dx, (float)y + (float)dy );
                     const vec4 dL = cv_utils::imsample32F<vec4>( dPrev, 0.5f * dpos );
                     if ( isnan( dL.y ) || ( dL.y == 0.0 ) )
                     {
                        vec4 dL2 = cv_utils::imsample32F<vec4>( dPrev, 0.5f * dpos );
                     }
                     assert( !isnan( dL.y ) );
                     assert( dL.y > 0.0 );
                     const vec3 cL = cv_utils::imsample32F<vec3>( iPrev, 0.5f * dpos );

                     const float sp_dist = distance( pos, dpos );
                     const float col_dist = distance( cL, cH );

                     const float z =
                         exp( -sp_dist * sp_dist / sp_z ) * exp( -col_dist * col_dist / col_z );
                     w += z;
                     assert( !isnan( w ) );
                     val += z * dL;
                  }
               }

               dH = ( dH.y == 0.0 ) ? val / w : mix( dH, val / w, filter );
               assert( !isnan( dH.y ) );
               assert( dH.y > 0.0 );
               assert( dH.x <= 1.0 );
            }
         }
      }
   }

#pragma omp parallel for
   for ( unsigned y = 0; y < depth.rows; y++ )
   {
      float* depthPtr = depth.ptr<float>( y );
      vec4* dPtr = dPyr.front().ptr<vec4>( y );
      for ( unsigned x = 0; x < depth.cols; x++ )
      {
         depthPtr[x] = dPtr[x].x;
      }
   }
}

//------------------------------------------------------------------------------
//
void processDepth(
    Mat& depth,
    const Mat& img,
    const float undef = 0.0,
    const int rad = 5,
    const float sigCol = 0.1 )
{
   depth =
       depth( Rect( 150, depth.rows / 3 + 20, depth.cols - 300, 2 * depth.rows / 3 - 20 ) ).clone();

   Mat depthOut = depth.clone();

   const uvec2 maxTileSz( 64, 64 );
   const uvec2 nTiles( depth.cols / maxTileSz.x + 1, depth.rows / maxTileSz.y + 1 );
   const vec2 tileSz( (float)depth.cols / nTiles.x, (float)depth.rows / nTiles.y );

   for ( unsigned tileY = 0; tileY < nTiles.y; ++tileY )
   {
      for ( unsigned tileX = 0; tileX < nTiles.x; ++tileX )
      {
         const vec2 tileId( tileX, tileY );
         const vec2 tileStart = max( tileId * tileSz - (float)rad, vec2( 0.0f ) );
         const vec2 tileEnd =
             min( ( tileId + 1.0f ) * tileSz + (float)rad, vec2( depth.cols - 1, depth.rows - 1 ) );

         const Rect tile(
             tileStart.x, tileStart.y, tileEnd.x - tileStart.x + 1, tileEnd.y - tileStart.y + 1 );

         Mat tileDepth = depth( tile );
         Mat tileDepthOut = depthOut( tile );
         Mat tileImg = img( tile );

         /*imshow("depth", depth);
         imshow("tile", tileDepth);
         waitKey(0);*/

         // ------------
         // solve the system || Wdx - Wdb ||^2
         // W is the affinitiy matrix between pixels :
         // w(x,y) = exp( -|I(x)-I(y)|^2 / sig^2 ) if ||x-y||^2 < th
         //        = 0 otherwise
         // dx are the unknown depth values
         // db are the known depth values

         // set the undefined depth position
         std::vector<uvec2> undefIdx;
         {
            vector<vector<uvec2> > yxUndefIdx( tileDepth.rows );
            vector<unsigned> cumUndef( tileDepth.rows );
            //#pragma omp parallel for
            for ( unsigned y = 0; y < tileDepth.rows; y++ )
            {
               const float* depthPtr = tileDepth.ptr<float>( y );
               vector<uvec2>& xUndefIdx = yxUndefIdx[y];
               unsigned& nUndef = cumUndef[y];
               nUndef = 0;
               xUndefIdx.reserve( tileDepth.cols );
               for ( size_t x = 0; x < tileDepth.cols; x++ )
               {
                  if ( depthPtr[x] != undef ) continue;
                  xUndefIdx.emplace_back( x, y );
                  nUndef++;
               }
            }
            for ( unsigned y = 1; y < tileDepth.rows; y++ ) cumUndef[y] += cumUndef[y - 1];
            undefIdx.resize( cumUndef.back() );
            //#pragma omp parallel for
            for ( unsigned y = 0; y < tileDepth.rows - 1; y++ )
            {
               std::cout << undefIdx.size() << "/" << cumUndef[y] << "/" << yxUndefIdx[y].size()
                         << endl;
               memcpy(
                   &undefIdx[cumUndef[y]],
                   &yxUndefIdx[y][0],
                   sizeof( uvec2 ) * yxUndefIdx[y].size() );
            }
         }

         // create the system
         const unsigned nUndef = undefIdx.size();
         MatrixXf W = MatrixXf::Zero( nUndef, nUndef );
         MatrixXf B = MatrixXf::Zero( nUndef, 1 );
         //#pragma omp parallel for
         for ( unsigned u = 0; u < nUndef; ++u )
         {
            const uvec2& upos = undefIdx[u];
            const vec3 ucol = tileImg.at<vec3>( upos.x, upos.y );

            // compute B from all neigboring defined pixels
            float sb_w = 0.0;
            float wb = 0.0;
            W( u, u ) = -1.0;
            for ( int ny = std::max( (int)( upos.y - rad ), 0 );
                  ny <= std::min( (int)( upos.y + rad ), tileDepth.rows - 1 );
                  ++ny )
            {
               for ( int nx = std::max( (int)( upos.x - rad ), 0 );
                     nx <= std::min( (int)( upos.x + rad ), tileDepth.cols - 1 );
                     ++nx )
               {
                  const uvec2 vpos( nx, ny );
                  if ( vpos == upos ) continue;
                  const float vd = tileDepth.at<float>( vpos.x, vpos.y );
                  if ( vd != undef )
                  {
                     const vec3 dcol = tileImg.at<vec3>( vpos.x, vpos.y ) - ucol;
                     const float w = exp( -1.0 * dot( dcol, dcol ) / sigCol );
                     sb_w += w;
                     wb += w * vd;
                  }
               }
            }
            B( u, 0 ) = sb_w > 0.0 ? wb / sb_w : 0.0;

            // compute W from all neigboring undefined pixels
            float sw_w = 0.0;
            for ( unsigned v = 0; v < nUndef; ++v )
            {
               if ( v == u ) continue;
               const uvec2& vpos = undefIdx[v];
               if ( distance( vec2( upos ), vec2( vpos ) ) > rad ) continue;
               const vec3 dcol = tileImg.at<vec3>( vpos.x, vpos.y ) - ucol;
               const float w = exp( -1.0 * dot( dcol, dcol ) / sigCol );
               sw_w += w;
               W( u, v ) = w;
            }
            if ( sw_w > 0.0 ) W.row( u ) = W.row( u ) / sw_w;
         }

         // solve the system
         MatrixXf WtW( nUndef, nUndef );
         WtW.template triangularView<Lower>() = W.transpose() * W;
         MatrixXf WtB = W.transpose() * B;
         WtW.ldlt().solveInPlace( WtB );

         // set the missing value
         //#pragma omp parallel for
         for ( unsigned u = 0; u < nUndef; ++u )
         {
            const uvec2& pos = undefIdx[u];
            tileDepthOut.at<float>( pos.x, pos.y ) = WtB( u, 0 );
         }

         imshow( "tile", tileDepth );
         imshow( "outtile", tileDepthOut );
         waitKey( 0 );
      }
   }

   depth = depthOut;
}
}
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------

const string keys =
    "{help h usage ? |         | print this message   }"
    "{@imgFileLst   |         | images list   }"
    "{@imgRootDir   |         | images root dir   }"
    "{@imgOutDir    |         | images output dir   }"
    "{@nbSamples      |         |    }";

int main( int argc, char* argv[] )
{
   CommandLineParser parser( argc, argv, keys );
   if ( parser.has( "help" ) )
   {
      parser.printMessage();
      return ( 0 );
   }

   const bool toLinear = false;
   int nbSamples = parser.get<int>( "@nbSamples" );

   const filesystem::path outRootPath( parser.get<string>( "@imgOutDir" ) );
   filesystem::path outGroupRootPath = outRootPath;

   // Create the list of image triplets

   ImgNFileLst<6> imgLst(
       parser.get<string>( "@imgFileLst" ).c_str(), parser.get<string>( "@imgRootDir" ).c_str() );
   if ( imgLst.size() == 0 )
   {
      cerr << "Invalid dataset : " << parser.get<string>( "@imgFileLst" ) << endl;
      return -1;
   }

   unsigned startIdx = 0;
   const int nMaxRendersPerGroup = 10000;

   // Loop through the data
   nbSamples = nbSamples <= 0 ? imgLst.size() : nbSamples;
   for ( size_t i = 0; i < nbSamples; ++i )
   {
      const auto& data = imgLst[i];

      if ( ( ( startIdx + i ) % nMaxRendersPerGroup ) == 0 )
      {
         const unsigned renderGroupId = ( startIdx + i ) / nMaxRendersPerGroup;
         char dirname[7];
         sprintf( dirname, "%06d", renderGroupId );
         outGroupRootPath = outRootPath / filesystem::path( std::string( dirname ) );
         if ( !filesystem::create_directory( outGroupRootPath ) )
         {
            cerr << "Cannot create directory : " << outGroupRootPath.string() << endl;
         }
      }

      char sampleIdName[7];
      sprintf(sampleIdName,"%06d",i);
      const string outPrefix = string(sampleIdName) + "_";
         //filesystem::path( data[1] ).parent_path().stem().string() + "_";

      // for ( size_t j = 0; j < 3; ++j )
      size_t j = 1;
      {
         // load the current image
         Mat img = cv::imread( data[j * 2], cv::IMREAD_UNCHANGED );
         Mat depth = cv::imread( data[j * 2 + 1], cv::IMREAD_UNCHANGED );

         processImg( img );

         Mat mask = processDepthMask( depth );

         // normalize(depth, depth, 1.0, 0.0, NORM_MINMAX);

         // imshow( "InDepth", depth( Rect( 150, depth.rows / 3 + 20, depth.cols - 300, 2 *
         // depth.rows / 3 - 20 ) ) );  processDepth( depth, img, 0.0f, 1.0, 0.1, 0.23  );

         // display
         /*imshow( "Img", img );
         imshow( "Depth", depth );
         imshow( "Mask", mask );*/

         const string outBasename = outPrefix + filesystem::path( data[j * 2] ).stem().string();
         const filesystem::path fRight( outBasename + string( "_rgb" ) + ".png" );
         const filesystem::path fDepth( outBasename + string( "_d" ) + ".png" );
         const filesystem::path fMask( outBasename + string( "_a" ) + ".png" );

         imwrite( filesystem::path( outGroupRootPath / fRight ).string().c_str(), img );
         imwrite( filesystem::path( outGroupRootPath / fDepth ).string().c_str(), depth );
         imwrite( filesystem::path( outGroupRootPath / fMask ).string().c_str(), mask );

         cout << fRight.string() << " " << fDepth.string() << " " << fMask.string();
         // if ( j == 2 )
         cout << endl;
         /*else
          */ cout << " ";

         //waitKey( 0 );
      }

      // waitKey( 0 );
   }

   return ( 0 );
}


#ifndef BOOSTGRAPH
#define BOOSTGRAPH
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/edge_connectivity.hpp>
#include <boost/graph/exterior_property.hpp>
#include <boost/graph/clustering_coefficient.hpp>
#include <boost/graph/dominator_tree.hpp>
#include <boost/graph/cuthill_mckee_ordering.hpp>
#include <boost/graph/king_ordering.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include <boost/graph/bellman_ford_shortest_paths.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/johnson_all_pairs_shortest.hpp>
#include <boost/graph/floyd_warshall_shortest.hpp>
#include <boost/graph/biconnected_components.hpp>
#include <boost/graph/properties.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/graph/edge_list.hpp>

#include <map>
#include <iostream>
#include <utility>

typedef int v_index;
typedef long e_index;

// This struct is the output of the edge connectivity Boost algorithm.
typedef struct {
    v_index ec; // The edge connectivity
    std::vector<v_index> edges; // The edges in a minimum cut, stored as a list of
                       // nodes. For instance, if the minimum cut is
                       // {(1,2),(3,4)}, the output vector will be (1,2,3,4).
} result_ec;

// This struct is the output of the clustering coefficient Boost algorithm.
typedef struct {
    double average_clustering_coefficient; // The average clustering coefficient
    std::vector<double> clust_of_v;             // The clustering coefficient of each node.
} result_cc;

// This struct is the output of the edge connectivity Boost algorithm.
typedef struct {
    std::vector<double> distances; // An array with all distances from the starting vertex
    std::vector<v_index> predecessors; // For each vertex v, the first vertex in a shortest
                                  // path from the starting vertex to v.
} result_distances;

template <class OutEdgeListS, // How neighbors are stored
          class VertexListS,  // How vertices are stored
          class DirectedS,    // The kind of graph (undirectedS, directedS, or bidirectionalS)
          class EdgeListS,    // How the list of edges is stored
          class EdgeProperty> // Properties of edges (weight)
class BoostGraph
/*
 * This generic class wraps a Boost graph, in order to make it Cython-friendly.
 *
 * In particular, it allows to "keep together" the Boost graph and the vector
 * *vertices: these two variables are generic, and Cython is not able to deal
 * with them properly, since it does not support generic classes.
 *
 * Vertices are numbers from 0 to n-1, where n is the total number of vertices:
 * this class takes care of the relation between number i and the corresponding
 * Boost vertex descriptor (which might be any object, depending on the value of
 * VertexListS). In particular, (*vertices)[i] contains the Boost vertex
 * corresponding to number i, while to transform a Boost vertex v into a number
 * we use vertex properties, and the syntax is (*graph)[v].
*/
{
private:
    typedef typename boost::adjacency_list<
        OutEdgeListS, VertexListS, DirectedS,
        boost::property<boost::vertex_index_t, v_index>,
        EdgeProperty, boost::no_property, EdgeListS> adjacency_list;
    typedef typename boost::graph_traits<adjacency_list>::vertex_descriptor vertex_descriptor;
    typedef typename boost::graph_traits<adjacency_list>::edge_descriptor edge_descriptor;
    typedef typename std::vector<edge_descriptor> edge_container;
    typedef typename boost::property_map<adjacency_list, boost::vertex_index_t>::type vertex_to_int_map;

    // This struct is used for biconnected_components function
    struct order_edges {
        bool operator()(const edge_descriptor& x, const edge_descriptor& y) const { return x.get_property() < y.get_property(); }
    };
    // This map is a parameter/output for biconnected_components function
    typedef typename std::map<edge_descriptor, int, order_edges> edge_map;

public:
    adjacency_list graph;
    std::vector<vertex_descriptor> vertices;
    vertex_to_int_map index;

    BoostGraph() {
    }

    v_index num_verts() {
        return num_vertices(graph);
    }

    e_index num_edges() {
        return boost::num_edges(graph);
    }

    void add_vertex() {
        vertices.push_back(boost::add_vertex(vertices.size(), graph));
    }

    void add_edge(v_index u, v_index v) {
        boost::add_edge(vertices[u], vertices[v], graph);
    }

    void add_edge(v_index u, v_index v, double weight) {
        boost::add_edge(vertices[u], vertices[v], weight, graph);
    }

    std::vector<std::pair<v_index, std::pair<v_index, double>>> edge_list() {
        std::vector<std::pair<v_index, std::pair<v_index, double>>> to_return;
        typename boost::graph_traits<adjacency_list>::edge_iterator ei, ei_end;
        for (boost::tie(ei, ei_end) = boost::edges(graph); ei != ei_end; ++ei) {
            to_return.push_back({index[boost::source(*ei, graph)],
                                 {index[boost::target(*ei, graph)],
                                  get(boost::edge_weight, graph, *ei)}});
        }
        return to_return;
    }

    result_ec edge_connectivity() {
        result_ec to_return;
        edge_container disconnecting_set;
        std::back_insert_iterator<edge_container> inserter(disconnecting_set);
        to_return.ec = boost::edge_connectivity(graph, inserter);

        for (size_t i = 0; i < disconnecting_set.size(); i++) {
            edge_descriptor edge = disconnecting_set[i];
            to_return.edges.push_back(index[boost::source(edge, graph)]);
            to_return.edges.push_back(index[boost::target(edge, graph)]);
        }
        return to_return;
    }

    double clustering_coeff(v_index v) {
        return clustering_coefficient(graph, vertices[v]);
    }

    result_cc clustering_coeff_all() {
        result_cc to_return;
        to_return.clust_of_v.resize(num_verts());
        to_return.average_clustering_coefficient = all_clustering_coefficients(graph,
            boost::make_iterator_property_map(to_return.clust_of_v.begin(), index));
        return to_return;
    }

    std::vector<v_index> dominator_tree(v_index v) {
        std::vector<v_index> fathers(num_verts());
        std::vector<vertex_descriptor> fathers_descr(num_verts(),
                    boost::graph_traits<adjacency_list>::null_vertex());

        lengauer_tarjan_dominator_tree(graph, vertices[v],
                                       boost::make_iterator_property_map(
                                           fathers_descr.begin(), index));

        for (v_index i = 0; i < num_verts(); i++) {
            vertex_descriptor v = fathers_descr[i];
            if (v == boost::graph_traits<adjacency_list>::null_vertex()) {
                fathers[i] = -1;
            } else {
                fathers[i] = index[v];
            }
        }
        return fathers;
    }

    // Works only in undirected graphs!
    std::vector<v_index> bandwidth_ordering(bool cuthill) {
        std::vector<v_index> to_return;
        std::vector<vertex_descriptor> inv_perm(num_vertices(graph));

        if (cuthill) {
            boost::cuthill_mckee_ordering(graph, inv_perm.rbegin());
        } else {
            boost::king_ordering(graph, inv_perm.rbegin());
        }

        for (size_t i = 0; i < inv_perm.size(); i++) {
            to_return.push_back(index[inv_perm[i]]);
        }
        return to_return;
    }

    // This function works only on undirected graphs.
    std::vector<v_index> kruskal_min_spanning_tree() {
        std::vector<v_index> to_return;
        std::vector<edge_descriptor> spanning_tree;
        kruskal_minimum_spanning_tree(graph, std::back_inserter(spanning_tree));

        for (unsigned int i = 0; i < spanning_tree.size(); i++) {
            to_return.push_back(index[source(spanning_tree[i], graph)]);
            to_return.push_back(index[target(spanning_tree[i], graph)]);
        }
        return to_return;
    }

    // This function works only on undirected graphs with no parallel edge.
    std::vector<v_index> prim_min_spanning_tree() {
        std::vector<v_index> to_return;
        std::vector<vertex_descriptor> predecessors(num_verts());
        prim_minimum_spanning_tree(graph, boost::make_iterator_property_map(predecessors.begin(), index));

        for (unsigned int i = 0; i < predecessors.size(); i++) {
            if (index[predecessors[i]] != i) {
                to_return.push_back(i);
                to_return.push_back(index[predecessors[i]]);
            }
        }
        return to_return;
    }

    // This function returns the biconnected components of the graph.
    std::vector<std::vector<v_index>> blocks_and_cut_vertices() {
        edge_map bicmp_map;
        boost::associative_property_map<edge_map> bimap(bicmp_map);
        std::size_t num_comps = biconnected_components(graph, bimap);

        // We iterate over every edge and add the vertices of block i into to_return[i].
        // to_return[i] could contain repetitions.
        std::vector<std::vector<v_index>> to_return(num_comps, std::vector<v_index>(0));
        typename boost::graph_traits<adjacency_list>::edge_iterator ei, ei_end;
        for (boost::tie(ei, ei_end) = edges(graph); ei != ei_end; ++ei) {
            to_return[bimap[*ei]].push_back(index[source(*ei, graph)]);
            to_return[bimap[*ei]].push_back(index[target(*ei, graph)]);
        }

        return to_return;
    }

    result_distances dijkstra_shortest_paths(v_index s) {
         v_index N = num_verts();
         result_distances to_return;
         std::vector<double> distances(N, (std::numeric_limits<double>::max)());
         std::vector<vertex_descriptor> predecessors(N);
         try {
             boost::dijkstra_shortest_paths(graph, vertices[s], distance_map(boost::make_iterator_property_map(distances.begin(), index))
                                            .predecessor_map(boost::make_iterator_property_map(predecessors.begin(), index)));
         } catch (boost::exception_detail::clone_impl<boost::exception_detail::error_info_injector<boost::negative_edge> > e) {
             return to_return;
         }

         to_return.distances = distances;

         for (int i = 0; i < N; i++) {
             to_return.predecessors.push_back(index[predecessors[i]]);
         }

         return to_return;
     }

     result_distances bellman_ford_shortest_paths(v_index s) {
         v_index N = num_verts();

         std::vector<double> distance(N, (std::numeric_limits<double>::max)());
         std::vector<vertex_descriptor> predecessors(N);
         result_distances to_return;
         typename boost::property_map<adjacency_list, boost::edge_weight_t>::type weight = get(boost::edge_weight, (graph));

         for (v_index i = 0; i < N; ++i)
             predecessors[i] = vertices[i];

         distance[s] = 0;
         bool r = boost::bellman_ford_shortest_paths
             (graph, N, boost::weight_map(weight).distance_map(boost::make_iterator_property_map(distance.begin(), index)).predecessor_map(boost::make_iterator_property_map(predecessors.begin(), index)));

         if (!r) {
             return to_return;
         }

         to_return.distances = distance;
         for (int i = 0; i < N; i++) {
             to_return.predecessors.push_back(index[predecessors[i]]);
         }
         return to_return;
     }
     std::vector<std::vector<double> > floyd_warshall_shortest_paths() {
         v_index N = num_verts();

         std::vector<std::vector<double> > D(N, std::vector<double>(N));
         if (floyd_warshall_all_pairs_shortest_paths(graph, D)) {
             return D;
         } else {
             return std::vector<std::vector<double> >();
         }
     }
     std::vector<std::vector<double> > johnson_shortest_paths() {
         v_index N = num_verts();

         std::vector<std::vector<double> > D(N, std::vector<double>(N));
         if (johnson_all_pairs_shortest_paths(graph, D)) {
             return D;
         } else {
             return std::vector<std::vector<double> >();
         }
     }
};

#endif // BOOSTGRAPH


#pragma once

// C++ standard library
#include <random>

// Armadillo
#include <armadillo>

namespace mant {
  arma::mat randomRotationMatrix(
      const arma::uword numberOfDimensions);

  arma::uvec randomPermutationVector(
      const arma::uword numberOfElements,
      const arma::uword cycleSize);
  arma::uvec randomPermutationVector(
      const arma::uword numberOfElements);

  arma::vec randomNeighbour(
      const arma::vec& parameter,
      const double minimalDistance,
      const double maximalDistance);
  arma::vec randomNeighbour(
      const arma::vec& parameter,
      const double maximalDistance);

  arma::mat uniformRandomNumbers(
      const arma::uword numberOfRows,
      const arma::uword numberOfColumns,
      std::uniform_real_distribution<double> distribution);
  arma::mat uniformRandomNumbers(
      const arma::uword numberOfRows,
      const arma::uword numberOfColumns);

  arma::vec uniformRandomNumbers(
      const arma::uword numberOfElements,
      std::uniform_real_distribution<double> distribution);
  arma::vec uniformRandomNumbers(
      const arma::uword numberOfElements);

  arma::mat normalRandomNumbers(
      const arma::uword numberOfRows,
      const arma::uword numberOfColumns,
      std::normal_distribution<double> distribution);
  arma::mat normalRandomNumbers(
      const arma::uword numberOfRows,
      const arma::uword numberOfColumns);

  arma::vec normalRandomNumbers(
      const arma::uword numberOfElements,
      std::normal_distribution<double> distribution);
  arma::vec normalRandomNumbers(
      const arma::uword numberOfElements);
}


#include <blitz/array.h>

using namespace blitz;

int main()
{
    Array<complex<float>, 2> Z(4,4);

    Z = complex<float>(0.0, 1.0);

    Z(4,4) = complex<float>(1.0, 0.0);

    return 0;
}


/**
 * \file boost/numeric/ublasx/operation/log2.hpp
 *
 * \brief Apply the \c std::log2 function to a vector or matrix expression.
 *
 * Copyright (c) 2011, Marco Guazzone
 *
 * 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)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_LOG2_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_LOG2_HPP

#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/expression/matrix_unary_functor.hpp>
#include <boost/numeric/ublasx/expression/vector_unary_functor.hpp>
#include <cmath>
#include <complex>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

namespace detail {

template <typename VectorExprT>
struct vector_log2_functor_traits
{
	typedef VectorExprT input_expression_type;
	typedef typename vector_traits<input_expression_type>::value_type signature_argument_type;
	typedef signature_argument_type signature_result_type;
	typedef vector_unary_functor_traits<
				input_expression_type,
				signature_result_type (signature_argument_type)
			> unary_functor_expression_type;
	typedef typename unary_functor_expression_type::result_type result_type;
	typedef typename unary_functor_expression_type::expression_type expression_type;
};

template <typename MatrixExprT>
struct matrix_log2_functor_traits
{
	typedef MatrixExprT input_expression_type;
	typedef typename matrix_traits<input_expression_type>::value_type signature_argument_type;
	typedef signature_argument_type signature_result_type;
	typedef matrix_unary_functor_traits<
				input_expression_type,
				signature_result_type (signature_argument_type)
			> unary_functor_expression_type;
	typedef typename unary_functor_expression_type::result_type result_type;
	typedef typename unary_functor_expression_type::expression_type expression_type;
};

namespace /*<unnamed>*/ {

/// Auxiliary function used to replace ::std::log2 when that is not available.
template <typename T>
BOOST_UBLAS_INLINE
T log2(T x)
{
  // C++0x and higher has std::log2 but it
  // doesn't work with complex numbers.
	return ::std::log(x)/::std::log(2);
}

} // Namespace <unnamed>

} // Namespace detail

/**
 * \brief Applies the \c std::log2 function to a given vector expression.
 *
 * \tparam VectorExprT The type of the input vector expression.
 *
 * \param ve The input vector expression.
 * \return A vector expression representing the application of \c std::log2 to
 *  each element of \a ve.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename VectorExprT>
BOOST_UBLAS_INLINE
typename detail::vector_log2_functor_traits<VectorExprT>::result_type log2(vector_expression<VectorExprT> const& ve)
{
	typedef typename detail::vector_log2_functor_traits<VectorExprT>::expression_type expression_type;
	typedef typename detail::vector_log2_functor_traits<VectorExprT>::signature_result_type signature_result_type;

	return expression_type(ve(), detail::log2<signature_result_type>);
}

/**
 * \brief Applies the \c std::log2 function to a given matrix expression.
 *
 * \tparam MatrixExprT The type of the input matrix expression.
 *
 * \param me The input matrix expression.
 * \return A matrix expression representing the application of \c std::log2 to
 *  each element of \a me.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename MatrixExprT>
BOOST_UBLAS_INLINE
typename detail::matrix_log2_functor_traits<MatrixExprT>::result_type log2(matrix_expression<MatrixExprT> const& me)
{
	typedef typename detail::matrix_log2_functor_traits<MatrixExprT>::expression_type expression_type;
	typedef typename detail::matrix_log2_functor_traits<MatrixExprT>::signature_result_type signature_result_type;

	return expression_type(me(), detail::log2<signature_result_type>);
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_LOG2_HPP


#define BOOST_TEST_MODULE vector

#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/mpl/list.hpp>

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

typedef boost::mpl::list<
	mla::vector::Dense<float>,
	mla::vector::Dense<double>,
	mla::vector::SparseCS<float>,
	mla::vector::SparseCS<double>
> vector_type_list;

BOOST_AUTO_TEST_SUITE(vector)

BOOST_AUTO_TEST_CASE_TEMPLATE( Declaration, VectorType, vector_type_list )
{
	VectorType v(3);

	BOOST_CHECK_EQUAL(v.size(), 3);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( Clearing, VectorType, vector_type_list )
{
	VectorType v(3);
	v.setZero();

	for(unsigned int i = 0; i < 3; i++)
	{
		BOOST_CHECK_EQUAL(v.getValue(i), 0.0);
	}

}

BOOST_AUTO_TEST_CASE_TEMPLATE( Assigning, VectorType, vector_type_list )
{
	VectorType v(3);
	v.setZero();

	v.setValue(1, 1.0);

	BOOST_CHECK_EQUAL(v.getValue(0), 0.0);
	BOOST_CHECK_EQUAL(v.getValue(1), 1.0);
	BOOST_CHECK_EQUAL(v.getValue(2), 0.0);
}

BOOST_AUTO_TEST_SUITE_END()


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

#ifndef BOOST_MATH_STATISTICS_RUNS_TEST_HPP
#define BOOST_MATH_STATISTICS_RUNS_TEST_HPP

#include <cmath>
#include <algorithm>
#include <utility>
#include <boost/math/statistics/univariate_statistics.hpp>
#include <boost/math/distributions/normal.hpp>

namespace boost::math::statistics {

template<class RandomAccessContainer>
auto runs_above_and_below_threshold(RandomAccessContainer const & v,
                          typename RandomAccessContainer::value_type threshold)
{
    using Real = typename RandomAccessContainer::value_type;
    using std::sqrt;
    using std::abs;
    if (v.size() <= 1)
    {
        throw std::domain_error("At least 2 samples are required to get number of runs.");
    }
    typedef boost::math::policies::policy<
          boost::math::policies::promote_float<false>,
          boost::math::policies::promote_double<false> >
          no_promote_policy;

    decltype(v.size()) nabove = 0;
    decltype(v.size()) nbelow = 0;

    decltype(v.size()) imin = 0;

    // Take care of the case that v[0] == threshold:
    while (imin < v.size() && v[imin] == threshold) {
        ++imin;
    }

    // Take care of the constant vector case:
    if (imin == v.size()) {
        return std::make_pair(std::numeric_limits<Real>::quiet_NaN(), Real(0));
    }

    bool run_up = (v[imin] > threshold);
    if (run_up) {
        ++nabove;
    } else {
        ++nbelow;
    }
    decltype(v.size()) runs = 1;
    for (decltype(v.size()) i = imin + 1; i < v.size(); ++i) {
      if (v[i] == threshold) {
        // skip values precisely equal to threshold (following R's randtests package)
        continue;
      }
      bool above = (v[i] > threshold);
      if (above) {
          ++nabove;
      } else {
          ++nbelow;
      }
      if (run_up == above) {
        continue;
      }
      else {
        run_up = above;
        runs++;
      }
    }

    // If you make n an int, the subtraction is gonna be bad in the variance:
    Real n = nabove + nbelow;

    Real expected_runs = Real(1) + Real(2*nabove*nbelow)/Real(n);
    Real variance = 2*nabove*nbelow*(2*nabove*nbelow-n)/Real(n*n*(n-1));

    // Bizarre, pathological limits:
    if (variance == 0)
    {
        if (runs == expected_runs)
        {
            Real statistic = 0;
            Real pvalue = 1;
            return std::make_pair(statistic, pvalue);
        }
        else
        {
            return std::make_pair(std::numeric_limits<Real>::quiet_NaN(), Real(0));
        }
    }

    Real sd = sqrt(variance);
    Real statistic = (runs - expected_runs)/sd;

    auto normal = boost::math::normal_distribution<Real, no_promote_policy>(0,1);
    Real pvalue = 2*boost::math::cdf(normal, -abs(statistic));
    return std::make_pair(statistic, pvalue);
}

template<class RandomAccessContainer>
auto runs_above_and_below_median(RandomAccessContainer const & v)
{
    using Real = typename RandomAccessContainer::value_type;
    using std::log;
    using std::sqrt;

    // We have to memcpy v because the median does a partial sort,
    // and that would be catastrophic for the runs test.
    auto w = v;
    Real median = boost::math::statistics::median(w);
    return runs_above_and_below_threshold(v, median);
}

}
#endif