#include <iostream>
#include <iomanip>
#include <fstream>
#include <vector>
#include <array>
#include <numeric>
#include <cmath>
#ifdef _OPENMP
#include <omp.h>
#endif

#include <boost/scope_exit.hpp>
#include <boost/program_options.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

#if defined(SOLVER_BACKEND_VEXCL)
#  include <amgcl/backend/vexcl.hpp>
   typedef amgcl::backend::vexcl<double> Backend;
#elif defined(SOLVER_BACKEND_CUDA)
#  include <amgcl/backend/cuda.hpp>
#  include <amgcl/relaxation/cusparse_ilu0.hpp>
   typedef amgcl::backend::cuda<double> Backend;
#else
#  ifndef SOLVER_BACKEND_BUILTIN
#    define SOLVER_BACKEND_BUILTIN
#  endif
#  include <amgcl/backend/builtin.hpp>
   typedef amgcl::backend::builtin<double> Backend;
#endif

#include <amgcl/mpi/direct_solver/runtime.hpp>
#include <amgcl/mpi/solver/runtime.hpp>
#include <amgcl/mpi/subdomain_deflation.hpp>
#include <amgcl/amg.hpp>
#include <amgcl/coarsening/runtime.hpp>
#include <amgcl/relaxation/runtime.hpp>
#include <amgcl/relaxation/as_preconditioner.hpp>
#include <amgcl/profiler.hpp>

#include "domain_partition.hpp"

namespace amgcl {
    profiler<> prof;
}

struct deflation_vectors {
    size_t nv;
    std::vector<double> x;
    std::vector<double> y;
    std::vector<double> z;

    deflation_vectors(ptrdiff_t n, size_t nv = 4) : nv(nv), x(n), y(n), z(n) {}

    size_t dim() const { return nv; }

    double operator()(ptrdiff_t i, int j) const {
        switch(j) {
            default:
            case 0:
                return 1;
            case 1:
                return x[i];
            case 2:
                return y[i];
            case 3:
                return z[i];
        }
    }
};

struct renumbering {
    const domain_partition<3> ∂
    const std::vector<ptrdiff_t> &dom;

    renumbering(
            const domain_partition<3> &p,
            const std::vector<ptrdiff_t> &d
            ) : part(p), dom(d)
    {}

    ptrdiff_t operator()(ptrdiff_t i, ptrdiff_t j, ptrdiff_t k) const {
        boost::array<ptrdiff_t, 3> p = {{i, j, k}};
        std::pair<int,ptrdiff_t> v = part.index(p);
        return dom[v.first] + v.second;
    }
};

int main(int argc, char *argv[]) {
    int provided;
    MPI_Init_thread(&argc, &argv, MPI_THREAD_MULTIPLE, &provided);
    BOOST_SCOPE_EXIT(void) {
        MPI_Finalize();
    } BOOST_SCOPE_EXIT_END

    amgcl::mpi::communicator world(MPI_COMM_WORLD);

    if (world.rank == 0)
        std::cout << "World size: " << world.size << std::endl;

    // Read configuration from command line
    ptrdiff_t n = 128;
    bool constant_deflation = false;

    amgcl::runtime::coarsening::type   coarsening       = amgcl::runtime::coarsening::smoothed_aggregation;
    amgcl::runtime::relaxation::type   relaxation       = amgcl::runtime::relaxation::spai0;
    amgcl::runtime::solver::type       iterative_solver = amgcl::runtime::solver::bicgstabl;
    amgcl::runtime::mpi::direct::type  direct_solver    = amgcl::runtime::mpi::direct::skyline_lu;

    bool just_relax = false;
    bool symm_dirichlet = true;
    std::string parameter_file;

    namespace po = boost::program_options;
    po::options_description desc("Options");

    desc.add_options()
        ("help,h", "show help")
        (
         "symbc",
         po::value<bool>(&symm_dirichlet)->default_value(symm_dirichlet),
         "Use symmetric Dirichlet conditions in laplace2d"
        )
        (
         "size,n",
         po::value<ptrdiff_t>(&n)->default_value(n),
         "domain size"
        )
        (
         "coarsening,c",
         po::value<amgcl::runtime::coarsening::type>(&coarsening)->default_value(coarsening),
         "ruge_stuben, aggregation, smoothed_aggregation, smoothed_aggr_emin"
        )
        (
         "relaxation,r",
         po::value<amgcl::runtime::relaxation::type>(&relaxation)->default_value(relaxation),
         "gauss_seidel, ilu0, iluk, ilut, damped_jacobi, spai0, spai1, chebyshev"
        )
        (
         "iter_solver,i",
         po::value<amgcl::runtime::solver::type>(&iterative_solver)->default_value(iterative_solver),
         "cg, bicgstab, bicgstabl, gmres"
        )
        (
         "dir_solver,d",
         po::value<amgcl::runtime::mpi::direct::type>(&direct_solver)->default_value(direct_solver),
         "skyline_lu"
#ifdef AMGCL_HAVE_EIGEN
         ", eigen_splu"
#endif
#ifdef AMGCL_HAVE_PASTIX
         ", pastix"
#endif
        )
        (
         "cd",
         po::bool_switch(&constant_deflation),
         "Use constant deflation (linear deflation is used by default)"
        )
        (
         "params,P",
         po::value<std::string>(&parameter_file),
         "parameter file in json format"
        )
        (
         "prm,p",
         po::value< std::vector<std::string> >()->multitoken(),
         "Parameters specified as name=value pairs. "
         "May be provided multiple times. Examples:\n"
         "  -p solver.tol=1e-3\n"
         "  -p precond.coarse_enough=300"
        )
        (
         "just-relax,0",
         po::bool_switch(&just_relax),
         "Do not create AMG hierarchy, use relaxation as preconditioner"
        )
        ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help")) {
        std::cout << desc << std::endl;
        return 0;
    }

    boost::property_tree::ptree prm;
    if (vm.count("params")) read_json(parameter_file, prm);

    if (vm.count("prm")) {
        for(const std::string &v : vm["prm"].as< std::vector<std::string> >()) {
            amgcl::put(prm, v);
        }
    }

    prm.put("isolver.type", iterative_solver);
    prm.put("dsolver.type", direct_solver);

    const ptrdiff_t n3 = n * n * n;

    boost::array<ptrdiff_t, 3> lo = { {0,   0,   0  } };
    boost::array<ptrdiff_t, 3> hi = { {n-1, n-1, n-1} };

    using amgcl::prof;

    prof.tic("partition");
    domain_partition<3> part(lo, hi, world.size);
    ptrdiff_t chunk = part.size( world.rank );

    std::vector<ptrdiff_t> domain(world.size + 1);
    MPI_Allgather(
            &chunk, 1, amgcl::mpi::datatype<ptrdiff_t>(),
            &domain[1], 1, amgcl::mpi::datatype<ptrdiff_t>(), world);
    std::partial_sum(domain.begin(), domain.end(), domain.begin());

    lo = part.domain(world.rank).min_corner();
    hi = part.domain(world.rank).max_corner();

    renumbering renum(part, domain);

    deflation_vectors def(chunk, constant_deflation ? 1 : 4);
    for(ptrdiff_t k = lo[2]; k <= hi[2]; ++k) {
        for(ptrdiff_t j = lo[1]; j <= hi[1]; ++j) {
            for(ptrdiff_t i = lo[0]; i <= hi[0]; ++i) {
                boost::array<ptrdiff_t, 3> p = {{i, j, k}};
                std::pair<int,ptrdiff_t> v = part.index(p);

                def.x[v.second] = (i - (lo[0] + hi[0]) / 2);
                def.y[v.second] = (j - (lo[1] + hi[1]) / 2);
                def.z[v.second] = (k - (lo[2] + hi[2]) / 2);
            }
        }
    }
    prof.toc("partition");

    prof.tic("assemble");
    std::vector<ptrdiff_t> ptr;
    std::vector<ptrdiff_t> col;
    std::vector<double>    val;
    std::vector<double>    rhs;

    ptr.reserve(chunk + 1);
    col.reserve(chunk * 7);
    val.reserve(chunk * 7);
    rhs.reserve(chunk);

    ptr.push_back(0);

    const double h2i  = (n - 1) * (n - 1);

    for(ptrdiff_t k = lo[2]; k <= hi[2]; ++k) {
        for(ptrdiff_t j = lo[1]; j <= hi[1]; ++j) {
            for(ptrdiff_t i = lo[0]; i <= hi[0]; ++i) {

                if (!symm_dirichlet && (i == 0 || j == 0 || k == 0 || i + 1 == n || j + 1 == n || k + 1 == n)) {
                    col.push_back(renum(i,j,k));
                    val.push_back(1);
                    rhs.push_back(0);
                } else {
                    if (k > 0)  {
                        col.push_back(renum(i,j,k-1));
                        val.push_back(-h2i);
                    }

                    if (j > 0)  {
                        col.push_back(renum(i,j-1,k));
                        val.push_back(-h2i);
                    }

                    if (i > 0) {
                        col.push_back(renum(i-1,j,k));
                        val.push_back(-h2i);
                    }

                    col.push_back(renum(i,j,k));
                    val.push_back(6 * h2i);

                    if (i + 1 < n) {
                        col.push_back(renum(i+1,j,k));
                        val.push_back(-h2i);
                    }

                    if (j + 1 < n) {
                        col.push_back(renum(i,j+1,k));
                        val.push_back(-h2i);
                    }

                    if (k + 1 < n) {
                        col.push_back(renum(i,j,k+1));
                        val.push_back(-h2i);
                    }

                    rhs.push_back(1);
                }
                ptr.push_back( col.size() );
            }
        }
    }
    prof.toc("assemble");

    Backend::params bprm;

#if defined(SOLVER_BACKEND_VEXCL)
    vex::Context ctx(vex::Filter::Env);
    std::cout << ctx << std::endl;
    bprm.q = ctx;
#elif defined(SOLVER_BACKEND_CUDA)
    cusparseCreate(&bprm.cusparse_handle);
#endif

    auto f = Backend::copy_vector(rhs, bprm);
    auto x = Backend::create_vector(chunk, bprm);

    amgcl::backend::clear(*x);

    size_t iters;
    double resid, tm_setup, tm_solve;

    std::function<double(ptrdiff_t, unsigned)> def_vec = std::cref(def);
    prm.put("num_def_vec", def.dim());
    prm.put("def_vec",     &def_vec);

    try {
    if (just_relax) {
        prm.put("local.type", relaxation);

        prof.tic("setup");
        typedef
            amgcl::mpi::subdomain_deflation<
                amgcl::relaxation::as_preconditioner<Backend, amgcl::runtime::relaxation::wrapper >,
                amgcl::runtime::mpi::solver::wrapper<Backend>,
                amgcl::runtime::mpi::direct::solver<double>
            > SDD;

        SDD solve(world, std::tie(chunk, ptr, col, val), prm, bprm);
        tm_setup = prof.toc("setup");

        prof.tic("solve");
        std::tie(iters, resid) = solve(*f, *x);
        tm_solve = prof.toc("solve");
    } else {
        prm.put("local.coarsening.type", coarsening);
        prm.put("local.relax.type", relaxation);

        prof.tic("setup");
        typedef
            amgcl::mpi::subdomain_deflation<
                amgcl::amg<Backend, amgcl::runtime::coarsening::wrapper, amgcl::runtime::relaxation::wrapper>,
                amgcl::runtime::mpi::solver::wrapper<Backend>,
                amgcl::runtime::mpi::direct::solver<double>
            > SDD;

        SDD solve(world, std::tie(chunk, ptr, col, val), prm, bprm);
        tm_setup = prof.toc("setup");

        prof.tic("solve");
        std::tie(iters, resid) = solve(*f, *x);
        tm_solve = prof.toc("solve");
    }
    } catch(const std::exception &e) {
        std::cerr << e.what() << std::endl;
        throw e;
    }

    if (world.rank == 0) {
        std::cout
            << "Iterations: " << iters << std::endl
            << "Error:      " << resid << std::endl
            << std::endl
            << prof << std::endl;

#ifdef _OPENMP
        int nt = omp_get_max_threads();
#else
        int nt = 1;
#endif
        std::ostringstream log_name;
        log_name << "log3d_" << n3 << "_" << nt << "_" << world.size << ".txt";
        std::ofstream log(log_name.str().c_str(), std::ios::app);
        log << n3 << "\t" << nt << "\t" << world.size
            << "\t" << tm_setup << "\t" << tm_solve
            << "\t" << iters << "\t" << std::endl;
    }

}


/******************************************************************************
 * Author:   Laurent Kneip                                                    *
 * Contact:  kneip.laurent@gmail.com                                          *
 * License:  Copyright (c) 2016 Laurent Kneip. All rights reserved.           *
 *                                                                            *
 * This is proprietary software. Use or redistribution, with or without       *
 * modifications, is not permitted unless an individual licensing agreement   *
 * with the owner has been made. Please contact the owner.                    *
 ******************************************************************************/

#ifndef POLYVIEW_DEMOS_RGBD_PLANETRACKER_HPP_
#define POLYVIEW_DEMOS_RGBD_PLANETRACKER_HPP_

#include <stdlib.h>
#include <vector>
#include <Eigen/Eigen>
#include <thread>

#include <polyview/containers/Multiframe.hpp>
#include <polyview/io/OpenCVExporter.hpp>

namespace polyview
{
namespace demos
{

class RGBD_PLANETRACKER
{
public:
    typedef std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > EigVec;
    typedef std::vector<std::vector<int>> VecVec;
    enum State
    {
      IDLE,
      TRACKING
    };

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    RGBD_PLANETRACKER();
    virtual ~RGBD_PLANETRACKER();

    double process(
        polyview::containers::Multiframe::Ptr & mf );

	State _state;
    polyview::io::OpenCVExporter _display;
	Eigen::Vector3d _normal;
	Eigen::Matrix3d _Rcan;
	Eigen::Vector3d _tcan;
    Eigen::Matrix3d _R;
    Eigen::Vector3d _t;
	Eigen::Matrix3d _Rbody;
	Eigen::Vector3d _tbody;
    Eigen::Vector3d _tlast;
    Eigen::Matrix3d _Rlast;
    Eigen::Vector3d _tbodylast;
    Eigen::Matrix3d _Rbodylast;
	Eigen::Vector2d _CentralPoint;

    double _alpha;
    double _depth;
	double _p2pDistanceThreshold;
    double _depthThreshold;
    double _chairThreshold;
    double _chairDistance;
    double _chairHeight;
    double _chairWeidth;
    bool _plot;
    bool _icp;
	size_t _thresholdNumberPointsOnPlane;
	size_t _refinementIterations;
    size_t _maxClass;
    size_t _minClass;
    VecVec _refClass;
    EigVec _refPoints;

private:
    void configure();
	void samplePoints(containers::Multiframe::Ptr & mf, Eigen::Vector3d & p_cam1, Eigen::Vector3d & p_cam2, Eigen::Vector3d & p_cam3 );
	void getPlaneHypothesis( Eigen::Vector3d & p_cam1, Eigen::Vector3d & p_cam2, Eigen::Vector3d & p_cam3);
	size_t countInliers(containers::Multiframe::Ptr & mf, bool plotting = false);
	void refineInliers( containers::Multiframe::Ptr & mf, size_t reserveSize);
    void findObjectOfInterest(containers::Multiframe::Ptr & mf);
    void plotElement( containers::Multiframe::Ptr & mf, Eigen::Vector3d & p1, Eigen::Vector3d & p2 );
    void boundObject(containers::Multiframe::Ptr & mf, EigVec & Object, VecVec & Class);
    void icp(containers::Multiframe::Ptr & mf, EigVec & CurObject, VecVec & CurClass);
};

}
}

#endif /* POLYVIEW_DEMOS_RGBD_SDVO_HPP_ */


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE DegreeOneReducerTests
#include <boost/test/unit_test.hpp>

#include <vector>
#include <set>

#include "graph/PBQPGraph.hpp"
#include "graph/Vector.hpp"
#include "graph/PBQPNode.hpp"
#include "graph/PBQPEdge.hpp"
#include "reduction/PBQPReduction.hpp"
#include "reduction/degree/DegreeOneReducer.hpp"
#include "graph/PBQPSolution.hpp"

#include "util/TestUtils.hpp"

namespace pbqppapa {

BOOST_AUTO_TEST_CASE(emptyGraphTest) {
	//make sure this doesnt explode
	PBQPGraph<int> graph;
	DegreeOneReducer<int> oneReducer(&graph);
	std::vector<PBQPGraph<int>*> result = oneReducer.reduce();
}

/*
BOOST_AUTO_TEST_CASE(simpleNodeReductionCount) {
	PBQPGraph<int> graph = PBQPGraph<int>();
	for (int i = 0; i < 100; i++) {
		int arr[] { 3, 1 };
		Vector<int> vek(2, arr);
		PBQPNode<int>* node = graph.addNode(vek);
	}
	auto iter = graph.getNodeBegin();
	PBQPNode<int>* first = *(graph.getNodeBegin());
	while (iter != graph.getNodeEnd()) {
		PBQPNode<int>* node = *iter++;
		int arr [] {2, 1, 5, 3};
		Matrix<int> mat (2, 2, arr);
		graph.addEdge(first, node, mat);
	}
	DegreeOneReducer<int> oneReducer(&graph);
	std::vector<PBQPGraph<int>*> result = oneReducer.reduce();
	BOOST_CHECK_EQUAL(result.size(), 1);
	PBQPGraph<int>* resultGraph = result[0];
	BOOST_CHECK_EQUAL(resultGraph->getNodeCount(), 1);
} */

BOOST_AUTO_TEST_CASE(simpleCalculation) {
	PBQPGraph<int> graph = PBQPGraph<int>();
	int node1Arr[] { 1, 2 };
	int node2Arr[] { 2, 3 };
	Vector<int> vek1 (2, node1Arr);
	Vector<int> vek2 (2, node2Arr);
	PBQPNode<int>* first = graph.addNode(vek1);
	PBQPNode<int>* second = graph.addNode(vek2);
	unsigned long firstIndex = first->getIndex();
	unsigned long secondIndex = second->getIndex();
	int edge1Arr[] {2,6,2,0};
	Matrix<int> mat1 (2, 2, edge1Arr);
	graph.addEdge(first, second, mat1);
	OnetoOneDependentSolution<int>* retSol = DegreeOneReducer<int>::reduceDegreeOne(second, &graph);
	BOOST_CHECK_EQUAL(graph.getNodeCount(), 1);
	BOOST_CHECK_EQUAL(graph.getEdgeCount(), 0);
	BOOST_CHECK_EQUAL(first->getVectorDegree(), 2);
	BOOST_CHECK_EQUAL(first->getVector().get(0), 5);
	BOOST_CHECK_EQUAL(first->getVector().get(1), 5);
	PBQPSolution<int> sol (2);
	sol.setSolution(firstIndex, 0);
	retSol->solve(&sol);
	BOOST_CHECK_EQUAL(sol.getSolution(secondIndex), 0);
	PBQPSolution<int> sol2 (2);
	sol2.setSolution(firstIndex, 1);
	retSol->solve(&sol2);
	BOOST_CHECK_EQUAL(sol2.getSolution(secondIndex), 1);
}

}


#ifndef TEST_UTILS_HPP
#define TEST_UTILS_HPP

#include <boost/graph/edge_list.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>

#include <kspwlo_ref/algorithms/kspwlo.hpp>

#include <arlib/path.hpp>
#include <arlib/type_traits.hpp>

#include <string_view>
#include <vector>

//===----------------------------------------------------------------------===//
//                      Utility functions for testing
//===----------------------------------------------------------------------===//

template <typename Graph>
bool one_regression_path_have_edges(std::vector<Path> &res_regression,
                                    Graph &G) {
  using boost::edges;
  using boost::source;
  using boost::target;
  auto first = edges(G).first;
  auto last = edges(G).second;

  bool has_them = false;
  for (auto &regr_path : res_regression) {
    int edges_count = 0;
    int nb_edges_regr_path_has = 0;
    for (auto it = first; it != last; ++it) {
      ++edges_count;
      NodeID u = source(*it, G);
      NodeID v = target(*it, G);

      if (regr_path.containsEdge(std::make_pair(u, v))) {
        ++nb_edges_regr_path_has;
      }
    }
    if (edges_count == nb_edges_regr_path_has) {
      has_them = true;
      break;
    }
  }
  return has_them;
}

template <typename Graph>
std::vector<arlib::edge_of_t<Graph>>
make_edge_list(arlib::Path<Graph> const &alt_path) {
  using namespace boost;
  auto res = std::vector<arlib::edge_of_t<Graph>>{};
  for (auto [first, last] = edges(alt_path); first != last; ++first) {
    res.push_back(*first);
  }
  return res;
}

template <typename Graph, typename WeightMap>
bool alternative_paths_are_dissimilar(
    std::vector<arlib::Path<Graph>> const &alt_paths, WeightMap const &weight,
    double theta) {
  // Build edge lists
  using Edge = arlib::edge_of_t<Graph>;
  auto edge_lists = std::vector<std::vector<Edge>>{};
  for (auto const &path : alt_paths) {
    edge_lists.push_back(make_edge_list(path));
  }

  // For each path_i, check its similarity with all the other paths.
  for (std::size_t i = 0; i < alt_paths.size(); ++i) {
    auto const &path_i = alt_paths[i];
    auto const &edges_i = edge_lists[i];
    for (std::size_t j = 0; j < i; ++j) {
      auto const &path_j = alt_paths[j];
      auto shared_len = 0.0;
      for (auto const &e : edges_i) {
        auto u = arlib::source(e, path_i);
        auto v = arlib::target(e, path_i);
        auto ok = arlib::edge(u, v, path_j).second;
        if (ok) {
          shared_len += weight[e];
        }
      }
      auto similarity = shared_len / path_j.length();
      if (similarity > theta) {
        return false;
      }
    }
  }
  return true;
}

constexpr std::string_view graph_gr = "d\n"
                                      "7 24\n"
                                      "0 1 6 0\n"
                                      "1 0 6 0\n"
                                      "0 3 3 0\n"
                                      "3 0 3 0\n"
                                      "0 2 4 0\n"
                                      "2 0 4 0\n"
                                      "1 6 6 0\n"
                                      "6 1 6 0\n"
                                      "3 4 5 0\n"
                                      "4 3 5 0\n"
                                      "3 1 2 0\n"
                                      "1 3 2 0\n"
                                      "3 2 3 0\n"
                                      "2 3 3 0\n"
                                      "2 4 5 0\n"
                                      "4 2 5 0\n"
                                      "3 5 3 0\n"
                                      "5 3 3 0\n"
                                      "4 5 1 0\n"
                                      "5 4 1 0\n"
                                      "4 6 3 0\n"
                                      "6 4 3 0\n"
                                      "5 6 2 0\n"
                                      "6 5 2 0\n";

constexpr std::string_view graph_gr_esx = "d\n"
                                          "7 24\n"
                                          "0 1 6 0\n"
                                          "1 0 6 0\n"
                                          "0 3 3 0\n"
                                          "3 0 3 0\n"
                                          "0 2 4 0\n"
                                          "2 0 4 0\n"
                                          "1 6 6 0\n"
                                          "6 1 6 0\n"
                                          "3 4 5 0\n"
                                          "4 3 5 0\n"
                                          "3 1 2 0\n"
                                          "1 3 2 0\n"
                                          "3 2 3 0\n"
                                          "2 3 3 0\n"
                                          "2 4 5 0\n"
                                          "4 2 5 0\n"
                                          "3 5 3 0\n"
                                          "5 3 3 0\n"
                                          "4 5 1 0\n"
                                          "5 4 1 0\n"
                                          "4 6 2 0\n"
                                          "6 4 2 0\n"
                                          "5 6 2 0\n"
                                          "6 5 2 0\n";

#endif


#ifndef STAN_ANALYZE_MCMC_COMPUTE_EFFECTIVE_SAMPLE_SIZE_HPP
#define STAN_ANALYZE_MCMC_COMPUTE_EFFECTIVE_SAMPLE_SIZE_HPP

#include <stan/math/prim/fun/Eigen.hpp>
#include <stan/analyze/mcmc/autocovariance.hpp>
#include <stan/analyze/mcmc/split_chains.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <algorithm>
#include <cmath>
#include <vector>
#include <limits>

namespace stan {
namespace analyze {
/**
 * Computes the effective sample size (ESS) for the specified
 * parameter across all kept samples.  The value returned is the
 * minimum of ESS and the number_total_draws *
 * log10(number_total_draws).
 *
 * See more details in Stan reference manual section "Effective
 * Sample Size". http://mc-stan.org/users/documentation
 *
 * Current implementation assumes draws are stored in contiguous
 * blocks of memory.  Chains are trimmed from the back to match the
 * length of the shortest chain.  Note that the effective sample size
 * can not be estimated with less than four draws.
 *
 * @param draws stores pointers to arrays of chains
 * @param sizes stores sizes of chains
 * @return effective sample size for the specified parameter
 */
inline double compute_effective_sample_size(std::vector<const double*> draws,
                                            std::vector<size_t> sizes) {
  int num_chains = sizes.size();
  size_t num_draws = sizes[0];
  for (int chain = 1; chain < num_chains; ++chain) {
    num_draws = std::min(num_draws, sizes[chain]);
  }

  if (num_draws < 4) {
    return std::numeric_limits<double>::quiet_NaN();
  }

  // check if chains are constant; all equal to first draw's value
  bool are_all_const = false;
  Eigen::VectorXd init_draw = Eigen::VectorXd::Zero(num_chains);

  for (int chain_idx = 0; chain_idx < num_chains; chain_idx++) {
    Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, 1>> draw(
        draws[chain_idx], sizes[chain_idx]);

    for (int n = 0; n < num_draws; n++) {
      if (!boost::math::isfinite(draw(n))) {
        return std::numeric_limits<double>::quiet_NaN();
      }
    }

    init_draw(chain_idx) = draw(0);

    if (draw.isApproxToConstant(draw(0))) {
      are_all_const |= true;
    }
  }

  if (are_all_const) {
    // If all chains are constant then return NaN
    // if they all equal the same constant value
    if (init_draw.isApproxToConstant(init_draw(0))) {
      return std::numeric_limits<double>::quiet_NaN();
    }
  }

  Eigen::Matrix<Eigen::VectorXd, Eigen::Dynamic, 1> acov(num_chains);
  Eigen::VectorXd chain_mean(num_chains);
  Eigen::VectorXd chain_var(num_chains);
  for (int chain = 0; chain < num_chains; ++chain) {
    Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, 1>> draw(
        draws[chain], sizes[chain]);
    autocovariance<double>(draw, acov(chain));
    chain_mean(chain) = draw.mean();
    chain_var(chain) = acov(chain)(0) * num_draws / (num_draws - 1);
  }

  double mean_var = chain_var.mean();
  double var_plus = mean_var * (num_draws - 1) / num_draws;
  if (num_chains > 1)
    var_plus += math::variance(chain_mean);
  Eigen::VectorXd rho_hat_s(num_draws);
  rho_hat_s.setZero();
  Eigen::VectorXd acov_s(num_chains);
  for (int chain = 0; chain < num_chains; ++chain)
    acov_s(chain) = acov(chain)(1);
  double rho_hat_even = 1.0;
  rho_hat_s(0) = rho_hat_even;
  double rho_hat_odd = 1 - (mean_var - acov_s.mean()) / var_plus;
  rho_hat_s(1) = rho_hat_odd;

  // Convert raw autocovariance estimators into Geyer's initial
  // positive sequence. Loop only until num_draws - 4 to
  // leave the last pair of autocorrelations as a bias term that
  // reduces variance in the case of antithetical chains.
  size_t s = 1;
  while (s < (num_draws - 4) && (rho_hat_even + rho_hat_odd) > 0) {
    for (int chain = 0; chain < num_chains; ++chain)
      acov_s(chain) = acov(chain)(s + 1);
    rho_hat_even = 1 - (mean_var - acov_s.mean()) / var_plus;
    for (int chain = 0; chain < num_chains; ++chain)
      acov_s(chain) = acov(chain)(s + 2);
    rho_hat_odd = 1 - (mean_var - acov_s.mean()) / var_plus;
    if ((rho_hat_even + rho_hat_odd) >= 0) {
      rho_hat_s(s + 1) = rho_hat_even;
      rho_hat_s(s + 2) = rho_hat_odd;
    }
    s += 2;
  }

  int max_s = s;
  // this is used in the improved estimate, which reduces variance
  // in antithetic case -- see tau_hat below
  if (rho_hat_even > 0)
    rho_hat_s(max_s + 1) = rho_hat_even;

  // Convert Geyer's initial positive sequence into an initial
  // monotone sequence
  for (int s = 1; s <= max_s - 3; s += 2) {
    if (rho_hat_s(s + 1) + rho_hat_s(s + 2) > rho_hat_s(s - 1) + rho_hat_s(s)) {
      rho_hat_s(s + 1) = (rho_hat_s(s - 1) + rho_hat_s(s)) / 2;
      rho_hat_s(s + 2) = rho_hat_s(s + 1);
    }
  }

  double num_total_draws = num_chains * num_draws;
  // Geyer's truncated estimator for the asymptotic variance
  // Improved estimate reduces variance in antithetic case
  double tau_hat = -1 + 2 * rho_hat_s.head(max_s).sum() + rho_hat_s(max_s + 1);
  return std::min(num_total_draws / tau_hat,
                  num_total_draws * std::log10(num_total_draws));
}

/**
 * Computes the effective sample size (ESS) for the specified
 * parameter across all kept samples.  The value returned is the
 * minimum of ESS and the number_total_draws *
 * log10(number_total_draws).
 *
 * See more details in Stan reference manual section "Effective
 * Sample Size". http://mc-stan.org/users/documentation
 *
 * Current implementation assumes draws are stored in contiguous
 * blocks of memory.  Chains are trimmed from the back to match the
 * length of the shortest chain.  Note that the effective sample size
 * can not be estimated with less than four draws.  Argument size
 * will be broadcast to same length as draws.
 *
 * @param draws stores pointers to arrays of chains
 * @param size size of chains
 * @return effective sample size for the specified parameter
 */
inline double compute_effective_sample_size(std::vector<const double*> draws,
                                            size_t size) {
  int num_chains = draws.size();
  std::vector<size_t> sizes(num_chains, size);
  return compute_effective_sample_size(draws, sizes);
}

/**
 * Computes the split effective sample size (ESS) for the specified
 * parameter across all kept samples.  The value returned is the
 * minimum of ESS and the number_total_draws *
 * log10(number_total_draws). When the number of total draws N is
 * odd, the (N+1)/2th draw is ignored.
 *
 * See more details in Stan reference manual section "Effective
 * Sample Size". http://mc-stan.org/users/documentation
 *
 * Current implementation assumes draws are stored in contiguous
 * blocks of memory.  Chains are trimmed from the back to match the
 * length of the shortest chain.  Note that the effective sample size
 * can not be estimated with less than four draws.
 *
 * @param draws stores pointers to arrays of chains
 * @param sizes stores sizes of chains
 * @return effective sample size for the specified parameter
 */
inline double compute_split_effective_sample_size(
    std::vector<const double*> draws, std::vector<size_t> sizes) {
  int num_chains = sizes.size();
  size_t num_draws = sizes[0];
  for (int chain = 1; chain < num_chains; ++chain) {
    num_draws = std::min(num_draws, sizes[chain]);
  }

  std::vector<const double*> split_draws = split_chains(draws, sizes);

  double half = num_draws / 2.0;
  std::vector<size_t> half_sizes(2 * num_chains, std::floor(half));

  return compute_effective_sample_size(split_draws, half_sizes);
}

/**
 * Computes the split effective sample size (ESS) for the specified
 * parameter across all kept samples.  The value returned is the
 * minimum of ESS and the number_total_draws *
 * log10(number_total_draws). When the number of total draws N is
 * odd, the (N+1)/2th draw is ignored.
 *
 * See more details in Stan reference manual section "Effective
 * Sample Size". http://mc-stan.org/users/documentation
 *
 * Current implementation assumes draws are stored in contiguous
 * blocks of memory.  Chains are trimmed from the back to match the
 * length of the shortest chain.  Note that the effective sample size
 * can not be estimated with less than four draws.  Argument size
 * will be broadcast to same length as draws.
 *
 * @param draws stores pointers to arrays of chains
 * @param size size of chains
 * @return effective sample size for the specified parameter
 */
inline double compute_split_effective_sample_size(
    std::vector<const double*> draws, size_t size) {
  int num_chains = draws.size();
  std::vector<size_t> sizes(num_chains, size);
  return compute_split_effective_sample_size(draws, sizes);
}

}  // namespace analyze
}  // namespace stan

#endif


/*
 * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
 * 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.
 *     * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * 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 Christian Gehring, Hannes Sommer, Paul Furgale,
 * Remo Diethelm 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.
 *
*/

#pragma once

#include <cmath>

#include <Eigen/Geometry>

#include "kindr/common/common.hpp"
#include "kindr/common/assert_macros_eigen.hpp"
#include "kindr/rotations/RotationBase.hpp"

namespace kindr {

/*! \class RotationMatrix
 *  \brief Implementation of matrix rotation based on Eigen::Matrix<Scalar, 3, 3>
 *
 *  The following four typedefs are provided for convenience:
 *   - \ref RotationMatrixAD "RotationMatrixD" for double primitive type
 *   - \ref RotationMatrixAF "RotationMatrixF" for float primitive type
 *
 *  \tparam PrimType_ the primitive type of the data (double or float)
 *
 *  \ingroup rotations
 */
template<typename PrimType_>
class RotationMatrix : public RotationBase<RotationMatrix<PrimType_>>, private Eigen::Matrix<PrimType_, 3, 3> {
 private:
  /*! \brief The base type.
   */
  typedef Eigen::Matrix<PrimType_, 3, 3> Base;
 public:
  /*! \brief The implementation type.
   *  The implementation type is always an Eigen object.
   */
  typedef Base Implementation;
  /*! \brief The primitive type.
   *  Float/Double
   */
  typedef PrimType_ Scalar;

  /*! \brief Default constructor using identity rotation.
   */
  RotationMatrix()
    : Base(Base::Identity()) {
  }

  /*! \brief Constructor using nine scalars.
   *  In debug mode, an assertion is thrown if the matrix is not a rotation matrix.
   *  \param r11     entry in row 1, col 1
   *  \param r12     entry in row 1, col 2
   *  \param r13     entry in row 1, col 3
   *  \param r21     entry in row 2, col 1
   *  \param r22     entry in row 2, col 2
   *  \param r23     entry in row 2, col 3
   *  \param r31     entry in row 3, col 1
   *  \param r32     entry in row 3, col 2
   *  \param r33     entry in row 3, col 3
   */
  RotationMatrix(Scalar r11, Scalar r12, Scalar r13,
                 Scalar r21, Scalar r22, Scalar r23,
                 Scalar r31, Scalar r32, Scalar r33) {

    *this << r11,r12,r13,r21,r22,r23,r31,r32,r33;

    KINDR_ASSERT_MATRIX_NEAR_DBG(std::runtime_error, this->toImplementation() * this->toImplementation().transpose(), Base::Identity(), static_cast<Scalar>(1e-4), "Input matrix is not orthogonal.");
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->determinant(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input matrix determinant is not 1.");
  }

  /*! \brief Constructor using Eigen::Matrix.
   *  In debug mode, an assertion is thrown if the rotation vector has not unit length.
   *  \param other   Eigen::Matrix<PrimType_,3,3>
   */
  explicit RotationMatrix(const Base& other)
  // : Base(other)
  {

    this->toImplementation() = other;

    KINDR_ASSERT_MATRIX_NEAR_DBG(std::runtime_error, other * other.transpose(), Base::Identity(), static_cast<Scalar>(1e-4), "Input matrix is not orthogonal.");
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, other.determinant(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input matrix determinant is not 1.");
  }

  /*! \brief Constructor using another rotation.
   *  \param other   other rotation
   */
  template<typename OtherDerived_>
  inline explicit RotationMatrix(const RotationBase<OtherDerived_>& other)
  // : Base(internal::ConversionTraits<RotationMatrix, OtherDerived_>::convert(other.derived()).toImplementation())
  {
    this->toImplementation() = internal::ConversionTraits<RotationMatrix, OtherDerived_>::convert(other.derived()).toImplementation();
  }

  /*! \brief Assignment operator using another rotation.
   *  \param other   other rotation
   *  \returns referece
   */
  template<typename OtherDerived_>
  RotationMatrix& operator =(const RotationBase<OtherDerived_>& other) {
    this->toImplementation() = internal::ConversionTraits<RotationMatrix, OtherDerived_>::convert(other.derived()).toImplementation();
    return *this;
  }

  /*! \brief Parenthesis operator to convert from another rotation.
   *  \param other   other rotation
   *  \returns reference
   */
  template<typename OtherDerived_>
  RotationMatrix& operator ()(const RotationBase<OtherDerived_>& other) {
    this->toImplementation() = internal::ConversionTraits<RotationMatrix, OtherDerived_>::convert(other.derived()).toImplementation();
    return *this;
  }

  /*! \brief Returns the inverse of the rotation.
   *  \returns the inverse of the rotation
   */
  RotationMatrix inverted() const {
    RotationMatrix matrix;
    matrix.toImplementation() = this->toImplementation().transpose();
    return matrix;
  }

  /*! \brief Inverts the rotation.
   *  \returns reference
   */
  RotationMatrix& invert() {
    *this = this->inverted();
    return *this;
  }

  /*! \brief Returns the transpose of the rotation matrix.
   *  \returns the inverse of the rotation
   */
  RotationMatrix transposed() const {
    RotationMatrix matrix;
    matrix.toImplementation() = this->toImplementation().transpose();
    return matrix;
  }

  /*! \brief Transposes the rotation matrix.
   *  \returns reference
   */
  RotationMatrix& transpose() {
    *this = this->transposed();
    return *this;
  }

  /*! \brief Returns the determinant of the rotation matrix.
   *  \returns determinant of the rotation matrix
   */
  Scalar determinant() const {
  return toImplementation().determinant();
  }

  /*! \brief Cast to the implementation type.
   *  \returns the implementation for direct manipulation (recommended only for advanced users)
   */
  inline Implementation& toImplementation() {
    return static_cast<Implementation&>(*this);
  }

  /*! \brief Cast to the implementation type.
   *  \returns the implementation for direct manipulation (recommended only for advanced users)
   */
  inline const Implementation& toImplementation() const {
    return static_cast<const Implementation&>(*this);
  }

  /*! \brief Reading access to the rotation matrix.
   *  \returns rotation matrix (matrix) with reading access
   */
  inline Implementation matrix() const {
      return this->toImplementation();
  }

  /*! \brief  Writing access to the rotation matrix.
   */
  inline void setMatrix(const Implementation & input) {
      this->toImplementation() = input;
  }

  /*! \brief  Writing access to the rotation matrix.
   */
  inline void setMatrix(Scalar r11, Scalar r12, Scalar r13,
                        Scalar r21, Scalar r22, Scalar r23,
                        Scalar r31, Scalar r32, Scalar r33) {

     *this << r11,r12,r13,r21,r22,r23,r31,r32,r33;

  }

  /*! \brief Sets the rotation to identity.
   *  \returns reference
   */
  RotationMatrix& setIdentity() {
    this->Implementation::setIdentity();
    return *this;
  }

  /*! \brief Returns a unique matrix rotation.
   *  A rotation matrix is always unique.
   *  This function is used to compare different rotations.
   *  \returns copy of the matrix rotation which is unique
   */
  RotationMatrix getUnique() const {
    return *this;
  }

  /*! \brief Modifies the matrix rotation such that it becomes unique.
   *  A rotation matrix is always unique.
   *  \returns reference
   */
  RotationMatrix& setUnique() {
    return *this;
  }

  /*! \brief Concenation operator.
   *  This is explicitly specified, because Eigen::Matrix provides also an operator*.
   *  \returns the concenation of two rotations
   */
  using RotationBase<RotationMatrix<PrimType_>>::operator*; // otherwise ambiguous RotationBase and Eigen

  /*! \brief Equivalence operator.
   *  This is explicitly specified, because Eigen::Matrix provides also an operator==.
   *  \returns true if two rotations are equal.
   */
  using RotationBase<RotationMatrix<PrimType_>>::operator==; // otherwise ambiguous RotationBase and Eigen

  /*! \brief Inequivalence operator.
   *  This is explicitly specified, because Eigen::Matrix provides also an operator!=.
   *  \returns true if two rotations are not equal.
   */
  using RotationBase<RotationMatrix<PrimType_>>::operator!=; // otherwise ambiguous RotationBase and Eigen

  /*! \brief Used for printing the object with std::cout.
   *  \returns std::stream object
   */
  friend std::ostream& operator << (std::ostream& out, const RotationMatrix& rotationMatrix) {
    out << rotationMatrix.toImplementation();
    return out;
  }
};

//! \brief Active matrix rotation with double primitive type
typedef RotationMatrix<double>  RotationMatrixD;
//! \brief Active matrix rotation with float primitive type
typedef RotationMatrix<float>  RotationMatrixF;
//! \brief Passive matrix rotation with double primitive type
typedef RotationMatrix<double> RotationMatrixPD;
//! \brief Passive matrix rotation with float primitive type
typedef RotationMatrix<float> RotationMatrixPF;

namespace internal {

template<typename PrimType_>
class get_scalar<RotationMatrix<PrimType_>> {
 public:
  typedef PrimType_ Scalar;
};

template<typename PrimType_>
class get_matrix3X<RotationMatrix<PrimType_>>{
 public:
  typedef int  IndexType;

  template <IndexType Cols>
  using Matrix3X = Eigen::Matrix<PrimType_, 3, Cols>;
};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Conversion Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
template<typename DestPrimType_, typename SourcePrimType_>
class ConversionTraits<RotationMatrix<DestPrimType_>, AngleAxis<SourcePrimType_>> {
 public:
  inline static RotationMatrix<DestPrimType_> convert(const AngleAxis<SourcePrimType_>& aa) {
    RotationMatrix<DestPrimType_> matrix;
    matrix.toImplementation() = (aa.toImplementation().template cast<DestPrimType_>()).toRotationMatrix();
    return matrix;
  }
};

template<typename DestPrimType_, typename SourcePrimType_>
class ConversionTraits<RotationMatrix<DestPrimType_>, RotationVector<SourcePrimType_>> {
 public:
  inline static RotationMatrix<DestPrimType_> convert(const RotationVector<SourcePrimType_>& rotationVector) {
    return RotationMatrix<DestPrimType_>(RotationQuaternion<DestPrimType_>(rotationVector));
  }
};

template<typename DestPrimType_, typename SourcePrimType_>
class ConversionTraits<RotationMatrix<DestPrimType_>, RotationQuaternion<SourcePrimType_>> {
 public:
  inline static RotationMatrix<DestPrimType_> convert(const RotationQuaternion<SourcePrimType_>& q) {
    RotationMatrix<DestPrimType_> matrix;
    matrix.toImplementation() = (q.toImplementation().template cast<DestPrimType_>()).toRotationMatrix();
    return matrix;
  }
};

template<typename DestPrimType_, typename SourcePrimType_>
class ConversionTraits<RotationMatrix<DestPrimType_>, RotationMatrix<SourcePrimType_>> {
 public:
  inline static RotationMatrix<DestPrimType_> convert(const RotationMatrix<SourcePrimType_>& R) {
    RotationMatrix<DestPrimType_> matrix;
    matrix.toImplementation() = R.toImplementation().template cast<DestPrimType_>();
    return matrix;
  }
};

template<typename DestPrimType_, typename SourcePrimType_>
class ConversionTraits<RotationMatrix<DestPrimType_>, EulerAnglesXyz<SourcePrimType_>> {
 public:
  inline static RotationMatrix<DestPrimType_> convert(const EulerAnglesXyz<SourcePrimType_>& xyz) {
      RotationMatrix<DestPrimType_> matrix;
      matrix.toImplementation() = RotationQuaternion<DestPrimType_>(xyz).toImplementation().toRotationMatrix();
      return matrix;
  }
};

template<typename DestPrimType_, typename SourcePrimType_>
class ConversionTraits<RotationMatrix<DestPrimType_>, EulerAnglesZyx<SourcePrimType_>> {
 public:
  inline static RotationMatrix<DestPrimType_> convert(const EulerAnglesZyx<SourcePrimType_>& zyx) {
    RotationMatrix<DestPrimType_> matrix;
    matrix.toImplementation() = RotationQuaternion<DestPrimType_>(zyx).toImplementation().toRotationMatrix();
    return matrix;
  }
};

/*! \brief Multiplication of two rotation matrices
 */
template<typename PrimType_>
class MultiplicationTraits<RotationBase<RotationMatrix<PrimType_>>, RotationBase<RotationMatrix<PrimType_>>> {
 public:
  inline static RotationMatrix<PrimType_> mult(const RotationMatrix<PrimType_>& lhs, const RotationMatrix<PrimType_>& rhs) {
      RotationMatrix<PrimType_> result;
      result.toImplementation() = lhs.toImplementation() * rhs.toImplementation();
      return result;
  }
};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Rotation Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Comparison Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Box Operations - required?
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Fixing Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
template<typename PrimType_>
class FixingTraits<RotationMatrix<PrimType_>> {
 public:
  inline static void fix(RotationMatrix<PrimType_>& R) {
    const PrimType_ factor = 1/pow(R.determinant(), 1.0/3.0);
    R.setMatrix(factor*R.matrix()(0,0),
                factor*R.matrix()(0,1),
                factor*R.matrix()(0,2),
                factor*R.matrix()(1,0),
                factor*R.matrix()(1,1),
                factor*R.matrix()(1,2),
                factor*R.matrix()(2,0),
                factor*R.matrix()(2,1),
                factor*R.matrix()(2,2));
  }
};

} // namespace internal
} // namespace kindr


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

  @copyright 2017 NumScale SAS

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

#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/function/max.hpp>
#include <boost/simd/function/min.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <boost/assert.hpp>
namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( clamp_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_< bd::arithmetic_<A0>, X>
                          , bs::pack_< bd::arithmetic_<A0>, X>
                          , bs::pack_< bd::arithmetic_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE
    A0 operator() ( A0 const& x, A0 const& lo, A0 const& hi) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG(assert_all(is_less_equal(lo, hi)), "lo is not less or equal to hi");
      return bs::min(bs::max(x, lo), hi);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( clamp_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pedantic_tag
                          , bs::pack_< bd::arithmetic_<A0>, X>
                          , bs::pack_< bd::arithmetic_<A0>, X>
                          , bs::pack_< bd::arithmetic_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE
    A0 operator() (pedantic_tag const&
                  , A0 const& x, A0 const& lo, A0 const& hi) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG(assert_all(is_less_equal(lo, hi)), "lo is not less or equal to hi");
      return if_else(x < lo, lo, if_else(hi < x, hi, x));
    }
  };

} } }

#endif


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/include/functions/ldexp.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <complex>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/complex/dry.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>
#include <nt2/sdk/complex/meta/as_dry.hpp>
#include <nt2/include/constants/four.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/i.hpp>

NT2_TEST_CASE_TPL ( ldexp_real,  NT2_REAL_TYPES)
{
  using nt2::ldexp;
  using nt2::tag::ldexp_;
  typedef typename std::complex<T> cT;
  typedef typename nt2::meta::as_dry<T>::type dT;
  typedef typename nt2::meta::as_integer<T>::type iT;
  NT2_TEST_TYPE_IS( (typename nt2::meta::call<ldexp_(cT, iT)>::type), cT );

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(ldexp(nt2::Inf<cT>(),  iT(2)), nt2::Inf<cT>());
  NT2_TEST_EQUAL(ldexp(nt2::Minf<cT>(), 2), nt2::Minf<cT>());
  NT2_TEST_EQUAL(ldexp(nt2::Nan<cT>(),  2), nt2::Nan<cT>());
  NT2_TEST_EQUAL(ldexp(nt2::Inf<dT>(),  2), nt2::Inf<dT>());
  NT2_TEST_EQUAL(ldexp(nt2::Minf<dT>(), 2), nt2::Minf<dT>());
  NT2_TEST_EQUAL(ldexp(nt2::Nan<dT>(),  2), nt2::Nan<dT>());
#endif
  NT2_TEST_EQUAL(ldexp(nt2::One<cT>(),  2), nt2::Four<cT>());
  NT2_TEST_EQUAL(ldexp(nt2::Zero<cT>(), 2), nt2::Zero<cT>());
  NT2_TEST_EQUAL(ldexp(cT(nt2::One<T>(),nt2::One<T>()) ,  2), cT(nt2::Four<T>(),nt2::Four<T>()) );
  NT2_TEST_EQUAL(ldexp(nt2::One<dT>(),  2), nt2::Four<dT>());
  NT2_TEST_EQUAL(ldexp(nt2::Zero<dT>(), 2), nt2::Zero<dT>());
}


#include <boost/units/base_units/metric/hour.hpp>
#include <boost/units/base_units/us/mile.hpp>
#include <boost/units/io.hpp>
#include <boost/units/is_quantity_of_dimension.hpp>
#include <boost/units/make_scaled_unit.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/si/prefixes.hpp>
#include <boost/units/systems/si/time.hpp>
#include <boost/units/systems/si/velocity.hpp>
#include <cassert>
#include <iostream>

namespace {

namespace bu = boost::units;

// h
using hour = bu::metric::hour_base_unit::unit_type;
BOOST_UNITS_STATIC_CONSTANT(hours, hour);

// km
using kilometer = bu::make_scaled_unit<bu::si::length, bu::scale<10, bu::static_rational<3>>>::type;

// km/h
using kilometer_per_hour = bu::divide_typeof_helper<kilometer, hour>::type;
BOOST_UNITS_STATIC_CONSTANT(kilometers_per_hour, kilometer_per_hour);

// mi
using mile = bu::us::mile_base_unit::unit_type;
BOOST_UNITS_STATIC_CONSTANT(miles, mile);

// mi/h
using mile_per_hour = bu::divide_typeof_helper<mile, hour>::type;
BOOST_UNITS_STATIC_CONSTANT(miles_per_hour, mile_per_hour);

template<typename Quantity, typename Dimension>
concept QuantityOf = bu::is_quantity_of_dimension<Quantity, Dimension>::value;

template<typename Quantity>
concept Length = QuantityOf<Quantity, bu::length_dimension>;

template<typename Quantity>
concept Time = QuantityOf<Quantity, bu::time_dimension>;

template<typename Quantity>
concept Velocity = QuantityOf<Quantity, bu::velocity_dimension>;

constexpr Velocity auto avg_speed(Length auto d, Time auto t)
{
  return d / t;
}

template<typename T>
void km_per_h(const T& v)
{
  std::cout << "v = " << v << '\n'; // prints "v = 110 k(m h^-1)"

  using namespace bu::si;
  assert(v.value() == 110);
  assert(v == 110 * kilometers_per_hour);
}

void km_per_h()
{
  using namespace bu::si;
  constexpr auto v = avg_speed(220 * kilo * meters, 2 * hours);
  km_per_h(v);
}

void km_per_h(double a, double b)
{
  using namespace bu::si;
  const auto v = avg_speed(a * kilo * meters, b * hours);
  km_per_h(v);
}

template<typename T>
void mi_per_h(const T& v)
{
  std::cout << "v = " << v << '\n'; // prints "v = 70 mi h^-1"

  using namespace bu::si;
  assert(v.value() == 70);
  assert(v == 70 * miles_per_hour);
}

void mi_per_h()
{
  using namespace bu::si;

  const auto v = avg_speed(140 * miles, 2 * hours);
  mi_per_h(v);
}

void mi_per_h(double a, double b)
{
  using namespace bu::si;

  const auto v = avg_speed(a * miles, b * hours);
  mi_per_h(v);
}

}  // namespace

int main()
{
  km_per_h();
  km_per_h(220, 2);
  mi_per_h();
  mi_per_h(140, 2);
}


// Copyright Aleksey Gurtovoy 2000-2004
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/mpl for documentation.

// $Id: distance.cpp 49268 2008-10-11 06:26:17Z agurtovoy $
// $Date: 2008-10-10 23:26:17 -0700 (Fri, 10 Oct 2008) $
// $Revision: 49268 $

#include <boost/mpl/distance.hpp>
#include <boost/mpl/list.hpp>
#include <boost/mpl/range_c.hpp>

#include <boost/mpl/aux_/test.hpp>

MPL_TEST_CASE()
{
    typedef list<char,short,int,long>::type list;
    typedef begin<list>::type first;
    typedef end<list>::type last;

    MPL_ASSERT_RELATION( (mpl::distance<first,last>::value), ==, 4 );
}

MPL_TEST_CASE()
{
    typedef range_c<int,0,10>::type range;
    typedef begin<range>::type first;
    typedef end<range>::type last;

    MPL_ASSERT_RELATION( (mpl::distance<first,last>::value), ==, 10 );
}


/**
 * Test Suite for RealNumber class
 * command for running tests:
 * ./real_number_test --log_level=test_suite
 * http://www.alittlemadness.com/2009/03/31/c-unit-testing-with-boosttest/
 * http://www.ibm.com/developerworks/aix/library/au-ctools1_boost/
 * http://www.boost.org/doc/libs/1_34_1/libs/test/doc/components/test_tools/reference/index.html
 *
 * Created by Marshall Farrier on 6/22/12.
 * Copyright (c) 2012 Marshall Farrier
 * license http://opensource.org/licenses/gpl-license.php GNU Public License
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <string>
#include <stdexcept>
#include "real_number.h"

struct RealNumberTestCases {
	std::string zero_str;
	std::string one_str;
	std::string two_str;
	std::string big_num_str;
	std::string small_num_str;
	// throws std::invalid_argument because it is too big
	std::string overflow_num_str;
	// sets maximum difference for 2 real numbers to be viewed as equal
	std::string very_small_num_str;
	std::string two_plus_small_str;

	RealNumber zero_rn;
	RealNumber one_rn;
	RealNumber two_rn;
	RealNumber big_num_rn;
	RealNumber small_num_rn;
	RealNumber very_small_num_rn;
	RealNumber two_plus_small_rn;

	RealNumberTestCases() {
		zero_str = "0.0";
		one_str = "1.0";
		two_str = "2.0";
		overflow_num_str = "10000000000.0";
		big_num_str = "900000000.0";
		// used as denominator in division test, so can't be so small as to cause overflow
		small_num_str = "0.001";
		// 10 ^ -50 should be fine in this context
		very_small_num_str = "0.00000000000000000000000000000000000000000000000001";
		two_plus_small_str = "2.001";

		zero_rn = RealNumber(zero_str);
		one_rn = RealNumber(one_str);
		two_rn = RealNumber(two_str);
		big_num_rn = RealNumber(big_num_str);
		small_num_rn = RealNumber(small_num_str);
		very_small_num_rn = RealNumber(very_small_num_str);
		two_plus_small_rn = RealNumber(two_plus_small_str);
	}
};

BOOST_FIXTURE_TEST_SUITE(constructor, RealNumberTestCases)
	BOOST_AUTO_TEST_CASE(default_constructor) {
		RealNumber default_zero;
		BOOST_REQUIRE_EQUAL(default_zero.to_decimal_string(), zero_rn.to_decimal_string());
	}

	BOOST_AUTO_TEST_CASE(equals1) {
		BOOST_REQUIRE_EQUAL(one_rn.to_decimal_string().substr(0, one_str.length()), one_str);
	}

	BOOST_AUTO_TEST_CASE(equals2) {
		BOOST_REQUIRE_EQUAL(two_rn.to_decimal_string().substr(0, two_str.length()), two_str);
	}

	BOOST_AUTO_TEST_CASE(equals_big_num_str) {
		BOOST_REQUIRE_EQUAL(big_num_rn.to_decimal_string().substr(0,
				big_num_str.length()), big_num_str);
	}

	BOOST_AUTO_TEST_CASE(equals_small_number1) {
		// tests whether all but the last digit match, as conversion will give back
		// "0.0009999999999..." on input "0.001"
		BOOST_REQUIRE_EQUAL(small_num_rn.to_decimal_string().substr(0,
				small_num_str.length() - 1), small_num_str.substr(0, small_num_str.length() - 1));
	}

	BOOST_AUTO_TEST_CASE(equals_small_number2) {
		// tests only the last digit for the small number and allows 2 possible values
		std::string output = small_num_rn.to_decimal_string();
		char original_digit = small_num_str[small_num_str.length() - 1];
		char output_digit = output[small_num_str.length() - 1];
		bool the_same = original_digit == output_digit;
		if (!the_same) {
			BOOST_TEST_MESSAGE("Last digit of input differs from corresponding digit of output:");
			BOOST_TEST_MESSAGE(std::string("input digit: ").append(1, original_digit));
			BOOST_TEST_MESSAGE(std::string("output digit: ").append(1, output_digit));
		}
		bool close_enough = original_digit - output_digit == 1;
		close_enough = close_enough && (output.substr(small_num_str.length(), 3) == "999");
		if (close_enough) {
			BOOST_TEST_MESSAGE("Difference between input and output is minimal and results from base conversion.");
			BOOST_TEST_MESSAGE(std::string("Relevant digits: ")
					.append(output.substr(small_num_str.length() - 1, 4)));
		}
		BOOST_REQUIRE(the_same || close_enough);
	}

	BOOST_AUTO_TEST_CASE(number_too_big) {
		BOOST_CHECK_THROW(RealNumber overflow_num_rn(overflow_num_str), std::invalid_argument);
	}

	BOOST_AUTO_TEST_CASE(copy_constructor) {
		RealNumber two_rn_copy(two_rn);
		BOOST_REQUIRE_EQUAL(two_rn_copy.to_decimal_string(), two_rn.to_decimal_string());
	}

	BOOST_AUTO_TEST_CASE(assignment_operator) {
		RealNumber one_rn_assigned = one_rn;
		BOOST_REQUIRE_EQUAL(one_rn_assigned.to_decimal_string(), one_rn.to_decimal_string());
	}
BOOST_AUTO_TEST_SUITE_END()

BOOST_FIXTURE_TEST_SUITE(comparison_operators, RealNumberTestCases)
	BOOST_AUTO_TEST_CASE(greater_than) {
		BOOST_CHECK(big_num_rn > two_rn);
		BOOST_CHECK(big_num_rn > small_num_rn);
		BOOST_CHECK(!(one_rn > two_rn));
		BOOST_CHECK(!(two_rn > two_rn));
	}

	BOOST_AUTO_TEST_CASE(greater_or_equal) {
		RealNumber one_rn_again(one_str);
		BOOST_CHECK(big_num_rn >= one_rn);
		BOOST_CHECK(one_rn_again >= one_rn);
		BOOST_CHECK(one_rn >= one_rn_again);
		BOOST_CHECK(two_rn >= two_rn);
		BOOST_CHECK(one_rn >= small_num_rn);
		BOOST_CHECK(!(one_rn >= big_num_rn));
	}

	BOOST_AUTO_TEST_CASE(less_than) {
		BOOST_CHECK(small_num_rn < one_rn);
		BOOST_CHECK(small_num_rn < big_num_rn);
		BOOST_CHECK(!(big_num_rn < one_rn));
		BOOST_CHECK(!(one_rn < one_rn));
	}

	BOOST_AUTO_TEST_CASE(less_or_equal) {
		RealNumber big_num_rn_again(big_num_str);
		BOOST_CHECK(small_num_rn <= one_rn);
		BOOST_CHECK(small_num_rn <= big_num_rn);
		BOOST_CHECK(one_rn <= one_rn);
		BOOST_CHECK(big_num_rn_again <= big_num_rn);
		BOOST_CHECK(!(two_rn <= one_rn));
	}

	BOOST_AUTO_TEST_CASE(not_equal) {
		RealNumber big_num_rn_copy(big_num_rn);
		BOOST_CHECK(big_num_rn != two_rn);
		BOOST_CHECK(big_num_rn != small_num_rn);
		BOOST_CHECK(small_num_rn != one_rn);
		BOOST_CHECK(small_num_rn != big_num_rn);
		BOOST_CHECK(!(one_rn != one_rn));
		BOOST_CHECK(!(big_num_rn_copy != big_num_rn));
	}

	BOOST_AUTO_TEST_CASE(equals) {
		RealNumber big_num_rn_copy(big_num_rn);
		RealNumber two_rn_copy(two_rn);
		RealNumber small_num_again(small_num_str);
		BOOST_CHECK(big_num_rn == big_num_rn_copy);
		BOOST_CHECK(two_rn_copy == two_rn);
		BOOST_CHECK(small_num_rn == small_num_again);
		BOOST_CHECK(!(two_rn == one_rn));
	}
BOOST_AUTO_TEST_SUITE_END()

BOOST_FIXTURE_TEST_SUITE(arithmetic_operators, RealNumberTestCases)
	BOOST_AUTO_TEST_CASE(addition) {
		BOOST_CHECK(one_rn + one_rn == two_rn);
		BOOST_CHECK(one_rn + zero_rn == one_rn);
		BOOST_CHECK(two_rn + small_num_rn == two_plus_small_rn);
		BOOST_CHECK(big_num_rn + very_small_num_rn > big_num_rn);
		BOOST_CHECK(big_num_rn + very_small_num_rn != big_num_rn);
	}

	BOOST_AUTO_TEST_CASE(subtraction) {
		RealNumber one_rn_copy(one_rn);
		BOOST_CHECK(one_rn - one_rn_copy == zero_rn);
		BOOST_CHECK(two_rn - one_rn == one_rn_copy);
		BOOST_CHECK(big_num_rn - very_small_num_rn < big_num_rn);
	}

	BOOST_AUTO_TEST_CASE(multiplication) {
		BOOST_CHECK(one_rn * two_rn == two_rn);
		BOOST_CHECK(big_num_rn * zero_rn == zero_rn);
		BOOST_CHECK(two_rn * two_rn == RealNumber("4.0"));
		BOOST_CHECK(big_num_rn * small_num_rn < big_num_rn);
		BOOST_CHECK(two_rn < two_rn * RealNumber("1.01"));
		BOOST_CHECK(two_rn * very_small_num_rn > very_small_num_rn);
	}

	BOOST_AUTO_TEST_CASE(division) {
		BOOST_CHECK(one_rn / two_rn < RealNumber("0.50000001"));
		BOOST_CHECK(one_rn / two_rn > RealNumber("0.49999999"));
		BOOST_CHECK(two_rn / big_num_rn > zero_rn);
		BOOST_CHECK(one_rn / small_num_rn > one_rn);
		BOOST_CHECK_THROW(one_rn / zero_rn, std::invalid_argument);
	}
BOOST_AUTO_TEST_SUITE_END()

BOOST_FIXTURE_TEST_SUITE(other_methods, RealNumberTestCases)
	BOOST_AUTO_TEST_CASE(difference_method) {
		BOOST_CHECK(big_num_rn.difference(two_rn) == big_num_rn - two_rn);
		BOOST_CHECK(two_rn.difference(big_num_rn) == big_num_rn - two_rn);
		BOOST_CHECK(one_rn.difference(small_num_rn) == one_rn - small_num_rn);
		BOOST_CHECK(small_num_rn.difference(one_rn) == one_rn - small_num_rn);
	}
	BOOST_AUTO_TEST_CASE(equals_method) {
		RealNumber big_num_rn_copy(big_num_rn);
		BOOST_CHECK(two_rn.equals(two_rn));
		BOOST_CHECK(big_num_rn.equals(big_num_rn_copy));
		BOOST_CHECK(big_num_rn_copy.equals(big_num_rn));
		BOOST_CHECK(!zero_rn.equals(small_num_rn));
		BOOST_CHECK(!very_small_num_rn.equals(zero_rn));
	}
	BOOST_AUTO_TEST_CASE(assign_value) {
		RealNumber tmp;
		tmp.assign_value(one_str);
		BOOST_CHECK(one_rn == tmp);
		tmp.assign_value(big_num_str);
		BOOST_CHECK(big_num_rn == tmp);
		tmp.assign_value(very_small_num_str);
		BOOST_CHECK(very_small_num_rn == tmp);
	}
BOOST_AUTO_TEST_SUITE_END()


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/Polygon_mesh_processing/remesh.h>
#include <CGAL/Polygon_mesh_processing/border.h>
#include <CGAL/Polygon_mesh_processing/IO/polygon_mesh_io.h>

#include <boost/iterator/function_output_iterator.hpp>

#include <iostream>
#include <string>
#include <vector>

typedef CGAL::Exact_predicates_inexact_constructions_kernel   K;
typedef CGAL::Surface_mesh<K::Point_3>                        Mesh;

typedef boost::graph_traits<Mesh>::halfedge_descriptor        halfedge_descriptor;
typedef boost::graph_traits<Mesh>::edge_descriptor            edge_descriptor;

namespace PMP = CGAL::Polygon_mesh_processing;

struct halfedge2edge
{
  halfedge2edge(const Mesh& m, std::vector<edge_descriptor>& edges)
    : m_mesh(m), m_edges(edges)
  {}
  void operator()(const halfedge_descriptor& h) const
  {
    m_edges.push_back(edge(h, m_mesh));
  }
  const Mesh& m_mesh;
  std::vector<edge_descriptor>& m_edges;
};

int main(int argc, char* argv[])
{
  const std::string filename = (argc > 1) ? argv[1] : CGAL::data_file_path("meshes/pig.off");

  Mesh mesh;
  if(!PMP::IO::read_polygon_mesh(filename, mesh) || !CGAL::is_triangle_mesh(mesh))
  {
    std::cerr << "Invalid input." << std::endl;
    return 1;
  }

  double target_edge_length = (argc > 2) ? std::stod(std::string(argv[2])) : 0.04;
  unsigned int nb_iter = 3;

  std::cout << "Split border...";

  std::vector<edge_descriptor> border;
  PMP::border_halfedges(faces(mesh), mesh, boost::make_function_output_iterator(halfedge2edge(mesh, border)));
  PMP::split_long_edges(border, target_edge_length, mesh);

  std::cout << "done." << std::endl;
  std::cout << "Start remeshing of " << filename
    << " (" << num_faces(mesh) << " faces)..." << std::endl;

  PMP::isotropic_remeshing(faces(mesh), target_edge_length, mesh,
                           CGAL::parameters::number_of_iterations(nb_iter)
                                            .protect_constraints(true)); //i.e. protect border, here

  std::cout << "Remeshing done." << std::endl;

  return 0;
}


/*
 * gdal_image.cpp -- Load GIS data into OpenCV Containers using the Geospatial Data Abstraction Library
*/
// OpenCV Headers
#include "opencv2/core.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/highgui.hpp"

// GDAL Dependencies
#include <cpl_conv.h>
#include <gdal_priv.h>

//  Boost Dependencies
#include <boost/filesystem.hpp>

// C++ Standard Libraries
#include <cmath>
#include <iostream>
#include <stdexcept>
#include <vector>

// STL Dependencies
#include <string>

#include <sstream>

using namespace cv;
using namespace std;

// define the corner points
//    Note that GDAL library can natively determine this
cv::Point2d tl( -122.441017, 37.815664 );
cv::Point2d tr( -122.370919, 37.815311 );
cv::Point2d bl( -122.441533, 37.747167 );
cv::Point2d br( -122.3715,   37.746814 );
// determine dem corners
cv::Point2d dem_bl( -122.0, 38);
cv::Point2d dem_tr( -123.0, 37);
// range of the heat map colors
std::vector<std::pair<cv::Vec3b,double> > color_range;
// List of all function prototypes
cv::Point2d lerp( const cv::Point2d&, const cv::Point2d&, const double& );
cv::Vec3b get_dem_color( const double& );
cv::Point2d world2dem( const cv::Point2d&, const cv::Size&);
cv::Point2d pixel2world( const int&, const int&, const cv::Size& );
void add_color( cv::Vec3b& pix, const uchar& b, const uchar& g, const uchar& r );
/*
 * Linear Interpolation
 * p1 - Point 1
 * p2 - Point 2
 * t  - Ratio from Point 1 to Point 2
*/
cv::Point2d lerp( cv::Point2d const& p1, cv::Point2d const& p2, const double& t ){
    return cv::Point2d( ((1-t)*p1.x) + (t*p2.x),
                        ((1-t)*p1.y) + (t*p2.y));
}
/*
 * Interpolate Colors
*/
template <typename DATATYPE, int N>
cv::Vec<DATATYPE,N> lerp( cv::Vec<DATATYPE,N> const& minColor,
                          cv::Vec<DATATYPE,N> const& maxColor,
                          double const& t ){
    cv::Vec<DATATYPE,N> output;
    for( int i=0; i<N; i++ ){
        output[i] = (uchar)(((1-t)*minColor[i]) + (t * maxColor[i]));
    }
    return output;
}
/*
 * Compute the dem color
*/
cv::Vec3b get_dem_color( const double& elevation ){
    // if the elevation is below the minimum, return the minimum
    if( elevation < color_range[0].second ){
        return color_range[0].first;
    }
    // if the elevation is above the maximum, return the maximum
    if( elevation > color_range.back().second ){
        return color_range.back().first;
    }
    // otherwise, find the proper starting index
    int idx=0;
    double t = 0;
    for( int x=0; x<(int)(color_range.size()-1); x++ ){
        // if the current elevation is below the next item, then use the current
        // two colors as our range
        if( elevation < color_range[x+1].second ){
            idx=x;
            t = (color_range[x+1].second - elevation)/
                (color_range[x+1].second - color_range[x].second);
            break;
        }
    }
    // interpolate the color
    return lerp( color_range[idx].first, color_range[idx+1].first, t);
}
/*
 * Given a pixel coordinate and the size of the input image, compute the pixel location
 * on the DEM image.
*/
cv::Point2d world2dem( cv::Point2d const& coordinate, const cv::Size& dem_size   ){
    // relate this to the dem points
    // ASSUMING THAT DEM DATA IS ORTHORECTIFIED
    double demRatioX = ((dem_tr.x - coordinate.x)/(dem_tr.x - dem_bl.x));
    double demRatioY = 1-((dem_tr.y - coordinate.y)/(dem_tr.y - dem_bl.y));
    cv::Point2d output;
    output.x = demRatioX * dem_size.width;
    output.y = demRatioY * dem_size.height;
    return output;
}
/*
 * Convert a pixel coordinate to world coordinates
*/
cv::Point2d pixel2world( const int& x, const int& y, const cv::Size& size ){
    // compute the ratio of the pixel location to its dimension
    double rx = (double)x / size.width;
    double ry = (double)y / size.height;
    // compute LERP of each coordinate
    cv::Point2d rightSide = lerp(tr, br, ry);
    cv::Point2d leftSide  = lerp(tl, bl, ry);
    // compute the actual Lat/Lon coordinate of the interpolated coordinate
    return lerp( leftSide, rightSide, rx );
}
/*
 * Add color to a specific pixel color value
*/
void add_color( cv::Vec3b& pix, const uchar& b, const uchar& g, const uchar& r ){
    if( pix[0] + b < 255 && pix[0] + b >= 0 ){ pix[0] += b; }
    if( pix[1] + g < 255 && pix[1] + g >= 0 ){ pix[1] += g; }
    if( pix[2] + r < 255 && pix[2] + r >= 0 ){ pix[2] += r; }
}

/**
 * Compute the scale factor for the conversion between image depths
 *
 * @param[in] gdalDepth   - GDAL Depth Type
 * @param[in] opencvDepth - OpenCV Depth Type
 * @return Scale factor between depths
 */
double gdal2OpenCVScale( const int& gdalDepth, const int& opencvDepth ){

    if( opencvDepth == CV_8U  && gdalDepth == GDT_Byte   ) return 1;
    if( opencvDepth == CV_8U  && gdalDepth == GDT_Int16  ) return 1/16.0;
    if( opencvDepth == CV_8U  && gdalDepth == GDT_UInt16 ) return 1/16.0;
    if( opencvDepth == CV_8U  && gdalDepth == GDT_Float32 ) return 1./255.;
    if( opencvDepth == CV_16U && gdalDepth == GDT_Byte   ) return 16;
    if( opencvDepth == CV_16U && gdalDepth == GDT_Int16  ) return 16;
    if( opencvDepth == CV_16U && gdalDepth == GDT_UInt16 ) return 1;
    if( opencvDepth == CV_16S && gdalDepth == GDT_Byte   ) return 16;
    if( opencvDepth == CV_16S && gdalDepth == GDT_Int16  ) return 16;
    if( opencvDepth == CV_16S && gdalDepth == GDT_UInt16 ) return 1;

    if( opencvDepth == CV_32F && gdalDepth == GDT_Float32 ) return 1.;

    throw string("Error: Unknown OpenCV Type or Unknown GDAL Type");
}

/**
 *  Convert and set the value from the gdal raster to the opencv image
 *
 * @param[in] val Pixel value from GDAL dataset
 * @param[in] image OpenCV Image to load
 * @param[in] point Pixel to set the value to
 * @param[in] scale Scale factor to convert with
*/
void GDAL2OpenCV( double const& val, Mat& image, Point const& point, const double& scale ){

    // get the required depth
    //int cvDepth = image.depth();

    // get the value for the image
    double pixVal = val * scale;

    // place the value
    if( image.depth() == CV_8U ){
        image.at<uchar>(point) = (uchar)pixVal;
    }
    else if( image.depth() == CV_16U ){
        image.at<ushort>(point) = (ushort)pixVal;
    }
    else if( image.depth() == CV_16S ){
        image.at<short>(point) = (short)pixVal;
    }
    else if( image.depth() == CV_32S ){
        image.at<int>(point) = (int)pixVal;
    }
    else if( image.depth() == CV_32F ){
        image.at<float>(point) = (float)pixVal;
    }
    else if( image.depth() == CV_64F ){
        image.at<double>(point) = (double)pixVal;
    }
    else{
        throw string("Error: Unknown Depth");
    }

}

/**
 * Convert Type 2 Depth
 *
 * @param[in] type OpenCV Type to convert
 * @return Associated depth
*/
int cvType2Depth( const int& type ){

    switch( type ){

        case CV_8UC1:
            return CV_8U;
        case CV_8UC2:
            return CV_8U;
        case CV_8UC3:
            return CV_8U;
        case CV_16UC1:
            return CV_16U;
        case CV_16UC2:
            return CV_16U;
        case CV_16UC3:
            return CV_16U;
        case CV_16SC1:
            return CV_16S;
        case CV_16SC2:
            return CV_16S;
        case CV_16SC3:
            return CV_16S;
        case CV_32SC1:
            return CV_32S;
        case CV_32SC2:
            return CV_32S;
        case CV_32SC3:
            return CV_32S;
        case CV_32FC1:
            return CV_32F;
        case CV_32FC2:
            return CV_32F;
        case CV_32FC3:
            return CV_32F;
        case CV_64FC1:
            return CV_64F;
        case CV_64FC2:
            return CV_64F;
        case CV_64FC3:
            return CV_64F;
        default:
            return -1;
    }
}

/**
 * Extract the number of channels from the type
 *
 * @param[in] type
 * @return Number of channels
*/
int cvType2Channels( const int& type ){

    switch( type ){

        case CV_8UC1:
        case CV_16UC1:
        case CV_16SC1:
        case CV_32SC1:
        case CV_32FC1:
        case CV_64FC1:
            return 1;
        case CV_8UC2:
        case CV_16UC2:
        case CV_16SC2:
        case CV_32SC2:
        case CV_32FC2:
        case CV_64FC2:
            return 2;
        case CV_8UC3:
        case CV_16UC3:
        case CV_16SC3:
        case CV_32SC3:
        case CV_32FC3:
        case CV_64FC3:
            return 3;

        default:
            return 0;
    }
}

/**
 * Convert Depth and Channels into Type
 *
 * @param[in] Depth OpenCV Depth value
 * @param[in] Channels Number of channels
 * @return  OpenCV Type
*/
int cvDepthChannel2Type( const int Depth, const int Channels ){

    if( Depth == CV_8U  && Channels == 3 )  return CV_8UC3;
    if( Depth == CV_8U  && Channels == 2 )  return CV_8UC2;
    if( Depth == CV_8U  && Channels == 1 )  return CV_8UC1;
    if( Depth == CV_16U && Channels == 3 ) return CV_16UC3;
    if( Depth == CV_16U && Channels == 2 ) return CV_16UC2;
    if( Depth == CV_16U && Channels == 1 ) return CV_16UC1;
    if( Depth == CV_16S && Channels == 3 ) return CV_16SC3;
    if( Depth == CV_16S && Channels == 2 ) return CV_16SC2;
    if( Depth == CV_16S && Channels == 1 ) return CV_16SC1;
    if( Depth == CV_32S && Channels == 3 ) return CV_32SC3;
    if( Depth == CV_32S && Channels == 2 ) return CV_32SC2;
    if( Depth == CV_32S && Channels == 1 ) return CV_32SC1;
    if( Depth == CV_32F && Channels == 1 ) return CV_32FC1;
    if( Depth == CV_64F && Channels == 1 ) return CV_64FC1;

    throw string("Error: combo not supported");

    return 0;
}

/**
 * Checks if the OpenCV Image Type is supported for this.
 *
 * 2 channel images are not supported only because I am not sure what
 * types of images these are.
*/
bool validOpenCVImageType( const int& imageType ){

    switch( imageType ){

        case CV_8UC1:
        case CV_8UC3:
        case CV_16UC1:
        case CV_16UC2:
        case CV_16UC3:
        case CV_16SC1:
        case CV_16SC2:
        case CV_16SC3:
        case CV_32SC1:
        case CV_32SC2:
        case CV_32SC3:
        case CV_32FC1:
        case CV_32FC2:
        case CV_32FC3:
        case CV_64FC1:
        case CV_64FC2:
        case CV_64FC3:
            return true;
        default:
            return false;
    }
}

/**
 * Read an image format using GDAL as the toolset.
 *
 * @param[in] filename Image filename
 * @param[in] flags Type of image you want to read
 */
Mat  imread_geo( const string& filename, const int& imageType ){

    // parse the flags to determine which color type we want to read
    if( validOpenCVImageType(imageType) == false ) throw string("Invalid Image Type");

    // ensure the file exists
    if( boost::filesystem::exists( filename ) == false ){
        throw string( "Error: File ") + filename + string(" does not exist" );
    }

    // register the gdal driver
    GDALAllRegister();

    // load the dataset
    GDALDataset*  dataset = (GDALDataset*) GDALOpen( filename.c_str(), GA_ReadOnly);

    // if the dataset returns null, then the file does not exist, or there was a read error
    if( dataset == NULL ){ throw string("Error:  GDAL Dataset returned null from read"); }

    // check if pixel data even exists
    if( dataset->GetRasterCount() <= 0 ) throw string("Error: File does not contain pixel data");

    //get the driver infomation
    //GDALDriver*  driver = dataset->GetDriver();

    // get raster image size
    Size imgSize( dataset->GetRasterXSize(), dataset->GetRasterYSize() );

    // create mats for each raster layer
    vector<Mat> layers( dataset->GetRasterCount() );
    for( size_t i=0; i<layers.size(); i++ )
        layers[i] = Mat( imgSize, cvDepthChannel2Type(cvType2Depth( imageType), 1));

    // iterate through each band
    for (int i = 0; i < dataset->GetRasterCount(); i++) {

        //create the band object
        GDALRasterBand *band = dataset->GetRasterBand(i + 1);

        // get the gdal band type
        int datatype = band->GetRasterDataType();

        // compute the scale factor
        double scale = gdal2OpenCVScale( datatype, layers[i].depth());

        //read each image row
        for ( int r = 0; r < layers[i].rows; r++) {

            float* pafScanline;
            pafScanline = (float*) CPLMalloc(sizeof (float) *layers[i].cols);
            CPLErr eErr = band->RasterIO(GF_Read, 0, r, layers[i].cols, 1, pafScanline, layers[i].cols, 1, GDT_Float32, 0, 0);
            if ( eErr != CE_None ) {
                        CPLFree(pafScanline);
                        Mat m;
                        return m;
                    }
            // iterate through each column
            for ( int c = 0; c < layers[i].cols; c++) {
                GDAL2OpenCV( pafScanline[c], layers[i], Point(c,r), scale );
            }
            CPLFree(pafScanline);
        }

    }

    // close our GDAL Dataset and clean up
    GDALClose( dataset );

    //merge channels into single image
    Mat output;
    if( layers.size() > 1 )
        cv::merge( layers, output );
    else
        output = layers[0].clone();

    // do channel conversions if necessary
    if( cvType2Channels(imageType) == output.channels() )
        return output;

    if( cvType2Channels(imageType) == 1 && output.channels() == 3 ){
        cvtColor( output, output, CV_BGR2GRAY );
        return output;
    }
    if( cvType2Channels(imageType) == 3 && output.channels() == 1 ){
        cvtColor( output, output, CV_GRAY2BGR);
        return output;
    }

    // Return the output image
    return output;
}

/*
 * Main Function
*/
int main( int argc, char* argv[] ){
    /*
     * Check input arguments
    */
    /*if( argc < 3 ){
        cout << "usage: " << argv[0] << " <image_name> <dem_model_name>" << endl;
        return -1;
    }
    */
    // load the image (note that we don't have the projection information.  You will
    // need to load that yourself or use the full GDAL driver.  The values are pre-defined
    // at the top of this file
    //cv::Mat image = cv::imread(argv[1], cv::IMREAD_LOAD_GDAL| cv::IMREAD_ANYDEPTH );
    //if (image.empty()){ throw std::runtime_error("ERROR loading image"); }

    //Mat inc_x = imread_geo( argv[1]+string("_0"), CV_32FC1 );
    //Mat inc_y = imread_geo( argv[1]+string("_1"), CV_32FC1 );
    //Mat inc_z = imread_geo( argv[1]+string("_2"), CV_32FC1 );
    //cout << "Using " << inc_z.size() << " channels" << endl;

    std::vector<cv::Mat> inc;
    inc.push_back(imread_geo( argv[1]+string("_0"), CV_32FC1 ));
    inc.push_back(imread_geo( argv[1]+string("_1"), CV_32FC1 ));
    inc.push_back(imread_geo( argv[1]+string("_2"), CV_32FC1 ));

    cout << "Using " << inc.size() << " Size" << endl;
    Mat3f mat_inc;
    cv::merge(inc,mat_inc);

    cout << "Using " << mat_inc.rows << " rows with " << mat_inc.cols << " cols and " << mat_inc.channels() << " channels with " << mat_inc.size() << " size each." << endl;

    //std::vector<Mat> emi;
    //Mat emi_x = imread_geo( argv[1]+string("_3"), CV_32FC1 );
    //Mat emi_y = imread_geo( argv[1]+string("_4"), CV_32FC1 );
    //Mat emi_z = imread_geo( argv[1]+string("_5"), CV_32FC1 );

    std::vector<Mat> norm;
    norm.push_back(imread_geo( argv[1]+string("_6"), CV_32FC1 ));
    norm.push_back(imread_geo( argv[1]+string("_7"), CV_32FC1 ));
    norm.push_back(imread_geo( argv[1]+string("_8"), CV_32FC1 ));
    Mat3f mat_norm;
    cv::merge(norm,mat_norm);

    //cv::Mat dot_array=[inc_norm, mat_norm];

    //Mat dot = Mat::dot(dot_array);
    // load the dem model
    /*cv::Mat dem = cv::imread(argv[2], cv::IMREAD_LOAD_GDAL | cv::IMREAD_ANYDEPTH );
    // create our output products
    cv::Mat output_dem(   image.size(), CV_8UC3 );
    cv::Mat output_dem_flood(   image.size(), CV_8UC3 );
    // for sanity sake, make sure GDAL Loads it as a signed short
    //if( dem.type() != CV_16SC1 ){ throw std::runtime_error("DEM image type must be CV_16SC1"); }
    // define the color range to create our output DEM heat map
    //  Pair format ( Color, elevation );  Push from low to high
    //  Note:  This would be perfect for a configuration file, but is here for a working demo.
    color_range.push_back( std::pair<cv::Vec3b,double>(cv::Vec3b( 188, 154,  46),   -1));
    color_range.push_back( std::pair<cv::Vec3b,double>(cv::Vec3b( 110, 220, 110), 0.25));
    color_range.push_back( std::pair<cv::Vec3b,double>(cv::Vec3b( 150, 250, 230),   20));
    color_range.push_back( std::pair<cv::Vec3b,double>(cv::Vec3b( 160, 220, 200),   75));
    color_range.push_back( std::pair<cv::Vec3b,double>(cv::Vec3b( 220, 190, 170),  100));
    color_range.push_back( std::pair<cv::Vec3b,double>(cv::Vec3b( 250, 180, 140),  200));
    // define a minimum elevation
    double minElevation = -13575;
    // iterate over each pixel in the image, computing the dem point
    for( int y=0; y<image.rows; y++ ){
    for( int x=0; x<image.cols; x++ ){
        // convert the pixel coordinate to lat/lon coordinates
        cv::Point2d coordinate = pixel2world( x, y, image.size() );
        // compute the dem image pixel coordinate from lat/lon
        cv::Point2d dem_coordinate = world2dem( coordinate, dem.size() );
        // extract the elevation
        double dz;
        if( dem_coordinate.x >=    0    && dem_coordinate.y >=    0     &&
            dem_coordinate.x < dem.cols && dem_coordinate.y < dem.rows ){
            dz = dem.at<short>(dem_coordinate);
        }else{
            dz = minElevation;
        }
        // write the pixel value to the file
        output_dem_flood.at<cv::Vec3b>(y,x) = image.at<cv::Vec3b>(y,x);
        // compute the color for the heat map output
        cv::Vec3b actualColor = get_dem_color(dz);
        output_dem.at<cv::Vec3b>(y,x) = actualColor;
        // show effect of a 10 meter increase in ocean levels
        if( dz < 10 ){
            add_color( output_dem_flood.at<cv::Vec3b>(y,x), 90, 0, 0 );
        }
        // show effect of a 50 meter increase in ocean levels
        else if( dz < 50 ){
            add_color( output_dem_flood.at<cv::Vec3b>(y,x), 0, 90, 0 );
        }
        // show effect of a 100 meter increase in ocean levels
        else if( dz < 100 ){
            add_color( output_dem_flood.at<cv::Vec3b>(y,x), 0, 0, 90 );
        }
    }}
    */
    // print our heat map
    cv::imwrite( "heat-map.tif"   ,  mat_inc );
    // print the flooding effect image
    //cv::imwrite( "flooded.jpg",  output_dem_flood);
    return 0;
}


#include <boost/graph/distributed/page_rank.hpp>


//
// Copyright Antoine Leblanc 2010 - 2015
// Distributed under the MIT license.
//
// http://nauths.fr
// http://github.com/nicuveo
// mailto://antoine.jp.leblanc@gmail.com
//

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Includes

#include <cstdlib>
#include <iostream>
#include <boost/test/unit_test.hpp>
#include "nauths/mml/mml.hh"

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Local code

namespace
{

  int next()
  {
    return sqrt(rand()) / 5;
  }

  typedef mml::Shape<mml::Num> Shape;
  typedef mml::Point<mml::Num> Point;
  typedef mml::Point<mml::Num> Vector;
  typedef mml::Rect<mml::Num> Rect;
  typedef mml::Circle<mml::Num> Circle;

}

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Implementation

BOOST_AUTO_TEST_SUITE(circle)

BOOST_AUTO_TEST_CASE(t0)
{
  srand(time(0));

  int x = next();
  int y = next();
  int r = next();

  Circle c0;
  Circle c1(x, y, r);
  Circle c2(c1);

  BOOST_REQUIRE_EQUAL(c0, Circle());
  BOOST_REQUIRE_EQUAL(c0, Circle(0, 0, 0));
  BOOST_REQUIRE_EQUAL(c0, Circle(Point(0, 0), 0));
  BOOST_REQUIRE_EQUAL(c0.center(), Point(0, 0));
  BOOST_REQUIRE_EQUAL(c0.radius(), 0);

  BOOST_REQUIRE_EQUAL(c1, Circle(x, y, r));
  BOOST_REQUIRE_EQUAL(c1, Circle(Point(x, y), r));
  BOOST_REQUIRE_EQUAL(c1.center(), Point(x, y));
  BOOST_REQUIRE_EQUAL(c1.radius(), r);

  BOOST_REQUIRE_EQUAL(c2, Circle(x, y, r));
  BOOST_REQUIRE_EQUAL(c2, Circle(Point(x, y), r));
  BOOST_REQUIRE_EQUAL(c2.center(), Point(x, y));
  BOOST_REQUIRE_EQUAL(c2.radius(), r);
}

BOOST_AUTO_TEST_CASE(t1)
{
  srand(time(0));

  int x = next();
  int y = next();
  int r = next();

  Circle c0;
  Circle c1(x, y, r);
  Circle c2(c1);

  BOOST_REQUIRE_EQUAL(c0.bounding_rect(), Rect(0, 0, 0, 0));
  BOOST_REQUIRE_EQUAL(c1.bounding_rect(), Rect(x - r, y - r, x + r, y + r));
  BOOST_REQUIRE_EQUAL(c2.bounding_rect(), Rect(x - r, y - r, x + r, y + r));
}

BOOST_AUTO_TEST_CASE(t2)
{
  srand(time(0));

  int x = next();
  int y = next();
  int r = next();

  Circle c0;
  Circle c1(x, y, r);
  Circle c2(c1);

  BOOST_REQUIRE_EQUAL(c0.area(), 0);
  BOOST_REQUIRE_EQUAL(c1.area(), c1.radius() * c1.radius() * mml::pi());
  BOOST_REQUIRE_EQUAL(c2.area(), c2.radius() * c2.radius() * mml::pi());
}

BOOST_AUTO_TEST_CASE(t3)
{
  srand(time(0));

  int x = next();
  int y = next();
  int r = next();

  Circle c0;
  Circle c1(x, y, r);
  Circle c2(c1);

  BOOST_REQUIRE_EQUAL(c0.perimeter(), 0);
  BOOST_REQUIRE_EQUAL(c1.perimeter(), 2 * c1.radius() * mml::pi());
  BOOST_REQUIRE_EQUAL(c2.perimeter(), 2 * c2.radius() * mml::pi());
}

BOOST_AUTO_TEST_SUITE_END()