#include <iostream>
#include <vector>

#include <boost/scope_exit.hpp>
#include <boost/program_options.hpp>

#include <amgcl/backend/builtin.hpp>
#include <amgcl/adapter/crs_tuple.hpp>
#include <amgcl/mpi/util.hpp>
#include <amgcl/mpi/distributed_matrix.hpp>
#include <amgcl/profiler.hpp>

#include "domain_partition.hpp"

namespace amgcl {
    profiler<> prof;
}

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

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

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

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

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

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

    // Read configuration from command line
    ptrdiff_t n = 128;

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

    desc.add_options()
        ("help,h", "show help")
        (
         "size,n",
         po::value<ptrdiff_t>(&n)->default_value(n),
         "domain size"
        )
        ;

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

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

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

    using amgcl::prof;

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

    std::vector<ptrdiff_t> domain = world.exclusive_sum(chunk);

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

    renumbering renum(part, domain);
    prof.toc("partition");

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

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

    ptr.push_back(0);

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

    for(ptrdiff_t k = lo[2]; k <= hi[2]; ++k) {
        for(ptrdiff_t j = lo[1]; j <= hi[1]; ++j) {
            for(ptrdiff_t i = lo[0]; i <= hi[0]; ++i) {
                if (k > 0)  {
                    col.push_back(renum(i,j,k-1));
                    val.push_back(-h2i);
                }

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

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

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

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

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

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

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

    typedef amgcl::backend::builtin<double>         Backend;
    typedef amgcl::mpi::distributed_matrix<Backend> Matrix;

    prof.tic("create distributed version");
    Matrix A(world, std::tie(chunk, ptr, col, val), chunk);
    prof.toc("create distributed version");

    prof.tic("distributed product");
    auto B = amgcl::mpi::product(A, A);
    prof.toc("distributed product");

    if (world.rank == 0) {
        if (world.size == 1) {
            typedef amgcl::backend::crs<double> matrix;
            matrix A(std::tie(chunk, ptr, col, val));
            prof.tic("openmp product");
            auto B = amgcl::backend::product(A, A);
            prof.toc("openmp product");
        }

        std::cout << prof << std::endl;
    }
}


/*
 This program is free software; you can redistribute it and/or modify it under
 the terms of the European Union Public Licence - EUPL v.1.1 as published by
 the European Commission.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE. See the European Union Public Licence - EUPL v.1.1
 for more details.

 You should have received a copy of the European Union Public Licence - EUPL v.1.1
 along with this program.

 Further information about the European Union Public Licence - EUPL v.1.1 can
 also be found on the world wide web at http://ec.europa.eu/idabc/eupl

*/

/*
 ------ Copyright (C) 2011 STA Steering Board (space.trajectory.analysis AT gmail.com) ----
*/
/*
------------------ Author: Guillermo Ortega  ----------------------------------------
 July 2011

 */

#include "serviceAngleUnit.h"

#include <Eigen/Core>

using namespace Eigen;

#include "QDebug"

DialogServiceAngleUnitFrame::DialogServiceAngleUnitFrame( QWidget * parent, Qt::WindowFlags f) : QFrame(parent,f)
{
	setupUi(this);
    angleUnitWidget = DialogServiceAngleUnitFrame::comboBoxAngleUnitsChoice;
    myPastUnits = 0;
    comboBoxAngleUnitsChoice->setCurrentIndex(myPastUnits);
}

DialogServiceAngleUnitFrame::~DialogServiceAngleUnitFrame()
{
}

// Index meaning is as follows:
// index = 0  is Degree
// index = 1  is radians

// Matrix coefficients as follows
// Deg->Deg  Rad->Deg
// Deg->Rad  Rad->Rad

static double angleConversionMatrixCoeffs[4] =
{1.0,            57.295779513,
 0.0174532925,   1.0};

static const Matrix<double, 2, 2> angleConversionMatrix(angleConversionMatrixCoeffs);

double DialogServiceAngleUnitFrame::convertAngle(int fromAngleUnit, int toAngleUnit, double distance)
{
    //qDebug() << distanceConversionMatrix(0,0) << distanceConversionMatrix(0,1)<< distanceConversionMatrix(0,2) << distanceConversionMatrix(0,3) << distanceConversionMatrix(0,4) << endl;
    double finalAngle = distance * angleConversionMatrix(fromAngleUnit, toAngleUnit);
    //qDebug() << fromAngleUnit << toAngleUnit << distanceConversionMatrix(fromAngleUnit, toAngleUnit) << distance << "--->" << finalAngle << endl;
    return finalAngle;
}

//// Sets the input distance, the output distance and the current index inside the method
void DialogServiceAngleUnitFrame::setInputAngle(double niceInputAngle)
{
    myPastAngle = niceInputAngle;
}

// Index meaning is as follows:
// index = 0  is Kilometers
// index = 1  is meters
// index = 2  is centi-meters
// index = 3  is mili-meters
// index = 4 is Astronomical Units
void DialogServiceAngleUnitFrame::on_comboBoxAngleUnitsChoice_currentIndexChanged(int myIndex)
{
    //qDebug() << myPastUnits << "==>" << myFutureUnits << "||||" << myPastAngle << "--->" << myFutureAngle << endl;
    myFutureUnits = myIndex;
    myFutureAngle = convertAngle(myPastUnits, myFutureUnits, myPastAngle);
    myPastAngle = myFutureAngle;
    myPastUnits = myFutureUnits;
    myRealAngleForXMLSchema = convertAngle (myPastUnits, 0, myFutureAngle);
}


#define BOOST_TEST_MODULE MyDLLTest
#include <iostream>
#include <boost/test/included/unit_test.hpp> //single-header
#include "MathLibrary.h" // project being tested

BOOST_AUTO_TEST_CASE(my_boost_test)
{
    // Initialize a Fibonacci relation sequence.
    fibonacci_init(1, 1);
    // Write out the sequence values until overflow.
    do {
        std::cout << fibonacci_index() << ": "
            << fibonacci_current() << std::endl;
    } while (fibonacci_next());
    // Report count of values written before overflow.
    std::cout << fibonacci_index() + 1 <<
        " Fibonacci sequence values fit in an " <<
        "unsigned 64-bit integer." << std::endl;
    BOOST_CHECK(92 == fibonacci_index());
}


#include "raycast.h"
#include <iostream>
#include <cmath>
#include <Eigen/Eigen>

int signum(int x) {
    return x == 0 ? 0 : x < 0 ? -1 : 1;
}

double mod(double value, double modulus) {
    return fmod(fmod(value, modulus) + modulus, modulus);
}

double intbound(double s, double ds) {
    // Find the smallest positive t such that s+t*ds is an integer.
    if (ds < 0) {
        return intbound(-s, -ds);
    } else {
        s = mod(s, 1);
        // problem is now s+t*ds = 1
        return (1 - s) / ds;
    }
}

bool RayIntersectsAABB(const Eigen::Vector3d &start, const Eigen::Vector3d &end, const Eigen::Vector3d &lb,
                       const Eigen::Vector3d &rt) {
    Eigen::Vector3d dir = (end - start).normalized();
    Eigen::Vector3d dirfrac(1.0f / dir.x(), 1.0f / dir.y(), 1.0f / dir.z());

    // r.dirs_ is unit dirs_ vector of ray
    // lb is the corner of AABB with minimal coordinates - left bottom, rt is maximal corner
    // start is origin of ray
    double t1 = (lb.x() - start.x()) * dirfrac.x();
    double t2 = (rt.x() - start.x()) * dirfrac.x();
    double t3 = (lb.y() - start.y()) * dirfrac.y();
    double t4 = (rt.y() - start.y()) * dirfrac.y();
    double t5 = (lb.z() - start.z()) * dirfrac.z();
    double t6 = (rt.z() - start.z()) * dirfrac.z();

    double tmin = fmax(fmax(fmin(t1, t2), fmin(t3, t4)), fmin(t5, t6));
    double tmax = fmin(fmin(fmax(t1, t2), fmax(t3, t4)), fmax(t5, t6));

    // if tmax < 0, ray (line) is intersecting AABB, but whole AABB is behing us
    if (tmax < 0) {
        return false;
    }

    // if tmin > tmax, ray doesn't intersect AABB
    if (tmin > tmax) {
        return false;
    }

    return true;
}

void Raycast(const Eigen::Vector3d &start, const Eigen::Vector3d &end,
             const Eigen::Vector3d &min, const Eigen::Vector3d &max,
             std::vector<Eigen::Vector3d> *output) {
//    std::cout << start << ' ' << end << std::endl;
    // From "A Fast Voxel Traversal Algorithm for Ray Tracing"
    // by John Amanatides and Andrew Woo, 1987
    // <http://www.cse.yorku.ca/~amana/research/grid.pdf>
    // <http://citeseer.ist.psu.edu/viewdoc/summary?doi=10.1.1.42.3443>
    // Extensions to the described algorithm:
    //   • Imposed a distance_ limit.
    //   • The face passed through to reach the current cube is provided to
    //     the callback.

    // The foundation of this algorithm is a parameterized representation of
    // the provided ray,
    //                    origin + t * dirs_,
    // except that t is not actually stored; rather, at any given point_ in the
    // traversal, we keep track of the *greater* t values which we would have
    // if we took a step sufficient to cross a cube boundary along that axis
    // (i.e. change the integer part of the coordinate) in the variables
    // tMaxX, tMaxY, and tMaxZ.

    // Cube containing origin point_.
    int x = (int) std::floor(start.x());
    int y = (int) std::floor(start.y());
    int z = (int) std::floor(start.z());
    int endX = (int) std::floor(end.x());
    int endY = (int) std::floor(end.y());
    int endZ = (int) std::floor(end.z());
    Eigen::Vector3d direction = (end - start);
    double maxDist = direction.squaredNorm();

    // Break out dirs_ vector.
    double dx = endX - x;
    double dy = endY - y;
    double dz = endZ - z;

    // Direction to increment x,y,z when stepping.
    int stepX = (int) signum((int) dx);
    int stepY = (int) signum((int) dy);
    int stepZ = (int) signum((int) dz);

    // See description above. The initial values depend on the fractional
    // part of the origin.
    double tMaxX = intbound(start.x(), dx);
    double tMaxY = intbound(start.y(), dy);
    double tMaxZ = intbound(start.z(), dz);

    // The change in t when taking a step (always positive).
    double tDeltaX = ((double) stepX) / dx;
    double tDeltaY = ((double) stepY) / dy;
    double tDeltaZ = ((double) stepZ) / dz;

    output->clear();

    // Avoids an infinite loop.
    if (stepX == 0 && stepY == 0 && stepZ == 0)
        return;

    double dist = 0;
    while (true) {

        if (x >= min.x() && x < max.x() &&
            y >= min.y() && y < max.y() &&
            z >= min.z() && z < max.z()) {
            output->push_back(Eigen::Vector3d(x, y, z));

            dist = (Eigen::Vector3d(x, y, z) - start).squaredNorm();

            if (dist > maxDist) return;

            if (output->size() > 1500) {
                std::cerr << "Error, too many racyast voxels." << std::endl;
                throw std::out_of_range("Too many RaycasMultithread voxels");
            }
        }

        if (x == endX && y == endY && z == endZ) break;

        // tMaxX stores the t-value at which we cross a cube boundary along the
        // X axis, and similarly for Y and Z. Therefore, choosing the least tMax
        // chooses the closest cube boundary. Only the first case of the four
        // has been commented in detail.
        if (tMaxX < tMaxY) {
            if (tMaxX < tMaxZ) {
                // Update which cube we are now in.
                x += stepX;
                // Adjust tMaxX to the next X-oriented boundary crossing.
                tMaxX += tDeltaX;
            } else {
                z += stepZ;
                tMaxZ += tDeltaZ;
            }
        } else {
            if (tMaxY < tMaxZ) {
                y += stepY;
                tMaxY += tDeltaY;
            } else {
                z += stepZ;
                tMaxZ += tDeltaZ;
            }
        }
    }
}


//
// Created by lirundong on 2019-04-28.
//

#ifndef POISSON_EDITING_INCLUDE_COMMON_HPP_
#define POISSON_EDITING_INCLUDE_COMMON_HPP_

#include <algorithm>
#include <iostream>
#include <vector>
#include <map>
#include <unordered_map>
#include <cstdint>
#include <cmath>

#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/Dense>
#include <Eigen/SparseCholesky>
#include <Eigen/PardisoSupport>

#define WITHIN(i, N) (0 <= (i) && (i) < (N))
#define CLAMP(x, lb, ub) (max((lb), min((x), (ub))))
#define TO_PIXEL(x) (static_cast<uint8_t>(CLAMP(x, 0., 255.)))

#ifdef EIGEN_USE_MKL_ALL
  #define EIGEN_SP_SOLVER Eigen::PardisoLDLT
#else
  #define EIGEN_SP_SOLVER Eigen::SimplicialLDLT
#endif

#define EIGEN_CHECK(exp, solver) do { \
  exp; \
  auto solver_info = solver.info(); \
  if (solver_info != Eigen::Success) { \
    auto err_info = EIGEN_COMPUTATION_ERROR.find(solver_info); \
    std::cerr << "`" #exp "` failed," << std::endl \
              << err_info->second << std::endl; \
    exit(-1); \
  } \
} while(0)

namespace poisson {

using std::max;
using std::min;
using std::vector;

using cv::Mat;
using cv::Size;
using cv::Rect;
using cv::Vec3b;
using cv::Vec3i;
using cv::Vec3d;
using cv::Point2i;
using cv::resize;
using Eigen::VectorXd;

using triplet = Eigen::Triplet<double>;
using triplets = std::vector<triplet>;
using spMat = Eigen::SparseMatrix<double>;
using vecMap = Eigen::Map<Eigen::VectorXd>;

static const std::unordered_map<Eigen::ComputationInfo, std::string>
    EIGEN_COMPUTATION_ERROR {
    {Eigen::NumericalIssue, "The provided data did not satisfy the "
                            "prerequisites"},
    {Eigen::NoConvergence,  "Iterative procedure did not converge"},
    {Eigen::InvalidInput,   "The inputs are invalid, or the algorithm has been "
                            "improperly called"},
};

static const double EPS = 1e-6;

template<typename T>
inline uint8_t real_to_pixel(const T value) {
  CV_Assert(0.0 <= value && value <= 1.0);
  return TO_PIXEL(value * 255.);
}

template<typename PointT>
inline decltype(auto) l2_dist(PointT &&p1, PointT &&p2) {
  auto diff = p2 - p1;
  return diff.x * diff.x + diff.y * diff.y;
}

template<typename PointT>
inline PointT find_nearest(const PointT &p, const vector<PointT> &border) {
  return std::min_element(border.cbegin(), border.cend(),
                          [&](PointT &&p1, PointT &&p2) -> bool {
                            return l2_dist(p1, p) < l2_dist(p2, p);
                          });
}

inline std::tuple<int, int> idx2yx(const int idx, const int W) {
  const int x = idx % W, y = idx / W;
  return {y, x};
}

inline int xy2idx(const int x, const int y, const int W) {
  return y * W + x;
}

inline int yx2idx(const int y, const int x, const int W) {
  return y * W + x;
}

inline Vec3d operator/(const Vec3d &lhs, const Vec3d &rhs) {
  return {lhs[0] / rhs[0], lhs[1] / rhs[1], lhs[2] / rhs[2]};
}

Mat fill_nearest(const Mat &img, const Mat &trimap, const int trimap_fore);

}

#endif //POISSON_EDITING_INCLUDE_COMMON_HPP_


/**
 * \file dh_key.cpp
 * \author Julien Kauffmann <julien.kauffmann@freelan.org>
 * \brief A DH key sample file.
 */

#include <cryptoplus/cryptoplus.hpp>
#include <cryptoplus/buffer.hpp>
#include <cryptoplus/pkey/dh_key.hpp>
#include <cryptoplus/hash/message_digest_context.hpp>
#include <cryptoplus/error/error_strings.hpp>

#include <boost/shared_ptr.hpp>

#include <iostream>
#include <string>
#include <cstdio>

#ifdef MSV
#include <openssl/applink.c>
#endif

using cryptoplus::buffer;

namespace
{
	int pem_passphrase_callback(char* buf, int buf_len, int rwflag, void*)
	{
		std::cout << "Passphrase (max: " << buf_len << " characters): " << std::flush;
		std::string passphrase;
		std::getline(std::cin, passphrase);

		if (passphrase.empty())
		{
			std::cerr << "Passphrase cannot be empty." << std::endl;
			return 0;
		}

		if (passphrase.size() > static_cast<size_t>(buf_len))
		{
			std::cerr << "Passphrase cannot exceed " << buf_len << " characters." << std::endl;
			return 0;
		}

		if (rwflag != 0)
		{
			std::cout << "Confirm: " << std::flush;
			std::string passphrase_confirmation;
			std::getline(std::cin, passphrase_confirmation);

			if (passphrase_confirmation != passphrase)
			{
				std::cerr << "The two passphrases do not match !" << std::endl;
				return 0;
			}
		}

		std::copy(passphrase.begin(), passphrase.end(), buf);
		return passphrase.size();
	}
}

int main()
{
	cryptoplus::crypto_initializer crypto_initializer;
	cryptoplus::algorithms_initializer algorithms_initializer;
	cryptoplus::error::error_strings_initializer error_strings_initializer;

	std::cout << "DH sample" << std::endl;
	std::cout << "=========" << std::endl;
	std::cout << std::endl;

	const int bits = 1024;
	const int generator = 2;

	std::cout << "Using DH keys of " << bits << " bits." << std::endl;

	const std::string parameters_filename = "parameters.pem";

	boost::shared_ptr<FILE> parameters_file(fopen(parameters_filename.c_str(), "w"), fclose);

	if (!parameters_file)
	{
		std::cerr << "Unable to open \"" << parameters_filename << "\" for writing." << std::endl;

		return EXIT_FAILURE;
	}

	try
	{
		std::cout << "Generating DH parameters. This can take some time..." << std::endl;

		cryptoplus::pkey::dh_key dh_key = cryptoplus::pkey::dh_key::generate_parameters(bits, generator);

		int codes = 0;

		dh_key.check(codes);

		if (codes != 0)
		{
			std::cerr << "Generation failed." << std::endl;

			if (codes & DH_CHECK_P_NOT_SAFE_PRIME)
			{
				std::cerr << "p is not a safe prime." << std::endl;
			}
			if (codes & DH_NOT_SUITABLE_GENERATOR)
			{
				std::cerr << "g is not a suitable generator." << std::endl;
			}

			if (codes & DH_UNABLE_TO_CHECK_GENERATOR)
			{
				std::cerr << "g is not a correct generator. Must be either 2 or 5." << std::endl;
			}

			return EXIT_FAILURE;
		}

		dh_key.write_parameters(parameters_file.get());

		std::cout << "DH parameters written succesfully to \"" << parameters_filename << "\"." << std::endl;
		std::cout << "Done." << std::endl;

		std::cout << "Generating DH key..." << std::endl;

		dh_key.generate_key();

		std::cout << "Done." << std::endl;

		parameters_file.reset(fopen(parameters_filename.c_str(), "r"), fclose);

		if (!parameters_file)
		{
			std::cerr << "Unable to open \"" << parameters_filename << "\" for reading." << std::endl;

			return EXIT_FAILURE;
		}

		std::cout << "Trying to read back the DH parameters from \"" << parameters_filename << "\"..." << std::endl;

		cryptoplus::pkey::dh_key dh_key2 = cryptoplus::pkey::dh_key::from_parameters(parameters_file.get(), pem_passphrase_callback);

		std::cout << "Done." << std::endl;

		std::cout << "Generating DH key..." << std::endl;

		dh_key2.generate_key();

		std::cout << "Done." << std::endl;

		std::cout << "Public key A: " << dh_key.public_key().to_dec() << std::endl;
		std::cout << "Public key B: " << dh_key2.public_key().to_dec() << std::endl;

		std::cout << "Computing key A..." << std::endl;

		buffer key_a = dh_key.compute_key(dh_key2.public_key());

		std::cout << "Done." << std::endl;

		std::cout << "Computing key B..." << std::endl;

		buffer key_b = dh_key2.compute_key(dh_key.public_key());

		std::cout << "Done." << std::endl;

		std::cout << "Comparing key A and key B: " << ((key_a == key_b) ? "IDENTICAL" : "DIFFERENT") << std::endl;
	}
	catch (std::exception& ex)
	{
		std::cerr << "Exception: " << ex.what() << std::endl;

		return EXIT_FAILURE;
	}

	return EXIT_SUCCESS;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TOOLBOX_TRIGONOMETRIC_FUNCTIONS_SCALAR_SINCPI_HPP_INCLUDED
#define NT2_TOOLBOX_TRIGONOMETRIC_FUNCTIONS_SCALAR_SINCPI_HPP_INCLUDED

#include <nt2/toolbox/trigonometric/functions/sincpi.hpp>
#include <nt2/include/functions/scalar/sinpi.hpp>
#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/include/functions/scalar/is_inf.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/invpi.hpp>
#include <boost/simd/sdk/config.hpp>

namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::sincpi_, tag::cpu_
                            , (A0)
                            , (scalar_< arithmetic_<A0> >)
                            )
  {

    typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;

    NT2_FUNCTOR_CALL(1)
    {
      return nt2::sincpi(result_type(a0));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is floating_
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::sincpi_, tag::cpu_
                            , (A0)
                            , (scalar_< floating_<A0> >)
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL(1)
    {
      #ifndef BOOST_SIMD_NO_INFINITIES
      if(nt2::is_inf(a0)) return nt2::Zero<A0>();
      #endif
      return (nt2::abs(a0) < nt2::Eps<A0>()) ? nt2::One<A0>() : nt2::Invpi<A0>()*nt2::sinpi(a0)/a0;
    }
  };
} }

#endif


#ifdef STAND_ALONE
#   define BOOST_TEST_MODULE RayTracerChallengeTests
#endif

#include <boost/test/unit_test.hpp>
#include <shared/Point.h>
#include <iostream>
#include <cmath>
#include <shared/Projectile.h>
#include <shared/Environment.h>
#include <shared/World.h>
#include "../src/shared/Tuple.h"
#include "shared/Vector.h"
#include "shared/Color.h"

BOOST_AUTO_TEST_SUITE(tuple_suite)

    BOOST_AUTO_TEST_CASE(point_test) {
        auto a = Tuple(4.3, -4.2, 3.1, 1.0);

        BOOST_CHECK_EQUAL(a.x, 4.3);
        BOOST_CHECK_EQUAL(a.y, -4.2);
        BOOST_CHECK_EQUAL(a.z, 3.1);
        BOOST_CHECK_EQUAL(a.w, 1.0);
        BOOST_CHECK_EQUAL(a.type, a.POINT);
        BOOST_CHECK_EQUAL(a.type, !a.VECTOR);
    }

    BOOST_AUTO_TEST_CASE(vector_test) {
        auto a = Tuple(4.3, -4.2, 3.1, 0);

        BOOST_CHECK_EQUAL(a.x, 4.3);
        BOOST_CHECK_EQUAL(a.y, -4.2);
        BOOST_CHECK_EQUAL(a.z, 3.1);
        BOOST_CHECK_EQUAL(a.w, 0.0);
        BOOST_CHECK_EQUAL(a.type, !a.POINT);
        BOOST_CHECK_EQUAL(a.type, a.VECTOR);
    }

    BOOST_AUTO_TEST_CASE(point_create_tuple_0_test) {
        auto a = Tuple(4, -3, 3, 0);
        auto b = Point(4, -3, 3);

        BOOST_TEST(a.equals(b));
    }

    BOOST_AUTO_TEST_CASE(vector_create_tuple_1_test) {
        auto a = Tuple(4, -3, 3, 1);
        auto b = Vector(4, -3, 3);

        BOOST_TEST(a.equals(b));
    }

    BOOST_AUTO_TEST_CASE(tuple_addition_test) {
        auto a1 = Tuple(3, -2, 5, 1);
        auto b1 = Tuple(-2, 3, 1, 0);
        auto c = a1.add(b1);

        BOOST_TEST(a1.add(b1).equals(Tuple(1, 1, 6, 1)));
    }

    BOOST_AUTO_TEST_CASE(point_subtraction_test) {
        auto p1 = Point(3, 2, 1);
        auto p2 = Point(5, 6, 7);
        auto c = p1.subtract(p2);

        BOOST_TEST(p1.subtract(p2).equals(Point(-2, -4, -6)));
    }

    BOOST_AUTO_TEST_CASE(vector_point_subtraction_test) {
        auto p1 = Point(3, 2, 1);
        auto p2 = Vector(5, 6, 7);
        auto c = p1.subtract(p2);

        BOOST_TEST(p1.subtract(p2).equals(Point(-2, -4, -6)));
    }

    BOOST_AUTO_TEST_CASE(vector_vector_subtraction_test) {
        auto p1 = Vector(3, 2, 1);
        auto p2 = Vector(5, 6, 7);
        auto c = p1.subtract(p2);
//        std::cout << c->x << std::endl;
//        std::cout << c->y << std::endl;
//        std::cout << c->z << std::endl;

        BOOST_TEST(p1.subtract(p2).equals(Vector(-2, -4, -6)));
    }

    BOOST_AUTO_TEST_CASE(tuple_negation_test) {
        auto a = Tuple(1, -2, 3, -4);

        BOOST_TEST(a.negate().equals(Tuple(-1, 2, -3, 4)));
    }

    BOOST_AUTO_TEST_CASE(multiplying_tuple_by_scalar_test) {
        auto a = Tuple(1, -2, 3, -4);
        auto b = a.multiply(3.5);

        BOOST_TEST(b.equals(Tuple(3.5, -7, 10.5, -14)));
    }

    BOOST_AUTO_TEST_CASE(multiplying_tuple_by_fraction_test) {
        auto a = Tuple(1, -2, 3, -4);
        auto b = a.multiply(0.5);

        BOOST_TEST(b.equals(Tuple(0.5, -1, 1.5, -2)));
    }

    BOOST_AUTO_TEST_CASE(divide_a_tuple_by_a_scaler_test) {
        auto a = Tuple(1, -2, 3, -4);
        auto b = a.divide(2);

        BOOST_TEST(b.equals(Tuple(0.5, -1, 1.5, -2)));
    }

    BOOST_AUTO_TEST_CASE(computing_the_magnitude_of_vector_1_0_0_test) {
        auto a = Vector(1, 0, 0);

        BOOST_CHECK_EQUAL(a.magnitude(), 1);
    }

    BOOST_AUTO_TEST_CASE(computing_the_magnitude_of_vector_m1_m20_m3_test) {
        auto a = Vector(-1, -2, -3);

        BOOST_CHECK_EQUAL(a.magnitude(), sqrt(14));
    }

    BOOST_AUTO_TEST_CASE(normalizing_vector_4_0_0_test) {
        auto a = Vector(4, 0, 0);

        BOOST_TEST(a.normalize().equals(Vector(1, 0, 0)));
    }

    BOOST_AUTO_TEST_CASE(dot_product_of_two_tuples_test) {
        auto a = Vector(1, 2, 3);
        auto b = Vector(2, 3, 4);

        BOOST_CHECK_EQUAL(a.dot(b), 20);
    }

    BOOST_AUTO_TEST_CASE(cross_product_of_two_tuples_test) {
        auto a = Vector(1, 2, 3);
        auto b = Vector(2, 3, 4);

        BOOST_TEST(a.cross(b).equals(Vector(-1, 2, -1)));
    }

    BOOST_AUTO_TEST_CASE(projectile_test) {
        auto projectile = Projectile(Point(0, 1, 0), Vector(1, 1, 0).normalize());
        auto e = Environment(Vector(0, -0.1, 0), Vector(0, -0.01, 0));
        auto w = World();
//        std::cout << "x " << projectile.position.x << " y  " << projectile.position.y << " z " << projectile.position.z << std::endl;
        int i=0;
        while(projectile.position.y>0 && i++<200){
//            std::cout <<"i " << i <<"x " << projectile.position.x << " y  " << projectile.position.y << " z " << projectile.position.z << std::endl;
            projectile=w.tick(e, projectile);
        }

       BOOST_TEST(projectile.position.y<=0);
    }

    BOOST_AUTO_TEST_CASE(colors_are_tuples_test) {
        auto a = Color(-0.5, 0.4, 1.7);

        BOOST_CHECK_EQUAL(a.red(), -0.5);
        BOOST_CHECK_EQUAL(a.green(), 0.4);
        BOOST_CHECK_EQUAL(a.blue(), 1.7);
    }

    BOOST_AUTO_TEST_CASE(adding_colors_test) {
        auto a = Color(0.9, 0.6, 0.75);
        auto b = Color(0.7, 0.1, 0.25);

        auto c=a+b;

        BOOST_TEST(c.equals(Color(1.6,0.7,1.0)));
    }

    BOOST_AUTO_TEST_CASE(subtract_colors_test) {
        auto a = Color(0.9, 0.6, 0.75);
        auto b = Color(0.7, 0.1, 0.25);

        auto c=a-b;

        BOOST_TEST(c.equals(Color(0.2,0.5,0.5)));
    }

    BOOST_AUTO_TEST_CASE(multiply_color_by_scalar_test) {
        auto a = Color(0.2, 0.3, 0.4);

        auto c=a*2;

        BOOST_TEST(c.equals(Color(0.4,0.6,0.8)));
    }

    BOOST_AUTO_TEST_CASE(multiply_colors_test) {
        auto a = Color(1,0.2,0.4);
        auto b = Color(0.9,1,0.1);

        auto c=a*b;

        BOOST_TEST(c.equals(Color(0.9,0.2,0.04)));
    }

BOOST_AUTO_TEST_SUITE_END()


// test_poisson.cpp

// Copyright Paul A. Bristow 2007.
// Copyright John Maddock 2006.

// 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)

// Basic sanity test for Poisson Cumulative Distribution Function.

#define BOOST_MATH_DISCRETE_QUANTILE_POLICY real

#if !defined(TEST_FLOAT) && !defined(TEST_DOUBLE) && !defined(TEST_LDOUBLE) && !defined(TEST_REAL_CONCEPT)
#  define TEST_FLOAT
#  define TEST_DOUBLE
#  define TEST_LDOUBLE
#  define TEST_REAL_CONCEPT
#endif

#ifdef _MSC_VER
#  pragma warning(disable: 4127) // conditional expression is constant.
#endif

#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp> // Boost.Test
#include <boost/test/floating_point_comparison.hpp>

#include <boost/math/concepts/real_concept.hpp> // for real_concept
#include <boost/math/distributions/poisson.hpp>
    using boost::math::poisson_distribution;
#include <boost/math/tools/test.hpp> // for real_concept

#include <boost/math/special_functions/gamma.hpp> // for (incomplete) gamma.
//   using boost::math::qamma_Q;
#include "table_type.hpp"
#include "test_out_of_range.hpp"

#include <iostream>
   using std::cout;
   using std::endl;
   using std::setprecision;
   using std::showpoint;
   using std::ios;
#include <limits>
  using std::numeric_limits;

template <class RealType> // Any floating-point type RealType.
void test_spots(RealType)
{
  // Basic sanity checks, tolerance is about numeric_limits<RealType>::digits10 decimal places,
   // guaranteed for type RealType, eg 6 for float, 15 for double,
   // expressed as a percentage (so -2) for BOOST_CHECK_CLOSE,

   int decdigits = numeric_limits<RealType>::digits10;
  // May eb >15 for 80 and 128-bit FP typtes.
  if (decdigits <= 0)
  { // decdigits is not defined, for example real concept,
    // so assume precision of most test data is double (for example, MathCAD).
     decdigits = numeric_limits<double>::digits10; // == 15 for 64-bit
  }
  if (decdigits > 15 ) // numeric_limits<double>::digits10)
  { // 15 is the accuracy of the MathCAD test data.
    decdigits = 15; // numeric_limits<double>::digits10;
  }

   decdigits -= 1; // Perhaps allow some decimal digit(s) margin of numerical error.
   RealType tolerance = static_cast<RealType>(std::pow(10., static_cast<double>(2-decdigits))); // 1e-6 (-2 so as %)
   tolerance *= 2; // Allow some bit(s) small margin (2 means + or - 1 bit) of numerical error.
   // Typically 2e-13% = 2e-15 as fraction for double.

   // Sources of spot test values:

  // Many be some combinations for which the result is 'exact',
  // or at least is good to 40 decimal digits.
   // 40 decimal digits includes 128-bit significand User Defined Floating-Point types,

   // Best source of accurate values is:
   // Mathworld online calculator (40 decimal digits precision, suitable for up to 128-bit significands)
   // http://functions.wolfram.com/webMathematica/FunctionEvaluation.jsp?name=GammaRegularized
   // GammaRegularized is same as gamma incomplete, gamma or gamma_q(a, x) or Q(a, z).

  // http://documents.wolfram.com/calculationcenter/v2/Functions/ListsMatrices/Statistics/PoissonDistribution.html

  // MathCAD defines ppois(k, lambda== mean) as k integer, k >=0.
  // ppois(0, 5) =  6.73794699908547e-3
  // ppois(1, 5) = 0.040427681994513;
  // ppois(10, 10) = 5.830397501929850E-001
  // ppois(10, 1) = 9.999999899522340E-001
  // ppois(5,5) = 0.615960654833065

  // qpois returns inverse Poission distribution, that is the smallest (floor) k so that ppois(k, lambda) >= p
  // p is real number, real mean lambda > 0
  // k is approximately the integer for which probability(X <= k) = p
  // when random variable X has the Poisson distribution with parameters lambda.
  // Uses discrete bisection.
  // qpois(6.73794699908547e-3, 5) = 1
  // qpois(0.040427681994513, 5) =

  // Test Poisson with spot values from MathCAD 'known good'.

  using boost::math::poisson_distribution;
  using  ::boost::math::poisson;
  using  ::boost::math::cdf;
  using  ::boost::math::pdf;

   // Check that bad arguments throw.
   BOOST_CHECK_THROW(
   cdf(poisson_distribution<RealType>(static_cast<RealType>(0)), // mean zero is bad.
      static_cast<RealType>(0)),  // even for a good k.
      std::domain_error); // Expected error to be thrown.

    BOOST_CHECK_THROW(
   cdf(poisson_distribution<RealType>(static_cast<RealType>(-1)), // mean negative is bad.
      static_cast<RealType>(0)),
      std::domain_error);

   BOOST_CHECK_THROW(
   cdf(poisson_distribution<RealType>(static_cast<RealType>(1)), // mean unit OK,
      static_cast<RealType>(-1)),  // but negative events is bad.
      std::domain_error);

  BOOST_CHECK_THROW(
     cdf(poisson_distribution<RealType>(static_cast<RealType>(0)), // mean zero is bad.
      static_cast<RealType>(99999)),  // for any k events.
      std::domain_error);

  BOOST_CHECK_THROW(
     cdf(poisson_distribution<RealType>(static_cast<RealType>(0)), // mean zero is bad.
      static_cast<RealType>(99999)),  // for any k events.
      std::domain_error);

  BOOST_CHECK_THROW(
     quantile(poisson_distribution<RealType>(static_cast<RealType>(0)), // mean zero.
      static_cast<RealType>(0.5)),  // probability OK.
      std::domain_error);

  BOOST_CHECK_THROW(
     quantile(poisson_distribution<RealType>(static_cast<RealType>(-1)),
      static_cast<RealType>(-1)),  // bad probability.
      std::domain_error);

  BOOST_CHECK_THROW(
     quantile(poisson_distribution<RealType>(static_cast<RealType>(1)),
      static_cast<RealType>(-1)),  // bad probability.
      std::domain_error);

  // Check some test values.

  BOOST_CHECK_CLOSE( // mode
     mode(poisson_distribution<RealType>(static_cast<RealType>(4))), // mode = mean = 4.
      static_cast<RealType>(4), // mode.
         tolerance);

  //BOOST_CHECK_CLOSE( // mode
  //   median(poisson_distribution<RealType>(static_cast<RealType>(4))), // mode = mean = 4.
  //    static_cast<RealType>(4), // mode.
      //   tolerance);
  poisson_distribution<RealType> dist4(static_cast<RealType>(40));

  BOOST_CHECK_CLOSE( // median
     median(dist4), // mode = mean = 4. median = 40.328333333333333
      quantile(dist4, static_cast<RealType>(0.5)), // 39.332839138842637
         tolerance);

  // PDF
  BOOST_CHECK_CLOSE(
     pdf(poisson_distribution<RealType>(static_cast<RealType>(4)), // mean 4.
      static_cast<RealType>(0)),
      static_cast<RealType>(1.831563888873410E-002), // probability.
         tolerance);

  BOOST_CHECK_CLOSE(
     pdf(poisson_distribution<RealType>(static_cast<RealType>(4)), // mean 4.
      static_cast<RealType>(2)),
      static_cast<RealType>(1.465251111098740E-001), // probability.
         tolerance);

  BOOST_CHECK_CLOSE(
     pdf(poisson_distribution<RealType>(static_cast<RealType>(20)), // mean big.
      static_cast<RealType>(1)),   //  k small
      static_cast<RealType>(4.122307244877130E-008), // probability.
         tolerance);

  BOOST_CHECK_CLOSE(
     pdf(poisson_distribution<RealType>(static_cast<RealType>(4)), // mean 4.
      static_cast<RealType>(20)),   //  K>> mean
      static_cast<RealType>(8.277463646553730E-009), // probability.
         tolerance);

  // CDF
  BOOST_CHECK_CLOSE(
     cdf(poisson_distribution<RealType>(static_cast<RealType>(1)), // mean unity.
      static_cast<RealType>(0)),  // zero k events.
      static_cast<RealType>(3.678794411714420E-1), // probability.
         tolerance);

  BOOST_CHECK_CLOSE(
     cdf(poisson_distribution<RealType>(static_cast<RealType>(1)), // mean unity.
      static_cast<RealType>(1)),  // one k event.
      static_cast<RealType>(7.357588823428830E-1), // probability.
         tolerance);

  BOOST_CHECK_CLOSE(
     cdf(poisson_distribution<RealType>(static_cast<RealType>(1)), // mean unity.
      static_cast<RealType>(2)),  // two k events.
      static_cast<RealType>(9.196986029286060E-1), // probability.
         tolerance);

  BOOST_CHECK_CLOSE(
     cdf(poisson_distribution<RealType>(static_cast<RealType>(1)), // mean unity.
      static_cast<RealType>(10)),  // two k events.
      static_cast<RealType>(9.999999899522340E-1), // probability.
         tolerance);

  BOOST_CHECK_CLOSE(
     cdf(poisson_distribution<RealType>(static_cast<RealType>(1)), // mean unity.
      static_cast<RealType>(15)),  // two k events.
      static_cast<RealType>(9.999999999999810E-1), // probability.
         tolerance);

  BOOST_CHECK_CLOSE(
     cdf(poisson_distribution<RealType>(static_cast<RealType>(1)), // mean unity.
      static_cast<RealType>(16)),  // two k events.
      static_cast<RealType>(9.999999999999990E-1), // probability.
         tolerance);

  BOOST_CHECK_CLOSE(
     cdf(poisson_distribution<RealType>(static_cast<RealType>(1)), // mean unity.
      static_cast<RealType>(17)),  // two k events.
      static_cast<RealType>(1.), // probability unity for double.
         tolerance);

  BOOST_CHECK_CLOSE(
     cdf(poisson_distribution<RealType>(static_cast<RealType>(1)), // mean unity.
      static_cast<RealType>(33)),  // k events at limit for float unchecked_factorial table.
      static_cast<RealType>(1.), // probability.
         tolerance);

  BOOST_CHECK_CLOSE(
     cdf(poisson_distribution<RealType>(static_cast<RealType>(100)), // mean 100.
      static_cast<RealType>(33)),  // k events at limit for float unchecked_factorial table.
      static_cast<RealType>(6.328271240363390E-15), // probability is tiny.
         tolerance * static_cast<RealType>(2e11)); // 6.3495253382825722e-015 MathCAD
      // Note that there two tiny probability are much more different.

   BOOST_CHECK_CLOSE(
     cdf(poisson_distribution<RealType>(static_cast<RealType>(100)), // mean 100.
      static_cast<RealType>(34)),  // k events at limit for float unchecked_factorial table.
      static_cast<RealType>(1.898481372109020E-14), // probability is tiny.
         tolerance*static_cast<RealType>(2e11)); //         1.8984813721090199e-014 MathCAD

 BOOST_CHECK_CLOSE(
     cdf(poisson_distribution<RealType>(static_cast<RealType>(33)), // mean = k
      static_cast<RealType>(33)),  // k events above limit for float unchecked_factorial table.
      static_cast<RealType>(5.461191812386560E-1), // probability.
         tolerance);

 BOOST_CHECK_CLOSE(
     cdf(poisson_distribution<RealType>(static_cast<RealType>(33)), // mean = k-1
      static_cast<RealType>(34)),  // k events above limit for float unchecked_factorial table.
      static_cast<RealType>(6.133535681502950E-1), // probability.
         tolerance);

 BOOST_CHECK_CLOSE(
     cdf(poisson_distribution<RealType>(static_cast<RealType>(1)), // mean unity.
      static_cast<RealType>(34)),  // k events above limit for float unchecked_factorial table.
      static_cast<RealType>(1.), // probability.
         tolerance);

  BOOST_CHECK_CLOSE(
     cdf(poisson_distribution<RealType>(static_cast<RealType>(5.)), // mean
      static_cast<RealType>(5)),  // k events.
      static_cast<RealType>(0.615960654833065), // probability.
         tolerance);
  BOOST_CHECK_CLOSE(
     cdf(poisson_distribution<RealType>(static_cast<RealType>(5.)), // mean
      static_cast<RealType>(1)),  // k events.
      static_cast<RealType>(0.040427681994512805), // probability.
         tolerance);

  BOOST_CHECK_CLOSE(
     cdf(poisson_distribution<RealType>(static_cast<RealType>(5.)), // mean
      static_cast<RealType>(0)),  // k events (uses special case formula, not gamma).
      static_cast<RealType>(0.006737946999085467), // probability.
         tolerance);

  BOOST_CHECK_CLOSE(
     cdf(poisson_distribution<RealType>(static_cast<RealType>(1.)), // mean
      static_cast<RealType>(0)),  // k events (uses special case formula, not gamma).
      static_cast<RealType>(0.36787944117144233), // probability.
         tolerance);

  BOOST_CHECK_CLOSE(
     cdf(poisson_distribution<RealType>(static_cast<RealType>(10.)), // mean
      static_cast<RealType>(10)),  // k events.
      static_cast<RealType>(0.5830397501929856), // probability.
         tolerance);

  BOOST_CHECK_CLOSE(
     cdf(poisson_distribution<RealType>(static_cast<RealType>(4.)), // mean
      static_cast<RealType>(5)),  // k events.
      static_cast<RealType>(0.785130387030406), // probability.
         tolerance);

  // complement CDF
  BOOST_CHECK_CLOSE( // Complement CDF
     cdf(complement(poisson_distribution<RealType>(static_cast<RealType>(4.)), // mean
      static_cast<RealType>(5))),  // k events.
      static_cast<RealType>(1 - 0.785130387030406), // probability.
         tolerance);

  BOOST_CHECK_CLOSE( // Complement CDF
     cdf(complement(poisson_distribution<RealType>(static_cast<RealType>(4.)), // mean
      static_cast<RealType>(0))),  // Zero k events (uses special case formula, not gamma).
      static_cast<RealType>(0.98168436111126578), // probability.
         tolerance);
  BOOST_CHECK_CLOSE( // Complement CDF
     cdf(complement(poisson_distribution<RealType>(static_cast<RealType>(1.)), // mean
      static_cast<RealType>(0))),  // Zero k events (uses special case formula, not gamma).
      static_cast<RealType>(0.63212055882855767), // probability.
         tolerance);

  // Example where k is bigger than max_factorial (>34 for float)
  // (therefore using log gamma so perhaps less accurate).
  BOOST_CHECK_CLOSE(
     cdf(poisson_distribution<RealType>(static_cast<RealType>(40.)), // mean
      static_cast<RealType>(40)),  // k events.
      static_cast<RealType>(0.5419181783625430), // probability.
         tolerance);

   // Quantile & complement.
  BOOST_CHECK_CLOSE(
    boost::math::quantile(
         poisson_distribution<RealType>(5),  // mean.
         static_cast<RealType>(0.615960654833065)),  //  probability.
         static_cast<RealType>(5.), // Expect k = 5
         tolerance/5); //

  // EQUAL is too optimistic - fails [5.0000000000000124 != 5]
  // BOOST_CHECK_EQUAL(boost::math::quantile( //
  //       poisson_distribution<RealType>(5.),  // mean.
  //       static_cast<RealType>(0.615960654833065)),  //  probability.
  //       static_cast<RealType>(5.)); // Expect k = 5 events.

  BOOST_CHECK_CLOSE(boost::math::quantile(
         poisson_distribution<RealType>(4),  // mean.
         static_cast<RealType>(0.785130387030406)),  //  probability.
         static_cast<RealType>(5.), // Expect k = 5 events.
         tolerance/5);

  // Check on quantile of other examples of inverse of cdf.
  BOOST_CHECK_CLOSE(
     cdf(poisson_distribution<RealType>(static_cast<RealType>(10.)), // mean
      static_cast<RealType>(10)),  // k events.
      static_cast<RealType>(0.5830397501929856), // probability.
         tolerance);

  BOOST_CHECK_CLOSE(boost::math::quantile( // inverse of cdf above.
         poisson_distribution<RealType>(10.),  // mean.
         static_cast<RealType>(0.5830397501929856)),  //  probability.
         static_cast<RealType>(10.), // Expect k = 10 events.
         tolerance/5);

  BOOST_CHECK_CLOSE(
     cdf(poisson_distribution<RealType>(static_cast<RealType>(4.)), // mean
      static_cast<RealType>(5)),  // k events.
      static_cast<RealType>(0.785130387030406), // probability.
         tolerance);

  BOOST_CHECK_CLOSE(boost::math::quantile( // inverse of cdf above.
         poisson_distribution<RealType>(4.),  // mean.
         static_cast<RealType>(0.785130387030406)),  //  probability.
         static_cast<RealType>(5.), // Expect k = 10 events.
         tolerance/5);

  //BOOST_CHECK_CLOSE(boost::math::quantile(
  //       poisson_distribution<RealType>(5),  // mean.
  //       static_cast<RealType>(0.785130387030406)),  //  probability.
  //        // 6.1882832344329559 result but MathCAD givest smallest integer ppois(k, mean) >= prob
  //       static_cast<RealType>(6.), // Expect k = 6 events.
  //       tolerance/5);

  //BOOST_CHECK_CLOSE(boost::math::quantile(
  //       poisson_distribution<RealType>(5),  // mean.
  //       static_cast<RealType>(0.77)),  //  probability.
  //        // 6.1882832344329559 result but MathCAD givest smallest integer ppois(k, mean) >= prob
  //       static_cast<RealType>(7.), // Expect k = 6 events.
  //       tolerance/5);

  //BOOST_CHECK_CLOSE(boost::math::quantile(
  //       poisson_distribution<RealType>(5),  // mean.
  //       static_cast<RealType>(0.75)),  //  probability.
  //        // 6.1882832344329559 result but MathCAD givest smallest integer ppois(k, mean) >= prob
  //       static_cast<RealType>(6.), // Expect k = 6 events.
  //       tolerance/5);

  BOOST_CHECK_CLOSE(
    boost::math::quantile(
         complement(
           poisson_distribution<RealType>(4),
           static_cast<RealType>(1 - 0.785130387030406))),  // complement.
           static_cast<RealType>(5), // Expect k = 5 events.
         tolerance/5);

  BOOST_CHECK_EQUAL(boost::math::quantile( // Check case when probability < cdf(0) (== pdf(0))
         poisson_distribution<RealType>(1),  // mean is small, so cdf and pdf(0) are about 0.35.
         static_cast<RealType>(0.0001)),  //  probability < cdf(0).
         static_cast<RealType>(0)); // Expect k = 0 events exactly.

  BOOST_CHECK_EQUAL(
    boost::math::quantile(
         complement(
           poisson_distribution<RealType>(1),
           static_cast<RealType>(0.9999))),  // complement, so 1-probability < cdf(0)
           static_cast<RealType>(0)); // Expect k = 0 events exactly.

  //
  // Test quantile policies against test data:
  //
#define T RealType
#include "poisson_quantile.ipp"

  for(unsigned i = 0; i < poisson_quantile_data.size(); ++i)
  {
     using namespace boost::math::policies;
     typedef policy<discrete_quantile<real> > P1;
     typedef policy<discrete_quantile<integer_round_down> > P2;
     typedef policy<discrete_quantile<integer_round_up> > P3;
     typedef policy<discrete_quantile<integer_round_outwards> > P4;
     typedef policy<discrete_quantile<integer_round_inwards> > P5;
     typedef policy<discrete_quantile<integer_round_nearest> > P6;
     RealType tol = boost::math::tools::epsilon<RealType>() * 20;
     if(!boost::is_floating_point<RealType>::value)
        tol *= 7;
     //
     // Check full real value first:
     //
     poisson_distribution<RealType, P1> p1(poisson_quantile_data[i][0]);
     RealType x = quantile(p1, poisson_quantile_data[i][1]);
     BOOST_CHECK_CLOSE_FRACTION(x, poisson_quantile_data[i][2], tol);
     x = quantile(complement(p1, poisson_quantile_data[i][1]));
     BOOST_CHECK_CLOSE_FRACTION(x, poisson_quantile_data[i][3], tol * 3);
     //
     // Now with round down to integer:
     //
     poisson_distribution<RealType, P2> p2(poisson_quantile_data[i][0]);
     x = quantile(p2, poisson_quantile_data[i][1]);
     BOOST_CHECK_EQUAL(x, floor(poisson_quantile_data[i][2]));
     x = quantile(complement(p2, poisson_quantile_data[i][1]));
     BOOST_CHECK_EQUAL(x, floor(poisson_quantile_data[i][3]));
     //
     // Now with round up to integer:
     //
     poisson_distribution<RealType, P3> p3(poisson_quantile_data[i][0]);
     x = quantile(p3, poisson_quantile_data[i][1]);
     BOOST_CHECK_EQUAL(x, ceil(poisson_quantile_data[i][2]));
     x = quantile(complement(p3, poisson_quantile_data[i][1]));
     BOOST_CHECK_EQUAL(x, ceil(poisson_quantile_data[i][3]));
     //
     // Now with round to integer "outside":
     //
     poisson_distribution<RealType, P4> p4(poisson_quantile_data[i][0]);
     x = quantile(p4, poisson_quantile_data[i][1]);
     BOOST_CHECK_EQUAL(x, poisson_quantile_data[i][1] < 0.5f ? floor(poisson_quantile_data[i][2]) : ceil(poisson_quantile_data[i][2]));
     x = quantile(complement(p4, poisson_quantile_data[i][1]));
     BOOST_CHECK_EQUAL(x, poisson_quantile_data[i][1] < 0.5f ? ceil(poisson_quantile_data[i][3]) : floor(poisson_quantile_data[i][3]));
     //
     // Now with round to integer "inside":
     //
     poisson_distribution<RealType, P5> p5(poisson_quantile_data[i][0]);
     x = quantile(p5, poisson_quantile_data[i][1]);
     BOOST_CHECK_EQUAL(x, poisson_quantile_data[i][1] < 0.5f ? ceil(poisson_quantile_data[i][2]) : floor(poisson_quantile_data[i][2]));
     x = quantile(complement(p5, poisson_quantile_data[i][1]));
     BOOST_CHECK_EQUAL(x, poisson_quantile_data[i][1] < 0.5f ? floor(poisson_quantile_data[i][3]) : ceil(poisson_quantile_data[i][3]));
     //
     // Now with round to nearest integer:
     //
     poisson_distribution<RealType, P6> p6(poisson_quantile_data[i][0]);
     x = quantile(p6, poisson_quantile_data[i][1]);
     BOOST_CHECK_EQUAL(x, floor(poisson_quantile_data[i][2] + 0.5f));
     x = quantile(complement(p6, poisson_quantile_data[i][1]));
     BOOST_CHECK_EQUAL(x, floor(poisson_quantile_data[i][3] + 0.5f));
  }
   check_out_of_range<poisson_distribution<RealType> >(1);
} // template <class RealType>void test_spots(RealType)

//

BOOST_AUTO_TEST_CASE( test_main )
{
  // Check that can construct normal distribution using the two convenience methods:
  using namespace boost::math;
  poisson myp1(2); // Using typedef
   poisson_distribution<> myp2(2); // Using default RealType double.

   // Basic sanity-check spot values.

  // Some plain double examples & tests:
  cout.precision(17); // double max_digits10
  cout.setf(ios::showpoint);

  poisson mypoisson(4.); // // mean = 4, default FP type is double.
  cout << "mean(mypoisson, 4.) == " << mean(mypoisson) << endl;
  cout << "mean(mypoisson, 0.) == " << mean(mypoisson) << endl;
  cout << "cdf(mypoisson, 2.) == " << cdf(mypoisson, 2.) << endl;
  cout << "pdf(mypoisson, 2.) == " << pdf(mypoisson, 2.) << endl;

  // poisson mydudpoisson(0.);
  // throws (if BOOST_MATH_DOMAIN_ERROR_POLICY == throw_on_error).

  BOOST_CHECK_THROW(poisson mydudpoisson(-1), std::domain_error);// Mean must be > 0.
  BOOST_CHECK_THROW(poisson mydudpoisson(-1), std::logic_error);// Mean must be > 0.
  // Passes the check because logic_error is a parent????
  // BOOST_CHECK_THROW(poisson mydudpoisson(-1), std::overflow_error); // fails the check
  // because overflow_error is unrelated - except from std::exception
  BOOST_CHECK_THROW(cdf(mypoisson, -1), std::domain_error); // k must be >= 0

  BOOST_CHECK_EQUAL(mean(mypoisson), 4.);
  BOOST_CHECK_CLOSE(
  pdf(mypoisson, 2.),  // k events = 2.
    1.465251111098740E-001, // probability.
      5e-13);

  BOOST_CHECK_CLOSE(
  cdf(mypoisson, 2.),  // k events = 2.
    0.238103305553545, // probability.
      5e-13);

#if 0
  // Compare cdf from finite sum of pdf and gamma_q.
  using boost::math::cdf;
  using boost::math::pdf;

  double mean = 4.;
  cout.precision(17); // double max_digits10
  cout.setf(ios::showpoint);
  cout << showpoint << endl;  // Ensure trailing zeros are shown.
  // This also helps show the expected precision max_digits10
  //cout.unsetf(ios::showpoint); // No trailing zeros are shown.

  cout << "k          pdf                     sum                  cdf                   diff" << endl;
  double sum = 0.;
  for (int i = 0; i <= 50; i++)
  {
   cout << i << ' ' ;
   double p =  pdf(poisson_distribution<double>(mean), static_cast<double>(i));
   sum += p;

   cout << p << ' ' << sum << ' '
   << cdf(poisson_distribution<double>(mean), static_cast<double>(i)) << ' ';
     {
       cout << boost::math::gamma_q<double>(i+1, mean); // cdf
       double diff = boost::math::gamma_q<double>(i+1, mean) - sum; // cdf -sum
       cout << setprecision (2) << ' ' << diff; // 0 0 to 4, 1 eps 5 to 9, 10 to 20 2 eps, 21 upwards 3 eps

     }
    BOOST_CHECK_CLOSE(
    cdf(mypoisson, static_cast<double>(i)),
      sum, // of pdfs.
      4e-14); // Fails at 2e-14
   // This call puts the precision etc back to default 6 !!!
   cout << setprecision(17) << showpoint;

     cout << endl;
  }

   cout << cdf(poisson_distribution<double>(5), static_cast<double>(0)) << ' ' << endl; // 0.006737946999085467
   cout << cdf(poisson_distribution<double>(5), static_cast<double>(1)) << ' ' << endl; // 0.040427681994512805
   cout << cdf(poisson_distribution<double>(2), static_cast<double>(3)) << ' ' << endl; // 0.85712346049854715

   { // Compare approximate formula in Wikipedia with quantile(half)
     for (int i = 1; i < 100; i++)
     {
       poisson_distribution<double> distn(static_cast<double>(i));
       cout << i << ' ' << median(distn) << ' ' << quantile(distn, 0.5) << ' '
         << median(distn) - quantile(distn, 0.5) << endl; // formula appears to be out-by-one??
     }  // so quantile(half) used via derived accressors.
   }
#endif

   // (Parameter value, arbitrarily zero, only communicates the floating-point type).
#ifdef TEST_POISSON
  test_spots(0.0F); // Test float.
#endif
#ifdef TEST_DOUBLE
  test_spots(0.0); // Test double.
#endif
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
  if (numeric_limits<long double>::digits10 > numeric_limits<double>::digits10)
  { // long double is better than double (so not MSVC where they are same).
#ifdef TEST_LDOUBLE
     test_spots(0.0L); // Test long double.
#endif
  }

#ifndef BOOST_MATH_NO_REAL_CONCEPT_TESTS
#ifdef TEST_REAL_CONCEPT
  test_spots(boost::math::concepts::real_concept(0.)); // Test real concept.
#endif
#endif
#endif

} // BOOST_AUTO_TEST_CASE( test_main )

/*

Output:

Autorun "i:\boost-06-05-03-1300\libs\math\test\Math_test\debug\test_poisson.exe"
Running 1 test case...
mean(mypoisson, 4.) == 4.0000000000000000
mean(mypoisson, 0.) == 4.0000000000000000
cdf(mypoisson, 2.) == 0.23810330555354431
pdf(mypoisson, 2.) == 0.14652511110987343
*** No errors detected

*/


#include <Eigen/Sparse>
#include <vector>
#include <cmath>
#include <iostream>
#include "day05.hpp"

typedef Eigen::SparseMatrix<int> SpMat;

void add_line_to_grid(std::vector<int> &data_row, SpMat &grid, bool diagonal=true) {
    int i, j, start, end;

    int step_horiz = data_row[2] - data_row[0];
    int step_verti = data_row[3] - data_row[1];
    int sign_horiz = sign(step_horiz);
    int sign_verti = sign(step_verti);

    if ((!diagonal) && !((step_horiz == 0) || (step_verti == 0)))
        return;

    if (((step_horiz != 0) && (step_verti != 0)) &&
            (std::abs(step_horiz) != std::abs(step_verti))) {
        std::cout << "Horiz step: " << step_horiz;
        std::cout << "  Vertical step: " << step_verti << std::endl;
        throw std::runtime_error("APW: Your assumption isn't right!");
    }

    int step = std::max(std::abs(step_horiz), std::abs(step_verti));
    for (int k=0; k < (step+1); k++) {
        i = sign_horiz * k * (step_horiz != 0) + data_row[0];
        j = sign_verti * k * (step_verti != 0) + data_row[1];
        grid.coeffRef(i, j) += 1;
    }

}

int main(int argc, char** argv) {
    std::cout << "---------------- Day 05 ----------------" << std::endl;

    if (argc != 2)
        throw std::runtime_error(
            "Invalid number of command-line arguments: Pass only the path to the data file");

    std::string datafile_path(argv[1]);
    auto data = parse_file(datafile_path);

    int grid_size = max(data) + 1;
    // print_vector2d<int>(data);
    std::cout << "grid size: " << grid_size << std::endl;

    int num_2s;
    SpMat grid_part1(grid_size, grid_size);
    SpMat grid_part2(grid_size, grid_size);

    // Part 1
    for (auto row : data)
        add_line_to_grid(row, grid_part1, false);

    // std::cout << grid_part1 << std::endl;

    num_2s = 0;
    for (int i=0; i < grid_size; i++)
        for (int j=0; j < grid_size; j++)
            num_2s += (grid_part1.coeff(i, j) >= 2);
    std::cout << "Part 1 answer: " << num_2s << std::endl;

    std::cout << "--------" << std::endl;

    // Part 2
    for (auto row : data)
        add_line_to_grid(row, grid_part2, true);

    // std::cout << grid_part2 << std::endl;

    num_2s = 0;
    for (int i=0; i < grid_size; i++)
        for (int j=0; j < grid_size; j++)
            num_2s += (grid_part2.coeff(i, j) >= 2);
    std::cout << "Part 2 answer: " << num_2s << std::endl;

}


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */

#include <Utils/Math/BSplines/BSpline.h>
#include <Utils/Math/BSplines/Splitter.h>
#include <gmock/gmock.h>
#include <Eigen/Core>

using namespace testing;
namespace Scine {
namespace Utils {
using namespace BSplines;
namespace Tests {

class ABSplineSplitterTest : public Test {
 public:
  unsigned p;
  BSpline bs, bsLeft, bsRight;
  Splitter bsSplitter;

  void SetUp() override {
  }
};

TEST_F(ABSplineSplitterTest, DisconnectednessDegree3) {
  unsigned p = 3;
  Eigen::MatrixXd controlPoints(5, 1);
  controlPoints << 1, 2, 3, 4, 5;

  Eigen::MatrixXd knotVector(9, 1);
  knotVector << 0, 0, 0, 0, 0.5, 1, 1, 1, 1;

  BSpline bs = BSpline(knotVector, controlPoints, p);

  double uSplit = 0.5;
  auto splitResult = bsSplitter.split(uSplit, bs, {false, false});

  bsLeft = splitResult.first;
  bsRight = splitResult.second;

  Eigen::VectorXd knotVectorLeft = bsLeft.getKnotVector();
  Eigen::MatrixXd controlPointsLeft = bsLeft.getControlPointMatrix();

  Eigen::VectorXd knotVectorRight = bsRight.getKnotVector();
  Eigen::MatrixXd controlPointsRight = bsRight.getControlPointMatrix();

  Eigen::VectorXd refKnotVectorLeft;
  refKnotVectorLeft.resize(8);
  refKnotVectorLeft << 0, 0, 0, 0, 0.5, 0.5, 0.5, 0.5;
  ASSERT_TRUE(knotVectorLeft.isApprox(refKnotVectorLeft));

  Eigen::VectorXd refControlPointsLeft;
  refControlPointsLeft.resize(4);
  refControlPointsLeft << 1, 2, 2.5, 3;
  ASSERT_TRUE(controlPointsLeft.isApprox(refControlPointsLeft));

  Eigen::VectorXd refKnotVectorRight;
  refKnotVectorRight.resize(8);
  refKnotVectorRight << 0.5, 0.5, 0.5, 0.5, 1, 1, 1, 1;
  ASSERT_TRUE(knotVectorRight.isApprox(refKnotVectorRight));

  Eigen::VectorXd refControlPointsRight;
  refControlPointsRight.resize(4);
  refControlPointsRight << 3, 3.5, 4, 5;
  ASSERT_TRUE(controlPointsRight.isApprox(refControlPointsRight));
}

TEST_F(ABSplineSplitterTest, Multiplicity0Degree3) {
  unsigned p = 3;
  Eigen::MatrixXd controlPoints(5, 1);
  controlPoints << 1, 2, 3, 4, 5;

  Eigen::MatrixXd knotVector(9, 1);
  knotVector << 0, 0, 0, 0, 0.5, 1, 1, 1, 1;

  BSpline bs = BSpline(knotVector, controlPoints, p);

  double uSplit = 0.75;
  auto splitResult = bsSplitter.split(uSplit, bs, {false, false});

  bsLeft = splitResult.first;
  bsRight = splitResult.second;

  Eigen::VectorXd knotVectorLeft = bsLeft.getKnotVector();
  Eigen::MatrixXd controlPointsLeft = bsLeft.getControlPointMatrix();

  Eigen::VectorXd knotVectorRight = bsRight.getKnotVector();
  Eigen::MatrixXd controlPointsRight = bsRight.getControlPointMatrix();

  Eigen::VectorXd refKnotVectorLeft;
  refKnotVectorLeft.resize(9);
  refKnotVectorLeft << 0, 0, 0, 0, 0.5, 0.75, 0.75, 0.75, 0.75;
  ASSERT_TRUE(knotVectorLeft.isApprox(refKnotVectorLeft));

  Eigen::VectorXd refControlPointsLeft;
  refControlPointsLeft.resize(5);
  refControlPointsLeft << 1, 2, 2.75, 3.5, 3.8125;
  ASSERT_TRUE(controlPointsLeft.isApprox(refControlPointsLeft));

  Eigen::VectorXd refKnotVectorRight;
  refKnotVectorRight.resize(8);
  refKnotVectorRight << 0.75, 0.75, 0.75, 0.75, 1, 1, 1, 1;
  ASSERT_TRUE(knotVectorRight.isApprox(refKnotVectorRight));

  Eigen::VectorXd refControlPointsRight;
  refControlPointsRight.resize(4);
  refControlPointsRight << 3.8125, 4.125, 4.5, 5;
  ASSERT_TRUE(controlPointsRight.isApprox(refControlPointsRight));
}

TEST_F(ABSplineSplitterTest, DisconnectednessDegree1) {
  unsigned p = 1;
  Eigen::MatrixXd controlPoints(5, 1);
  controlPoints << 1, 2, 3, 4, 5;

  Eigen::MatrixXd knotVector(7, 1);
  knotVector << 0, 0, 0.25, 0.5, 0.75, 1, 1;

  BSpline bs = BSpline(knotVector, controlPoints, p);

  double uSplit = 0.5;
  auto splitResult = bsSplitter.split(uSplit, bs, {false, false});

  bsLeft = splitResult.first;
  bsRight = splitResult.second;

  Eigen::VectorXd knotVectorLeft = bsLeft.getKnotVector();
  Eigen::MatrixXd controlPointsLeft = bsLeft.getControlPointMatrix();

  Eigen::VectorXd knotVectorRight = bsRight.getKnotVector();
  Eigen::MatrixXd controlPointsRight = bsRight.getControlPointMatrix();

  Eigen::VectorXd refKnotVectorLeft;
  refKnotVectorLeft.resize(5);
  refKnotVectorLeft << 0, 0, 0.25, 0.5, 0.5;
  ASSERT_TRUE(knotVectorLeft.isApprox(refKnotVectorLeft));

  Eigen::VectorXd refControlPointsLeft;
  refControlPointsLeft.resize(3);
  refControlPointsLeft << 1, 2, 3;
  ASSERT_TRUE(controlPointsLeft.isApprox(refControlPointsLeft));

  Eigen::VectorXd refKnotVectorRight;
  refKnotVectorRight.resize(5);
  refKnotVectorRight << 0.5, 0.5, 0.75, 1, 1;
  ASSERT_TRUE(knotVectorRight.isApprox(refKnotVectorRight));

  Eigen::VectorXd refControlPointsRight;
  refControlPointsRight.resize(3);
  refControlPointsRight << 3, 4, 5;
  ASSERT_TRUE(controlPointsRight.isApprox(refControlPointsRight));
}

} // namespace Tests
} // namespace Utils
} // namespace Scine


#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/search/kdtree.h>
#include <pcl/search/impl/kdtree.hpp>

#include <ros/ros.h>
#include <ros/console.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Eigen>
#include <math.h>
#include <random>

using namespace std;
using namespace Eigen;

ros::Publisher _all_map_pub;

int _obs_num, _cir_num;
double _x_size, _y_size, _z_size, _init_x, _init_y, _resolution, _sense_rate;
double _x_l, _x_h, _y_l, _y_h, _w_l, _w_h, _h_l, _h_h, _w_c_l, _w_c_h;

bool _has_map  = false;

sensor_msgs::PointCloud2 globalMap_pcd;
pcl::PointCloud<pcl::PointXYZ> cloudMap;

pcl::search::KdTree<pcl::PointXYZ> kdtreeMap;
vector<int>     pointIdxSearch;
vector<float>   pointSquaredDistance;

void RandomMapGenerate()
{
   random_device rd;
   default_random_engine eng(rd());

   uniform_real_distribution<double> rand_x = uniform_real_distribution<double>(_x_l, _x_h );
   uniform_real_distribution<double> rand_y = uniform_real_distribution<double>(_y_l, _y_h );
   uniform_real_distribution<double> rand_w = uniform_real_distribution<double>(_w_l, _w_h);
   uniform_real_distribution<double> rand_h = uniform_real_distribution<double>(_h_l, _h_h);

   uniform_real_distribution<double> rand_x_circle = uniform_real_distribution<double>(_x_l + 1.0, _x_h - 1.0);
   uniform_real_distribution<double> rand_y_circle = uniform_real_distribution<double>(_y_l + 1.0, _y_h - 1.0);
   uniform_real_distribution<double> rand_r_circle = uniform_real_distribution<double>(_w_c_l    , _w_c_h    );

   uniform_real_distribution<double> rand_roll      = uniform_real_distribution<double>(- M_PI,     + M_PI);
   uniform_real_distribution<double> rand_pitch     = uniform_real_distribution<double>(+ M_PI/4.0, + M_PI/2.0);
   uniform_real_distribution<double> rand_yaw       = uniform_real_distribution<double>(+ M_PI/4.0, + M_PI/2.0);
   uniform_real_distribution<double> rand_ellipse_c = uniform_real_distribution<double>(0.5, 2.0);
   uniform_real_distribution<double> rand_num       = uniform_real_distribution<double>(0.0, 1.0);

   pcl::PointXYZ pt_random;

   // firstly, we put some circles
   for(int i = 0; i < _cir_num; i ++)
   {
      double x0, y0, z0, R;
      std::vector<Vector3d> circle_set;

      x0   = rand_x_circle(eng);
      y0   = rand_y_circle(eng);
      z0   = rand_h(eng) / 2.0;
      R    = rand_r_circle(eng);

      if(sqrt( pow(x0-_init_x, 2) + pow(y0-_init_y, 2) ) < 2.0 )
         continue;

      double a, b;
      a = rand_ellipse_c(eng);
      b = rand_ellipse_c(eng);

      double x, y, z;
      Vector3d pt3, pt3_rot;
      for(double theta = -M_PI; theta < M_PI; theta += 0.025)
      {
         x = a * cos(theta) * R;
         y = b * sin(theta) * R;
         z = 0;
         pt3 << x, y, z;
         circle_set.push_back(pt3);
      }
      // Define a random 3d rotation matrix
      Matrix3d Rot;
      double roll,  pitch, yaw;
      double alpha, beta,  gama;
      roll  = rand_roll(eng); // alpha
      pitch = rand_pitch(eng); // beta
      yaw   = rand_yaw(eng); // gama

      alpha = roll;
      beta  = pitch;
      gama  = yaw;

      double p = rand_num(eng);
      if(p < 0.5)
      {
         beta = M_PI / 2.0;
         gama = M_PI / 2.0;
      }

      Rot << cos(alpha) * cos(gama)  - cos(beta) * sin(alpha) * sin(gama), - cos(beta) * cos(gama) * sin(alpha) - cos(alpha) * sin(gama),   sin(alpha) * sin(beta),
             cos(gama)  * sin(alpha) + cos(alpha) * cos(beta) * sin(gama),   cos(alpha) * cos(beta) * cos(gama) - sin(alpha) * sin(gama), - cos(alpha) * sin(beta),
             sin(beta)  * sin(gama),                                         cos(gama) * sin(beta),                                         cos(beta);

      for(auto pt: circle_set)
      {
         pt3_rot = Rot * pt;
         pt_random.x = pt3_rot(0) + x0 + 0.001;
         pt_random.y = pt3_rot(1) + y0 + 0.001;
         pt_random.z = pt3_rot(2) + z0 + 0.001;

         if(pt_random.z >= 0.0)
            cloudMap.points.push_back( pt_random );
      }
   }

   bool is_kdtree_empty = false;
   if(cloudMap.points.size() > 0)
      kdtreeMap.setInputCloud( cloudMap.makeShared() );
   else
      is_kdtree_empty = true;

   // then, we put some pilar
   for(int i = 0; i < _obs_num; i ++)
   {
      double x, y, w, h;
      x    = rand_x(eng);
      y    = rand_y(eng);
      w    = rand_w(eng);

      //if(sqrt( pow(x - _init_x, 2) + pow(y - _init_y, 2) ) < 2.0 )
      if(sqrt( pow(x - _init_x, 2) + pow(y - _init_y, 2) ) < 0.8 )
         continue;

      pcl::PointXYZ searchPoint(x, y, (_h_l + _h_h)/2.0);
      pointIdxSearch.clear();
      pointSquaredDistance.clear();

      if(is_kdtree_empty == false)
      {
         if ( kdtreeMap.nearestKSearch (searchPoint, 1, pointIdxSearch, pointSquaredDistance) > 0 )
         {
            if(sqrt(pointSquaredDistance[0]) < 1.0 )
               continue;
         }
      }

      x = floor(x/_resolution) * _resolution + _resolution / 2.0;
      y = floor(y/_resolution) * _resolution + _resolution / 2.0;

      int widNum = ceil(w/_resolution);
      for(int r = -widNum/2.0; r < widNum/2.0; r ++ )
      {
         for(int s = -widNum/2.0; s < widNum/2.0; s ++ )
         {
            h    = rand_h(eng);
            int heiNum = 2.0 * ceil(h/_resolution);
            for(int t = 0; t < heiNum; t ++ ){
               pt_random.x = x + (r+0.0) * _resolution + 0.001;
               pt_random.y = y + (s+0.0) * _resolution + 0.001;
               pt_random.z =     (t+0.0) * _resolution * 0.5 + 0.001;
               cloudMap.points.push_back( pt_random );
            }
         }
      }
   }

   cloudMap.width = cloudMap.points.size();
   cloudMap.height = 1;
   cloudMap.is_dense = true;

   _has_map = true;

   pcl::toROSMsg(cloudMap, globalMap_pcd);
   globalMap_pcd.header.frame_id = "world";
}

void pubSensedPoints()
{
   if( !_has_map ) return;

   _all_map_pub.publish(globalMap_pcd);
}

int main (int argc, char** argv)
{
   ros::init (argc, argv, "random_complex_scene");
   ros::NodeHandle n( "~" );

   _all_map_pub   = n.advertise<sensor_msgs::PointCloud2>("global_map", 1);

   n.param("init_state_x", _init_x,       0.0);
   n.param("init_state_y", _init_y,       0.0);

   n.param("map/x_size",  _x_size, 50.0);
   n.param("map/y_size",  _y_size, 50.0);
   n.param("map/z_size",  _z_size, 5.0 );

   n.param("map/obs_num",    _obs_num,  30);
   n.param("map/circle_num", _cir_num,  30);
   n.param("map/resolution", _resolution, 0.2);

   n.param("ObstacleShape/lower_rad", _w_l,   0.3);
   n.param("ObstacleShape/upper_rad", _w_h,   0.8);
   n.param("ObstacleShape/lower_hei", _h_l,   3.0);
   n.param("ObstacleShape/upper_hei", _h_h,   7.0);

   n.param("CircleShape/lower_circle_rad", _w_c_l, 0.3);
   n.param("CircleShape/upper_circle_rad", _w_c_h, 0.8);

   n.param("sensing/rate", _sense_rate, 1.0);

   _x_l = - _x_size / 2.0;
   _x_h = + _x_size / 2.0;

   _y_l = - _y_size / 2.0;
   _y_h = + _y_size / 2.0;

   RandomMapGenerate();
   ros::Rate loop_rate(_sense_rate);
   while (ros::ok())
   {
      pubSensedPoints();
      ros::spinOnce();
      loop_rate.sleep();
   }
}


#ifndef SPATHPP
#define SPATHPP

// -------------------------------------------------------
//
//   Spatially-regularized Levenberg Marquardt algorithm
//   Coded by J. de la Cruz Rodriguez (ISP-SU, 2020)
//
//   Reference: de la Cruz Rodriguez (2019):
//   https://ui.adsabs.harvard.edu/abs/2019A%26A...631A.153D/abstract
//
//   -------------------------------------------------------

#include <omp.h>
#include <vector>
#include <iostream>
#include <string>
#include <cstdio>
#include <cstring>
#include <chrono>

#include "line.hpp"
#include "Milne.hpp"
#include "lm.hpp"
#include "spatially_regularized_tools.hpp"

#include <Eigen/Sparse>
#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

namespace spa{

  // ************************************************************** //

  template<typename T, typename iType = long>
  class lms{
  protected:
    iType npar, ny, nx;

    Eigen::SparseMatrix<T, Eigen::RowMajor, iType> A;

    Eigen::Matrix<T,Eigen::Dynamic, 1> B;

    Eigen::SparseMatrix<T,Eigen::RowMajor, iType> L;
    Eigen::SparseMatrix<T,Eigen::RowMajor, iType> LL;

  public:
    lms(int const inpar, int const iny, int const inx):
      npar(inpar), ny(iny), nx(inx),  A(), B(){};

    // ------------------------------------------------------------ //

    static inline T checkLambda(T val, T const &mi, T const& ma)
    {return std::max<T>(std::min<T>(ma, val), mi);}

    // ------------------------------------------------------------ //

    inline static T get_one_JJ(int const ndat, const T* const __restrict__ Jy, const T* const __restrict__ Jx)
    {
      return static_cast<T>(ksumMult<T,double>(ndat, Jy, Jx));
    }

    // ------------------------------------------------------------ //

    void construct_system(int const npar, container<T> const& cont, T* const __restrict__ m,
			  T* const __restrict__ r,  Eigen::Matrix<T,Eigen::Dynamic,1> const& Reg_RHS)
    {

      iType const npix = cont.ny*cont.nx;
      iType const ndat = cont.nDat;
      iType const nthreads = cont.getNthreads();
      iType const nx = cont.nx;
      iType const ny = cont.ny;
      iType const Jstride = npar*ndat;

      // --- Build Sparse system --- //

      B.resize(npix*npar); B.setZero();
      A.resize(0,0); A.data().squeeze(); A.resize(npix*npar, npix*npar);
      A.reserve(Eigen::VectorXi::Constant(npix*npar,npar));

      T* __restrict__ iJ = NULL;
      iType ipix=0, tid=0, pp=0, ii=0, jj=0;
      T iSum = 0;

      // --- parallel block --- //

#pragma omp parallel default(shared) firstprivate(ipix, tid, iSum, pp, ii, jj, iJ) num_threads(nthreads)
      {

	tid = omp_get_thread_num();
	iJ = new T [npar*ndat](); // Allocate thread buffer for derivatives

#pragma omp for
	for(ipix=0; ipix<npix; ++ipix){

	  // --- synthesize_one pixel with derivatives --- //

	  cont.synthesize_der_one(npar, &m[ipix*npar], &r[ipix*ndat], iJ, tid, ipix);

	  // --- Fill in subspace in sparse Hessian matrix --- //

	  for(jj=0; jj<npar; ++jj){

	    // --- RHS of the equation --- //

	    B[ipix*npar+jj] = ksumMult<T,double>(ndat, &iJ[jj*ndat], &r[ipix*ndat]) - Reg_RHS[ipix*npar+jj];

	    for(ii=0; ii<=jj;++ii){

	      // --- Matrix subspaces --- //

	      iSum = get_one_JJ(ndat, &iJ[jj*ndat], &iJ[ii*ndat]);
	      A.insert(ipix*npar + jj, ipix*npar + ii) = iSum;

	      if(ii != jj) // The matrix is symmetric but avoid inserting the diagonal term twice
	       	A.insert(ipix*npar + ii, ipix*npar + jj) = iSum;

	    } // ii
	  } // jj

	} // ipix

	delete [] iJ;
	iJ = NULL;

      }// parallel

    }

    // ------------------------------------------------------------ //

    Chi2<T> getCorrection(container<T> const& cont, T* const __restrict__ m,
			  T* const __restrict__ syn, T* const __restrict__ r, T iLam, int const method)const
    {

      iType const npix = cont.ny*cont.nx;
      iType const ndat = cont.nDat;

      Eigen::SparseMatrix<T,Eigen::RowMajor,iType> Atot = A+LL;

      // --- damp diagonal and get correction --- //

      iType const nDiag = iType(npar)*iType(npix);
      for(iType kk =0; kk<nDiag; ++kk)
	Atot.coeffRef(kk,kk) *= (1+iLam);

      Eigen::Matrix<T,Eigen::Dynamic,1> dx;

      // --- Solve for corrections --- //

      if(method == 0){
	Eigen::ConjugateGradient<Eigen::SparseMatrix<T,Eigen::RowMajor,iType>, Eigen::Lower| Eigen::Upper> solver(Atot);
	dx = solver.solve(B);
      }else if(method == 1){
	Eigen::BiCGSTAB<Eigen::SparseMatrix<T,Eigen::RowMajor,iType>> solver(Atot);
	dx = solver.solve(B);
      }else if(method == 2){
	Eigen::SparseLU<Eigen::SparseMatrix<T,Eigen::RowMajor,iType>> solver(Atot);
	dx = solver.solve(B);
      }

      // --- Check corrections --- //

      for(iType pp=0; pp<npar; ++pp)
	for(iType ipix = 0; ipix<npix; ++ipix){
	  m[ipix*npar+pp] += dx[ipix*npar+pp];
	  cont.Pinfo[pp].CheckNormalized(m[ipix*npar+pp]);

	}

      // --- compute chi2 --- //

      std::vector<T> rnew(ndat*npix,0);
      cont.fx(npar, m, syn, &rnew[0]);
      Eigen::Matrix<T, Eigen::Dynamic, 1> Gam = cont.getGamma(npar, m);

      Chi2<T> chi2(ksum2<T,double>(npix*ndat, &rnew[0]), ksum2<T,double>(Gam.size(), &Gam[0]));

      return chi2;
    }

    // ------------------------------------------------------------ //

    Chi2<T> getStep(container<T> const& cont, T* const __restrict__ m,
		    T* const __restrict__ syn, T* const __restrict__ r, T& iLam, bool bracket,
		    T const minLam, T const maxLam, T const Lam_step,  Eigen::Matrix<T,Eigen::Dynamic,1> const& Reg_RHS,
		    Chi2<T> const& bestChi2, int const method)
    {

      // --- if no bracketing just compute one correction --- //

      if(!bracket){
	return getCorrection(cont, m, syn, r, iLam, method);
      }else{

	// --- Bracketing optimal lambda value --- //

	int const npix = cont.nx*cont.ny;
	std::vector<Chi2<T>> iChi2;
	std::vector<T> Lambdas;

	int idx = 0;

	Eigen::Map<Eigen::Matrix<T,Eigen::Dynamic,1>> input_model(m, npix*npar);
	Eigen::Matrix<T,Eigen::Dynamic,1> Model = input_model;

	Chi2<T> chi2 = getCorrection(cont, &Model[0], syn, r, iLam, method);
	if(chi2.value() > bestChi2.value()) return chi2;

	Eigen::Matrix<T,Eigen::Dynamic,1> bestModel = Model;

	iChi2.emplace_back(chi2);
	Lambdas.emplace_back(iLam);

	// --- First try to bracket by decreasing lambda --- //
	int iter = 0;
	while((iter < 1) || ((iter < 4) && (iChi2[iter].value()<iChi2[iter-1].value()) && (Lambdas[iter] > minLam))){
	  iLam = checkLambda(iLam / Lam_step, minLam, maxLam);
	  Model = input_model;

	  Lambdas.emplace_back(iLam);
	  iChi2.emplace_back(getCorrection(cont, &Model[0], syn, r, iLam, method));
	  if(iChi2[iter+1].value() < iChi2[idx].value()){
	    idx = iter+1;
	    bestModel = Model;
	  }

	  ++iter;

	}// while

	// --- if the best Chi2 is not in the first element we consider it bracketed --- //

	if(idx == 0){
	  // --- Go in the opposite direction, increasing lambda --- //
	  iter = 0;
	  while((iter == 0) ||( (iter++ <= 5) && (Lambdas[0] < maxLam))){
	    Model = input_model;
	    iLam = checkLambda(iLam * Lam_step*Lam_step, minLam, maxLam);

	    Lambdas.insert(Lambdas.begin(), iLam);
	    iChi2.insert(iChi2.begin(), getCorrection(cont, &Model[0], syn, r, iLam, method) );

	    if(iChi2[0].value() < iChi2[1].value()){
	      bestModel = Model;
	      idx = 0;
	    }else{
	      idx += 1;
	      break;
	    }

	  }// while
	}

	input_model = bestModel;
	iLam = Lambdas[idx];
	return iChi2[idx];
      }

    }

    // ------------------------------------------------------------ //

    T fitData(container<T> const& cont, int const npar, T* __restrict__ bestModel,
	      T* __restrict__ bestSyn, int const max_iter = 20, T iLam = 10,
	      T const Chi2_thres = 1.0, T const fx_thres = 2.e-3, int const delay_braket = 2,
	      bool verbose = true, int const method = 0)
    {
      int const nthreads = int(cont.Me.size());
      Eigen::initParallel();
      Eigen::setNbThreads(nthreads);

      static constexpr T const facLam = 3.1622776601683795;
      static constexpr T const maxLam = 1000.;
      static constexpr T const minLam =  3.1622776601683795e-3;
      static constexpr int const max_n_reject = 6;

      // --- Init temporary variables --- //

      Chi2<T> bestChi2(1.e34,1.e34);
      Chi2<T> chi2 = bestChi2;

      iType const npix = cont.ny*cont.nx;
      iType const ndat = cont.nDat;
      iType const nJ = long(npix)*long(npar)*long(ndat);

      T* const __restrict__ r   = new T [npix*ndat]();
      T* const __restrict__ m   = new T [npix*npar]();

      // --- check pars --- //

      cont.checkPars(npar, bestModel);
      cont.NormalizePars(npar, bestModel);
      std::memcpy(m, bestModel, npix*npar*sizeof(T));

      // --- Init residue "r" --- //

      std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
      cont.fx(npar, m, bestSyn, r);

      // --- precompute L (only needed once) --- //

      if(verbose)
	fprintf(stdout, "lms::fitData: pre-computing regularization derivatives matrix ... ");

      L  = cont.get_L(npar, m);
      LL = L.transpose()*L;
      Eigen::Matrix<T,Eigen::Dynamic,1> Reg_RHS;

      // --- Init total Chi2 --- //
      {
	Eigen::Matrix<T, Eigen::Dynamic, 1> Gam = cont.getGamma(npar, m);
	bestChi2 = Chi2<T>(ksum2<T,double>(npix*ndat, r), ksum2<T,double>(Gam.size(), &Gam[0]));
	Reg_RHS = L.transpose()*Gam; // Init RHS regularization term. Vector that only needs to be computed once per successfull iteration.
      }

      // --- Initialize sparse linear system for the first iteration --- //

      construct_system(npar, cont, m, r, Reg_RHS);

      if(verbose)
	fprintf(stdout,"done\n");

      // --- Init iteration --- //

      if(verbose)
	fprintf(stderr, "\nlms::fitData: [Init] Chi2=%s\n", bestChi2.formatted().c_str());
      int iter = 0, n_rejected = 0;
      bool quit = false, tooSmall = false;
      T oLam = 0, dfx = 0;

      iLam = checkLambda(iLam, minLam, maxLam);

      // --- Iterate the solution --- //

      while(iter < max_iter){

	bool do_bracket =  ((delay_braket > iter)? false : true);

      	oLam = iLam;
	std::memcpy(m, bestModel, npar*npix*sizeof(T));

	// --- Get model correction --- //

	chi2 = getStep(cont, m, bestSyn, r, iLam, do_bracket, minLam, maxLam, facLam, Reg_RHS, bestChi2, method);

	// --- have we improved? --- //

	if(chi2.value() < bestChi2.value()){

	  oLam = iLam;
	  dfx = (bestChi2.value() - chi2.value()) / bestChi2.value();
	  bestChi2 = chi2;

	  std::memcpy(bestModel,   m, npar*npix*sizeof(T));

	  if(!do_bracket)
	    if(iLam*1.00001 > minLam)
	      iLam = checkLambda(iLam/facLam, minLam, maxLam);
	    else
	      iLam *= facLam*facLam;
	  else
	    iLam = facLam*iLam;

	  if(dfx < fx_thres){
	    if(tooSmall) quit = true;
	    else tooSmall = true;
	  }
	  n_rejected = 0;

	}else{

	  iLam = checkLambda(iLam*SQ<T>(facLam), minLam, maxLam);
	  n_rejected += 1;
	  if(verbose)
	    fprintf(stderr,"lms::fitData: ----> Chi2=%s > %s -> Increasing lambda %f -> %f\n",  chi2.formatted().c_str(), bestChi2.formatted().c_str(), oLam, iLam);

	  if(n_rejected<max_n_reject) continue;

	} // else

	std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();
	double dt = std::chrono::duration_cast<std::chrono::milliseconds>(end - begin).count();
	begin = end;

	// --- Check what has happened with Chi2 --- //

	if(n_rejected >= max_n_reject){
	  if(verbose)
	    fprintf(stderr, "lms::fitData: maximum number of rejected iterations reached, finishing inversion");
	  break;
	}

	if(verbose)
	  fprintf(stderr, "lms::fitData: [%3d] Chi2=%s, lambda=%e, dtime=%6.1fs\n", iter, chi2.formatted().c_str(), oLam, dt/1000.);

	if(bestChi2.value() < Chi2_thres){
	  if(verbose)
	    fprintf(stderr, "lms::fitData: Chi2 (%f) < Chi2_threshold (%f), finishing inversion", bestChi2.value(), Chi2_thres);
	  break;
	}

	if(quit){
	  if(verbose)
	    fprintf(stderr, "lms::fitData: Chi2 improvement too small for 2-iterations, finishing inversion\n");
	  break;
	}

	iter++;
	if(iter >= max_iter){
	  break;
	}

	// --- init next iteration --- //

	std::memcpy(m, bestModel, npix*npar*sizeof(T));

	{
	  Eigen::Matrix<T, Eigen::Dynamic, 1> Gam = cont.getGamma(npar, m);
	  Reg_RHS = L.transpose()*Gam;
	}

	// --- Construct sparse matrix with the new model estimate --- //

	construct_system(npar, cont, m, r,  Reg_RHS);
      }

      // --- Synthesize with best model --- //

      cont.fx(npar, bestModel, bestSyn, r);

      // --- scale model parameters ---- //

      cont.ScalePars(npar, bestModel);

      // --- Clean up --- //

      delete [] r;
      delete [] m;

      return bestChi2.value();
    }

    // ------------------------------------------------------------ //

  };

}

#endif


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

#include <boost/config.hpp>
#define BOOST_TEST_MAIN
#include <boost/array.hpp>
#include <boost/math/tools/polynomial.hpp>
#include <boost/math/common_factor_rt.hpp>
#include <boost/mpl/list.hpp>
#include <boost/mpl/joint_view.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <utility>

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

template <typename T>
struct answer
{
    answer(std::pair< polynomial<T>, polynomial<T> > const &x) :
    quotient(x.first), remainder(x.second) {}

    polynomial<T> quotient;
    polynomial<T> remainder;
};

boost::array<double, 4> const d3a = {{10, -6, -4, 3}};
boost::array<double, 4> const d3b = {{-7, 5, 6, 1}};
boost::array<double, 4> const d3c = {{10.0/3.0, -2.0, -4.0/3.0, 1.0}};
boost::array<double, 2> const d1a = {{-2, 1}};
boost::array<double, 3> const d2a = {{-2, 2, 3}};
boost::array<double, 3> const d2b = {{-7, 5, 6}};
boost::array<double, 3> const d2c = {{31, -21, -22}};
boost::array<double, 1> const d0a = {{6}};
boost::array<double, 2> const d0a1 = {{0, 6}};
boost::array<double, 6> const d0a5 = {{0, 0, 0, 0, 0, 6}};
boost::array<double, 1> const d0b = {{3}};

boost::array<int, 9> const d8 = {{-5, 2, 8, -3, -3, 0, 1, 0, 1}};
boost::array<int, 9> const d8b = {{0, 2, 8, -3, -3, 0, 1, 0, 1}};
boost::array<int, 7> const d6 = {{21, -9, -4, 0, 5, 0, 3}};
boost::array<int, 3> const d2 = {{-6, 0, 9}};
boost::array<int, 6> const d5 = {{-9, 0, 3, 0, -15}};

BOOST_AUTO_TEST_CASE( test_construction )
{
    polynomial<double> const a(d3a.begin(), d3a.end());
    polynomial<double> const b(d3a.begin(), 3);
    BOOST_CHECK_EQUAL(a, b);
}

#if !defined(BOOST_NO_CXX11_HDR_INITIALIZER_LIST) && !BOOST_WORKAROUND(BOOST_GCC_VERSION, < 40500)
BOOST_AUTO_TEST_CASE( test_initializer_list_construction )
{
    polynomial<double> a(begin(d3a), end(d3a));
    polynomial<double> b = {10, -6, -4, 3};
    polynomial<double> c{{10, -6, -4, 3}};
    polynomial<double> d{{10, -6, -4, 3, 0, 0}};
    BOOST_CHECK_EQUAL(a, b);
    BOOST_CHECK_EQUAL(b, c);
    BOOST_CHECK_EQUAL(d.degree(), 3u);
}

BOOST_AUTO_TEST_CASE( test_initializer_list_assignment )
{
    polynomial<double> a(begin(d3a), end(d3a));
    polynomial<double> b;
    b = {10, -6, -4, 3, 0, 0};
    BOOST_CHECK_EQUAL(b.degree(), 3u);
    BOOST_CHECK_EQUAL(a, b);
}
#endif

BOOST_AUTO_TEST_CASE( test_degree )
{
    polynomial<double> const zero;
    polynomial<double> const a(d3a.begin(), d3a.end());
    BOOST_CHECK_THROW(zero.degree(), std::logic_error);
    BOOST_CHECK_EQUAL(a.degree(), 3u);
}

BOOST_AUTO_TEST_CASE( test_division_over_field )
{
    polynomial<double> const a(d3a.begin(), d3a.end());
    polynomial<double> const b(d1a.begin(), d1a.end());
    polynomial<double> const q(d2a.begin(), d2a.end());
    polynomial<double> const r(d0a.begin(), d0a.end());
    polynomial<double> const c(d3b.begin(), d3b.end());
    polynomial<double> const d(d2b.begin(), d2b.end());
    polynomial<double> const e(d2c.begin(), d2c.end());
    polynomial<double> const f(d0b.begin(), d0b.end());
    polynomial<double> const g(d3c.begin(), d3c.end());
    polynomial<double> const zero;
    polynomial<double> const one(1.0);

    answer<double> result = quotient_remainder(a, b);
    BOOST_CHECK_EQUAL(result.quotient, q);
    BOOST_CHECK_EQUAL(result.remainder, r);
    BOOST_CHECK_EQUAL(a, q * b + r); // Sanity check.

    result = quotient_remainder(a, c);
    BOOST_CHECK_EQUAL(result.quotient, f);
    BOOST_CHECK_EQUAL(result.remainder, e);
    BOOST_CHECK_EQUAL(a, f * c + e); // Sanity check.

    result = quotient_remainder(a, f);
    BOOST_CHECK_EQUAL(result.quotient, g);
    BOOST_CHECK_EQUAL(result.remainder, zero);
    BOOST_CHECK_EQUAL(a, g * f + zero); // Sanity check.
    // Check that division by a regular number gives the same result.
    BOOST_CHECK_EQUAL(a / 3.0, g);
    BOOST_CHECK_EQUAL(a % 3.0, zero);

    // Sanity checks.
    BOOST_CHECK_EQUAL(a / a, one);
    BOOST_CHECK_EQUAL(a % a, zero);
    // BOOST_CHECK_EQUAL(zero / zero, zero); // TODO
}

BOOST_AUTO_TEST_CASE( test_division_over_ufd )
{
    polynomial<int> const zero;
    polynomial<int> const one(1);
    polynomial<int> const aa(d8.begin(), d8.end());
    polynomial<int> const bb(d6.begin(), d6.end());
    polynomial<int> const q(d2.begin(), d2.end());
    polynomial<int> const r(d5.begin(), d5.end());

    answer<int> result = quotient_remainder(aa, bb);
    BOOST_CHECK_EQUAL(result.quotient, q);
    BOOST_CHECK_EQUAL(result.remainder, r);

    // Sanity checks.
    BOOST_CHECK_EQUAL(aa / aa, one);
    BOOST_CHECK_EQUAL(aa % aa, zero);
}

BOOST_AUTO_TEST_CASE( test_gcd )
{
    /* NOTE: Euclidean gcd is not yet customized to return THE greatest
     * common polynomial divisor. If d is THE greatest common divisior of u and
     * v, then gcd(u, v) will return d or -d according to the algorithm.
     * By convention, it should return d, as for example Maxima and Wolfram
     * Alpha do.
     * This test is an example of the fact that it returns -d.
     */
    boost::array<double, 9> const d8 = {{105, 278, -88, -56, 16}};
    boost::array<double, 7> const d6 = {{70, 232, -44, -64, 16}};
    boost::array<double, 7> const d2 = {{-35, 24, -4}};
    polynomial<double> const u(d8.begin(), d8.end());
    polynomial<double> const v(d6.begin(), d6.end());
    polynomial<double> const w(d2.begin(), d2.end());
    polynomial<double> const d = boost::math::gcd(u, v);
    BOOST_CHECK_EQUAL(w, d);
}

// Sanity checks to make sure I didn't break it.
typedef boost::mpl::list<int, long
#if !BOOST_WORKAROUND(BOOST_MSVC, <= 1500)
   , boost::multiprecision::cpp_int
#endif
> integral_test_types;
typedef boost::mpl::list<double
#if !BOOST_WORKAROUND(BOOST_MSVC, <= 1500)
   , boost::multiprecision::cpp_rational, boost::multiprecision::cpp_bin_float_single, boost::multiprecision::cpp_dec_float_50
#endif
> non_integral_test_types;
typedef boost::mpl::joint_view<integral_test_types, non_integral_test_types> all_test_types;

BOOST_AUTO_TEST_CASE_TEMPLATE( test_addition, T, all_test_types )
{
    polynomial<T> const a(d3a.begin(), d3a.end());
    polynomial<T> const b(d1a.begin(), d1a.end());
    polynomial<T> const zero;

    polynomial<T> result = a + b; // different degree
    boost::array<T, 4> tmp = {{8, -5, -4, 3}};
    polynomial<T> expected(tmp.begin(), tmp.end());
    BOOST_CHECK_EQUAL(result, expected);
    BOOST_CHECK_EQUAL(a + zero, a);
    BOOST_CHECK_EQUAL(a + b, b + a);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( test_subtraction, T, all_test_types )
{
    polynomial<T> const a(d3a.begin(), d3a.end());
    polynomial<T> const zero;

    BOOST_CHECK_EQUAL(a - T(0), a);
    BOOST_CHECK_EQUAL(T(0) - a, -a);
    BOOST_CHECK_EQUAL(a - zero, a);
    BOOST_CHECK_EQUAL(zero - a, -a);
    BOOST_CHECK_EQUAL(a - a, zero);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( test_multiplication, T, all_test_types )
{
    polynomial<T> const a(d3a.begin(), d3a.end());
    polynomial<T> const b(d1a.begin(), d1a.end());
    polynomial<T> const zero;
    boost::array<T, 7> const d3a_sq = {{100, -120, -44, 108, -20, -24, 9}};
    polynomial<T> const a_sq(d3a_sq.begin(), d3a_sq.end());

    BOOST_CHECK_EQUAL(a * T(0), zero);
    BOOST_CHECK_EQUAL(a * zero, zero);
    BOOST_CHECK_EQUAL(zero * T(0), zero);
    BOOST_CHECK_EQUAL(zero * zero, zero);
    BOOST_CHECK_EQUAL(a * b, b * a);
    polynomial<T> aa(a);
    aa *= aa;
    BOOST_CHECK_EQUAL(aa, a_sq);
    BOOST_CHECK_EQUAL(aa, a * a);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( test_arithmetic_relations, T, all_test_types )
{
    polynomial<T> const a(d8b.begin(), d8b.end());
    polynomial<T> const b(d1a.begin(), d1a.end());

    BOOST_CHECK_EQUAL(a * T(2), a + a);
    BOOST_CHECK_EQUAL(a - b, -b + a);
    BOOST_CHECK_EQUAL(a, (a * a) / a);
    BOOST_CHECK_EQUAL(a, (a / a) * a);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_non_integral_arithmetic_relations, T, non_integral_test_types )
{
    polynomial<T> const a(d8b.begin(), d8b.end());
    polynomial<T> const b(d1a.begin(), d1a.end());

    BOOST_CHECK_EQUAL(a * T(0.5), a / T(2));
}

BOOST_AUTO_TEST_CASE_TEMPLATE( test_self_multiply_assign, T, all_test_types )
{
    polynomial<T> a(d3a.begin(), d3a.end());
    polynomial<T> const b(a);
    boost::array<double, 7> const d3a_sq = {{100, -120, -44, 108, -20, -24, 9}};
    polynomial<T> const asq(d3a_sq.begin(), d3a_sq.end());

    a *= a;

    BOOST_CHECK_EQUAL(a, b*b);
    BOOST_CHECK_EQUAL(a, asq);

    a *= a;

    BOOST_CHECK_EQUAL(a, b*b*b*b);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_right_shift, T, all_test_types )
{
    polynomial<T> a(d8b.begin(), d8b.end());
    polynomial<T> const aa(a);
    polynomial<T> const b(d8b.begin() + 1, d8b.end());
    polynomial<T> const c(d8b.begin() + 5, d8b.end());
    a >>= 0u;
    BOOST_CHECK_EQUAL(a, aa);
    a >>= 1u;
    BOOST_CHECK_EQUAL(a, b);
    a = a >> 4u;
    BOOST_CHECK_EQUAL(a, c);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_left_shift, T, all_test_types )
{
    polynomial<T> a(d0a.begin(), d0a.end());
    polynomial<T> const aa(a);
    polynomial<T> const b(d0a1.begin(), d0a1.end());
    polynomial<T> const c(d0a5.begin(), d0a5.end());
    a <<= 0u;
    BOOST_CHECK_EQUAL(a, aa);
    a <<= 1u;
    BOOST_CHECK_EQUAL(a, b);
    a = a << 4u;
    BOOST_CHECK_EQUAL(a, c);
    polynomial<T> zero;
    // Multiplying zero by x should still be zero.
    zero <<= 1u;
    BOOST_CHECK_EQUAL(zero, zero_element(multiplies< polynomial<T> >()));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_odd_even, T, all_test_types)
{
    polynomial<T> const zero;
    BOOST_CHECK_EQUAL(odd(zero), false);
    BOOST_CHECK_EQUAL(even(zero), true);
    polynomial<T> const a(d0a.begin(), d0a.end());
    BOOST_CHECK_EQUAL(odd(a), true);
    BOOST_CHECK_EQUAL(even(a), false);
    polynomial<T> const b(d0a1.begin(), d0a1.end());
    BOOST_CHECK_EQUAL(odd(b), false);
    BOOST_CHECK_EQUAL(even(b), true);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( test_pow, T, all_test_types )
{
    polynomial<T> a(d3a.begin(), d3a.end());
    polynomial<T> const one(T(1));
    boost::array<double, 7> const d3a_sqr = {{100, -120, -44, 108, -20, -24, 9}};
    boost::array<double, 10> const d3a_cub =
        {{1000, -1800, -120, 2124, -1032, -684, 638, -18, -108, 27}};
    polynomial<T> const asqr(d3a_sqr.begin(), d3a_sqr.end());
    polynomial<T> const acub(d3a_cub.begin(), d3a_cub.end());

    BOOST_CHECK_EQUAL(pow(a, 0), one);
    BOOST_CHECK_EQUAL(pow(a, 1), a);
    BOOST_CHECK_EQUAL(pow(a, 2), asqr);
    BOOST_CHECK_EQUAL(pow(a, 3), acub);
    BOOST_CHECK_EQUAL(pow(a, 4), pow(asqr, 2));
    BOOST_CHECK_EQUAL(pow(a, 5), asqr * acub);
    BOOST_CHECK_EQUAL(pow(a, 6), pow(acub, 2));
    BOOST_CHECK_EQUAL(pow(a, 7), acub * acub * a);

    BOOST_CHECK_THROW(pow(a, -1), std::domain_error);
    BOOST_CHECK_EQUAL(pow(one, 137), one);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_bool, T, all_test_types)
{
    polynomial<T> const zero;
    polynomial<T> const a(d0a.begin(), d0a.end());
    BOOST_CHECK_EQUAL(bool(zero), false);
    BOOST_CHECK_EQUAL(bool(a), true);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_set_zero, T, all_test_types)
{
    polynomial<T> const zero;
    polynomial<T> a(d0a.begin(), d0a.end());
    a.set_zero();
    BOOST_CHECK_EQUAL(a, zero);
    a.set_zero(); // Ensure that setting zero to zero is a no-op.
    BOOST_CHECK_EQUAL(a, zero);
}


#include "visualization/constant-velocity-smoother.h"

#include <chrono>

#include <Eigen/Dense>
#include <glog/logging.h>
#include <maplab-common/conversions.h>

DEFINE_double(
    pose_smoother_cutoff_frequency, 40,
    "Cutoff frequency for the "
    "velocity smoothing [Hz].");

namespace visualization {
ConstantVelocitySmoother::ConstantVelocitySmoother()
    : ConstantVelocitySmoother(FLAGS_pose_smoother_cutoff_frequency) {}

ConstantVelocitySmoother::ConstantVelocitySmoother(double cutoff_frequency)
    : cutoff_frequency_(cutoff_frequency),
      is_initialized_(false),
      n_samples_(0),
      filtered_position_(Eigen::Vector3d::Constant(0.0)),
      filtered_velocity_(Eigen::Vector3d::Constant(0.0)),
      last_sample_(Eigen::Vector3d::Constant(0.0)) {
  CHECK_GT(cutoff_frequency_, 0.0);
  time_constant_ = 1. / (2. * M_PI * cutoff_frequency_);
}
void ConstantVelocitySmoother::addSample(const Eigen::Vector3d& sample) {
  if (!is_initialized_) {
    filtered_position_ = sample;
    last_sample_ = sample;
    is_initialized_ = true;
    last_time_ = steady_clock::now();
    ++n_samples_;
    return;
  }

  using std::chrono::duration_cast;
  steady_clock::time_point current = steady_clock::now();
  double delta_t_seconds =
      kMilliSecondsToSeconds *
      duration_cast<std::chrono::milliseconds>(current - last_time_).count();
  last_time_ = current;

  if (delta_t_seconds == 0.0) {
    return;
  }

  Eigen::Vector3d current_velocity = (sample - last_sample_) / delta_t_seconds;

  double alpha = delta_t_seconds / (time_constant_ + delta_t_seconds);
  filtered_velocity_ =
      alpha * current_velocity + (1. - alpha) * filtered_velocity_;

  Eigen::Vector3d predicted_position =
      filtered_position_ + filtered_velocity_ * delta_t_seconds;
  filtered_position_ = alpha * sample + (1. - alpha) * predicted_position;

  ++n_samples_;
  last_sample_ = sample;
}

const Eigen::Vector3d& ConstantVelocitySmoother::getCurrentPosition() const {
  return filtered_position_;
}
}  // namespace visualization