#ifndef TVTML_DATA3D_HPP
#define TVTML_DATA3D_HPP

// system includes
#include <cassert>
#include <limits>
#include <cmath>
#include <random>

#include <string>
#include <iostream>
#include <sstream>
#include <fstream>

//Eigen includes
#include <Eigen/Core>
#include <Eigen/Geometry>

// OpenCV includes
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#ifdef TV_DATA_DEBUG
    #include <opencv2/highgui/highgui.hpp>
#endif

// video++ includes
#include <vpp/vpp.hh>
#include <vpp/utils/opencv_bridge.hh>

// own includes
#include "data3d_utils.hpp"
#include "manifold.hpp"

namespace tvmtl{

// Specialization 3D Data
template < typename MANIFOLD >
class Data<MANIFOLD, 3>{

    public:
	static const int img_dim;

	// Manifold typedefs
	typedef typename MANIFOLD::value_type value_type;
	typedef typename MANIFOLD::scalar_type scalar_type;

	// Storage typedefs
	typedef vpp::image3d<value_type> storage_type;

	typedef double weights_type;
	typedef vpp::image3d<weights_type> weights_mat;

	typedef bool inp_type;
	typedef vpp::image3d<inp_type> inp_mat;

	inline bool doInpaint() const { return inpaint_; }

	// Data Init functions
	inline void initEdgeweights();
	inline void initInp();

	// Input functions
	void rgb_slice_reader(std::string filename, int num_slides);
	void readMatrixDataFromCSV(std::string filename, const int nz, const int ny, const int nx);
	void readRawVolumeData(std::string filename, const int nz, const int ny, const int nx);

	// Noise functions
	void add_gaussian_noise(double stdev);

	// Creation functions
	void create_noisy_gray(const int nz, const int ny, const int nx, double color=0.5, double stdev=0.1);
	void create_noisy_rgb(const int nz, const int ny, const int nx, int color=1, double stdev=0.1);

	void setEdgeWeights(const weights_mat&);

	//Output functions
	template <class IMG>
	void output_matval_img(const IMG& img, std::string filename) const;

//  private:
	storage_type img_;
	storage_type noise_img_;
	weights_mat edge_weights_;

	bool inpaint_;
	inp_mat inp_;
};

/*----- Implementation 3D Data ------*/
template < typename MANIFOLD >
const int Data<MANIFOLD, 3>::img_dim = 3;

template < typename MANIFOLD >
void Data<MANIFOLD, 3>::setEdgeWeights(const weights_mat& w){
    clone3d(w, edge_weights_);
}

template < typename MANIFOLD >
void Data<MANIFOLD, 3>::initInp(){
    inp_ = inp_mat(noise_img_.domain());
    fill3d(inp_, false);
    inpaint_ = false;
}

template < typename MANIFOLD >
void Data<MANIFOLD, 3>::initEdgeweights(){
    edge_weights_ = weights_mat(noise_img_.domain());
    fill3d(edge_weights_, 1.0);
}

template <typename MANIFOLD>
void Data<MANIFOLD, 3>::add_gaussian_noise(double stdev){

    std::random_device rd;
    std::mt19937 gen(rd());
    std::normal_distribution<typename MANIFOLD::scalar_type> rand(0.0, stdev);

    auto generate = [&] (typename MANIFOLD::scalar_type entry){
	return entry + rand(gen);
    };

    if(MANIFOLD::non_isometric_embedding)
	pixel_wise3d([&] (value_type& i){ MANIFOLD::interpolation_preprocessing(i); }, noise_img_);

    pixel_wise3d([&] (value_type& i) {if(i.norm() > 0.07 ) i = i.unaryExpr(generate); }, noise_img_);

    if(MANIFOLD::non_isometric_embedding)
	pixel_wise3d([&] (value_type& i){ MANIFOLD::interpolation_postprocessing(i); }, noise_img_);

    pixel_wise3d([&] (value_type& i) { MANIFOLD::projector(i); }, noise_img_);

    clone3d(noise_img_, img_);
}

template < typename MANIFOLD >
void Data<MANIFOLD, 3>::create_noisy_gray(const int nz, const int ny, const int nx, double color, double stdev){
    static_assert(MANIFOLD::value_dim == 1, "Method is only callable for Manifolds with embedding dimension 1");
    noise_img_ = storage_type(nz, ny, nx);
    img_ = storage_type(noise_img_.domain());

    std::random_device rd;
    std::mt19937 gen(rd());
    std::normal_distribution<typename MANIFOLD::scalar_type> rand(0.0, stdev);
    auto insert = [&] (value_type& i) { i.setConstant(color + rand(gen)); MANIFOLD::projector(i); };

    pixel_wise3d(insert, noise_img_);
    clone3d(noise_img_, img_);
    initInp();
    initEdgeweights();
}

template < typename MANIFOLD >
void Data<MANIFOLD, 3>::create_noisy_rgb(const int nz, const int ny, const int nx, int color, double stdev){
    static_assert(MANIFOLD::value_dim == 3, "Method is only callable for Manifolds with embedding dimension 3");
    noise_img_ = storage_type(nz, ny, nx);
    img_ = storage_type(noise_img_.domain());

    std::random_device rd;
    std::mt19937 gen(rd());
    std::normal_distribution<typename MANIFOLD::scalar_type> rand(0.0, stdev);
    value_type v(0.7 + rand(gen), 0.3 + rand(gen), 0.3 + rand(gen));
    auto insert = [&] (value_type& i) { i = v; MANIFOLD::projector(i); };

    pixel_wise3d(insert, noise_img_);
    clone3d(noise_img_, img_);
    initInp();
    initEdgeweights();
}

template < typename MANIFOLD >
void Data<MANIFOLD, 3>::rgb_slice_reader(std::string filename, int num_slices){
	static_assert(MANIFOLD::value_dim == 3,"ERROR: RGB Input requires a Manifold with embedding dimension N=3!");
	std::string fname(filename);

	int last_dot_pos = fname.find_last_of(".");
	std::string basefilename  = fname.substr(0, last_dot_pos-1);
	std::string ext = fname.substr(last_dot_pos, fname.length());

	vpp::image2d<vpp::vuchar3> input_image = vpp::clone(vpp::from_opencv<vpp::vuchar3 >(cv::imread(basefilename + std::to_string(0) + ext)));
	vpp::image2d<value_type> input_image_double(input_image.domain());
	int nr = input_image.nrows();
	int nc = input_image.ncols();
	noise_img_ = storage_type(num_slices, nr, nc);

	#ifdef TV_DATA_DEBUG
	    std::cout << "\nReading slice sequence " << basefilename + std::to_string(0) + ext << " containing " << num_slices << " Slices. " << std::endl;
	    std::cout << "Dimensions (Slices, Rows, Cols): " << num_slices << " X " << nr << " X " << nc << std::endl;
	#endif

	// Convert Picture of uchar to double
	auto converter =  [] (const auto& i, auto& d) {
	    value_type v = value_type::Zero();
	    vpp::vuchar3 vu = i;
	    // TODO: insert manifold scalar type
	    v[0]=static_cast<double>(vu[2]); //opencv saves as BGR
	    v[1]=static_cast<double>(vu[1]);
	    v[2]=static_cast<double>(vu[0]);
	    d = v / static_cast<double>(std::numeric_limits<unsigned char>::max());
	};

	for(int s = 0; s < num_slices; ++s ){
	#ifdef TV_DATA_DEBUG
	    std::cout << "Reading slice number " << s << " with name " << basefilename + std::to_string(s) + ext << std::endl;
	#endif
	    input_image = vpp::clone(vpp::from_opencv<vpp::vuchar3 >(cv::imread(basefilename + std::to_string(s) + ext)));
	    vpp::pixel_wise(input_image, input_image_double) | converter;

	    #pragma omp parallel for
	    for(int r = 0; r < nr; ++r){
		value_type* row_pointer3d = &noise_img_(s, r, 0);
		value_type* row_pointer2d = &input_image_double(r,0);
		for(int c = 0; c < nc; ++c)
		    row_pointer3d[c] = row_pointer2d[c];
	    }
	}
    img_ = storage_type(noise_img_.domain());
    clone3d(noise_img_, img_);

    initInp();
    initEdgeweights();
}

template <typename MANIFOLD>
void Data<MANIFOLD, 3>::readMatrixDataFromCSV(std::string filename, const int nz, const int ny, const int nx){
    #ifdef TV_DATA_DEBUG
	std::cout << "ReadMatrixData from CSV File..." << std::endl;
    #endif
    noise_img_ = storage_type(nz, ny, nx);
    //fill3d(noise_img_, MANIFOLD::value_type::Zero());

    const int N = MANIFOLD::value_type::RowsAtCompileTime;
    const int N2 = MANIFOLD::value_dim;
    int cols, rows = 0;

    std::ifstream infile(filename, std::ifstream::in);

    std::string line = "";
    while (std::getline(infile, line)){
	std::stringstream strstr(line);
	std::string word = "";

	if(cols == 0)
	    while (std::getline(strstr, word, ','))
		cols++;

	rows++;
    }

    assert(N2 == cols);
    assert(nz*nx*ny == rows);

    infile.clear();
    infile.seekg(0, std::ios_base::beg);

    Eigen::Matrix<typename MANIFOLD::scalar_type, N2, 1> vectorizedMat;
    vectorizedMat.setZero();

    auto it = noise_img_.begin();
    while (std::getline(infile, line)){
	std::stringstream strstr(line);
	std::string word = "";
	int j=0;
	while (std::getline(strstr, word,',')){
	    typename MANIFOLD::scalar_type entry= static_cast<typename MANIFOLD::scalar_type>(std::stod(word));
	    vectorizedMat(j) = entry;
	    ++j;
	}
	*it = Eigen::Map<typename MANIFOLD::value_type>(vectorizedMat.data());
	it.next();
    }
    img_ = storage_type(noise_img_.domain());
    clone3d(noise_img_, img_);

    initInp();
    initEdgeweights();
}

template <typename MANIFOLD>
void Data<MANIFOLD, 3>::readRawVolumeData(std::string filename, const int nz, const int ny, const int nx){

    static_assert(MANIFOLD::MyType == EUCLIDIAN, "readRawVolumeData is only Implemented for Euclidian Manifolds");
    static_assert(MANIFOLD::value_dim == 1, "readMatrixDataFromCSV is only for grayscale volume picture input");

    int pixel_num = nz * ny * nx;
    noise_img_ = storage_type(nz, ny, nx);

    std::string fname(filename);
    std::fstream file;
    file.open(fname, std::ios::in|std::ios::binary|std::ios::ate);

    std::streampos size;
    char* buffer;
    bool read_failure = true;

    std::cout << "Reading file " << fname << " with dimensions(Slices, Rows, Cols) " << nz << " X " << ny << " X " << nx << std::endl;
    std::cout << "Number of pixels = " << pixel_num << std::endl;

    if(file.is_open()){
	size = file.tellg();
	buffer = new char[size];
	file.seekg(0, std::ios::beg);
	file.read(buffer, size);
	file.close();
	read_failure = false;
    }

    if(read_failure){
	std::cout << "File import not successfull!" << std::endl;
	return;
	}

    std::cout << "File successfully imported! File Size = " << size << " Bytes" <<std::endl;

    assert(size == pixel_num);

    int k = 0;
    for(auto& p : noise_img_){
	double px = static_cast<double>(buffer[k]) / static_cast<double>(std::numeric_limits<unsigned char>::max());
	p.setConstant(px);
	++k;
    }

    img_ = storage_type(noise_img_.domain());
    clone3d(noise_img_, img_);

    initInp();
    initEdgeweights();

}

template < typename MANIFOLD >
template < class IMG >
void Data<MANIFOLD, 3>::output_matval_img(const IMG& img, std::string filename) const{
    int ns = img.nslices();
    int nr = img.nrows();
    int nc = img.ncols();

    std::fstream f;
    f.open(filename, std::fstream::out);
    Eigen::IOFormat CommaInitFmt(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "", "\n");
    for (int s=0; s < ns; s++){
	for (int r=0; r < nr; r++){
	    const auto* cur = &img(s, r, 0);
	    for (int c=0; c < nc; c++)
		f << cur[c].format(CommaInitFmt);
	}
    }
/*
for (int c=0; c < nc; c++)
for (int r=0; r < nr; r++)
for (int s=0; s < ns; s++)
    f << img(s, r, c).format(CommaInitFmt);

    f.close();*/
}

}// end namespace tvmtl

#endif


//
//  geometry.cpp
//  autd3
//
//  Created by Seki Inoue on 6/8/16.
//
//

#include <stdio.h>
#include <map>
#include <Eigen/Geometry>
#include "autd3.hpp"
#include "privdef.hpp"

class Device {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    Device(int device_id, Eigen::Vector3f position, Eigen::Vector3f euler_angles)
    : device_id(device_id), position(position), euler_angles(euler_angles) {
        Eigen::Quaternionf quo =
            Eigen::AngleAxisf(euler_angles.x(), Eigen::Vector3f::UnitZ()) *
            Eigen::AngleAxisf(euler_angles.y(), Eigen::Vector3f::UnitY()) *
            Eigen::AngleAxisf(euler_angles.z(), Eigen::Vector3f::UnitZ());

        transform_matrix = Eigen::Translation3f(position) * quo;
        z_direction = quo * Eigen::Vector3f(0, 0, 1);

        int index = 0;
        for (int y = 0; y < NUM_TRANS_Y; y++)
            for (int x = 0; x < NUM_TRANS_X; x++)
                if (!IS_MISSING_TRANSDUCER(x, y))
                    local_trans_positions.col(index++) = Eigen::Vector3f(x*TRANS_SIZE, y*TRANS_SIZE, 0);

        global_trans_positions = transform_matrix * local_trans_positions;
    };
    int device_id;
    Eigen::Vector3f position;
    Eigen::Matrix<float, 3, NUM_TRANS_IN_UNIT> local_trans_positions;
    Eigen::Matrix<float, 3, NUM_TRANS_IN_UNIT> global_trans_positions;
    Eigen::Vector3f euler_angles;
    Eigen::Vector3f z_direction;
    Eigen::Affine3f transform_matrix;
};

class autd::Geometry::impl {
public:
    std::vector<std::shared_ptr<Device> > devices;
    std::shared_ptr<Device> device(int transducer_id) {
        int eid = transducer_id/NUM_TRANS_IN_UNIT;
        return this->devices[eid];
    }
};

autd::GeometryPtr autd::Geometry::Create() {
    return GeometryPtr(new Geometry);
}

autd::Geometry::Geometry() {
    this->_pimpl = std::unique_ptr<impl>(new impl());
}

autd::Geometry::~Geometry() {

}

int autd::Geometry::AddDevice(Eigen::Vector3f position, Eigen::Vector3f euler_angles) {
    int device_id = this->_pimpl->devices.size();
    this->_pimpl->devices.push_back(std::shared_ptr<Device>(new Device(device_id, position, euler_angles)));
    return device_id;
}

void autd::Geometry::DelDevice(int device_id) {
    auto itr = this->_pimpl->devices.begin();
    while (itr != this->_pimpl->devices.end())
    {
        if((*itr)->device_id == device_id) itr = this->_pimpl->devices.erase(itr);
        else itr++;
    }
}

const int autd::Geometry::numDevices() {
    return this->_pimpl->devices.size();
}

const int autd::Geometry::numTransducers() {
    return this->numDevices()*NUM_TRANS_IN_UNIT;
}

const Eigen::Vector3f autd::Geometry::position(int transducer_id) {
    const int local_trans_id = transducer_id%NUM_TRANS_IN_UNIT;
    auto device = this->_pimpl->device(transducer_id);
    return device->global_trans_positions.col(local_trans_id);
}

const Eigen::Vector3f &autd::Geometry::direction(int transducer_id) {
    return this->_pimpl->devices[this->deviceIdForTransIdx(transducer_id)]->z_direction;
}

const int autd::Geometry::deviceIdForDeviceIdx(int device_idx) {
    return this->_pimpl->devices[device_idx]->device_id;
}

const int autd::Geometry::deviceIdForTransIdx(int transducer_id) {
    return this->_pimpl->device(transducer_id)->device_id;
}

float  autd::Geometry::frequency() {
    return FPGA_CLOCK / (640.0 + this->_freq_shift);
}

void   autd::Geometry::SetFrequency(float freq) {
    this->_freq_shift = std::min(std::max(FPGA_CLOCK / freq - 640, (float)std::numeric_limits<int8_t>::min()), (float)std::numeric_limits<int8_t>::max());
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

/// \file
/// Declares function RootFinder::toms748

#pragma once

#include <boost/math/tools/roots.hpp>
#include <functional>
#include <limits>

#include "DataStructures/DataVector.hpp"
#include "ErrorHandling/Exceptions.hpp"

namespace RootFinder {

/*!
 * \ingroup NumericalAlgorithmsGroup
 * \brief Finds the root of the function `f` with the TOMS_748 method.
 *
 * `f` is a unary invokable that takes a `double` which is the current value at
 * which to evaluate `f`. An example is below.
 *
 * \snippet Test_TOMS748.cpp double_root_find
 *
 * The TOMS_748 algorithm searches for a root in the interval [`lower_bound`,
 * `upper_bound`], and will throw if this interval does not bracket a root,
 * i.e. if `f(lower_bound) * f(upper_bound) > 0`.
 *
 * The arguments `f_at_lower_bound` and `f_at_upper_bound` are optional, and
 * are the function values at `lower_bound` and `upper_bound`. These function
 * values are often known because the user typically checks if a root is
 * bracketed before calling `toms748`; passing the function values here saves
 * two function evaluations.
 *
 * See the [Boost](http://www.boost.org/) documentation for more details.
 *
 * \requires Function `f` is invokable with a `double`
 *
 * \throws `std::domain_error` if the bounds do not bracket a root.
 * \throws `convergence_error` if the requested tolerance is not met after
 *                            `max_iterations` iterations.
 */
template <typename Function>
double toms748(const Function& f, const double lower_bound,
               const double upper_bound, const double f_at_lower_bound,
               const double f_at_upper_bound, const double absolute_tolerance,
               const double relative_tolerance,
               const size_t max_iterations = 100) {
  ASSERT(relative_tolerance > std::numeric_limits<double>::epsilon(),
         "The relative tolerance is too small.");

  boost::uintmax_t max_iters = max_iterations;

  // This solver requires tol to be passed as a termination condition. This
  // termination condition is equivalent to the convergence criteria used by the
  // GSL
  auto tol = [absolute_tolerance, relative_tolerance](double lhs, double rhs) {
    return (fabs(lhs - rhs) <=
            absolute_tolerance +
                relative_tolerance * fmin(fabs(lhs), fabs(rhs)));
  };
  // clang-tidy: internal boost warning, can't fix it.
  auto result = boost::math::tools::toms748_solve(  // NOLINT
      f, lower_bound, upper_bound, f_at_lower_bound, f_at_upper_bound, tol,
      max_iters);
  if (max_iters >= max_iterations) {
    throw convergence_error(
        "toms748 reached max iterations without converging");
  }
  return result.first + 0.5 * (result.second - result.first);
}

/*!
 * \ingroup NumericalAlgorithmsGroup
 * \brief Finds the root of the function `f` with the TOMS_748 method,
 * where function values are not supplied at the lower and upper
 * bounds.
 */
template <typename Function>
double toms748(const Function& f, const double lower_bound,
               const double upper_bound, const double absolute_tolerance,
               const double relative_tolerance,
               const size_t max_iterations = 100) {
  return toms748(f, lower_bound, upper_bound, f(lower_bound), f(upper_bound),
                 absolute_tolerance, relative_tolerance, max_iterations);
}

namespace detail {
template <typename Function>
DataVector toms748_impl(const Function& f, const DataVector& lower_bound,
                        const DataVector& upper_bound,
                        const DataVector& f_at_lower_bound,
                        const DataVector& f_at_upper_bound,
                        const double absolute_tolerance,
                        const double relative_tolerance,
                        const size_t max_iterations,
                        const bool function_values_are_supplied) {
  ASSERT(relative_tolerance > std::numeric_limits<double>::epsilon(),
         "The relative tolerance is too small.");
  // This solver requires tol to be passed as a termination condition. This
  // termination condition is equivalent to the convergence criteria used by the
  // GSL
  auto tol = [absolute_tolerance, relative_tolerance](const double lhs,
                                                      const double rhs) {
    return (fabs(lhs - rhs) <=
            absolute_tolerance +
                relative_tolerance * fmin(fabs(lhs), fabs(rhs)));
  };
  DataVector result_vector{lower_bound.size()};
  for (size_t i = 0; i < result_vector.size(); ++i) {
    // toms748_solver modifies the max_iters after the root is found to the
    // number of iterations that it took to find the root, so we reset it to
    // max_iterations after each root find.
    boost::uintmax_t max_iters = max_iterations;
    auto result = function_values_are_supplied
                      ?
                      // clang-tidy: internal boost warning, can't fix it.
                      boost::math::tools::toms748_solve(  // NOLINT
                          [&f, i](double x) { return f(x, i); }, lower_bound[i],
                          upper_bound[i], f_at_lower_bound[i],
                          f_at_upper_bound[i], tol, max_iters)
                      :
                      // clang-tidy: internal boost warning, can't fix it.
                      boost::math::tools::toms748_solve(  // NOLINT
                          [&f, i](double x) { return f(x, i); }, lower_bound[i],
                          upper_bound[i], tol, max_iters);
    if (max_iters >= max_iterations) {
      throw convergence_error(
          "toms748 reached max iterations without converging");
    }
    result_vector[i] = result.first + 0.5 * (result.second - result.first);
  }
  return result_vector;
}
}  // namespace detail

/*!
 * \ingroup NumericalAlgorithmsGroup
 * \brief Finds the root of the function `f` with the TOMS_748 method on each
 * element in a `DataVector`.
 *
 * `f` is a binary invokable that takes a `double` as its first argument and a
 * `size_t` as its second. The `double` is the current value at which to
 * evaluate `f`, and the `size_t` is the current index into the `DataVector`s.
 * Below is an example of how to root find different functions by indexing into
 * a lambda-captured `DataVector` using the `size_t` passed to `f`.
 *
 * \snippet Test_TOMS748.cpp datavector_root_find
 *
 * For each index `i` into the DataVector, the TOMS_748 algorithm searches for a
 * root in the interval [`lower_bound[i]`, `upper_bound[i]`], and will throw if
 * this interval does not bracket a root,
 * i.e. if `f(lower_bound[i], i) * f(upper_bound[i], i) > 0`.
 *
 * See the [Boost](http://www.boost.org/) documentation for more details.
 *
 * \requires Function `f` be callable with a `double` and a `size_t`
 *
 * \throws `std::domain_error` if, for any index, the bounds do not bracket a
 * root.
 * \throws `convergence_error` if, for any index, the requested tolerance is not
 * met after `max_iterations` iterations.
 */
template <typename Function>
DataVector toms748(const Function& f, const DataVector& lower_bound,
                   const DataVector& upper_bound,
                   const double absolute_tolerance,
                   const double relative_tolerance,
                   const size_t max_iterations = 100) {
  return detail::toms748_impl(f, lower_bound, upper_bound, DataVector{},
                              DataVector{}, absolute_tolerance,
                              relative_tolerance, max_iterations, false);
}

/*!
 * \ingroup NumericalAlgorithmsGroup
 * \brief Finds the root of the function `f` with the TOMS_748 method on each
 * element in a `DataVector`, where function values are supplied at the lower
 * and upper bounds.
 *
 * Supplying function values is an optimization that saves two
 * function calls per point.  The function values are often available
 * because one often checks if the root is bracketed before calling `toms748`.
 */
template <typename Function>
DataVector toms748(const Function& f, const DataVector& lower_bound,
                   const DataVector& upper_bound,
                   const DataVector& f_at_lower_bound,
                   const DataVector& f_at_upper_bound,
                   const double absolute_tolerance,
                   const double relative_tolerance,
                   const size_t max_iterations = 100) {
  return detail::toms748_impl(f, lower_bound, upper_bound, f_at_lower_bound,
                              f_at_upper_bound, absolute_tolerance,
                              relative_tolerance, max_iterations, true);
}

}  // namespace RootFinder


/**
 * Created on Tue Jan 04 2022
 *
 * Shane Flandermeyer, shaneflandermeyer@gmail.com
 *
 * A simple example usage of the CFAR detection object when the input is
 * gaussian noise with a time-varying variance. This example is adapted from
 * https://www.mathworks.com/help/phased/ug/constant-false-alarm-rate-cfar-detection.html
 */

#include "cfar1d.h"
#include "matrix_utils.h"

#include <random>
#include <Eigen/Dense>
#include <matplot/matplot.h>

using namespace plasma;
using namespace matplot;
using namespace Eigen;
int main() {

  //! CFAR parameters
  auto method = "CA";
  size_t num_guard_cells = 2;
  size_t num_train_cells = 200;
  auto pfa = 1e-3;
  CFARDetector cfar{num_train_cells, num_guard_cells, pfa};

  //! Generate data
  // The input data is a square-law input signal with increasing noise power
  auto noise_power = pow(10, -10 / 10);
  size_t num_points = 10e3;
  // std::default_random_engine gen;
  MatrixXcd rsamp(num_points, 1);
  MatrixXd x(rsamp.rows(), rsamp.cols());
  // Start with samples from a complex standard normal distribution
  std::mt19937 gen{1000};
  std::normal_distribution<> normal{0, 1};
  for (size_t i = 0; i < num_points; ++i)
    rsamp(i) = std::complex<double>(normal(gen), normal(gen));
  // Create the square-law input signal
  ArrayXd ramp = ArrayXd::LinSpaced(num_points, 1, 10);
  x = abs2(sqrt(noise_power * ramp / 2) * rsamp.array()).matrix();
  // x = hcat(x,x);

  //! Write to a file
  // std::vector<double> filevec(x.data(), x.data() + x.size());
  // write_binary<double>("/home/shane/bin.dat",filevec);

  //! Do CFAR
  DetectionReport det = cfar.detect(x);

  //! Figures
  // Input data
  std::vector<double> xvec(x.data(), x.data() + x.size());
  std::vector<double> threshvec(det.threshold.data(),
                                det.threshold.data() + det.threshold.size());
  std::vector<size_t> indices(det.indices.col(0).data(),
                              det.indices.col(0).data() +
                                  det.indices.col(0).size());
  std::vector<double> detvec(det.indices.size());
  for (size_t i = 0; i < det.indices.size(); ++i)
    detvec[i] = x(det.indices(i));

  std::cout << "Number of Detections: " << det.num_detections << std::endl;

  plot(xvec);
  hold(true);
  plot(threshvec);
  plot(indices, detvec, "o");
  hold(false);
  show();
  return 0;
}


#include <iostream>

#include <unistd.h>
#include <cuda.h>
#include <cuda_runtime_api.h>

#include <blitz.h>

using namespace blitz;
// M K
Shape left_shape(2);
// K N
Shape right_shape(2);
// M N
Shape output_shape(2);

void compare_cpu_gpu(size_t size, float* output_cpu, float* output_gpu) {
  for (size_t i = 0; i < size; ++i) {
    if (output_cpu[i] > output_gpu[i] + 1e-3 ||
      output_cpu[i] < output_gpu[i] - 1e-3) {
      std::cout << "Index: " << i << ", CPU: " << output_cpu[i] <<
        ", GPU: " << output_gpu[i] << std::endl;
    }
  }
}

void multiply(size_t m, size_t n, size_t k, BLITZ_ALGORITHM algorithm, string trans) {
  bool left_trans, right_trans;
  // set shapes
  if (trans == "nn") {
    left_trans = false;
    right_trans = false;
    left_shape[0] = m;
    left_shape[1] = k;
    right_shape[0] = k;
    right_shape[1] = n;
    output_shape[0] = m;
    output_shape[1] = n;
  } else if (trans == "tn") {
    left_trans = true;
    right_trans = false;
    left_shape[0] = k;
    left_shape[1] = m;
    right_shape[0] = k;
    right_shape[1] = n;
    output_shape[0] = m;
    output_shape[1] = n;
  } else if (trans == "nt") {
    left_trans = false;
    right_trans = true;
    left_shape[0] = m;
    left_shape[1] = k;
    right_shape[0] = n;
    right_shape[1] = k;
    output_shape[0] = m;
    output_shape[1] = n;
  } else {
    std::cerr << "No such transform method!" << std::endl;
    exit(1);
  }
  // set up cpu
  CPUTensor<float> left_cpu(left_shape);
  CPUTensor<float> right_cpu(right_shape);
  CPUTensor<float> output_cpu(output_shape);
  // set up gpu
  GPUTensor<float> left_gpu(left_shape);
  GPUTensor<float> right_gpu(right_shape);
  GPUTensor<float> output_gpu(output_shape);
  CPUTensor<float> output_copy(output_shape);
  // init values
  Backend<CPUTensor, float>::UniformDistributionFunc(&left_cpu, 0.0, 1.0);
  Backend<CPUTensor, float>::UniformDistributionFunc(&right_cpu, 0.0, 1.0);
  cudaMemcpy(left_gpu.data(), left_cpu.data(),
    left_cpu.size() * sizeof(float), cudaMemcpyHostToDevice);
  cudaMemcpy(right_gpu.data(), right_cpu.data(),
    right_cpu.size() * sizeof(float), cudaMemcpyHostToDevice);
  // transpose
  Backend<CPUTensor, float>::MatrixMultiplyFunc(&left_cpu, &right_cpu, &output_cpu, left_trans, right_trans, 1.0, 0.0, algorithm);
  Backend<GPUTensor, float>::MatrixMultiplyFunc(&left_gpu, &right_gpu, &output_gpu, left_trans, right_trans, 1.0, 0.0, algorithm);
  // copy from gpu to cpu
  cudaMemcpy(output_copy.data(), output_gpu.data(),
    output_gpu.size() * sizeof(float), cudaMemcpyDeviceToHost);
  compare_cpu_gpu(output_cpu.size(), output_cpu.data(), output_copy.data());
}

int main(int argc, char** argv) {
  const size_t NUM_ARGS = 5;
  // M N K
  if (argc != NUM_ARGS + 1) {
    std::cerr << "Not matchable args!" << std::endl;
    exit(1);
  }
  std::string kernel = std::string(argv[1]);
  std::string trans = std::string(argv[2]);
  const size_t M = atoi(argv[3]);
  const size_t N = atoi(argv[4]);
  const size_t K = atoi(argv[5]);
  // run
  multiply(M, N, K, BlitzParseAlgorithm(kernel), trans);
  return 0;
}


// -----------------------------------------------------------------------------
// Filename:    Coord_Process.hpp
// Revision:    None
// Date:        2018/09/24 - 10:10
// Author:      Haixiang HOU
// Email:       hexid26@outlook.com
// Website:     [NULL]
// Notes:       [NULL]
// -----------------------------------------------------------------------------
// Copyright:   2018 (c) Haixiang
// License:     GPL
// -----------------------------------------------------------------------------
// Version [1.0]
// Calculate the coordinates of senders/receivers and sample points.

#include <boost/asio.hpp>
#include <boost/atomic.hpp>
#include <boost/thread.hpp>
#include <chrono>
#include <cmath>
#include <fstream>
#include <iostream>

#include "define.hpp"

class Coord_Process {
  private:
    boost::atomic_int saved_files;
    float dis_farest_point;                            // 成像点到圆心的距离
    unsigned long long pre_malloc_points_per_triangle; // 预先用来保存单个三角形点坐标的空间大小
    uint16_t point_sum_per_line;                       // 成像点每行的个数，图为正方形
    float coord_step;                                  // 当前成像分辨率下坐标标准方向上的步进
    void initialize(uint16_t num);
    void calc_ele_coords();
    void calc_all_triangles();
    inline float change_coord_to_int(float num, float step);
    void calc_sample_points_coords_by_ele_snd_id(uint16_t triangle_id);
    void save_sample_points_in_triangle(uint16_t ele_snd_id);

  public:
    float ele_coord_array_x[ELE_NO], ele_coord_array_y[ELE_NO]; //振元坐标
    float *triangle_vertex_x, *triangle_vertex_y;               // 所有三角形顶点坐标
    unsigned long long temp_sample_points_coords_cnt[ELE_NO];        //每个发射源扫描区的点数
    uint16_t **temp_sample_points_coords;              // 二维数组[ELE_NO][采样点数]
    void calc_all_sample_points_coords(int thread_sum);         // 多线程计算所有扫描区中，点的坐标
    void save_all_triangles(int thread_sum);                    // 多线程保存文件
    void print_ele_coords();
    void print_triangles_vertex();
    void print_sample_points_info();
    Coord_Process(uint16_t num);
    ~Coord_Process();
};

Coord_Process::Coord_Process(uint16_t num) { initialize(num); }

// num 是成像分辨率单维点数
void Coord_Process::initialize(uint16_t num) {
    dis_farest_point = RADIUS - 0.01; // 距离探头 10mm 的不成像
    point_sum_per_line = num;
    coord_step = IMAGE_WIDTH / (point_sum_per_line - 1);
    pre_malloc_points_per_triangle =
        (unsigned long long)(num * num * RADIUS * RADIUS * tan(PI / 18) / IMAGE_WIDTH / IMAGE_WIDTH);
    printf("Points per triangle: %llu\nPre malloc memory: %.2fGB\n", pre_malloc_points_per_triangle,
           pre_malloc_points_per_triangle * ELE_NO * 2 * 2 / 1024.0 / 1024.0 / 1024.0);
    calc_ele_coords();
    triangle_vertex_x = (float *)calloc(3 * sizeof(float), ELE_NO);
    triangle_vertex_y = (float *)calloc(3 * sizeof(float), ELE_NO);
    temp_sample_points_coords = new uint16_t *[ELE_NO];
    for (uint16_t ele_snd_id = 0; ele_snd_id < ELE_NO; ele_snd_id++) {
        temp_sample_points_coords[ele_snd_id] =
            (uint16_t *)calloc(2 * sizeof(uint16_t), pre_malloc_points_per_triangle);
    }
    calc_all_triangles();
}

Coord_Process::~Coord_Process() {
    free(triangle_vertex_x);
    free(triangle_vertex_y);
    for (uint16_t index = 0; index < point_sum_per_line; index++) {
        free(temp_sample_points_coords[index]);
    }
}

// 计算所有发射源的坐标
void Coord_Process::calc_ele_coords() {
    float raduis = RADIUS;
    float ele_angle_offset = (2 * PI * 43.4695 / (256 - 1)) / 360; //阵元间隔角度
    float start_ele_angle = 2 * PI * (45 - 43.4695) / 360;         //第一个阵元角度
    /* for (int start_ele_id = 0; start_ele_id < 256; start_ele_id++) {
        ele_coord_array_x[start_ele_id] =
            raduis * cos(start_ele_angle + start_ele_id * ele_angle_offset);
        ele_coord_array_y[start_ele_id] =
            -raduis * sin(start_ele_angle + start_ele_id * ele_angle_offset);
    } */
    // 把上面的 for 循环融入到下面会产生误差，但是上面的结果不符合可90度旋转原理
    for (int start_ele_id = 0; start_ele_id < ELE_NO; start_ele_id++) {
        ele_coord_array_x[start_ele_id] = raduis * cos(start_ele_angle + (start_ele_id % 256) * ele_angle_offset +
                                                       (int)(start_ele_id / 256) * PI / 4);
        ele_coord_array_y[start_ele_id] = -raduis * sin(start_ele_angle + (start_ele_id % 256) * ele_angle_offset +
                                                        (int)(start_ele_id / 256) * PI / 4);
    }
}

// 计算所有发射源扫描区(三角形)的三点坐标
void Coord_Process::calc_all_triangles() {
    float radius = RADIUS;
    float temp_point[3][2];   // 3个点，每个点2个坐标值
    uint16_t high_id, low_id; // 最高点和最低点的 id
    for (uint16_t start_ele_id = 0; start_ele_id < ELE_NO; start_ele_id++) {
        temp_point[0][0] = ele_coord_array_x[start_ele_id];
        temp_point[0][1] = ele_coord_array_y[start_ele_id];
        temp_point[1][0] = sin(PI / 18) * radius *
                           sqrtf(ele_coord_array_y[start_ele_id] * ele_coord_array_y[start_ele_id] / (radius * radius));
        temp_point[1][1] = -(temp_point[0][0] * temp_point[1][0]) / temp_point[0][1];
        temp_point[2][0] = -temp_point[1][0];
        temp_point[2][1] = -temp_point[1][1];
        // 排序

        for (uint16_t index = 0; index < 3; index++) {
            if (temp_point[index][1] == std::max(std::max(temp_point[0][1], temp_point[1][1]), temp_point[2][1])) {
                high_id = index;
            }
        }
        for (uint16_t index = 0; index < 3; index++) {
            if (temp_point[index][1] == std::min(std::min(temp_point[0][1], temp_point[1][1]), temp_point[2][1])) {
                low_id = index;
            }
        }

        triangle_vertex_x[3 * start_ele_id] = temp_point[high_id][0];
        triangle_vertex_y[3 * start_ele_id] = temp_point[high_id][1];
        triangle_vertex_x[3 * start_ele_id + 1] = temp_point[3 - high_id - low_id][0];
        triangle_vertex_y[3 * start_ele_id + 1] = temp_point[3 - high_id - low_id][1];
        triangle_vertex_x[3 * start_ele_id + 2] = temp_point[low_id][0];
        triangle_vertex_y[3 * start_ele_id + 2] = temp_point[low_id][1];
    }
    return;
}

// 把三角形三点坐标转移到整数空间
inline float Coord_Process::change_coord_to_int(float num, float step) {
    return (num + (point_sum_per_line - 1) / 2 * step) / step;
}

// 扫描单个三角形中的点
void Coord_Process::calc_sample_points_coords_by_ele_snd_id(uint16_t triangle_id) {
    float high_point_x = triangle_vertex_x[3 * triangle_id];
    float mid_point_x = triangle_vertex_x[3 * triangle_id + 1];
    float low_point_x = triangle_vertex_x[3 * triangle_id + 2];
    float high_point_y = triangle_vertex_y[3 * triangle_id];
    float mid_point_y = triangle_vertex_y[3 * triangle_id + 1];
    float low_point_y = triangle_vertex_y[3 * triangle_id + 2];
    high_point_x = change_coord_to_int(high_point_x, coord_step);
    mid_point_x = change_coord_to_int(mid_point_x, coord_step);
    low_point_x = change_coord_to_int(low_point_x, coord_step);
    high_point_y = change_coord_to_int(high_point_y, coord_step);
    mid_point_y = change_coord_to_int(mid_point_y, coord_step);
    low_point_y = change_coord_to_int(low_point_y, coord_step);

    unsigned long long sample_points_sum = 0;

    float temp1, temp2;
    int min_horizontal_id, max_horizontal_id;
    for (int row_id = std::max((int)(low_point_y + 1), 0); row_id <= std::min((int)mid_point_y, point_sum_per_line - 1);
         row_id++) {
        temp1 = ((float)row_id - low_point_y) * (mid_point_x - low_point_x) / (mid_point_y - low_point_y) + low_point_x;
        temp2 =
            ((float)row_id - low_point_y) * (high_point_x - low_point_x) / (high_point_y - low_point_y) + low_point_x;
        min_horizontal_id = (int)(std::min(temp1, temp2) + 1);
        max_horizontal_id = (int)(std::max(temp1, temp2));
        min_horizontal_id = std::max(min_horizontal_id, 0);
        min_horizontal_id = std::min(min_horizontal_id, point_sum_per_line - 1);
        max_horizontal_id = std::min(max_horizontal_id, point_sum_per_line - 1);
        max_horizontal_id = std::max(max_horizontal_id, 0);
        for (uint16_t index = min_horizontal_id; index <= max_horizontal_id; index++) {
            if (sqrtf(((float)index - point_sum_per_line / 2) * ((float)index - point_sum_per_line / 2) +
                      ((float)row_id - point_sum_per_line / 2) * ((float)row_id - point_sum_per_line / 2)) *
                    coord_step >
                dis_farest_point) {
                continue;
            }
            temp_sample_points_coords[triangle_id][2 * sample_points_sum] = (uint16_t)index;
            temp_sample_points_coords[triangle_id][2 * sample_points_sum + 1] = (uint16_t)row_id;
            sample_points_sum++;
        }
    }

    for (int row_id = std::max((int)(mid_point_y + 1), 0);
         row_id <= std::min((int)high_point_y, point_sum_per_line - 1); row_id++) {
        temp1 =
            ((float)row_id - high_point_y) * (high_point_x - mid_point_x) / (high_point_y - mid_point_y) + high_point_x;
        temp2 =
            ((float)row_id - low_point_y) * (high_point_x - low_point_x) / (high_point_y - low_point_y) + low_point_x;
        min_horizontal_id = (int)(std::min(temp1, temp2) + 1);
        max_horizontal_id = (int)(std::max(temp1, temp2));
        min_horizontal_id = std::max(min_horizontal_id, 0);
        min_horizontal_id = std::min(min_horizontal_id, point_sum_per_line - 1);
        max_horizontal_id = std::min(max_horizontal_id, point_sum_per_line - 1);
        max_horizontal_id = std::max(max_horizontal_id, 0);
        for (uint16_t index = min_horizontal_id; index <= max_horizontal_id; index++) {
            if (sqrtf(((float)index - point_sum_per_line / 2.0) * ((float)index - point_sum_per_line / 2.0) +
                      ((float)row_id - point_sum_per_line / 2.0) * ((float)row_id - point_sum_per_line / 2.0)) *
                    coord_step >
                dis_farest_point) {
                continue;
            }
            temp_sample_points_coords[triangle_id][2 * sample_points_sum] = (uint16_t)index;
            temp_sample_points_coords[triangle_id][2 * sample_points_sum + 1] = (uint16_t)row_id;
            sample_points_sum++;
        }
    }
    temp_sample_points_coords_cnt[triangle_id] = sample_points_sum;
}

// 多线程扫描所有的(2048)三角形，thread_sum 通过进程参数设置
void Coord_Process::calc_all_sample_points_coords(int thread_sum) {
    boost::asio::thread_pool thread_pool_calc(thread_sum);
    for (uint16_t ele_snd_id = 0; ele_snd_id < ELE_NO; ele_snd_id++) {
        boost::asio::post(thread_pool_calc,
                          boost::bind(&Coord_Process::calc_sample_points_coords_by_ele_snd_id, this, ele_snd_id));
    }
    thread_pool_calc.join();

    // for (uint16_t ele_snd_id = 0; ele_snd_id < ELE_NO; ele_snd_id++) {
    //     calc_sample_points_coords_by_ele_snd_id(ele_snd_id);
    // }
}

// 打印所有探头的坐标
void Coord_Process::print_ele_coords() {
    for (uint16_t index = 0; index < ELE_NO; index++) {
        printf("[%04d] = %10.8f, %10.8f; [%04d] = %10.8f, %10.8f; [%04d] = %10.8f, %10.8f; [%04d] = %10.8f, %10.8f;\n",
               index, ele_coord_array_x[index], ele_coord_array_y[index], index + ELE_NO / 4,
               ele_coord_array_x[index + ELE_NO / 4], ele_coord_array_y[index + ELE_NO / 4], index + ELE_NO * 2 / 4,
               ele_coord_array_x[index + ELE_NO * 2 / 4], ele_coord_array_y[index + ELE_NO * 2 / 4],
               index + ELE_NO * 3 / 4, ele_coord_array_x[index + ELE_NO * 3 / 4],
               ele_coord_array_y[index + ELE_NO * 3 / 4]);
    }
    return;
}

void Coord_Process::print_triangles_vertex() {
    for (uint16_t triangle_id = 0; triangle_id < ELE_NO; triangle_id++) {
        if (triangle_id % 64 == 0) {
            printf(
                "[[%10.8f, %10.8f, %10.8f, %10.8f],[%10.8f, %10.8f, %10.8f, "
                "%10.8f]]\n",
                triangle_vertex_x[3 * triangle_id], triangle_vertex_x[3 * triangle_id + 1],
                triangle_vertex_x[3 * triangle_id + 2], triangle_vertex_x[3 * triangle_id],
                triangle_vertex_y[3 * triangle_id], triangle_vertex_y[3 * triangle_id + 1],
                triangle_vertex_y[3 * triangle_id + 2], triangle_vertex_y[3 * triangle_id]);
        }
    }
}

void Coord_Process::print_sample_points_info() {
    unsigned long long point_sum = 0;
    for (uint16_t index = 0; index < ELE_NO; index++) {
        printf("Tri_%04u :: Sample point sum = %llu\n", index, temp_sample_points_coords_cnt[index]);
        point_sum += temp_sample_points_coords_cnt[index];
    }
}

void Coord_Process::save_sample_points_in_triangle(uint16_t ele_snd_id) {
    char *file_name = new char[17];
    sprintf(file_name, "Tri/Tri_%04d.txt", ele_snd_id);
    std::ofstream f_stream(file_name, std::fstream::out);
    std::string content = "";
    for (unsigned long long index = 0; index < temp_sample_points_coords_cnt[ele_snd_id]; index++) {
        content += std::to_string(temp_sample_points_coords[ele_snd_id][2 * index]) + "," +
                   std::to_string(temp_sample_points_coords[ele_snd_id][2 * index + 1]) + "\n";
    }
    f_stream << content;
    f_stream.close();
    saved_files++;
}

void Coord_Process::save_all_triangles(int thread_sum) {
    boost::asio::thread_pool thread_pool_save(40);
    saved_files = 0;
    for (uint16_t ele_snd_id = 0; ele_snd_id < ELE_NO; ele_snd_id++) {
        boost::asio::post(thread_pool_save,
                          boost::bind(&Coord_Process::save_sample_points_in_triangle, this, ele_snd_id));
    }
    while (saved_files < ELE_NO) {
        printf("\r%06.3f%%", (float)saved_files / ELE_NO * 100);
        fflush(stdout);
        sleep(1);
    }
    printf("\r%06.3f%%\n", (float)saved_files / ELE_NO * 100);
}


#include <chrono>
#include <iomanip>
#include <thread>
#include <eigen3/Eigen/Dense>

#include "common/plugin.hpp"
#include "common/switchboard.hpp"
#include "common/data_format.hpp"
#include "common/threadloop.hpp"

#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/navigation/AHRSFactor.h>
#include <gtsam/navigation/CombinedImuFactor.h>  // Used if IMU combined is off.
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/navigation/ImuFactor.h>
#include <boost/smart_ptr/shared_ptr.hpp>
#include <boost/smart_ptr/make_shared.hpp>

// IMU sample time to live in seconds
#define IMU_TTL 5

using PimUniquePtr = std::unique_ptr<gtsam::PreintegrationType>;
using ImuBias = gtsam::imuBias::ConstantBias;
using namespace ILLIXR;

typedef struct {
	double timestamp;
	Eigen::Matrix<double, 3, 1> wm;
	Eigen::Matrix<double, 3, 1> am;
} imu_type;

class imu_integrator : public threadloop {
public:
	imu_integrator(std::string name_, phonebook* pb_)
		: threadloop{name_, pb_}
		, sb{pb->lookup_impl<switchboard>()}
		, _m_imu_cam{sb->subscribe_latest<imu_cam_type>("imu_cam")}
		, _m_in{sb->subscribe_latest<imu_integrator_seq>("imu_integrator_seq")}
		, _m_imu_integrator_input{sb->subscribe_latest<imu_integrator_input>("imu_integrator_input")}
		, _m_imu_raw{sb->publish<imu_raw_type>("imu_raw")}
		, _seq_expect(1)
	{}

	virtual skip_option _p_should_skip() override {
		auto in = _m_in->get_latest_ro();
		if (!in || in->seq == _seq_expect-1) {
			// No new data, sleep to keep CPU utilization low
			std::this_thread::sleep_for(std::chrono::milliseconds{1});
			return skip_option::skip_and_yield;
		} else {
			if (in->seq != _seq_expect) {
				_stat_missed = in->seq - _seq_expect;
			} else {
				_stat_missed = 0;
			}
			_stat_processed++;
			_seq_expect = in->seq+1;
			return skip_option::run;
		}
	}

	void _p_one_iteration() override {
		const imu_cam_type *datum = _m_imu_cam->get_latest_ro();
		double timestamp_in_seconds = (double(datum->dataset_time) / NANO_SEC);

		imu_type data;
        data.timestamp = timestamp_in_seconds;
        data.wm = (datum->angular_v).cast<double>();
        data.am = (datum->linear_a).cast<double>();
		_imu_vec.emplace_back(data);

		clean_imu_vec(timestamp_in_seconds);
        propagate_imu_values(timestamp_in_seconds, datum->time);
	}

private:
	const std::shared_ptr<switchboard> sb;

	// IMU Data, Sequence Flag, and State Vars Needed
	std::unique_ptr<reader_latest<imu_cam_type>> _m_imu_cam;
	std::unique_ptr<reader_latest<imu_integrator_seq>> _m_in;
	std::unique_ptr<reader_latest<imu_integrator_input>> _m_imu_integrator_input;

	// Write IMU Biases for PP
	std::unique_ptr<writer<imu_raw_type>> _m_imu_raw;

	std::vector<imu_type> _imu_vec;
  	PimUniquePtr pim_ = NULL;

	[[maybe_unused]] double last_cam_time = 0;
	double last_imu_offset = 0;
	long long _seq_expect, _stat_processed, _stat_missed;

	// Remove IMU values older than 'IMU_TTL' from the imu buffer
	void clean_imu_vec(double timestamp) {
		auto imu_iterator = _imu_vec.begin();

		// Since the vector is ordered oldest to latest, keep deleting until you
		// hit a value less than 'IMU_TTL' seconds old
		while (imu_iterator != _imu_vec.end()) {
			if (timestamp-(*imu_iterator).timestamp < IMU_TTL) {
				break;
			}

			imu_iterator = _imu_vec.erase(imu_iterator);
		}
	}

	// Timestamp we are propagating the biases to (new IMU reading time)
	void propagate_imu_values(double timestamp, time_type real_time) {
		const imu_integrator_input *input_values = _m_imu_integrator_input->get_latest_ro();
		if (input_values == NULL) {
			return;
		}

		ImuBias imu_bias = ImuBias(input_values->biasAcc, input_values->biasGyro);
		if (pim_ == NULL) {
			boost::shared_ptr<gtsam::PreintegratedCombinedMeasurements::Params> params =
          			boost::make_shared<gtsam::PreintegratedCombinedMeasurements::Params>(input_values->params.n_gravity);

			params->setGyroscopeCovariance(std::pow(input_values->params.gyro_noise, 2.0) * Eigen::Matrix3d::Identity());
  			params->setAccelerometerCovariance(std::pow(input_values->params.acc_noise, 2.0) * Eigen::Matrix3d::Identity());
  			params->setIntegrationCovariance(std::pow(input_values->params.imu_integration_sigma, 2.0) * Eigen::Matrix3d::Identity());
			params->biasAccCovariance = std::pow(input_values->params.acc_walk, 2.0) * Eigen::Matrix3d::Identity();
			params->biasOmegaCovariance = std::pow(input_values->params.gyro_walk, 2.0) * Eigen::Matrix3d::Identity();

			pim_ = std::make_unique<gtsam::PreintegratedCombinedMeasurements>(params, imu_bias);
			last_imu_offset = input_values->t_offset;
		}

#ifndef NDEBUG
		if (input_values->last_cam_integration_time > last_cam_time) {
			std::cout << "New slow pose has arrived!\n";
			last_cam_time = input_values->last_cam_integration_time;
		}
#endif
		pim_->resetIntegrationAndSetBias(imu_bias);

		double time_begin = input_values->last_cam_integration_time + last_imu_offset;
		double time_end = timestamp + input_values->t_offset;

		std::vector<imu_type> prop_data = select_imu_readings(_imu_vec, time_begin, time_end);
		if (prop_data.size() < 2) {
			return;
		}

   		ImuBias prev_bias = pim_->biasHat();
		ImuBias bias = pim_->biasHat();

#ifndef NDEBUG
		std::cout << "Integrating over " << prop_data.size() << " IMU samples\n";
#endif

		for (unsigned i = 0; i < prop_data.size()-1; i++) {
			const gtsam::Vector3& measured_acc = prop_data.at(i).am;
			const gtsam::Vector3& measured_omega = prop_data.at(i).wm;

			// Delta T should be in seconds
			const double& delta_t = prop_data.at(i+1).timestamp - prop_data.at(i).timestamp;
			pim_->integrateMeasurement(measured_acc, measured_omega, delta_t);

			prev_bias = bias;
			bias = pim_->biasHat();
		}

		gtsam::NavState navstate_lkf(gtsam::Pose3(gtsam::Rot3(input_values->quat), input_values->position), input_values->velocity);
		gtsam::NavState navstate_k = pim_->predict(navstate_lkf, imu_bias);
		gtsam::Pose3 out_pose = navstate_k.pose();

#ifndef NDEBUG
		std::cout << "Base Position (x, y, z) = "
				<< input_values->position(0) << ", "
				<< input_values->position(1) << ", "
				<< input_values->position(2) << std::endl;

		std::cout << "New  Position (x, y, z) = "
				<< out_pose.x() << ", "
				<< out_pose.y() << ", "
				<< out_pose.z() << std::endl;
#endif

		_m_imu_raw->put(new imu_raw_type{
			prev_bias.gyroscope(),
			prev_bias.accelerometer(),
			bias.gyroscope(),
			bias.accelerometer(),
			out_pose.translation(), // Position
			navstate_k.velocity(), // Velocity
			out_pose.rotation().toQuaternion(), // Eigen Quat
			real_time
		});
	}

	// Select IMU readings based on timestamp similar to how OpenVINS selects IMU values to propagate
	std::vector<imu_type> select_imu_readings(const std::vector<imu_type>& imu_data, double time_begin, double time_end) {
		std::vector<imu_type> prop_data;
		if (imu_data.size() < 2) {
			return prop_data;
		}

		for (unsigned i = 0; i < imu_data.size()-1; i++) {

			// If time_begin comes inbetween two IMUs (A and B), interpolate A forward to time_begin
			if (imu_data.at(i+1).timestamp > time_begin && imu_data.at(i).timestamp < time_begin) {
				imu_type data = interpolate_imu(imu_data.at(i), imu_data.at(i+1), time_begin);
				prop_data.push_back(data);
				continue;
			}

			// IMU is within time_begin and time_end
			if (imu_data.at(i).timestamp >= time_begin && imu_data.at(i+1).timestamp <= time_end) {
				prop_data.push_back(imu_data.at(i));
				continue;
			}

			// IMU is past time_end
			if (imu_data.at(i+1).timestamp > time_end) {
				imu_type data = interpolate_imu(imu_data.at(i), imu_data.at(i+1), time_end);
				prop_data.push_back(data);
				break;
			}
		}

		// Loop through and ensure we do not have an zero dt values
		// This would cause the noise covariance to be Infinity
		for (size_t i = 0; i < prop_data.size()-1; i++) {
			if (std::abs(prop_data.at(i+1).timestamp-prop_data.at(i).timestamp) < 1e-12) {
				prop_data.erase(prop_data.begin()+i);
				i--;
			}
		}

		return prop_data;
	}

	// For when an integration time ever falls inbetween two imu measurements (modeled after OpenVINS)
	static imu_type interpolate_imu(const imu_type imu_1, imu_type imu_2, double timestamp) {
		imu_type data;
		data.timestamp = timestamp;

		double lambda = (timestamp - imu_1.timestamp) / (imu_2.timestamp - imu_1.timestamp);
		data.am = (1 - lambda) * imu_1.am + lambda * imu_2.am;
		data.wm = (1 - lambda) * imu_1.wm + lambda * imu_2.wm;

		return data;
	}
};

PLUGIN_MAIN(imu_integrator)


/**
 * @file snapshot_ensembles.cpp
 * @author Marcus Edel
 *
 * Test file for SGDR with snapshot ensembles.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/core/optimizers/sgdr/snapshot_ensembles.hpp>
#include <mlpack/core/optimizers/sgdr/snapshot_sgdr.hpp>
#include <mlpack/methods/logistic_regression/logistic_regression.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace std;
using namespace arma;
using namespace mlpack;
using namespace mlpack::optimization;

using namespace mlpack::distribution;
using namespace mlpack::regression;

BOOST_AUTO_TEST_SUITE(SnapshotEnsemblesTest);

/*
 * Test that the step size resets after a specified number of epochs.
 */
BOOST_AUTO_TEST_CASE(SnapshotEnsemblesResetTest)
{
  const double stepSize = 0.5;
  arma::mat iterate;

  // Now run cyclical decay policy with a couple of multiplicators and initial
  // restarts.
  for (size_t restart = 5; restart < 100; restart += 10)
  {
    for (size_t mult = 2; mult < 5; ++mult)
    {
      double epochStepSize = stepSize;

      SnapshotEnsembles snapshotEnsembles(restart,
          double(mult), stepSize, 1000, 2);

      snapshotEnsembles.EpochBatches() = 10 / (double)1000;
      // Create all restart epochs.
      arma::Col<size_t> nextRestart(1000 / 10 /  mult);
      nextRestart(0) = restart;
      for (size_t j = 1; j < nextRestart.n_elem; ++j)
        nextRestart(j) = nextRestart(j - 1) * mult;

      for (size_t i = 0; i < 1000; ++i)
      {
        snapshotEnsembles.Update(iterate, epochStepSize, iterate);
        if (i <= restart || arma::accu(arma::find(nextRestart == i)) > 0)
        {
          BOOST_CHECK_EQUAL(epochStepSize, stepSize);
        }
      }

      BOOST_CHECK_EQUAL(snapshotEnsembles.Snapshots().size(), 2);
    }
  }
}

/**
 * Run SGDR with snapshot ensembles on logistic regression and make sure the
 * results are acceptable.
 */
BOOST_AUTO_TEST_CASE(LogisticRegressionTest)
{
  // Generate a two-Gaussian dataset.
  GaussianDistribution g1(arma::vec("1.0 1.0 1.0"), arma::eye<arma::mat>(3, 3));
  GaussianDistribution g2(arma::vec("9.0 9.0 9.0"), arma::eye<arma::mat>(3, 3));

  arma::mat data(3, 1000);
  arma::Row<size_t> responses(1000);
  for (size_t i = 0; i < 500; ++i)
  {
    data.col(i) = g1.Random();
    responses[i] = 0;
  }
  for (size_t i = 500; i < 1000; ++i)
  {
    data.col(i) = g2.Random();
    responses[i] = 1;
  }

  // Shuffle the dataset.
  arma::uvec indices = arma::shuffle(arma::linspace<arma::uvec>(0,
      data.n_cols - 1, data.n_cols));
  arma::mat shuffledData(3, 1000);
  arma::Row<size_t> shuffledResponses(1000);
  for (size_t i = 0; i < data.n_cols; ++i)
  {
    shuffledData.col(i) = data.col(indices[i]);
    shuffledResponses[i] = responses[indices[i]];
  }

  // Create a test set.
  arma::mat testData(3, 1000);
  arma::Row<size_t> testResponses(1000);
  for (size_t i = 0; i < 500; ++i)
  {
    testData.col(i) = g1.Random();
    testResponses[i] = 0;
  }
  for (size_t i = 500; i < 1000; ++i)
  {
    testData.col(i) = g2.Random();
    testResponses[i] = 1;
  }

  // Now run SGDR with snapshot ensembles on a couple of batch sizes.
  for (size_t batchSize = 5; batchSize < 50; batchSize += 5)
  {
    SnapshotSGDR<> sgdr(50, 2.0, batchSize, 0.01, 10000, 1e-3);
    LogisticRegression<> lr(shuffledData, shuffledResponses, sgdr, 0.5);

    // Ensure that the error is close to zero.
    const double acc = lr.ComputeAccuracy(data, responses);
    BOOST_REQUIRE_CLOSE(acc, 100.0, 0.3); // 0.3% error tolerance.

    const double testAcc = lr.ComputeAccuracy(testData, testResponses);
    BOOST_REQUIRE_CLOSE(testAcc, 100.0, 0.6); // 0.6% error tolerance.
  }
}

BOOST_AUTO_TEST_SUITE_END();


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

#include <iostream>
#include <random>

#include <boost/program_options.hpp>

namespace po = boost::program_options;

using std::cerr;
using std::cout;
using std::endl;
using std::exception;
using std::mt19937;
using std::uniform_real_distribution;

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

        po::options_description graph_options{ "Graph options" };
        graph_options.add_options()
            ("seed",      po::value<int>(),              "Specify a random seed")
            ("directed",                                 "Generate a directed graph")
            ("loops",     po::value<double>(),           "Generate loops with this probability")
            ;
        display_options.add(graph_options);

        po::options_description all_options{ "All options" };
        all_options.add_options()
            ("vertices",          po::value<int>(),    "Specify the number of vertices")
            ("edge-probability",  po::value<double>(), "Specify the edge probability")
            ;

        all_options.add(display_options);

        po::positional_options_description positional_options;
        positional_options
            .add("vertices", 1)
            .add("edge-probability", 1)
            ;

        po::variables_map options_vars;
        po::store(po::command_line_parser(argc, argv)
                .options(all_options)
                .positional(positional_options)
                .run(), options_vars);
        po::notify(options_vars);

        /* --help? Show a message, and exit. */
        if (options_vars.count("help")) {
            cout << "Usage: " << argv[0] << " [options] number-of-vertices edge-probability" << endl;
            cout << endl;
            cout << display_options << endl;
            return EXIT_SUCCESS;
        }

        if (! options_vars.count("vertices") || ! options_vars.count("edge-probability")) {
            cout << "Usage: " << argv[0] << " [options] number-of-vertices edge-probability" << endl;
            return EXIT_FAILURE;
        }

        int seed = 0;
        if (options_vars.count("seed"))
            seed = options_vars["seed"].as<int>();

        int vertices = options_vars["vertices"].as<int>();
        double density = options_vars["edge-probability"].as<double>();
        double loops = options_vars.count("loops") ? options_vars["loops"].as<double>() : 0;

        bool directed = options_vars.count("directed");

        mt19937 rand;
        rand.seed(seed);
        uniform_real_distribution<double> dist(0.0, 1.0);

        for (int v = 0 ; v < vertices ; ++v) {
            cout << "v" << v << "," << endl;
            if (loops > dist(rand))
                cout << "v" << v << "," << "v" << v << endl;
            for (int w = (directed ? 0 : v + 1) ; w < vertices ; ++w)
                if (v != w && density > dist(rand))
                    cout << "v" << v << (directed ? ">" : ",") << "v" << w << endl;
        }

        return EXIT_SUCCESS;
    }
    catch (const po::error & e) {
        cerr << "Error: " << e.what() << endl;
        cerr << "Try " << argv[0] << " --help" << endl;
        return EXIT_FAILURE;
    }
    catch (const exception & e) {
        cerr << "Error: " << e.what() << endl;
        return EXIT_FAILURE;
    }
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/algorithms/transform.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/io/wkt/read.hpp>
#include <boost/geometry/strategies/strategies.hpp>

#include <boost/geometry/extensions/gis/latlong/point_ll.hpp>
#include <boost/geometry/extensions/gis/geographic/strategies/area_huiller_earth.hpp>
#include <boost/geometry/extensions/gis/projections/epsg.hpp>
#include <boost/geometry/extensions/gis/projections/parameters.hpp>
#include <boost/geometry/extensions/gis/projections/proj/sterea.hpp>

//#include <test_common/test_point.hpp>

template <typename PRJ, typename XY, typename LL>
void add_to_ring(PRJ const& prj, LL const& ll,
                 bg::model::ring<LL>& ring_ll,
                 bg::model::ring<XY>& ring_xy)
{
    ring_ll.push_back(ll);

    XY xy;
    prj.forward(ll, xy);
    ring_xy.push_back(xy);
}

template <typename XY, typename LL>
void test_area_polygon_ll(bool concave, bool hole, double perc)
{
    BOOST_ASSERT(! (concave && hole) );

    typedef typename bg::coordinate_type<LL>::type T;

    // Amsterdam, Rotterdam, The Hague, Utrecht,
    // these cities together are the city group "Randstad"

    LL a, r, h, u;
    // Amsterdam 52 22'23"N 4 53'32"E
    a.lat(bg::dms<bg::north, T>(52, 22, 23));
    a.lon(bg::dms<bg::east, T>(4, 53, 32));

    // Rotterdam 51 55'51"N 4 28'45"E
    r.lat(bg::dms<bg::north, T>(51, 55, 51));
    r.lon(bg::dms<bg::east, T>(4, 28, 45));

    // The hague: 52 4' 48" N, 4 18' 0" E
    h.lat(bg::dms<bg::north, T>(52, 4, 48));
    h.lon(bg::dms<bg::east, T>(4, 18, 0));

    // Utrecht
    u.lat(bg::dms<bg::north, T>(52, 5, 36));
    u.lon(bg::dms<bg::east, T>(5, 7, 10));

    // For checking calculated area, use the Dutch projection (RD), this is EPSG code 28992
    bg::projections::sterea_ellipsoid<LL, XY> dutch_prj(bg::projections::init(28992));

    // Add them in clockwise direction
    bg::model::polygon<LL> randstad;
    bg::model::polygon<XY> randstad_xy;
    add_to_ring(dutch_prj, a, randstad.outer(), randstad_xy.outer());
    add_to_ring(dutch_prj, u, randstad.outer(), randstad_xy.outer());

    // Concave case
    if (concave)
    {
        // Add the city "Alphen" to create a concave case
        // Alphen 52 7' 48" N, 4 39' 0" E
        LL alphen(
            bg::latitude<T>(bg::dms<bg::north, T>(52, 7, 48)),
            bg::longitude<T>(bg::dms<bg::east, T>(4, 39)));
        add_to_ring(dutch_prj, alphen, randstad.outer(), randstad_xy.outer());
    }

    add_to_ring(dutch_prj, r, randstad.outer(), randstad_xy.outer());
    add_to_ring(dutch_prj, h, randstad.outer(), randstad_xy.outer());
    add_to_ring(dutch_prj, a, randstad.outer(), randstad_xy.outer());

    // Hole case
    if (hole)
    {
        // Gouda 52 1' 12" N, 4 42' 0" E
        LL gouda(
            bg::latitude<T>(bg::dms<bg::north, T>(52, 1, 12)),
            bg::longitude<T>(bg::dms<bg::east, T>(4, 42)));
        // Woerden 52 5' 9" N, 4 53' 0" E
        LL woerden(
            bg::latitude<T>(bg::dms<bg::north, T>(52, 5, 9)),
            bg::longitude<T>(bg::dms<bg::east, T>(4, 53, 0)));
        // Uithoorn 52 13' 48" N, 4 49' 48" E
        LL uithoorn(bg::latitude<T>
            (bg::dms<bg::north, T>(52, 13, 48)),
            bg::longitude<T>(bg::dms<bg::east, T>(4, 49, 48)));
        // Alphen 52 7' 48" N, 4 39' 0" E
        LL alphen(bg::latitude<T>(
            bg::dms<bg::north, T>(52, 7, 48)),
            bg::longitude<T>(bg::dms<bg::east, T>(4, 39)));

        randstad.inners().resize(1);
        randstad_xy.inners().resize(1);

        typename bg::model::polygon<LL>::ring_type& ring = randstad.inners()[0];
        typename bg::model::polygon<XY>::ring_type& ring_xy = randstad_xy.inners()[0];

        // Add them in counter-clockwise direction (see map of the Netherlands)
        add_to_ring(dutch_prj, gouda, ring, ring_xy);
        add_to_ring(dutch_prj, woerden, ring, ring_xy);
        add_to_ring(dutch_prj, uithoorn, ring, ring_xy);
        add_to_ring(dutch_prj, alphen, ring, ring_xy);
        add_to_ring(dutch_prj, gouda, ring, ring_xy);
    }

    // Check the area in square KM
    static const double KM2 = 1.0e6;
    double d_ll = bg::area(randstad) / KM2;
    double d_xy = bg::area(randstad_xy) / KM2;

    BOOST_CHECK_CLOSE(d_ll, d_xy, 1.0);
    if (hole)
    {
        BOOST_CHECK_CLOSE(d_ll, 1148.210, perc);
        BOOST_CHECK_CLOSE(d_xy, 1151.573, perc);
    }
    else
    {
        BOOST_CHECK_CLOSE(d_ll, concave ? 977.786 : 1356.168, perc);
        BOOST_CHECK_CLOSE(d_xy, concave ? 980.658 : 1360.140, perc);

        // No hole: area of outer should be equal to area of ring
        double r_ll = bg::area(randstad.outer()) / KM2;
        double r_xy = bg::area(randstad_xy.outer()) / KM2;

        BOOST_CHECK_CLOSE(d_ll, r_ll, perc);
        BOOST_CHECK_CLOSE(d_xy, r_xy, perc);
    }

    // Calculate are using specified strategy, here with radius in KM
    // We then don't have to divide by KM*KM to get the same result
    bg::strategy::area::huiller<LL, long double> strategy(6372.8);
    d_ll = bg::area(randstad, strategy);
    BOOST_CHECK_CLOSE(d_ll, d_xy, 1.0);
}

template <typename T>
void test_latlong(double perc)
{
    test_area_polygon_ll<bg::model::d2::point_xy<T>, bg::model::ll::point<bg::degree, T> >(false, false, perc);

    // with concavities
    test_area_polygon_ll<bg::model::d2::point_xy<T>, bg::model::ll::point<bg::degree, T> >(true, false, perc);

    // with holes
    test_area_polygon_ll<bg::model::d2::point_xy<T>, bg::model::ll::point<bg::degree, T> >(false, true, perc);

}

int test_main(int, char* [])
{
    test_latlong<double>(0.01);
    //test_latlong<float>(0.3); // LL area calculations using projections differ

    return 0;
}


#pragma once

#include <Eigen/Dense>
#include <tinyply.h>
#include <vector>
#include <cstdint>

class TriangleMesh {
public:
	TriangleMesh(const char* path);

	void print_debug_info() const;

	void write_mesh_ply(const char* fileName, const std::vector<Eigen::Array3<uint8_t>>& colors = {}) const;

	void write_mesh_vertices_sequence_ply(const char* fileName) const;

	const std::vector<Eigen::Vector3f>& get_vertices() const {
		return m_vertices;
	}

	const std::vector<Eigen::Array3i>& get_faces() const {
		return m_faces;
	}

	void rearrange_vertices(const std::vector<uint32_t>& old2new);

	void sort_faces();

private:

	void parse_ply(const char* path);

	// Variables
	std::vector<Eigen::Vector3f> m_vertices;
	std::vector<Eigen::Array3i> m_faces;

};


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

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

#include <numeric>
#include <functional>
#include <vector>

#include <boost/compute/functional.hpp>
#include <boost/compute/lambda.hpp>
#include <boost/compute/system.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/algorithm/copy.hpp>
#include <boost/compute/algorithm/exclusive_scan.hpp>
#include <boost/compute/algorithm/inclusive_scan.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/iterator/counting_iterator.hpp>
#include <boost/compute/iterator/transform_iterator.hpp>

#include "check_macros.hpp"
#include "context_setup.hpp"

namespace bc = boost::compute;

BOOST_AUTO_TEST_CASE(inclusive_scan_int)
{
    int data[] = { 1, 2, 1, 2, 3 };
    bc::vector<int> vector(data, data + 5, queue);
    BOOST_CHECK_EQUAL(vector.size(), size_t(5));

    bc::vector<int> result(5, context);
    BOOST_CHECK_EQUAL(result.size(), size_t(5));

    // inclusive scan
    bc::inclusive_scan(vector.begin(), vector.end(), result.begin(), queue);
    CHECK_RANGE_EQUAL(int, 5, result, (1, 3, 4, 6, 9));

    // in-place inclusive scan
    CHECK_RANGE_EQUAL(int, 5, vector, (1, 2, 1, 2, 3));
    bc::inclusive_scan(vector.begin(), vector.end(), vector.begin(), queue);
    CHECK_RANGE_EQUAL(int, 5, vector, (1, 3, 4, 6, 9));
}

BOOST_AUTO_TEST_CASE(exclusive_scan_int)
{
    int data[] = { 1, 2, 1, 2, 3 };
    bc::vector<int> vector(data, data + 5, queue);
    BOOST_CHECK_EQUAL(vector.size(), size_t(5));

    bc::vector<int> result(5, context);
    BOOST_CHECK_EQUAL(vector.size(), size_t(5));

    // exclusive scan
    bc::exclusive_scan(vector.begin(), vector.end(), result.begin(), queue);
    CHECK_RANGE_EQUAL(int, 5, result, (0, 1, 3, 4, 6));

    // in-place exclusive scan
    CHECK_RANGE_EQUAL(int, 5, vector, (1, 2, 1, 2, 3));
    bc::exclusive_scan(vector.begin(), vector.end(), vector.begin(), queue);
    CHECK_RANGE_EQUAL(int, 5, vector, (0, 1, 3, 4, 6));
}

BOOST_AUTO_TEST_CASE(inclusive_scan_int2)
{
    using boost::compute::int2_;

    int data[] = { 1, 2,
                   3, 4,
                   5, 6,
                   7, 8,
                   9, 0 };

    boost::compute::vector<int2_> input(reinterpret_cast<int2_*>(data),
                                        reinterpret_cast<int2_*>(data) + 5,
                                        queue);
    BOOST_CHECK_EQUAL(input.size(), size_t(5));

    boost::compute::vector<int2_> output(5, context);
    boost::compute::inclusive_scan(input.begin(), input.end(), output.begin(),
                                   queue);
    CHECK_RANGE_EQUAL(
        int2_, 5, output,
        (int2_(1, 2), int2_(4, 6), int2_(9, 12), int2_(16, 20), int2_(25, 20))
    );
}

BOOST_AUTO_TEST_CASE(inclusive_scan_counting_iterator)
{
    bc::vector<int> result(10, context);
    bc::inclusive_scan(bc::make_counting_iterator(1),
                       bc::make_counting_iterator(11),
                       result.begin(), queue);
    CHECK_RANGE_EQUAL(int, 10, result, (1, 3, 6, 10, 15, 21, 28, 36, 45, 55));
}

BOOST_AUTO_TEST_CASE(exclusive_scan_counting_iterator)
{
    bc::vector<int> result(10, context);
    bc::exclusive_scan(bc::make_counting_iterator(1),
                       bc::make_counting_iterator(11),
                       result.begin(), queue);
    CHECK_RANGE_EQUAL(int, 10, result, (0, 1, 3, 6, 10, 15, 21, 28, 36, 45));
}

BOOST_AUTO_TEST_CASE(inclusive_scan_transform_iterator)
{
    float data[] = { 1.0f, 2.0f, 3.0f, 4.0f, 5.0f };
    bc::vector<float> input(data, data + 5, queue);
    bc::vector<float> output(5, context);

    // normal inclusive scan of the input
    bc::inclusive_scan(input.begin(), input.end(), output.begin(), queue);
    bc::system::finish();
    BOOST_CHECK_CLOSE(float(output[0]), 1.0f, 1e-4f);
    BOOST_CHECK_CLOSE(float(output[1]), 3.0f, 1e-4f);
    BOOST_CHECK_CLOSE(float(output[2]), 6.0f, 1e-4f);
    BOOST_CHECK_CLOSE(float(output[3]), 10.0f, 1e-4f);
    BOOST_CHECK_CLOSE(float(output[4]), 15.0f, 1e-4f);

    // inclusive scan of squares of the input
    using ::boost::compute::_1;

    bc::inclusive_scan(bc::make_transform_iterator(input.begin(), pown(_1, 2)),
                       bc::make_transform_iterator(input.end(), pown(_1, 2)),
                       output.begin(), queue);
    bc::system::finish();
    BOOST_CHECK_CLOSE(float(output[0]), 1.0f, 1e-4f);
    BOOST_CHECK_CLOSE(float(output[1]), 5.0f, 1e-4f);
    BOOST_CHECK_CLOSE(float(output[2]), 14.0f, 1e-4f);
    BOOST_CHECK_CLOSE(float(output[3]), 30.0f, 1e-4f);
    BOOST_CHECK_CLOSE(float(output[4]), 55.0f, 1e-4f);
}

BOOST_AUTO_TEST_CASE(inclusive_scan_doctest)
{
//! [inclusive_scan_int]
// setup input
int data[] = { 1, 2, 3, 4 };
boost::compute::vector<int> input(data, data + 4, queue);

// setup output
boost::compute::vector<int> output(4, context);

// scan values
boost::compute::inclusive_scan(
    input.begin(), input.end(), output.begin(), queue
);

// output = [ 1, 3, 6, 10 ]
//! [inclusive_scan_int]

    CHECK_RANGE_EQUAL(int, 4, output, (1, 3, 6, 10));
}

BOOST_AUTO_TEST_CASE(exclusive_scan_doctest)
{
//! [exclusive_scan_int]
// setup input
int data[] = { 1, 2, 3, 4 };
boost::compute::vector<int> input(data, data + 4, queue);

// setup output
boost::compute::vector<int> output(4, context);

// scan values
boost::compute::exclusive_scan(
    input.begin(), input.end(), output.begin(), queue
);

// output = [ 0, 1, 3, 6 ]
//! [exclusive_scan_int]

    CHECK_RANGE_EQUAL(int, 4, output, (0, 1, 3, 6));
}

BOOST_AUTO_TEST_CASE(inclusive_scan_int_multiplies)
{
//! [inclusive_scan_int_multiplies]
// setup input
int data[] = { 1, 2, 1, 2, 3 };
boost::compute::vector<int> input(data, data + 5, queue);

// setup output
boost::compute::vector<int> output(5, context);

// inclusive scan with multiplication
boost::compute::inclusive_scan(
    input.begin(), input.end(), output.begin(),
    boost::compute::multiplies<int>(), queue
);

// output = [1, 2, 2, 4, 12]
//! [inclusive_scan_int_multiplies]

    BOOST_CHECK_EQUAL(input.size(), size_t(5));
    BOOST_CHECK_EQUAL(output.size(), size_t(5));

    CHECK_RANGE_EQUAL(int, 5, output, (1, 2, 2, 4, 12));

    // in-place inclusive scan
    CHECK_RANGE_EQUAL(int, 5, input, (1, 2, 1, 2, 3));
    boost::compute::inclusive_scan(input.begin(), input.end(), input.begin(),
                                   boost::compute::multiplies<int>(), queue);
    CHECK_RANGE_EQUAL(int, 5, input, (1, 2, 2, 4, 12));
}

BOOST_AUTO_TEST_CASE(exclusive_scan_int_multiplies)
{
//! [exclusive_scan_int_multiplies]
// setup input
int data[] = { 1, 2, 1, 2, 3 };
boost::compute::vector<int> input(data, data + 5, queue);

// setup output
boost::compute::vector<int> output(5, context);

// exclusive_scan with multiplication
// initial value equals 10
boost::compute::exclusive_scan(
    input.begin(), input.end(), output.begin(),
    int(10), boost::compute::multiplies<int>(), queue
);

// output = [10, 10, 20, 20, 40]
//! [exclusive_scan_int_multiplies]

    BOOST_CHECK_EQUAL(input.size(), size_t(5));
    BOOST_CHECK_EQUAL(output.size(), size_t(5));

    CHECK_RANGE_EQUAL(int, 5, output, (10, 10, 20, 20, 40));

    // in-place exclusive scan
    CHECK_RANGE_EQUAL(int, 5, input, (1, 2, 1, 2, 3));
    bc::exclusive_scan(input.begin(), input.end(), input.begin(),
                       int(10), bc::multiplies<int>(), queue);
    CHECK_RANGE_EQUAL(int, 5, input, (10, 10, 20, 20, 40));
}

BOOST_AUTO_TEST_CASE(inclusive_scan_int_multiplies_long_vector)
{
    size_t size = 1000;
    bc::vector<int> device_vector(size, int(2), queue);
    BOOST_CHECK_EQUAL(device_vector.size(), size);
    bc::inclusive_scan(device_vector.begin(), device_vector.end(),
                       device_vector.begin(), bc::multiplies<int>(), queue);

    std::vector<int> host_vector(size, 2);
    BOOST_CHECK_EQUAL(host_vector.size(), size);
    bc::copy(device_vector.begin(), device_vector.end(),
             host_vector.begin(), queue);

    std::vector<int> test(size, 2);
    BOOST_CHECK_EQUAL(test.size(), size);
    std::partial_sum(test.begin(), test.end(),
                     test.begin(), std::multiplies<int>());

    BOOST_CHECK_EQUAL_COLLECTIONS(host_vector.begin(), host_vector.end(),
                                  test.begin(), test.end());
}

BOOST_AUTO_TEST_CASE(exclusive_scan_int_multiplies_long_vector)
{
    size_t size = 1000;
    bc::vector<int> device_vector(size, int(2), queue);
    BOOST_CHECK_EQUAL(device_vector.size(), size);
    bc::exclusive_scan(device_vector.begin(), device_vector.end(),
                       device_vector.begin(), int(10), bc::multiplies<int>(),
                       queue);

    std::vector<int> host_vector(size, 2);
    BOOST_CHECK_EQUAL(host_vector.size(), size);
    bc::copy(device_vector.begin(), device_vector.end(),
             host_vector.begin(), queue);

    std::vector<int> test(size, 2);
    BOOST_CHECK_EQUAL(test.size(), size);
    test[0] = 10;
    std::partial_sum(test.begin(), test.end(),
                     test.begin(), std::multiplies<int>());

    BOOST_CHECK_EQUAL_COLLECTIONS(host_vector.begin(), host_vector.end(),
                                  test.begin(), test.end());
}

BOOST_AUTO_TEST_CASE(inclusive_scan_int_custom_function)
{
    BOOST_COMPUTE_FUNCTION(int, multi, (int x, int y),
    {
        return x * y * 2;
    });

    int data[] = { 1, 2, 1, 2, 3 };
    bc::vector<int> vector(data, data + 5, queue);
    BOOST_CHECK_EQUAL(vector.size(), size_t(5));

    bc::vector<int> result(5, context);
    BOOST_CHECK_EQUAL(result.size(), size_t(5));

    // inclusive scan
    bc::inclusive_scan(vector.begin(), vector.end(), result.begin(),
                       multi, queue);
    CHECK_RANGE_EQUAL(int, 5, result, (1, 4, 8, 32, 192));

    // in-place inclusive scan
    CHECK_RANGE_EQUAL(int, 5, vector, (1, 2, 1, 2, 3));
    bc::inclusive_scan(vector.begin(), vector.end(), vector.begin(),
                       multi, queue);
    CHECK_RANGE_EQUAL(int, 5, vector, (1, 4, 8, 32, 192));
}

BOOST_AUTO_TEST_CASE(exclusive_scan_int_custom_function)
{
    BOOST_COMPUTE_FUNCTION(int, multi, (int x, int y),
    {
        return x * y * 2;
    });

    int data[] = { 1, 2, 1, 2, 3 };
    bc::vector<int> vector(data, data + 5, queue);
    BOOST_CHECK_EQUAL(vector.size(), size_t(5));

    bc::vector<int> result(5, context);
    BOOST_CHECK_EQUAL(result.size(), size_t(5));

    // exclusive_scan
    bc::exclusive_scan(vector.begin(), vector.end(), result.begin(),
                       int(1), multi, queue);
    CHECK_RANGE_EQUAL(int, 5, result, (1, 2, 8, 16, 64));

    // in-place exclusive scan
    CHECK_RANGE_EQUAL(int, 5, vector, (1, 2, 1, 2, 3));
    bc::exclusive_scan(vector.begin(), vector.end(), vector.begin(),
                       int(1), multi, queue);
    CHECK_RANGE_EQUAL(int, 5, vector, (1, 2, 8, 16, 64));
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * @ file avgvalboundary_main.cc
 * @ brief NPDE homework AvgValBoundary code
 * @ author Simon Meierhans
 * @ date 11.03.2019
 * @ copyright Developed at ETH Zurich
 */

#include <lf/assemble/assemble.h>
#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/utils/utils.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <iostream>
#include <memory>

#include "avgvalboundary.h"

/* SAM_LISTING_BEGIN_1 */
int main() {
  // read in mesh and set up finite element space
  auto mesh_factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::io::GmshReader reader(std::move(mesh_factory),
                            CURRENT_SOURCE_DIR "/../meshes/square.msh");
  auto mesh = reader.mesh();
  // obtain dofh for lagrangian finite element space
  auto fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh);
  const lf::assemble::DofHandler &dofh{fe_space->LocGlobMap()};
  // Solve test problem
  Eigen::VectorXd mu = AvgValBoundary::solveTestProblem(dofh);
  // compute H1 seminorm of the solution
  double h1s_norm = AvgValBoundary::compH1seminorm(dofh, mu);
  // compute boundary functional
  auto w = [](Eigen::Vector2d x) -> double { return 1.0; };
  double boundary_functional =
      AvgValBoundary::compBoundaryFunctional(dofh, mu, w);

  std::cout << "H1s-norm: " << h1s_norm << "\n";
  std::cout << "F: " << boundary_functional << "\n";

  //====================
  // Your code goes here
  //====================
  return 0;
}
/* SAM_LISTING_END_1 */


#ifndef _MST_HXX_
#define _MST_HXX_

#include <utility>
#include <boost/shared_array.hpp>

struct UnionFind {

    struct Entry {
        int parent;
        int depth;
        int size;
    };

    int N;
    boost::shared_array<Entry> partitions;

    UnionFind(int N = 0);

    int Find(int elem) const;
    int Depth(int elem) const { return partitions[Find(elem)].depth; }
    int ComponentSize(int elem) const { return partitions[Find(elem)].size; }
    void Merge(int e1, int e2);
    int Size() const { return N; }
};

inline UnionFind::UnionFind(int N)
    : N(N)
{
    partitions = boost::shared_array<Entry>(new Entry[N]);
    for (int i = 0; i < N; ++i) {
        partitions[i].parent = i;
        partitions[i].depth = 1;
        partitions[i].size = 1;
    }
}

inline int UnionFind::Find(int elem) const {
    int parent = partitions[elem].parent;
    while (parent != elem) {
        elem = parent;
        parent = partitions[elem].parent;
    }
    return elem;
}

inline void UnionFind::Merge(int e1, int e2) {
    int p1 = Find(e1);
    int p2 = Find(e2);
    if (p1 == p2)
        return;
    int d1 = partitions[p1].depth;
    int d2 = partitions[p2].depth;
    if (d1 < d2) {
        partitions[p1].parent = p2;
        partitions[p2].size += partitions[p1].size;
    } else if (d2 < d1) {
        partitions[p2].parent = p1;
        partitions[p1].size += partitions[p2].size;
    } else {
        partitions[p2].parent = p1;
        partitions[p1].size += partitions[p2].size;
        partitions[p1].depth++;
    }
}

#endif


//  (C) Copyright Eric Niebler 2005.
//  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)

// Test case for weighted_p_square_quantile.hpp

#include <cmath> // for std::exp()
#include <boost/random.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/accumulators/numeric/functional/vector.hpp>
#include <boost/accumulators/numeric/functional/complex.hpp>
#include <boost/accumulators/numeric/functional/valarray.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/weighted_p_square_quantile.hpp>

using namespace boost;
using namespace unit_test;
using namespace boost::accumulators;

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    typedef accumulator_set<double, stats<tag::weighted_p_square_quantile>, double> accumulator_t;

    // tolerance in %
    double epsilon = 1;

    // some random number generators
    double mu4 = -1.0;
    double mu5 = -1.0;
    double mu6 = 1.0;
    double mu7 = 1.0;
    boost::lagged_fibonacci607 rng;
    boost::normal_distribution<> mean_sigma4(mu4, 1);
    boost::normal_distribution<> mean_sigma5(mu5, 1);
    boost::normal_distribution<> mean_sigma6(mu6, 1);
    boost::normal_distribution<> mean_sigma7(mu7, 1);
    boost::variate_generator<boost::lagged_fibonacci607&, boost::normal_distribution<> > normal4(rng, mean_sigma4);
    boost::variate_generator<boost::lagged_fibonacci607&, boost::normal_distribution<> > normal5(rng, mean_sigma5);
    boost::variate_generator<boost::lagged_fibonacci607&, boost::normal_distribution<> > normal6(rng, mean_sigma6);
    boost::variate_generator<boost::lagged_fibonacci607&, boost::normal_distribution<> > normal7(rng, mean_sigma7);

    accumulator_t acc0(quantile_probability = 0.001);
    accumulator_t acc1(quantile_probability = 0.025);
    accumulator_t acc2(quantile_probability = 0.975);
    accumulator_t acc3(quantile_probability = 0.999);

    accumulator_t acc4(quantile_probability = 0.001);
    accumulator_t acc5(quantile_probability = 0.025);
    accumulator_t acc6(quantile_probability = 0.975);
    accumulator_t acc7(quantile_probability = 0.999);

    for (std::size_t i=0; i<100000; ++i)
    {
        double sample = rng();
        acc0(sample, weight = 1.);
        acc1(sample, weight = 1.);
        acc2(sample, weight = 1.);
        acc3(sample, weight = 1.);

        double sample4 = normal4();
        double sample5 = normal5();
        double sample6 = normal6();
        double sample7 = normal7();
        acc4(sample4, weight = std::exp(-mu4 * (sample4 - 0.5 * mu4)));
        acc5(sample5, weight = std::exp(-mu5 * (sample5 - 0.5 * mu5)));
        acc6(sample6, weight = std::exp(-mu6 * (sample6 - 0.5 * mu6)));
        acc7(sample7, weight = std::exp(-mu7 * (sample7 - 0.5 * mu7)));
    }
    // check for uniform distribution with weight = 1
    BOOST_CHECK_CLOSE( weighted_p_square_quantile(acc0), 0.001, 28 );
    BOOST_CHECK_CLOSE( weighted_p_square_quantile(acc1), 0.025, 5 );
    BOOST_CHECK_CLOSE( weighted_p_square_quantile(acc2), 0.975, epsilon );
    BOOST_CHECK_CLOSE( weighted_p_square_quantile(acc3), 0.999, epsilon );

    // check for shifted standard normal distribution ("importance sampling")
    BOOST_CHECK_CLOSE( weighted_p_square_quantile(acc4), -3.090232, epsilon );
    BOOST_CHECK_CLOSE( weighted_p_square_quantile(acc5), -1.959963, epsilon );
    BOOST_CHECK_CLOSE( weighted_p_square_quantile(acc6),  1.959963, epsilon );
    BOOST_CHECK_CLOSE( weighted_p_square_quantile(acc7),  3.090232, epsilon );
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("weighted_p_square_quantile test");

    test->add(BOOST_TEST_CASE(&test_stat));

    return test;
}


/**
 * ****************************************************************************
 * Copyright (c) 2015, Robert Lukierski.
 * 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 copyright holder 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 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.
 *
 * ****************************************************************************
 * Fitting things to data.
 * ****************************************************************************
 */

#ifndef VISIONCORE_MATH_FITTING_HPP
#define VISIONCORE_MATH_FITTING_HPP

#include <VisionCore/Platform.hpp>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#include <Eigen/SVD>
#include <Eigen/LU>

#include <VisionCore/Math/Statistics.hpp>
#include <VisionCore/Types/Hypersphere.hpp>

namespace vc
{

namespace math
{

/**
 * Fitting plane from 3D points.
 */
template<typename T>
class PlaneFitting
{
public:
    typedef MultivariateStats<Eigen::Matrix<T,3,1>> StatsT;
    typedef typename StatsT::VectorType VectorT;
    typedef typename StatsT::CovarianceType CovarianceMatrixT;
    typedef Eigen::Hyperplane<T,3> PlaneT;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    PlaneFitting() { reset(); }

    inline void reset() { stats.reset(); }
    inline std::size_t count() const { return stats.count(); }

    inline void operator()(const VectorT& x)
    {
        stats(x);
    }

    bool getPlane(PlaneT& p, T& curvature) const
    {
        return getPlane(stats, p, curvature);
    }

private:
    bool getPlane(const StatsT& ss, PlaneT& p, T& curvature) const
    {
        if(ss.count() < 3)
        {
            return false;
        }

        const VectorT mean_point = ss.mean();
        const CovarianceMatrixT cm = ss.covariance();

        getPlane(cm, mean_point, p, curvature);

        return true;
    }

    void getPlane(const CovarianceMatrixT& cm, const VectorT& mean_point, PlaneT& p, T& curvature) const
    {
        using Eigen::numext::abs;

        Eigen::SelfAdjointEigenSolver<CovarianceMatrixT> es(cm);

        const T eigen_value = es.eigenvalues()(0);
        const VectorT eigen_vector = es.eigenvectors().col(0);

        p.normal() = eigen_vector;

        T eig_sum = cm.coeff(0) + cm.coeff(4) + cm.coeff(8);
        if(eig_sum != T(0.0))
        {
            curvature = abs(eigen_value / eig_sum);
        }
        else
        {
            curvature = T(0.0);
        }

        // Hessian form (D = nc . p_plane (centroid here) + p)
        p.offset() = T(1.0) * eigen_vector.dot(mean_point);
    }

    math::MultivariateStats<VectorT> stats;
};

/**
 * Circle from 3 points.
 */
template<typename T>
EIGEN_DEVICE_FUNC static inline types::CircleT<T> circleFrom3Points(const Eigen::Matrix<T,2,1>& p1, const Eigen::Matrix<T,2,1>& p2, const Eigen::Matrix<T,2,1>& p3)
{
    T ma = (p2(1) - p1(1))/(p2(0) - p1(0));
    T mb = (p3(1) - p2(1))/(p3(0) - p2(0));

    T cx = (ma*mb * (p1(1) - p3(1)) + mb * (p1(0) + p2(0)) - ma * (p2(0) + p3(0)))/(2 * (mb - ma));
    T cy = (mb*p3(1)+(mb-ma)*p2(1)-ma*p1(1)+p3(0)-p1(0))/(2*(mb-ma));

    types::CircleT<T> ret;

    ret.coeff() << cx , cy;
    ret.radius() = ret(p1);

    return ret;
}

// ****************** AFFINE FROM 3D POINT CORRESPONDANCES ***********

/**
 * Establish transform from 3D corresponding points.
 */
template<typename T>
class TransformationFrom3DPointPairs
{
public:
    typedef Eigen::Matrix<T,3,1> VectorT;
    typedef Eigen::Matrix<T,3,3> MatrixT;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    inline void reset()
    {
        sample_count = 0;
        accumulated_weight_ = 0.0;
        mean1.fill(0);
        mean2.fill(0);
        covariance.fill(0);
    }

    inline std::size_t count() const { return sample_count; }

    inline void operator()(const VectorT& p1, const VectorT& p2, T weight = T(1.0))
    {
        if(weight == T(0.0))
            return;

        ++sample_count;
        accumulated_weight_ += weight;
        T alpha = weight/accumulated_weight_;

        VectorT diff1 = p1 - mean1, diff2 = p2 - mean2;
        covariance = (T(1.0) - alpha)*(covariance + alpha * (diff2 * diff1.transpose()));

        mean1 += alpha * (diff1);
        mean2 += alpha * (diff2);
    }

    inline Eigen::Affine3f getTransformation ()
    {
        Eigen::JacobiSVD<MatrixT> svd (covariance, Eigen::ComputeFullU | Eigen::ComputeFullV);
        const MatrixT& u = svd.matrixU(),& v = svd.matrixV();

        MatrixT s;
        s.setIdentity();
        if(u.determinant()*v.determinant() < T(0.0f))
        {
            s(2,2) = T(-1.0);
        }

        MatrixT r = u * s * v.transpose();
        VectorT t = mean2 - r*mean1;

        Eigen::Transform<T,3,Eigen::Affine> ret(Eigen::Matrix<T,4,4>::Zero());
        ret.block<3,3>(0,0) = r;
        ret.block<3,1>(0,3) = t;

        return ret;
    }
private:
    std::size_t sample_count;
    T accumulated_weight_;
    VectorT mean1, mean2;
    MatrixT covariance;
};

}

}

#endif // VISIONCORE_MATH_FITTING_HPP


/**
 * @file clempiricflux.cc
 * @brief NPDE exam problem summer 2019 "CLEmpiricFlux" code
 * @author Oliver Rietmann
 * @date 18.07.2019
 * @copyright Developed at ETH Zurich
 */

#include "clempiricflux.h"

#include <Eigen/Core>
#include <cassert>

namespace CLEmpiricFlux {

/**
 * @brief Bisection algorithm for root finding of an increasing function.
 *
 * @param g continuous changing sign in the interval [v, w]
 * @param v lower bound of the interval containg the root
 * @param w upper bound of the interval containg the root
 * @param tol error tolerance for stopping criterion
 * @return approximate root x in [v, w]
 */
/* SAM_LISTING_BEGIN_8 */
template <typename FUNCTOR>
double findRoots(double v, double w, FUNCTOR &&g, double tol = 1.0E-6) {
  double x = v;  // approximate root
  const double len = w - v;
  constexpr static const int maxN = 1000;
  double gv = g(v), gw = g(w);
  // Ensure that function changes sign
  assert(gv * gw <= 0);
  for (int N = 0; (std::abs(w - v) > tol * len) && N < maxN; N++) {
    x = (v + w) / 2.0;
    const double gx = g(x);
    if (gv * gx < 0.0) {
      // Sign change in left half of [v,w]
      w = x;
      gw = gx;
    } else {
      // Sign change in right half of [v,w]
      v = x;
      gv = gx;
    }
  }
  return x;
}
/* SAM_LISTING_END_8 */

GodunovFlux::GodunovFlux(const UniformCubicSpline &f) : _f(f){};

/* SAM_LISTING_BEGIN_9 */
double GodunovFlux::operator()(double v, double w) const {
  double result;
  //====================
  // Your code goes here
  //====================
  return result;
}

/* SAM_LISTING_END_9 */

}  // namespace CLEmpiricFlux


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Polyhedron_3.h>
#include <CGAL/Polygon_mesh_processing/compute_normal.h>
#include <boost/property_map/property_map.hpp>

#include <map>
// #include <CGAL/Unique_hash_map.h>
// #include <boost/unordered_map.hpp>

#include <iostream>
#include <fstream>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_3 Point;
typedef K::Vector_3 Vector;

typedef CGAL::Polyhedron_3<K> Polyhedron;
typedef boost::graph_traits<Polyhedron>::vertex_descriptor vertex_descriptor;
typedef boost::graph_traits<Polyhedron>::face_descriptor   face_descriptor;

int main(int argc, char* argv[])
{
  const char* filename = (argc > 1) ? argv[1] : "data/eight.off";
  std::ifstream input(filename);

  Polyhedron mesh;
  if (!input || !(input >> mesh) || mesh.is_empty()) {
    std::cerr << "Not a valid off file." << std::endl;
    return 1;
  }

  std::map<face_descriptor,Vector> fnormals;
  std::map<vertex_descriptor,Vector> vnormals;
  // Instead of std::map you may use std::unordered_map, boost::unordered_map
  // or CGAL::Unique_hash_map
  // CGAL::Unique_hash_map<face_descriptor,Vector> fnormals;
  // boost::unordered_map<vertex_descriptor,Vector> vnormals;

  CGAL::Polygon_mesh_processing::compute_normals(mesh,
                                                 boost::make_assoc_property_map(vnormals),
                                                 boost::make_assoc_property_map(fnormals));

  std::cout << "Face normals :" << std::endl;
  for(face_descriptor fd: faces(mesh)){
    std::cout << fnormals[fd] << std::endl;
  }
  std::cout << "Vertex normals :" << std::endl;
  for(vertex_descriptor vd: vertices(mesh)){
    std::cout << vnormals[vd] << std::endl;
  }
  return 0;
}


#define BOOST_TEST_MODULE eigen_cuda

#include "cudamatrix.hpp"
#include "cudapipeline.hpp"
#include <boost/test/unit_test.hpp>

using eigencuda::CudaMatrix;
using eigencuda::CudaPipeline;
using eigencuda::Index;

BOOST_AUTO_TEST_CASE(create_cudamatrix) {
  // Call the class to handle GPU resources
  CudaPipeline cp;

  // Call matrix multiplication GPU
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(2, 2);
  Eigen::MatrixXd B = Eigen::MatrixXd::Zero(3, 2);
  Eigen::MatrixXd X = Eigen::MatrixXd::Zero(3, 2);

  // Define matrices
  A << 1., 2., 3., 4.;
  B << 5., 6., 7., 8., 9., 10.;
  X << 23., 34., 31., 46., 39., 58.;

  // Copy matrix back and for to the GPU
  CudaMatrix cumatrix{B, cp.get_stream()};
  Eigen::MatrixXd tmp = cumatrix;
  Eigen::MatrixXd result = tmp * A;

  // Expected results
  BOOST_TEST(X.isApprox(result));
}

BOOST_AUTO_TEST_CASE(matrix_multiplication) {

  Index dim = 200;
  Eigen::MatrixXd A = Eigen::MatrixXd::Random(dim, dim);
  Eigen::MatrixXd B = Eigen::MatrixXd::Random(dim, dim);

  CudaPipeline cuda_pip;
  CudaMatrix cuma_A{A, cuda_pip.get_stream()};
  CudaMatrix cuma_B{B, cuda_pip.get_stream()};
  CudaMatrix cuma_C{dim, dim, cuda_pip.get_stream()};

  cuda_pip.gemm(cuma_A, cuma_B, cuma_C);

  Eigen::MatrixXd C = cuma_C;
  Eigen::MatrixXd result = A * B;

  BOOST_TEST(C.isApprox(result));
}

BOOST_AUTO_TEST_CASE(right_matrix_multiplication) {
  // Call the class to handle GPU resources
  CudaPipeline cuda_pip;

  // Call matrix multiplication GPU
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(2, 2);
  Eigen::MatrixXd B = Eigen::MatrixXd::Zero(3, 2);
  Eigen::MatrixXd C = Eigen::MatrixXd::Zero(3, 2);
  Eigen::MatrixXd D = Eigen::MatrixXd::Zero(3, 2);
  Eigen::MatrixXd X = Eigen::MatrixXd::Zero(3, 2);
  Eigen::MatrixXd Y = Eigen::MatrixXd::Zero(3, 2);
  Eigen::MatrixXd Z = Eigen::MatrixXd::Zero(3, 2);

  // Define matrices
  A << 1., 2., 3., 4.;
  B << 5., 6., 7., 8., 9., 10.;
  C << 9., 10., 11., 12., 13., 14.;
  D << 13., 14., 15., 16., 17., 18.;
  X << 23., 34., 31., 46., 39., 58.;
  Y << 39., 58., 47., 70., 55., 82.;
  Z << 55., 82., 63., 94., 71., 106.;

  std::vector<Eigen::MatrixXd> tensor{B, C, D};
  std::vector<Eigen::MatrixXd> results(3, Eigen::MatrixXd::Zero(3, 2));
  CudaMatrix cuma_A{A, cuda_pip.get_stream()};
  CudaMatrix cuma_B{3, 2, cuda_pip.get_stream()};
  CudaMatrix cuma_C{3, 2, cuda_pip.get_stream()};

  for (Index i = 0; i < 3; i++) {
    cuma_B.copy_to_gpu(tensor[i]);
    cuda_pip.gemm(cuma_B, cuma_A, cuma_C);
    results[i] = cuma_C;
  }
  // Expected results
  BOOST_TEST(X.isApprox(results[0]));
  BOOST_TEST(Y.isApprox(results[1]));
  BOOST_TEST(Z.isApprox(results[2]));
}

BOOST_AUTO_TEST_CASE(wrong_shape_cublas) {
  Eigen::MatrixXd A = Eigen::MatrixXd::Random(2, 2);
  Eigen::MatrixXd B = Eigen::MatrixXd::Random(5, 5);

  CudaPipeline cuda_pip;
  CudaMatrix cuma_A{A, cuda_pip.get_stream()};
  CudaMatrix cuma_B{B, cuda_pip.get_stream()};
  CudaMatrix cuma_C{2, 5, cuda_pip.get_stream()};

  BOOST_REQUIRE_THROW(cuda_pip.gemm(cuma_A, cuma_B, cuma_C),
                      std::runtime_error);
}


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

  @copyright 2016 NumScale SAS

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

/*!
  @ingroup group-constant
  @defgroup constant-Fact_10 Fact_10 (function template)

  Generates the @c 10! constant

  @headerref{<boost/simd/constant/fact_10.hpp>}

  @par Description

  1.  @code
      template<typename T> T Fact_10();
      @endcode

  2.  @code
      template<typename T> T Fact_10( boost::simd::as_<T> const& target );
      @endcode

  Generates a value of type @c T that evaluates to 10!.

  @par Parameters

  | Name                | Description                                                         |
  |--------------------:|:--------------------------------------------------------------------|
  | **target**          | a [placeholder](@ref type-as) value encapsulating the constant type |

  @par Return Value
  A value of type @c T that evaluates to @c T(3628800).

  @par Requirements
  - **T** models Value
**/

#include <boost/simd/constant/scalar/fact_10.hpp>
#include <boost/simd/constant/simd/fact_10.hpp>

#endif