//
// Created by lancelot on 3/15/17.
//

#ifndef PNP_PNPSIMULATION_H
#define PNP_PNPSIMULATION_H

#include <boost/math/distributions.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/random.hpp>

#include <random>
#include <vector>
#include <Eigen/Dense>
#include <sophus/se3.hpp>



/*
template<typename T, int N>
void sampleGauss(Eigen::Matrix<T, N, 1>& mean,
                 Eigen::Matrix<T, N, N>& var,
                 std::vector<Eigen::Matrix<T, N, 1>>& vec, int num = 50) {
    g2o::GaussianSampler<Eigen::Matrix<T, N, 1>, Eigen::Matrix<T, N, N>> gaussSampler;
    gaussSampler.setDistribution(var);
    for (int i = 0; i < num; ++i) {
        Eigen::Matrix<T, N, 1> v = mean + gaussSampler.generateSample();
        vec.push_back(v);
    }
}

template <typename T, int N>
Eigen::Matrix<T, N, 1> oneSampleGauss(Eigen::Matrix<T, N, 1>& mean,
                 Eigen::Matrix<T, N, N>& var) {
    g2o::GaussianSampler<Eigen::Matrix<T, N, 1>, Eigen::Matrix<T, N, N>> gaussSampler;
    gaussSampler.setDistribution(var);
    return mean + gaussSampler.generateSample();
}


template <typename T, int N>
void sampleUniformMeans(T start, T end, std::vector<Eigen::Matrix<T, N, 1>>& means, int num = 50) {
    static boost::mt19937 rng(static_cast<unsigned>(std::time(0)));
    boost::uniform_real<T> uni_dist(start, end);
    Eigen::Matrix<T, N, 1> mean;
    for (int i = 0; i < num; ++i) {
        for(int dim = 0; dim < N; dim++)
            mean(dim) = uni_dist(rng);
        means.push_back(mean);
    }
}

template<typename T, int N>
void sampleGauss(std::vector<Eigen::Matrix<T, N, 1>>& means,
                 Eigen::Matrix<T, N, N>& var,
                 std::vector<Eigen::Matrix<T, N, 1>>& vec) {
    g2o::GaussianSampler<Eigen::Matrix<T, N, 1>, Eigen::Matrix<T, N, N>> gaussSampler;
    gaussSampler.setDistribution(var);
    for (size_t i = 0; i < means.size(); ++i) {
        Eigen::Matrix<T, N, 1> v = means[i] + gaussSampler.generateSample();
        vec.push_back(v);
    }
}
*/
template <typename RealType = double, int N>
void sampleUniformMeans(RealType start, RealType end, std::vector<Eigen::Matrix<RealType,N,1> >& means,int num = 50)
{
	std::random_device rd; // will be used to obtain a seed for the random number engine
	std::mt19937 gen(rd()); // seeded with rd()
	std::uniform_real_distribution<RealType> dis(start, end);
	Eigen::Matrix<RealType, N, 1> mean;
	for (int i = 0; i < num; ++i) {
		for (int dim = 0; dim < N; dim++)
			mean(dim) = dis(gen);
		means.push_back(mean);
	}
}

template<typename RealType = double, int N>
Eigen::Matrix<RealType, N, 1> sampleGaussian( Eigen::Matrix<RealType, N, 1>& mean, Eigen::Matrix<RealType,N, N>& var)
{
	// first find a cholesky decomposition ...
	Eigen::LLT<Eigen::Matrix<RealType, N, N> > cholDecomp(var);
	assert(cholDecomp.info() != Eigen::NumericalIssue);
	Eigen::Matrix<RealType, N, N> choleskyFactor = cholDecomp.matrixL();

	std::random_device rd;
	std::mt19937 gen(rd());
	std::normal_distribution<RealType> d(0, 1);

	Eigen::Matrix<RealType, N, 1> sampleResult;
	for (int i = 0; i < N; ++i)
		sampleResult(i) = d(gen);
	return mean + choleskyFactor*sampleResult;
}


struct Camera {
    Camera(double fx, double fy, double cx, double cy) {
        fx_ = fx;
        fy_ = fy;
        cx_ = cx;
        cy_ = cy;
    }

    Eigen::Vector2d project(double x, double y, double z);
    Eigen::Vector2d project(Eigen::Vector3d point);
    Eigen::Vector3d bacProject(Eigen::Vector2d uv, double d);

    double fx_;
    double fy_;
    double cx_;
    double cy_;
};


class PNPSimulation {
public:
    PNPSimulation(Sophus::SE3d& se3, Eigen::Matrix<double, 2, 2>& Var);
    ~PNPSimulation();

    void start();

private:
    Sophus::SE3d real_;
    Eigen::Matrix2d information_;
};


#endif //PNP_PNPSIMULATION_H