/* MIT License

Copyright (c) 2020 FORNO

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/
#include <cmath>
#include <cstddef>
#include <filesystem>
#include <iostream>
#include <limits>
#include <utility>

#include <boost/optional.hpp>
#include <boost/program_options.hpp>
#include <boost/property_tree/ini_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/tokenizer.hpp>

#include <Eigen/Dense>
#include <Eigen/Geometry>

constexpr std::size_t marker_count {29};

struct index_holder
{
  std::size_t left_asis;
  std::size_t right_asis;
  std::size_t v_sacral;
};

index_holder read_indexies(std::filesystem::path path);

int main(int argc, char** argv)
{
  std::ios::sync_with_stdio(false);
  std::cin.tie(nullptr);
  std::cout << std::fixed;

  namespace po = boost::program_options;

  po::options_description description("Options");
  description.add_options()
    ("header,h", po::value<bool>()->default_value(true), "Dose the CSV has header?")
    ("config,c", po::value<std::string>(), "Config file path")
    ("l2w,l", po::value<bool>()->default_value(false), "Local to World flag")
    ("help,H", "Help")
    ;

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

  if (argc < 2 || vm.count("help")) {
    std::cerr << "Usage: " << *argv << " --config config/Cooking < cooking.csv\n";
    return 1;
  }

  try {
    const auto config_path = vm["config"].as<std::string>();
    const auto indexies {read_indexies(config_path)};
    if (vm["header"].as<bool>()) {
      std::cin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
    }
    if (!vm["l2w"].as<bool>()) {
      for (std::string line; std::getline(std::cin, line);) {
        boost::tokenizer<boost::escaped_list_separator<char>> tokens(line);
        Eigen::Matrix<float, marker_count, 3, Eigen::RowMajor> values;
        {
          auto it = tokens.begin();
          for (std::size_t i {0}; i < marker_count * 3; ++i) {
            try {
              values(i) = std::stof(*it);
            } catch (std::invalid_argument&) {
              values(i) = std::numeric_limits<float>::quiet_NaN();
            }
            ++it;
          }
        }

        const Eigen::Vector3f center_asis = values.row(indexies.left_asis) / 2 + values.row(indexies.right_asis) / 2;
        for (std::size_t i {0}; i < marker_count; ++i) {
          values.row(i) -= center_asis;
        }

        const Eigen::Vector3f forward = -values.row(indexies.v_sacral);
        const auto w2l_rotation = Eigen::Quaternionf::FromTwoVectors(forward, Eigen::Vector3f{0, 0, 1.f});

        for (std::size_t i {0}; i < marker_count; ++i) {
          values.row(i) = w2l_rotation * values.row(i);
        }

        auto is_first {true};
        for (std::size_t i {0}; i < static_cast<std::size_t>(values.size()); ++i) {
          if (!std::exchange(is_first, false)) {
            std::cout.put(',');
          }
          if (!isnan(values(i))) {
            std::cout << values(i);
          }
        }
        const auto l2w_translation {-center_asis};
        for (std::size_t i {0}; i < static_cast<std::size_t>(l2w_translation.size()); ++i) {
          std::cout << ',' << l2w_translation(i);
        }
        const auto l2w_rotation {w2l_rotation.inverse()};
        const auto& l2w_coeffs {l2w_rotation.coeffs()};
        for (std::size_t i {0}; i < static_cast<std::size_t>(l2w_coeffs.size()); ++i) {
          std::cout << ',' << l2w_coeffs(i);
        }
        std::cout.put('\n');
      }
    } else {
      for (std::string line; std::getline(std::cin, line);) {
        Eigen::Matrix<float, marker_count, 3, Eigen::RowMajor> values;
        Eigen::Vector3f translation;
        Eigen::Quaternionf rotation;
        {
          boost::tokenizer<boost::escaped_list_separator<char>> tokens(line);
          auto it = tokens.begin();
          for (std::size_t i {0}; i < static_cast<std::size_t>(values.size()); ++i) {
            try {
              values(i) = std::stof(*it);
            } catch (std::invalid_argument&) {
              values(i) = std::numeric_limits<float>::quiet_NaN();
            }
            ++it;
          }
          for (std::size_t i {0}; i < static_cast<std::size_t>(translation.size()); ++i) {
            try {
              translation(i) = std::stof(*it);
            } catch (std::invalid_argument&) {
              translation(i) = std::numeric_limits<float>::quiet_NaN();
            }
            ++it;
          }
          Eigen::Vector4f rotation_coeffs ;
          for (std::size_t i {0}; i < static_cast<std::size_t>(rotation_coeffs.size()); ++i) {
            try {
              rotation_coeffs(i) = std::stof(*it);
            } catch (std::invalid_argument&) {
              rotation_coeffs(i) = std::numeric_limits<float>::quiet_NaN();
            }
            ++it;
          }
          rotation = Eigen::Quaternionf(rotation_coeffs);
        }
        for (std::size_t i {0}; i < marker_count; ++i) {
          values.row(i) = rotation * values.row(i) + translation;
        }
        auto is_first {true};
        for (std::size_t i {0}; i < static_cast<std::size_t>(values.size()); ++i) {
          if (!std::exchange(is_first, false)) {
            std::cout.put(',');
          }
          if (!isnan(values(i))) {
            std::cout << values(i);
          }
        }
        std::cout.put('\n');
      }
    }
  } catch (boost::wrapexcept<boost::property_tree::ptree_bad_path>& e) {
    std::cerr << "Invalid indexies format: " << e.what() << '\n';
    return 1;
  } catch (boost::wrapexcept<boost::property_tree::ini_parser::ini_parser_error>& e) {
    std::cerr << "Invalid indexies file: " << e.what() << '\n';
    return 1;
  }

  return 0;
}

index_holder read_indexies(std::filesystem::path path)
{
  using namespace boost::property_tree;
  ptree pt;
  read_ini(path.native(), pt);
  return index_holder {pt.get<std::size_t>("Indexies.LeftAsis"), pt.get<std::size_t>("Indexies.RightAsis"), pt.get<std::size_t>("Indexies.VSacral")};
}


// Copyright (c) 2017-2018 Steven Watanabe
//
// Distributed under the Boost Software License Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_UNITS2_UNIT_HPP_INCLUDED
#define BOOST_UNITS2_UNIT_HPP_INCLUDED

#include <boost/units2/detail/merge.hpp>
#include <boost/mp11/algorithm.hpp>
#include <boost/mp11/utility.hpp>
#include <boost/integer/common_factor_ct.hpp>
#include <type_traits>
#include <limits>
#include <cstdint>
#include <cmath>

// Design goals:
// - Can represent any unit.
// - Can convert between any units with the same dimensions.
// - New units can be defined in terms of any existing units.
//   Which unit is the base unit and which unit is the derived
//   unit shall not change the behavior of any user code outside
//   the definition itself.
// - The set of dimensions shall be extensible.
// - Every unit shall be represented by one and only one type.
// - To the maximum extent possible, conversions shall not
//   lose precision.
// - The raw type of a unit should be (somewhat) intelligible.
//
// Implementation Notes:
// - All units are ultimately seen as a scaled combination of base units.
// - There is exactly one base unit for each dimension.
// - Conversions reduce both sides to the base form.
// - To avoid loss-of-precision, all conversion factors
//   are tracked and identical components are combined/canceled
//   before any evaluation is done.  In addition, all calculations
//   are carried out using exact (rational) arithmetic wherever possible.
// - The struct that a user defines is a unit, with no wrapping required.
// - All units are reduced to normalized form after every operation.
// - The different types of units can be processed using a visitor via the
//   alias _boost_units2_apply.
//
// Possible improvements:
// - Allow floating point scaling to have greater precision than double.
//   This also means passing the value_type through all the conversion logic.

namespace boost {
namespace units2 {

/*
 * Base class for scales
 */
struct scale_base {
    /// INTERNAL ONLY
    using _boost_units2_is_scale = void;
};

/**
 * Base class for all units.
 */
template<class Derived>
struct unit_base {
    /// INTERNAL ONLY
    /// Tag for use with SFINAE
    using _boost_units2_is_unit = void;
    /// INTERNAL ONLY
    /// visitor to help code that needs to switch on the kind of unit
    template<class F, class T>
    using _boost_units2_apply = typename F::template apply_base<T>;
    /// INTERNAL ONLY
    using _boost_units2_type = Derived;
    /// INTERNAL ONLY
    auto operator<=>(const unit_base&) const = default;
};

/**
 * Represents a unit that is a scaled version of another unit.
 * \pre Base is a Unit
 * \pre Scale is either a std::ratio or a type with a nested static constexpr double value();
 */
template<class Base, class Scale>
struct scaled_unit : unit_base<scaled_unit<Base, Scale> > {
    /// INTERNAL ONLY
    template<class F, class T>
    using _boost_units2_apply = typename F::template apply_scaled<Base, Scale>;
    /// INTERNAL ONLY
    auto operator<=>(const scaled_unit&) const = default;
};

/**
 * Represents a unit that is a combination of other units.
 * \pre All the elements of P must be specializations of dim
 */
template<class... P>
struct compound_unit : unit_base<compound_unit<P...>> {
    /// INTERNAL ONLY
    auto operator<=>(const compound_unit&) const = default;
};

/// INTERNAL ONLY
/// Specializations to make compound_unit useable with merge.
template<>
struct compound_unit<> : unit_base<compound_unit<>> {
    static constexpr const bool empty = true;
    template<class F, class T>
    using _boost_units2_apply = typename F::template apply_compound<>;
    /// INTERNAL ONLY
    auto operator<=>(const compound_unit&) const = default;
};
/// INTERNAL ONLY
template<class P0, class... P>
struct compound_unit<P0, P...> : unit_base<compound_unit<P0,P...>> {
    static constexpr const bool empty = false;
    using front = P0;
    using pop_front = compound_unit<P...>;
    template<class F, class T>
    using _boost_units2_apply = typename F::template apply_compound<P0, P...>;
    /// INTERNAL ONLY
    auto operator<=>(const compound_unit&) const = default;
};

namespace detail {

template<class F, class T>
using visit = typename T::template _boost_units_apply2<F, T>;

constexpr int const_strcmp(const char * lhs, const char * rhs)
{
    return (*lhs && *rhs) ?
        (*lhs == *rhs ? const_strcmp(lhs+1, rhs+1) : (*lhs < *rhs ? -1 : 1)) :
        ((!*lhs && !*rhs) ? 0 : (!*lhs ? -1 : 1));
}

template<class T, class U>
struct scale_compare {
    static constexpr const int value = T::value() < U::value?-1:(T::value()>U::value()?1:0);
};
template<class T, long long N, long long D>
struct scale_compare<T,std::ratio<N,D>>
{
    static const constexpr int value = 1;
};
template<class T, long long N, long long D>
struct scale_compare<std::ratio<N,D>,T>
{
    static const constexpr int value = -1;
};
template<long long N1, long long D1, long long N2, long long D2>
struct scale_compare<std::ratio<N1,D1>,std::ratio<N2,D2>>
{
    static const constexpr int value = std::ratio_less<std::ratio<N1,D1>,std::ratio<N2,D2>>::value?-1:
        (std::ratio_less<std::ratio<N2,D2>,std::ratio<N1,D1>>::value?1:0);
};

// For two user-defined units, compare by name
template<class T, class U>
struct unit_compare_impl {
    static constexpr const int value = const_strcmp(T::name, U::name);
    static_assert(std::is_same<T, U>::value || value != 0, "Different units cannot have the same name.");
};

// Compare compond units lexicographically
template<class T0, class... T, class U0, class... U>
struct unit_compare_impl<compound_unit<T0, T...>, compound_unit<U0, U...> >
{
    static constexpr const int value = unit_compare_impl<typename T0::base, typename U0::base>::value?
        unit_compare_impl<typename T0::base, typename U0::base>::value:
        (std::ratio_less<typename T0::exponent, typename U0::exponent>::value?-1:1);
};
template<class T0, class... T, class... U>
struct unit_compare_impl<compound_unit<T0, T...>, compound_unit<T0, U...> >
{
    static constexpr const int value = unit_compare_impl<compound_unit<T...>, compound_unit<U...>>::value;
};
template<class... T>
struct unit_compare_impl<compound_unit<T...>, compound_unit<> >
{
    static constexpr const int value = 1;
};
template<class... T>
struct unit_compare_impl<compound_unit<>, compound_unit<T...> >
{
    static constexpr const int value = -1;
};
template<>
struct unit_compare_impl<compound_unit<>, compound_unit<> >
{
    static constexpr const int value = 0;
};

// For scaled_unit compare the base first, then the exponent
template<class B1, class E1, class B2, class E2>
struct unit_compare_impl<scaled_unit<B1, E1>, scaled_unit<B2, E2> >
{
    static const constexpr int value = (unit_compare_impl<B1, B2>::value != 0)?
        (unit_compare_impl<B1, B2>::value != 0):
        scale_compare<E1, E2>::value;
};

// cross type comparisons
// user-defined unit < scaled_unit < compound_unit
template<class... T, class U>
struct unit_compare_impl<compound_unit<T...>, U>
{
    static const constexpr int value = 1;
};
template<class T, class... U>
struct unit_compare_impl<T, compound_unit<U...>>
{
    static const constexpr int value = -1;
};
template<class... T, class B, class E>
struct unit_compare_impl<compound_unit<T...>, scaled_unit<B,E> >
{
    static const constexpr int value = 1;
};
template<class B, class E, class... U>
struct unit_compare_impl<scaled_unit<B, E>, compound_unit<U...>>
{
    static const constexpr int value = -1;
};
template<class T, class B, class E>
struct unit_compare_impl<T, scaled_unit<B,E> >
{
    static const constexpr int value = -1;
};
template<class B, class E, class U>
struct unit_compare_impl<scaled_unit<B, E>, U>
{
    static const constexpr int value = 1;
};

// pre: T and U are both compound_units
template<class T, class U>
using compound_unit_multiply = detail::merge<unit_compare_impl, compound_unit, T, U>;

// Wrap any class in a compound_unit iff it is not already a compound_unit.
template<class T>
struct as_compound_unit_impl { typedef compound_unit<dim<T, std::ratio<1, 1> > > type; };
template<class... T>
struct as_compound_unit_impl<compound_unit<T...> > { typedef compound_unit<T...> type; };
template<class T>
using as_compound_unit = typename as_compound_unit_impl<T>::type;

// Unwrap any compound units of the form U^1.
// Folds scaled_units that use std::ratio.
// May perform other normalization as needed in the future.
// Note: It is assumed that this normalization is applied
// consistently, so we never need to fix more than the outer layer.
template<class T>
struct simplify_unit_impl { using type = T; };
template<class T>
struct simplify_unit_impl<compound_unit<dim<T, std::ratio<1, 1> > > > { using type = T; };
template<class T, std::intmax_t N1, std::intmax_t D1, std::intmax_t N2, std::intmax_t D2>
struct simplify_unit_impl<scaled_unit<scaled_unit<T, std::ratio<N1,D1>>,std::ratio<N2,D2>>> {
    using new_scale = std::ratio_multiply<std::ratio<N1,D1>,std::ratio<N2,D2>>;
    using type = boost::mp11::mp_if_c<(new_scale::num==1&&new_scale::den==1), T, scaled_unit<T, new_scale> >;
};
template<class T>
struct simplify_unit_impl<scaled_unit<T,std::ratio<1,1>>> { using type = T; };
// resolve ambiguity
template<class T, std::intmax_t N, std::intmax_t D>
struct simplify_unit_impl<scaled_unit<scaled_unit<T,std::ratio<N,D>>,std::ratio<1,1>>> {
    using type = scaled_unit<T,std::ratio<N,D>>;
};
template<class T>
using simplify_unit = typename simplify_unit_impl<T>::type;

// The result of multiplying two units
template<class T, class U>
using unit_multiply = simplify_unit<compound_unit_multiply<as_compound_unit<T>, as_compound_unit<U> > >;

template<class T, class E>
struct unit_pow_impl;
template<class... T, class... E, class R>
struct unit_pow_impl<compound_unit<dim<T, E>...>, R> {
    using type = compound_unit<dim<T, std::ratio_multiply<E, R> >...>;
};
template<class... T, class... E>
struct unit_pow_impl<compound_unit<dim<T, E>...>, std::ratio<0>> {
    using type = compound_unit<>;
};
template<class T, class E>
using unit_pow = simplify_unit<typename unit_pow_impl<as_compound_unit<T>, E>::type>;

template<class T, class U>
using unit_divide = unit_multiply<T, unit_pow<U, std::ratio<-1>>>;

// determine whether a type is a unit
template<class T>
using requires_unit = typename T::_boost_units2_is_unit;

template<class T, class E = void>
struct is_unit_impl {
    using type = std::false_type;
};
template<class T>
struct is_unit_impl<T, requires_unit<T>> {
    using type = std::true_type;
};

// A scale must either be a std::ratio or inherit from scale_base
// This only handles the latter.
template<class T>
using requires_scale = typename T::_boost_units2_is_scale;

} // namespace detail

template<class T, class U, class = detail::requires_unit<T>, class = detail::requires_unit<U> >
constexpr auto operator*(T, U) -> detail::unit_multiply<T, U>
{ return {}; }

template<class T, class U, class = detail::requires_unit<T>, class = detail::requires_unit<U> >
constexpr auto operator/(T, U) -> detail::unit_divide<T, U>
{ return {}; }

// multiplying a unit by a std::ratio creates a scaled_unit
template<class T, long long N, long long D, class = detail::requires_unit<T>>
constexpr auto operator*(T, std::ratio<N,D>) -> detail::simplify_unit<scaled_unit<T, typename std::ratio<N,D>::type>>
{ return {}; }
template<class T, long long N, long long D, class = detail::requires_unit<T>>
constexpr auto operator*(std::ratio<N,D>, T) -> detail::simplify_unit<scaled_unit<T, typename std::ratio<N,D>::type>>
{ return {}; }

// multiplying a unit by any scale gives a scaled unit
template<class T, class U, class = detail::requires_unit<T>, class = detail::requires_scale<U> >
constexpr auto operator*(T, U) -> detail::simplify_unit<scaled_unit<T, U>>
{ return {}; }
template<class T, class U, class = detail::requires_scale<T>, class = detail::requires_unit<U> >
constexpr auto operator*(T, U) -> detail::simplify_unit<scaled_unit<U,T>>
{ return {}; }

template<std::intmax_t N, class T, class = detail::requires_unit<T>>
constexpr auto pow(T) -> detail::unit_pow<T, std::ratio<N>>
{ return {}; }

    template<class T, std::intmax_t N, std::intmax_t D, class = detail::requires_unit<T>>
constexpr auto pow(T, std::ratio<N,D>) -> detail::unit_pow<T, std::ratio<N,D>>
{ return {}; }

// Conversion support

namespace detail {

template<class... D>
struct scale_list;

template<>
struct scale_list<> {
    static const constexpr bool empty = true;
};

template<class D0, class... D>
struct scale_list<D0, D...> {
    static const constexpr bool empty = false;
    using front = D0;
    using pop_front = scale_list<D...>;
};

template<class T, class E>
struct scale_list_pow_impl;
template<class... T, class... E, class R>
struct scale_list_pow_impl<scale_list<dim<T, E>...>, R> {
    using type = scale_list<dim<T, std::ratio_multiply<E, R> >...>;
};
template<class T, class E>
using scale_list_pow = typename scale_list_pow_impl<T, E>::type;

template<class T, class U>
using scale_list_multiply = detail::merge<scale_compare, scale_list, T, U>;

struct flatten_scale_impl;
template<class T>
using flatten_scale = visit<flatten_scale_impl, T>;
struct flatten_scale_impl
{
    template<class T>
    using apply_base = scale_list<>;

    template<class Base, class Scale>
    using apply_scaled = scale_list_multiply<flatten_scale<Base>, scale_list<dim<Scale, std::ratio<1>>>>;

    template<class... T>
    using apply_compound = boost::mp11::mp_fold<boost::mp11::mp_list<scale_list_pow<flatten_scale<typename T::base>, typename T::exponent>...>, scale_list<>, scale_list_multiply>;
};

struct dimension_check_impl;
template<class T>
using dimension_check = visit<dimension_check_impl, T>;
struct dimension_check_impl
{
    template<class T>
    using apply_base = typename T::_boost_units2_type;

    template<class Base, class Scale>
    using apply_scaled = dimension_check<Base>;

    template<class... T>
    using apply_compound = ::boost::mp11::mp_fold< boost::mp11::mp_list<unit_pow<dimension_check<typename T::base>, typename T::exponent>...>, compound_unit<>, unit_multiply>;
};

template<class T>
constexpr double get_value(T) { return T::value(); }
template<long long N, long long D>
constexpr double get_value(std::ratio<N, D>) { return static_cast<double>(N)/D; }

template<class T, class U>
struct multiplier {
    static constexpr double value() { return ::boost::units2::detail::get_value(T()) * ::boost::units2::detail::get_value(U()); }
};

template<class B, class E>
struct power {
    static /*constexpr*/ double value() { return ::std::pow(::boost::units2::detail::get_value(B()), ::boost::units2::detail::get_value(E())); }
};

// Returns 0 if overflow would happen
// precondition: all ratios are positive
template<class T, class U>
struct safe_ratio_multiply
{
    static const constexpr long long gcd1 = ::boost::integer::static_gcd<T::num, U::den>::value;
    static const constexpr long long gcd2 = ::boost::integer::static_gcd<U::num, T::den>::value;
    static const constexpr bool overflow =
        (std::numeric_limits<long long>::max()/(T::num/gcd1) > (U::num/gcd2)) ||
        (std::numeric_limits<long long>::max()/(T::den/gcd1) > (U::den/gcd1));
    using type = std::ratio<
        overflow?0:(T::num/gcd1)*(U::num/gcd2),
        overflow?1:(T::den/gcd1)*(U::den/gcd1)>;
};

constexpr long long safe_multiply(long long lhs, long long rhs)
{
    return (std::numeric_limits<long long>::max)()/lhs >= rhs? lhs*rhs : 0;
}
constexpr long long safe_square(long long arg)
{
    return safe_multiply(arg, arg);
}
constexpr long long safe_power(long long base, long long exponent) {
    return exponent == 1? base : safe_multiply(safe_square(safe_power(base, exponent/2)), (exponent%2?base:1));
}

template<class B, long long E>
struct safe_ratio_pow {
    static const constexpr long long abs_exponent = E < 0? -E : E;
    static const constexpr long long num = safe_power(E<0?B::den:B::num, abs_exponent);
    static const constexpr long long den = safe_power(E<0?B::num:B::den, abs_exponent);
    static const constexpr bool overflow = num==0||den==0;
    using type = std::ratio<overflow?0:num,overflow?1:den>;
};

template<class T, class U>
struct fold_conversion_impl { using type = multiplier<T, U>; };

template<long long N1, long long D1, long long N2, long long D2>
struct fold_conversion_impl<std::ratio<N1,D1>,std::ratio<N2,D2>>
{
    using result1 = typename safe_ratio_multiply<std::ratio<N1, D1>, std::ratio<N2, D2> >::type;
    using type = ::boost::mp11::mp_if_c<result1::num!=0, result1, multiplier<std::ratio<N1,D1>,std::ratio<N2,D2> > >;
};

template<class T>
struct evaluate_power;
template<class Base, class Exponent>
struct evaluate_power<dim<Base, Exponent> >
{
    using type = power<Base, Exponent>;
};
template<long long N, long long D, long long E>
struct evaluate_power<dim<std::ratio<N,D>, std::ratio<E> > >
{
    using result1 = typename safe_ratio_pow<std::ratio<N,D>, E>::type;
    using type = ::boost::mp11::mp_if_c<result1::num!=0, result1, power<std::ratio<N,D>, std::ratio<E> > >;
};

template<class T, class U>
using conversion_fold_op = typename fold_conversion_impl<T,U>::type;

template<class... T>
struct fold_conversion;
template<class... T>
struct fold_conversion<scale_list<T...>> {
    using type = boost::mp11::mp_fold<boost::mp11::mp_list<typename evaluate_power<T>::type...>,std::ratio<1>,conversion_fold_op>;
};

template<class T, class U>
void check_conversion() {
    static_assert(std::is_same<T, U>::value,
        "Cannot convert units with different dimensions.");
}

} // namespace detail

template<class T, class U, class = detail::requires_unit<T>, class = detail::requires_unit<U>>
constexpr bool has_same_dimension(T, U)
{
    return std::is_same<detail::dimension_check<T>, detail::dimension_check<U>>::value;
}

template<class T, class U, class = detail::requires_unit<T>, class = detail::requires_unit<U>>
constexpr double conversion_factor(T, U)
{
    // Indirection to make sure that the reduced dimensions appear
    // in the template backtrace.
    detail::check_conversion<detail::dimension_check<T>, detail::dimension_check<U>>();
    return ::boost::units2::detail::get_value(typename detail::fold_conversion<detail::flatten_scale<detail::unit_divide<T, U>>>::type());
}

}
}

#endif


// -----------------------------------------------------------------------
// RTToolbox - DKFZ radiotherapy quantitative evaluation library
//
// Copyright (c) German Cancer Research Center (DKFZ),
// Software development for Integrated Diagnostics and Therapy (SIDT).
// ALL RIGHTS RESERVED.
// See rttbCopyright.txt or
// http://www.dkfz.de/en/sidt/projects/rttb/copyright.html
//
// This software is distributed WITHOUT ANY WARRANTY; without even
// the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
// PURPOSE.  See the above copyright notices for more information.
//
//------------------------------------------------------------------------

#include "CompareDVH.h"

#include <boost/make_shared.hpp>

namespace rttb
{

	namespace testing
	{

		bool checkEqualDVH(DVHPointer aDVH1, DVHPointer aDVH2)
		{

			bool result;
			const double errorConstantDVH = 1e-4;
			result = lit::AreClose(aDVH1->getDeltaD(), aDVH2->getDeltaD(), errorConstantDVH);
			result = result && lit::AreClose(aDVH1->getDeltaV(), aDVH2->getDeltaV(), errorConstantDVH);
			result = result && (aDVH1->getDoseID() == aDVH2->getDoseID());
			result = result && (aDVH1->getStructureID() == aDVH2->getStructureID());
			result = result && lit::AreClose(aDVH1->getMaximum(), aDVH2->getMaximum(), errorConstantDVH);
			result = result && lit::AreClose(aDVH1->getMinimum(), aDVH2->getMinimum(), errorConstantDVH);
			result = result && lit::AreClose(aDVH1->getMean(), aDVH2->getMean(), errorConstantDVH);
			result = result && (aDVH1->getDataDifferential().size() == aDVH2->getDataDifferential().size());

			for (size_t i = 0; i < aDVH1->getDataDifferential().size(); i++)
			{
				result = result
				         && lit::AreClose(aDVH1->getDataDifferential().at(i), aDVH2->getDataDifferential().at(i),
							 errorConstantDVH);
			}

			return result;

		}

        rttb::testing::DVHPointer computeDiffDVH(DVHPointer aDVH1, DVHPointer aDVH2)
        {
            if (aDVH1->getDeltaD() == aDVH2->getDeltaD() && aDVH1->getDeltaV() == aDVH2->getDeltaV()){

                rttb::core::DVH::DataDifferentialType dvhData1 = aDVH1->getDataDifferential();
                rttb::core::DVH::DataDifferentialType dvhData2 = aDVH2->getDataDifferential();
                rttb::core::DVH::DataDifferentialType dvhDataDifference;

                auto it1 = dvhData1.cbegin();
                auto it2 = dvhData2.cbegin();

                while (it1 != dvhData1.cend() && it2 != dvhData2.cend())
                {
                    dvhDataDifference.push_back(*it1-*it2);
                    ++it1;
                    ++it2;
                }

                auto differenceDVH = ::boost::make_shared<core::DVH>(dvhDataDifference, aDVH1->getDeltaD(), aDVH1->getDeltaV(), aDVH1->getStructureID(), aDVH1->getDoseID());
                return differenceDVH;
            }
            else {
                return aDVH1;
            }
        }

    }//testing
}//rttb


// Copyright (c) Microsoft Corporation.
// SPDX-License-Identifier: Apache-2.0 WITH LLVM-exception

#include <cerrno>
#include <cmath>
#include <limits>
#include <type_traits>
#include <utility>

#pragma warning(push)
#pragma warning(disable : 4619) // #pragma warning: there is no warning number '%d'
#pragma warning(disable : 4643) // Forward declaring '%s' in namespace std is not permitted by the C++ Standard
#pragma warning(disable : 4702) // unreachable code
#pragma warning(disable : 5219) // implicit conversion from '%s' to '%s', possible loss of data

#define BOOST_CHRONO_HEADER_ONLY
#define BOOST_CONFIG_SUPPRESS_OUTDATED_MESSAGE
#define BOOST_MATH_DOMAIN_ERROR_POLICY   errno_on_error
#define BOOST_MATH_OVERFLOW_ERROR_POLICY ignore_error

// Using headers from Boost.Math
#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/beta.hpp>
#include <boost/math/special_functions/ellint_1.hpp>
#include <boost/math/special_functions/ellint_2.hpp>
#include <boost/math/special_functions/ellint_3.hpp>
#include <boost/math/special_functions/expint.hpp>
#include <boost/math/special_functions/hermite.hpp>
#include <boost/math/special_functions/laguerre.hpp>
#include <boost/math/special_functions/legendre.hpp>
#include <boost/math/special_functions/spherical_harmonic.hpp>
#include <boost/math/special_functions/zeta.hpp>
#include <boost/math/tools/config.hpp>
#include <boost/math/tools/precision.hpp>

#pragma warning(pop)

namespace {
    template <class _Func>
    _NODISCARD auto _Boost_call(const _Func& _Fn) noexcept {
        _TRY_BEGIN
        return _Fn();
        _CATCH_ALL
        errno     = EDOM;
        using _Ty = _STD decay_t<decltype(_Fn())>;
        return _STD numeric_limits<_Ty>::quiet_NaN();
        _CATCH_END
    }
} // unnamed namespace

_EXTERN_C
_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_assoc_laguerre(
    const unsigned int _Pn, const unsigned int _Pm, const double _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::laguerre(_Pn, _Pm, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_assoc_laguerref(
    const unsigned int _Pn, const unsigned int _Pm, const float _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::laguerre(_Pn, _Pm, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_assoc_legendre(
    const unsigned int _Pl, const unsigned int _Pm, const double _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] {
        auto _Result = ::boost::math::legendre_p(_Pl, _Pm, _Px);
        // Boost includes the Condon-Shortley phase term (-1)^m, std does not
        if ((_Pm & 1u) != 0) {
            _Result = -_Result;
        }
        return _Result;
    });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_assoc_legendref(
    const unsigned int _Pl, const unsigned int _Pm, const float _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] {
        auto _Result = ::boost::math::legendre_p(_Pl, _Pm, _Px);
        // Boost includes the Condon-Shortley phase term (-1)^m, std does not
        if ((_Pm & 1u) != 0) {
            _Result = -_Result;
        }
        return _Result;
    });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_beta(const double _Px, const double _Py) noexcept {
    return _Boost_call([=] { return ::boost::math::beta(_Px, _Py); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_betaf(const float _Px, const float _Py) noexcept {
    return _Boost_call([=] { return ::boost::math::beta(_Px, _Py); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_comp_ellint_1(const double _Pk) noexcept {
    return _Boost_call([=] { return ::boost::math::ellint_1(_Pk); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_comp_ellint_1f(const float _Pk) noexcept {
    return _Boost_call([=] { return ::boost::math::ellint_1(_Pk); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_comp_ellint_2(const double _Pk) noexcept {
    if (_STD isnan(_Pk)) {
        return _Pk;
    }

    return _Boost_call([=] { return ::boost::math::ellint_2(_Pk); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_comp_ellint_2f(const float _Pk) noexcept {
    if (_STD isnan(_Pk)) {
        return _Pk;
    }

    return _Boost_call([=] { return ::boost::math::ellint_2(_Pk); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_comp_ellint_3(const double _Pk, const double _Pnu) noexcept {
    if (_STD isnan(_Pk)) {
        return _Pk;
    }

    if (_STD isnan(_Pnu)) {
        return _Pnu;
    }

    return _Boost_call([=] { return ::boost::math::ellint_3(_Pk, _Pnu); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_comp_ellint_3f(const float _Pk, const float _Pnu) noexcept {
    if (_STD isnan(_Pk)) {
        return _Pk;
    }

    if (_STD isnan(_Pnu)) {
        return _Pnu;
    }

    return _Boost_call([=] { return ::boost::math::ellint_3(_Pk, _Pnu); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_cyl_bessel_i(const double _Pnu, const double _Px) noexcept {
    if (_STD isnan(_Pnu)) {
        return _Pnu;
    }

    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::cyl_bessel_i(_Pnu, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_cyl_bessel_if(const float _Pnu, const float _Px) noexcept {
    if (_STD isnan(_Pnu)) {
        return _Pnu;
    }

    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::cyl_bessel_i(_Pnu, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_cyl_bessel_j(const double _Pnu, const double _Px) noexcept {
    if (_STD isnan(_Pnu)) {
        return _Pnu;
    }

    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::cyl_bessel_j(_Pnu, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_cyl_bessel_jf(const float _Pnu, const float _Px) noexcept {
    if (_STD isnan(_Pnu)) {
        return _Pnu;
    }

    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::cyl_bessel_j(_Pnu, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_cyl_bessel_k(const double _Pnu, const double _Px) noexcept {
    if (_STD isnan(_Pnu)) {
        return _Pnu;
    }

    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::cyl_bessel_k(_Pnu, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_cyl_bessel_kf(const float _Pnu, const float _Px) noexcept {
    if (_STD isnan(_Pnu)) {
        return _Pnu;
    }

    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::cyl_bessel_k(_Pnu, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_cyl_neumann(const double _Pnu, const double _Px) noexcept {
    if (_STD isnan(_Pnu)) {
        return _Pnu;
    }

    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::cyl_neumann(_Pnu, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_cyl_neumannf(const float _Pnu, const float _Px) noexcept {
    if (_STD isnan(_Pnu)) {
        return _Pnu;
    }

    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::cyl_neumann(_Pnu, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_ellint_1(const double _Pk, const double _Pphi) noexcept {
    if (_STD isnan(_Pk)) {
        return _Pk;
    }

    if (_STD isnan(_Pphi)) {
        return _Pphi;
    }

    return _Boost_call([=] { return ::boost::math::ellint_1(_Pk, _Pphi); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_ellint_1f(const float _Pk, const float _Pphi) noexcept {
    if (_STD isnan(_Pk)) {
        return _Pk;
    }

    if (_STD isnan(_Pphi)) {
        return _Pphi;
    }

    return _Boost_call([=] { return ::boost::math::ellint_1(_Pk, _Pphi); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_ellint_2(const double _Pk, const double _Pphi) noexcept {
    if (_STD isnan(_Pk)) {
        return _Pk;
    }

    if (_STD isnan(_Pphi)) {
        return _Pphi;
    }

    return _Boost_call([=] { return ::boost::math::ellint_2(_Pk, _Pphi); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_ellint_2f(const float _Pk, const float _Pphi) noexcept {
    if (_STD isnan(_Pk)) {
        return _Pk;
    }

    if (_STD isnan(_Pphi)) {
        return _Pphi;
    }

    return _Boost_call([=] { return ::boost::math::ellint_2(_Pk, _Pphi); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_ellint_3(
    const double _Pk, const double _Pnu, const double _Pphi) noexcept {
    if (_STD isnan(_Pk)) {
        return _Pk;
    }

    if (_STD isnan(_Pnu)) {
        return _Pnu;
    }

    if (_STD isnan(_Pphi)) {
        return _Pphi;
    }

    return _Boost_call([=] { return ::boost::math::ellint_3(_Pk, _Pnu, _Pphi); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_ellint_3f(
    const float _Pk, const float _Pnu, const float _Pphi) noexcept {
    if (_STD isnan(_Pk)) {
        return _Pk;
    }

    if (_STD isnan(_Pnu)) {
        return _Pnu;
    }

    if (_STD isnan(_Pphi)) {
        return _Pphi;
    }

    return _Boost_call([=] { return ::boost::math::ellint_3(_Pk, _Pnu, _Pphi); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_expint(const double _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::expint(_Px); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_expintf(const float _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::expint(_Px); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_hermite(const unsigned int _Pn, const double _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::hermite(_Pn, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_hermitef(const unsigned int _Pn, const float _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::hermite(_Pn, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_laguerre(const unsigned int _Pn, const double _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::laguerre(_Pn, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_laguerref(const unsigned int _Pn, const float _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::laguerre(_Pn, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_legendre(const unsigned int _Pl, const double _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::legendre_p(_Pl, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_legendref(const unsigned int _Pl, const float _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::legendre_p(_Pl, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_riemann_zeta(const double _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::zeta(_Px); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_riemann_zetaf(const float _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::zeta(_Px); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_sph_bessel(const unsigned int _Pn, const double _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::sph_bessel(_Pn, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_sph_besself(const unsigned int _Pn, const float _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::sph_bessel(_Pn, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_sph_legendre(
    const unsigned int _Pl, const unsigned int _Pm, const double _Ptheta) noexcept {
    if (_STD isnan(_Ptheta)) {
        return _Ptheta;
    }

    return _Boost_call([=] { return ::boost::math::spherical_harmonic_r(_Pl, _Pm, _Ptheta, 0.0); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_sph_legendref(
    const unsigned int _Pl, const unsigned int _Pm, const float _Ptheta) noexcept {
    if (_STD isnan(_Ptheta)) {
        return _Ptheta;
    }

    return _Boost_call([=] { return ::boost::math::spherical_harmonic_r(_Pl, _Pm, _Ptheta, 0.0f); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_sph_neumann(const unsigned int _Pn, const double _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::sph_neumann(_Pn, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_sph_neumannf(const unsigned int _Pn, const float _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::sph_neumann(_Pn, _Px); });
}
_END_EXTERN_C

namespace {
    template <class _Ty>
    _NODISCARD _Ty _Hypot3(_Ty _Dx, _Ty _Dy, _Ty _Dz) noexcept { // type-oblivious backend for 3-arg hypot
        static_assert(_STD is_floating_point_v<_Ty>);
        _Dx = _STD abs(_Dx);
        _Dy = _STD abs(_Dy);
        _Dz = _STD abs(_Dz);

        constexpr _Ty _Inf = _STD numeric_limits<_Ty>::infinity();
        if (_Dx == _Inf || _Dy == _Inf || _Dz == _Inf) {
            return _Inf;
        }

        if (_Dy > _Dx) {
            _STD swap(_Dx, _Dy);
        }

        if (_Dz > _Dx) {
            _STD swap(_Dx, _Dz);
        }

        constexpr _Ty _Eps = ::boost::math::tools::epsilon<_Ty>();
        if (_Dx * _Eps >= _Dy && _Dx * _Eps >= _Dz) {
            return _Dx;
        }

        const auto _DyDx = _Dy / _Dx;
        const auto _DzDx = _Dz / _Dx;

        return _Dx * _STD sqrt(1 + _DyDx * _DyDx + _DzDx * _DzDx);
    }
} // unnamed namespace

_EXTERN_C
_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_hypot3(
    const double _Dx, const double _Dy, const double _Dz) noexcept {
    return _Hypot3(_Dx, _Dy, _Dz);
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_hypot3f(
    const float _Dx, const float _Dy, const float _Dz) noexcept {
    return _Hypot3(_Dx, _Dy, _Dz);
}
_END_EXTERN_C


#include <boost/function_output_iterator.hpp>
#include <boost/geometry/geometry.hpp>
#include <chrono>
#include <iostream>
#include <random>
#include <vector>

// number of dimensions (must be 2 or 3)
const int D = 2;

// default parameters (documented below)
const double DefaultParticleSpacing = 1;
const double DefaultAttractionDistance = 3;
const double DefaultMinMoveDistance = 1;
const int DefaultStubbornness = 0;
const double DefaultStickiness = 1;

// boost is used for its spatial index
using BoostPoint = boost::geometry::model::point<
    double, D, boost::geometry::cs::cartesian>;

using IndexValue = std::pair<BoostPoint, int>;

using Index = boost::geometry::index::rtree<
    IndexValue, boost::geometry::index::linear<4>>;

// Vector represents a point or a vector
class Vector {
public:
    Vector() :
        m_X(0), m_Y(0), m_Z(0) {}

    Vector(double x, double y) :
        m_X(x), m_Y(y), m_Z(0) {}

    Vector(double x, double y, double z) :
        m_X(x), m_Y(y), m_Z(z) {}

    double X() const {
        return m_X;
    }

    double Y() const {
        return m_Y;
    }

    double Z() const {
        return m_Z;
    }

    BoostPoint ToBoost() const {
        return BoostPoint(m_X, m_Y, m_Z);
    }

    double Length() const {
        return std::sqrt(m_X * m_X + m_Y * m_Y + m_Z * m_Z);
    }

    double LengthSquared() const {
        return m_X * m_X + m_Y * m_Y + m_Z * m_Z;
    }

    double Distance(const Vector &v) const {
        const double dx = m_X - v.m_X;
        const double dy = m_Y - v.m_Y;
        const double dz = m_Z - v.m_Z;
        return std::sqrt(dx * dx + dy * dy + dz * dz);
    }

    Vector Normalized() const {
        const double m = 1 / Length();
        return Vector(m_X * m, m_Y * m, m_Z * m);
    }

    Vector operator+(const Vector &v) const {
        return Vector(m_X + v.m_X, m_Y + v.m_Y, m_Z + v.m_Z);
    }

    Vector operator-(const Vector &v) const {
        return Vector(m_X - v.m_X, m_Y - v.m_Y, m_Z - v.m_Z);
    }

    Vector operator*(const double a) const {
        return Vector(m_X * a, m_Y * a, m_Z * a);
    }

    Vector &operator+=(const Vector &v) {
        m_X += v.m_X; m_Y += v.m_Y; m_Z += v.m_Z;
        return *this;
    }

private:
    double m_X;
    double m_Y;
    double m_Z;
};

// Lerp linearly interpolates from a to b by distance.
Vector Lerp(const Vector &a, const Vector &b, const double d) {
    return a + (b - a).Normalized() * d;
}

// Random returns a uniformly distributed random number between lo and hi
double Random(const double lo = 0, const double hi = 1) {
    static thread_local std::mt19937 gen(
        std::chrono::high_resolution_clock::now().time_since_epoch().count());
    std::uniform_real_distribution<double> dist(lo, hi);
    return dist(gen);
}

// RandomInUnitSphere returns a random, uniformly distributed point inside the
// unit sphere (radius = 1)
Vector RandomInUnitSphere() {
    while (true) {
        const Vector p = Vector(
            Random(-1, 1),
            Random(-1, 1),
            D == 2 ? 0 : Random(-1, 1));
        if (p.LengthSquared() < 1) {
            return p;
        }
    }
}

// Model holds all of the particles and defines their behavior.
class Model {
public:
    Model() :
        m_ParticleSpacing(DefaultParticleSpacing),
        m_AttractionDistance(DefaultAttractionDistance),
        m_MinMoveDistance(DefaultMinMoveDistance),
        m_Stubbornness(DefaultStubbornness),
        m_Stickiness(DefaultStickiness),
        m_BoundingRadius(0) {}

    void SetParticleSpacing(const double a) {
        m_ParticleSpacing = a;
    }

    void SetAttractionDistance(const double a) {
        m_AttractionDistance = a;
    }

    void SetMinMoveDistance(const double a) {
        m_MinMoveDistance = a;
    }

    void SetStubbornness(const int a) {
        m_Stubbornness = a;
    }

    void SetStickiness(const double a) {
        m_Stickiness = a;
    }

    // Add adds a new particle with the specified parent particle
    void Add(const Vector &p, const int parent = -1) {
        const int id = m_Points.size();
        m_Index.insert(std::make_pair(p.ToBoost(), id));
        m_Points.push_back(p);
        m_JoinAttempts.push_back(0);
        m_BoundingRadius = std::max(
            m_BoundingRadius, p.Length() + m_AttractionDistance);
        std::cout
            << id << "," << parent << ","
            << p.X() << "," << p.Y() << "," << p.Z() << std::endl;
    }

    // Nearest returns the index of the particle nearest the specified point
    int Nearest(const Vector &point) const {
        int result = -1;
        m_Index.query(
            boost::geometry::index::nearest(point.ToBoost(), 1),
            boost::make_function_output_iterator([&result](const auto &value) {
                result = value.second;
            }));
        return result;
    }

    // RandomStartingPosition returns a random point to start a new particle
    Vector RandomStartingPosition() const {
        const double d = m_BoundingRadius;
        return RandomInUnitSphere().Normalized() * d;
    }

    // ShouldReset returns true if the particle has gone too far away and
    // should be reset to a new random starting position
    bool ShouldReset(const Vector &p) const {
        return p.Length() > m_BoundingRadius * 2;
    }

    // ShouldJoin returns true if the point should attach to the specified
    // parent particle. This is only called when the point is already within
    // the required attraction distance.
    bool ShouldJoin(const Vector &p, const int parent) {
        m_JoinAttempts[parent]++;
        if (m_JoinAttempts[parent] < m_Stubbornness) {
            return false;
        }
        return Random() <= m_Stickiness;
    }

    // PlaceParticle computes the final placement of the particle.
    Vector PlaceParticle(const Vector &p, const int parent) const {
        return Lerp(m_Points[parent], p, m_ParticleSpacing);
    }

    // MotionVector returns a vector specifying the direction that the
    // particle should move for one iteration. The distance that it will move
    // is determined by the algorithm.
    Vector MotionVector(const Vector &p) const {
        return RandomInUnitSphere();
    }

    // AddParticle diffuses one new particle and adds it to the model
    void AddParticle() {
        // compute particle starting location
        Vector p = RandomStartingPosition();

        // do the random walk
        while (true) {
            // get distance to nearest other particle
            const int parent = Nearest(p);
            const double d = p.Distance(m_Points[parent]);

            // check if close enough to join
            if (d < m_AttractionDistance) {
                if (!ShouldJoin(p, parent)) {
                    // push particle away a bit
                    p = Lerp(m_Points[parent], p,
                        m_AttractionDistance + m_MinMoveDistance);
                    continue;
                }

                // adjust particle position in relation to its parent
                p = PlaceParticle(p, parent);

                // add the point
                Add(p, parent);
                return;
            }

            // move randomly
            const double m = std::max(
                m_MinMoveDistance, d - m_AttractionDistance);
            p += MotionVector(p).Normalized() * m;

            // check if particle is too far away, reset if so
            if (ShouldReset(p)) {
                p = RandomStartingPosition();
            }
        }
    }

private:
    // m_ParticleSpacing defines the distance between particles that are
    // joined together
    double m_ParticleSpacing;

    // m_AttractionDistance defines how close together particles must be in
    // order to join together
    double m_AttractionDistance;

    // m_MinMoveDistance defines the minimum distance that a particle will move
    // during its random walk
    double m_MinMoveDistance;

    // m_Stubbornness defines how many interactions must occur before a
    // particle will allow another particle to join to it.
    int m_Stubbornness;

    // m_Stickiness defines the probability that a particle will allow another
    // particle to join to it.
    double m_Stickiness;

    // m_BoundingRadius defines the radius of the bounding sphere that bounds
    // all of the particles
    double m_BoundingRadius;

    // m_Points stores the final particle positions
    std::vector<Vector> m_Points;

    // m_JoinAttempts tracks how many times other particles have attempted to
    // join with each finalized particle
    std::vector<int> m_JoinAttempts;

    // m_Index is the spatial index used to accelerate nearest neighbor queries
    Index m_Index;
};

int main() {
    // create the model
    Model model;

    // add seed point(s)
    model.Add(Vector());

    // {
    //     const int n = 3600;
    //     const double r = 1000;
    //     for (int i = 0; i < n; i++) {
    //         const double t = (double)i / n;
    //         const double a = t * 2 * M_PI;
    //         const double x = std::cos(a) * r;
    //         const double y = std::sin(a) * r;
    //         model.Add(Vector(x, y, 0));
    //     }
    // }

    // run diffusion-limited aggregation
    for (int i = 0; i < 100000; i++) {
        model.AddParticle();
    }

    return 0;
}


/**	\file gridmap.h
 * \brief Implementation an occupancymap updated from laser range scans for mobile robot.
 * The included is an Astar pathfinder, such that optimal obstacle-free paths can be
 * generated from the robots location to any goal location. OpenCV and Eigen are required.
 *
 * \author Hjalte Bested Møller
 * \date 4. April 2018
 *
*/

#ifndef GRIDMAP_H
#define GRIDMAP_H

#include <iostream>
#include <Eigen/Dense>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#define ASTAR_USE_OPENCV
#include "astar.hpp"

using namespace std;
using namespace Eigen;
using namespace cv;

/*
#ifndef USE_CV_DISTANCE_TRANSFORM
%define USE_CV_DISTANCE_TRANSFORM
#endif
*/

/** Class representing a laser scanner:
* It contains data structures for maintaining the laser scan data and the scanners pose in the inertial frame,
* functions for doing various coordinate systems transformations, and also implements
* a simulated laser scanner that facilitates development
*/
class LaserScanner {
public:
    /** Construct LaserScanner object */
    LaserScanner() {
        // Laser Parameters
        this->maxDistance = 4.0f;
        this->resolDeg = 0.36f;
        this->fov = 180.0f;
        this->init();
    }

    /**  Max Distance (Laser Measurement Range) in meters - If points are detected further from the scanner than this value, the point is ignored */
    float maxDistance;
    /** Minimum Distance (Laser Measurement Range) in meters - If points are detected closer to the scanner than this value, the point is ignored */
    float minDistance;
    /** Laser Scanner Resolution in Deg (Only Used By Simulator) */
    float resolDeg;
    /** Laser Scanner Resolution in Rad (Only Used By Simulator) */
    float resolRad;
    /** Laser Scanner Field-of-view in Deg (Only Used By Simulator) */
    float fov;

    /** Number of scanning lines (deduced from scan width and resolution in case of ) */
    int nScanLines;

    /** Matrix for holding the laser scan data format is scanData(2,nScanLines), where phi(i) = scanData(0,i), r(i) = scanData(1,i) */
    MatrixXd scanData;

    /** Matrix for holding the laser scan data format is scanCart(2,nScanLines), where x^s(i) = scanData(0,i), y^s(i) = scanData(1,i) */
    MatrixXd scanCart;

    /** Matrix for holding the laser scan data format is scanWorld(2,nScanLines), where x^i(i) = scanData(0,i), y^i(i) = scanData(1,i) */
    MatrixXd scanWorld;

    /** Initialise LaserScanner: Calculate the resolution in radian and determine the number of scanning lines nScanLines */
    void init(){
        this->resolRad = resolDeg*M_PI/180.0;
        this->nScanLines = round(fov/resolDeg+1);
    }

    /** Scan Pose in world coordinates (x,y,theta) */
    Vector3f pose;

    /** Set Scan Pose in world coordinates (x,y,theta) */
    void setPose(Vector3f const& newpose){
        pose = newpose;
    }
    /** Set Scan Pose in world coordinates (x,y,theta) */
    void setPose(float x, float y, float th){
        pose << x,y,th;
    }

    /** Return pose */
    Vector3f getPose(){
        return pose;
    }

    /** Convert polar coordinates to cartesian coordinates */
    MatrixXd polarToCart(MatrixXd const& scanPolar){
        const int nScanLines = scanPolar.cols();
        scanCart.resize(2,nScanLines);
        for(int i=0; i<nScanLines; i++){
            double const& phi = scanPolar(0,i);
            double const& rho = scanPolar(1,i);
            double x = rho * cos(phi);
            double y = rho * sin(phi);
            scanCart(0,i) = x;
            scanCart(1,i) = y;
        }
        return scanCart;
    }

    /** Convert cartesian coordinates in the scanner frame to cartesian coordinates in the world frame */
    MatrixXd cartToWorld(MatrixXd const& scanCart, Vector3f const& pose){
        const int& nScanLines = scanCart.cols();
        scanWorld.resize(2,nScanLines);
        double const& x = pose(0);
        double const& y = pose(1);
        double const& th = pose(2);
        double costh = cos(th);
        double sinth = sin(th);

        // Transform all the coordinates and store them in scanWorld
        for(int i=0; i<nScanLines; i++){
            double const& xs = scanCart(0,i);
            double const& ys = scanCart(1,i);
            double xw = xs*costh - ys*sinth + x;
            double yw = ys*costh + xs*sinth + y;
            scanWorld(0,i) = xw;
            scanWorld(1,i) = yw;
        }
        return scanWorld;
    }

    /** Convert polar coordinates in the scanner frame to cartesian coordinates in the world frame directly */
    inline MatrixXd polarToWorld(MatrixXd const& scanPolar, Vector3f const& pose){
        return cartToWorld(polarToCart(scanPolar),pose);
    }

    /** Overloaded convinience method: Convert cartesian coordinates in the scanner frame to cartesian coordinates in the world frame */
    inline MatrixXd cartToWorld(MatrixXd const& scanCart){
        return cartToWorld(scanCart, this->pose);
    }

    /** Overloaded convinience method: Convert polar coordinates in the scanner frame to cartesian coordinates in the world frame directly */
    inline MatrixXd polarToWorld(MatrixXd const& scanPolar){
        return polarToWorld(scanPolar,pose);
    }

    /** Return scan in polar (default) coordinates */
    inline MatrixXd getScanPolar(){
        return scanData;
    }

    /** Return scan in Laser local cartesian coordinates */
    inline MatrixXd getScanCart(){
        return polarToCart(scanData);
    }

    /** Return scan in world cartesian coordinates, based on the internal pose */
    inline MatrixXd getScanCartWorld(){
        return polarToWorld(scanData, pose);
    }

    /** Resize MatrixXd scanData such that it has two rows and nScanLines columns */
    void resize(int nScanLines){
        this->nScanLines = nScanLines;
        scanData.resize(2,nScanLines);
    }

    /** When working with a real laserscanner this is the way to fill the in the scanData */
    void setScanPoint(int i, double phi, double r){
        scanData(0,i) = phi;
        scanData(1,i) = r;
    }

    /** Simulate a laser scan - The lines are defined as a matrix where each column represents a line and the rows are [x1, y1, x2, y2] */
    MatrixXd simScan(Vector3f const& pose, MatrixXd const& lines){
        int nLines = lines.cols();
        double x = pose(0);
        double y = pose(1);
        double th = pose(2);
        double costh = cos(th);
        double sinth = sin(th);
        // cout << "th=" << th << endl;

        /* ----------------------------------------
         * ---- Pre-allocate the memory needs -----
         * --------------------------------------*/

        // Global system coordinates
        VectorXd xStart(nLines);
        VectorXd yStart(nLines);
        VectorXd xEnd(nLines);
        VectorXd yEnd(nLines);

        // Laser scanner local system coordinates
        VectorXd xStartTrans(nLines);
        VectorXd yStartTrans(nLines);
        VectorXd xEndTrans(nLines);
        VectorXd yEndTrans(nLines);

        // Parameters for the line equations of the form: a*x+ b*y=c
        VectorXd a(nLines);
        VectorXd b(nLines);
        VectorXd c(nLines);

        // Rezise the scanData matrix, if the size is correct already, Eigen
        // ensures that this is a no-operation
        scanData.resize(2,nScanLines);

        /* ------------------------------------------------------------------------
         * --- Conversion of the lines from global frame to scanner local frame ---
         * --------------------------------------------------------------------- */

        // For each line:
        // cout << "nLines" << nLines << endl;

        for(int i=0; i<nLines; i++){
            // Start and end values for the lines ending points.
            xStart(i) = lines(0,i);
            yStart(i) = lines(1,i);
            xEnd(i)   = lines(2,i);
            yEnd(i)   = lines(3,i);
            // cout << "Line = (" << xStart(i) << "," << yStart(i) << "," << xEnd(i) << "," << yEnd(i) << ")" << endl;

            // Transformation of the lines to the laser scanner's coordinate system.
            // Lines are converted to the new coordinate system.
            xStartTrans(i) = (xStart(i)-x)*costh + (yStart(i)-y)*sinth;
            yStartTrans(i) = (yStart(i)-y)*costh - (xStart(i)-x)*sinth;
            xEndTrans(i) = (xEnd(i)-x)*costh   + (yEnd(i)-y)*sinth;
            yEndTrans(i) = (yEnd(i)-y)*costh   - (xEnd(i)-x)*sinth;
            // cout << "LineTrans = (" << xStartTrans(i) << "," << yStartTrans(i) << "," << xEndTrans(i) << "," << yEndTrans(i) << ")" << endl;

            // Starting and ending points are swapped if the x value of the
            // starting point is bigger than the x value of the ending point.
            if( xStartTrans(i) > xEndTrans(i)){
                double temp = xStartTrans(i);
                xStartTrans(i) = xEndTrans(i);
                xEndTrans(i) = temp;
                temp = yStartTrans(i);
                yStartTrans(i) = yEndTrans(i);
                yEndTrans(i) = temp;
            }

            if( xStartTrans(i) == xEndTrans(i)){
                xEndTrans(i) += 1e-6;
            }
            // cout << "LineTrans = (" << xStartTrans(i) << "," << yStartTrans(i) << "," << xEndTrans(i) << "," << yEndTrans(i) << ")" << endl;

            // The line equations are calculated
            // a*x+ b*y=c - IMPORTANT: Variables related to the line equations must
            // be double precition to avoid numerical issues!
            a(i) = yStartTrans(i)-yEndTrans(i);
            b(i) = xEndTrans(i)-xStartTrans(i);
            double a2 = a(i)*a(i);
            double b2 = b(i)*b(i);
            double l = sqrt(a2+b2);
            a(i) = a(i)/l;
            b(i) = b(i)/l;
            c(i) = a(i)*xStartTrans(i)+b(i)*yStartTrans(i);
            // cout << "Line " << i << ": " << a(i) << "x + " << b(i) << "y = " << c(i)<< endl;
        }

        /* -------------------------------------------------------------------
         * Conversion of what the laser scanner sees to polar coordinates.
         * -------------------------------------------------------------------*/

        // For each laser scanner angle:
        for(int i=0; i<nScanLines; i++){
            // Laser scanner maximum measured distance maxDistance(in meters)
            // Closest distance from the laser scanner.
            double min_dist = maxDistance;

            // Current laser scanner angle
            double phi = i*resolRad-fov/2.0*M_PI/180.0;
            // Find distance from the lines to laser scanner in the current angle
            double cosphi = cos(phi);
            double sinphi = sin(phi);
            // cout << "cos(phi)=" << cosphi << ", sin(phi)=" << sinphi << endl;
            for(int j=0; j<nLines; j++){
                double temp = a(j)*cosphi + b(j)*sinphi;
                if(abs(temp) > 1e-8){
                    double t = c(j)/temp;
                    if(t>0 && t<min_dist){
                        if(abs(xStartTrans(j)-xEndTrans(j)) > 1e-8){
                            if(t*cosphi < xEndTrans(j) && t*cosphi > xStartTrans(j)) min_dist=t;
                        else
                            if(yEndTrans(j) > yStartTrans(j)){
                                if(t*sinphi < yEndTrans(j) && t*sinphi > yStartTrans(j)) min_dist=t;
                            } else {
                                if(t*sinphi > yEndTrans(j) && t*sinphi < yStartTrans(j)) min_dist=t;
                            }

                        }
                    }
                }
            }

            // The polar coordinates returned
            scanData(0,i) = phi;
            scanData(1,i) = min_dist;
        }
        return scanData;
    }

    /** Simulate a laser scan - The lines are defined as a matrix where each column represents a line and the rows are [x1, y1, x2, y2] */
    inline MatrixXd simScan(MatrixXd const& lines){
        return simScan(pose,lines);
    }
};

/** Implementation an occupancy grid-map updated from laser range scans including path finder for mobile robot.
 * The included is an Astar pathfinder, such that optimal obstacle-free paths can be
 * generated from the robots location to any goal location.
 * OpenCV and Eigen are required.
 */
class GridMap {
public:
    /** The raw map data: -1:Unknown, 0: Free, 1:Obstacle */
    Mat mapData;
    /** The dialated map data: -1:Unknown, 0: Free, 1:Obstacle */
    Mat dialatedMap;
    /** OpenCV Structuring Element used for map dilation.  */
    Mat strel;

    #ifdef USE_CV_DISTANCE_TRANSFORM
    /** The inverse of the dialated map, used for distance transform */
    Mat invDialatedMap;
    /** The distance transformed map as a 32bit float */
    Mat distanceMap_32F;
    /** The distance transformed map as char */
    Mat distanceMap;
    #endif

    /** If wrapMap = true, the map can wrap around the edges such that the map can be used locally */
    bool wrapMap = true;

    /** The size of the map cells in square meters */
    float cellSize = 1;
    /** The with of the map in pixels */
    unsigned long width = 0;
    /** The height of the map in pixels */
    unsigned long height = 0;
    /** The size of the map in pixels, i.e. width*height */
    unsigned long size = 0;

    /** The position of x=0 in the map, specified in meters */
    float xoffset = 0;
    /** The position of y=0 in the map, specified in meters */
    float yoffset = 0;

    /** Flag that determines how cells are set to occupied in the map */
    int fillMode = -1;
    /** Flag that determines how cells are set to free in the map */
    int clearMode = 0;

    /** A* search algorithm */
    Astar astar;
    /** The current cell position of the LaserScanner */
    Point scanPosCell;

    /** Construct an uninitialized GridMap object */
    GridMap() { }
    /** Construct an initialized GridMap object */
    GridMap(float widthInMeters, float heightInMeters, float cellSize) {
        this->resize(widthInMeters, heightInMeters, cellSize);
    }

    /** Resize the and all the related data structures */
    void resize(float widthInMeters, float heightInMeters, float cellSize){
        this->cellSize = cellSize;
        this->height = ceil(heightInMeters/cellSize);
        this->width  = ceil(widthInMeters/cellSize);
        this->size = width * height;
        this->setOffset(widthInMeters/2.0,heightInMeters/2.0);
        mapData.create(height,width,CV_8SC1);
        dialatedMap.create(height,width,CV_8SC1);
        #ifdef USE_CV_DISTANCE_TRANSFORM
        invDialatedMap.create(height,width,CV_8SC1);
        distanceMap_32F.create(height,width,CV_32F);
        distanceMap.create(height,width,CV_8SC1);
        #endif
        astar.resize(height, width);
        astar.wrapMap = wrapMap;
        this->clear();
    }

    /** Clear the map */
    void clear(){
        mapData.setTo(-1);
        dialatedMap.setTo(0);
        #ifdef USE_CV_DISTANCE_TRANSFORM
        distanceMap.setTo(0);
        #endif
        astar.clearAll();
    }

    /** Return the width of the map in meters, i.e. width*cellSize */
    float widthInMeters() {
        return width*cellSize;
    }
    /** Return the height of the map in meters, i.e. height*cellSize */
    float heightInMeters() {
        return height*cellSize;
    }

    /** Set the position of (x=0,y=0) in the map, specified in meters */
    void setOffset(float xoffset, float yoffset){
        this->xoffset = xoffset;
        this->yoffset = yoffset;
    }

    /** If wrapMap is enabled the map is wrapped around the edges. This allows the map to be used in a more local sence. */
    void setWrapMap(bool wrapMap){
        this->wrapMap = wrapMap;
        astar.wrapMap = wrapMap;
    }

    /** Make Structuring Element for Map dilation - Dilates as circle by default */
    void makeStrel(float width, int dialation_type = MORPH_ELLIPSE){
        int dilationSize = round(0.5*(width-cellSize)/cellSize);
        cout << "dilationSize=" << dilationSize << endl;
        strel = getStructuringElement(
            dialation_type,
            Size( 2*dilationSize+1, 2*dilationSize+1 ),
            Point( dilationSize, dilationSize ) );
    }

    /** An implementation of Bresenham's line algorithm, see http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm */
    vector<Point> bresenhamPoints(int x0, int y0, int x1, int y1){
        Point point;
        vector<Point> points;
        int ystep = -1;

        bool steep=abs(y1-y0)>abs(x1-x0);

        if(steep){ // slope more than 1
            int tempx0=x0; x0 = y0; y0 = tempx0; // Swapping
            int tempx1=x1; x1 = y1; y1 = tempx1; // Swapping
        }
        if(x0>x1){ // line goes to the left
            int tempx0=x0; x0=x1; x1=tempx0; // Swapping
            int tempy0=y0; y0=y1; y1=tempy0; // Swapping
        }

        int deltaX = x1-x0;
        int deltaY = abs(y1-y0);
        int N = deltaX+1;
        float error=-N/2;

        int x=x0;
        int y=y0;

        if(y0<y1) ystep=1;

        for(int i=0; i<N; i++){
            if(steep){
                point.x = y;
                point.y = x;
            } else {
                point.x = x;
                point.y = y;
            }
            x++;
            error=error+deltaY;
            if(error >= 0){
                y=y+ystep;
                error=error-deltaX;
            }
            points.push_back(point);
        }
        return points;
    }

    /** Round towards zero */
    int fix(float val){
        if(val > 0) return floor(val);
        if(val < 0) return ceil(val);
        return val;
    }

    /** Convert a point from world coordinates to map cell coordinates */
    inline Point worldToCell(float x, float y){
        int& rows = mapData.rows;
        x = x+xoffset;
        y = y+yoffset;
        Point cell;
        cell.x = x/cellSize;
        cell.y = rows - floor(y/cellSize + 1);
        return cell;
    }

    /** Convert a point from world coordinates to map cell coordinates */
    inline Point worldToCell(Point2f world){
        return worldToCell(world.x,world.y);
    }

    /** Convert a point from map cell coordinates to world coordinates */
    inline Point2f cellToWorld(int x, int y){
        Point2f world;
        // Translate from map coordinates to world coordinates (center of cell)
        world.x =  (x+0.5) * cellSize - xoffset;
        world.y =  (mapData.rows-y-0.5) * cellSize - yoffset;
        return world;
    }

    /** Convert a point from map cell coordinates to world coordinates */
    inline Point2f cellToWorld(Point cell){
        return cellToWorld(cell.x,cell.y);
    }

    /** wrap a point around the edges of the map. */
    inline Point wrap(Point cell){
        // Wrapping the map
        while(cell.x < 0)               cell.x += mapData.cols;
        while(cell.x >= mapData.cols)   cell.x -= mapData.cols;
        while(cell.y < 0)               cell.y += mapData.rows;
        while(cell.y >= mapData.rows)   cell.y -= mapData.rows;
        return cell;
    }

    /** wrap an x-coordinate around the edges of the map. If wrapMap is false this is a no-operation. */
    inline int wx(int x){
        if(wrapMap){ // Wrapping the map
            while(x < 0)                x += mapData.cols;
            while(x >= mapData.cols)    x -= mapData.cols;
        }
        return x;
    }
    /** wrap an y-coordinate around the edges of the map. If wrapMap is false this is a no-operation. */
    inline int wy(int y){
        if(wrapMap){ // Wrapping the map
            while(y < 0)                y += mapData.rows;
            while(y >= mapData.rows)    y -= mapData.rows;
        }
        return y;
    }

    /** Reset a MapNode in the Astar instance and set its (x,y) and type this is needed for wrapMap to work */
    inline void resetMapNode(int x, int y, char value){
        MapNode*node = astar.mapAt(x,y);
        if(node != NULL){
            node->x = x;
            node->y = y;
            node->clear(value);
        }
    }

    /** Set a cell to a new value (nodetype) */
    void setCell(int const& x, int const& y, char const& value, int const& mode){
        int rows = mapData.rows;
        int cols = mapData.cols;
        int xm = wx(x);
        int ym = wy(y);

        if((0 <= xm && xm < cols) && (0 <= ym && ym < rows)){
            resetMapNode(x,y,value);
            mapData.at<char>(ym,xm) = value;

            int xm1=wx(xm-1); int xp1=wx(xm+1); int ym1=wy(ym-1); int yp1=wy(ym+1);
            if(value > 0 && mode == -1){
                if(0 <= xm1   && mapData.at<char>(ym,xm1)<0){  mapData.at<char>(ym,xm1) = value; resetMapNode(x-1,y,value);}
                if(xp1 < cols && mapData.at<char>(ym,xp1)<0){  mapData.at<char>(ym,xp1) = value; resetMapNode(x+1,y,value);}
                if(0 <= ym1   && mapData.at<char>(ym1,xm)<0){  mapData.at<char>(ym1,xm) = value; resetMapNode(x,y-1,value);}
                if(yp1 < rows && mapData.at<char>(yp1,xm)<0){  mapData.at<char>(yp1,xm) = value; resetMapNode(x,y+1,value);}
                if(0 <= xm1   && 0 <= ym1   && mapData.at<char>(ym1,xm1)<0){ mapData.at<char>(ym1,xm1)=value; resetMapNode(x-1,y-1,value);}
                if(0 <= xm1   && yp1 < rows && mapData.at<char>(yp1,xm1)<0){ mapData.at<char>(yp1,xm1)=value; resetMapNode(x-1,y+1,value);}
                if(xp1 < cols && 0 <= ym-1  && mapData.at<char>(ym1,xp1)<0){ mapData.at<char>(ym1,xp1)=value; resetMapNode(x+1,y-1,value);}
                if(xp1 < cols && yp1 < rows && mapData.at<char>(yp1,xp1)<0){ mapData.at<char>(yp1,xp1)=value; resetMapNode(x+1,y+1,value);}
            }

            if(mode > 0){
                xm1=wx(xm-1); xp1=wx(xm+1); ym1=wy(ym-1); yp1=wy(ym+1);
                if(0 <= xm1){    mapData.at<char>(ym,xm1) = value; resetMapNode(x-1,y,value);}
                if(xp1 < cols){  mapData.at<char>(ym,xp1) = value; resetMapNode(x+1,y,value);}
                if(0 <= ym1){    mapData.at<char>(ym1,xm) = value; resetMapNode(x,y-1,value);}
                if(yp1 < rows){  mapData.at<char>(yp1,xm) = value; resetMapNode(x,y+1,value);}
            }
            if(mode > 1){
                if(0 <= xm1 && 0 <= ym1){    mapData.at<char>(ym1,xm1)=value;  resetMapNode(x-1,y-1,value);}
                if(0 <= xm1 && yp1 < rows){  mapData.at<char>(yp1,xm1)=value;  resetMapNode(x-1,y+1,value);}
                if(xp1 < cols && 0 <= ym1){  mapData.at<char>(ym1,xp1)=value;  resetMapNode(x+1,y-1,value);}
                if(xp1 < cols && yp1 < rows){mapData.at<char>(yp1,xp1)=value;  resetMapNode(x+1,y+1,value);}
            }
            // cout << "SetCell (" << x << "," << y << ") = " << int(value) << ",mode=" << mode << endl;
        }
    }

    /** Set cells along a line from (x1,y1) to (x2,y2) to a new value (nodetype) */
    void setLine(int x1, int y1, int x2, int y2, int value, int mode=0){
        vector<Point> pointsToSet = bresenhamPoints(x1, y1, x2, y2);
        for(uint i=0; i<pointsToSet.size(); i++){
            setCell(pointsToSet[i].x, pointsToSet[i].y, value, mode);
        }
    }

    /** Set a square of cells with center point (x,y) to a new value (nodetype) */
    void setSquare(int x, int y, int size, int value, int mode=0){
        for(int iy=y-size; iy<=y+size; iy++){
            for(int ix=x-size; ix<=x+size; ix++){
                setCell(ix,iy,value, mode);
                setCell(ix,iy,value, mode);
            }
        }
    }

    /** Return the value of the obstacle map at coordinate (x,y) */
    inline char mapAt(int x, int y){
        // Wrapping the map
        if(wrapMap){
            while(x < 0)                x += mapData.cols;
            while(x >= mapData.cols)    x -= mapData.cols;
            while(y < 0)                y += mapData.rows;
            while(y >= mapData.rows)    y -= mapData.rows;
            return mapData.at<char>(y,x);
        }

        if((0 <= x && x < mapData.cols) && (0 <= y && y < mapData.rows)){
            return mapData.at<char>(y,x);
        }

        return 0;
    }

    /** Return the value of the dialated map at coordinate (x,y) */
    inline uchar dialatedMapAt(int x, int y){
        int& rows = mapData.rows;
        int& cols = mapData.cols;

        // Wrapping the map
        if(wrapMap){
            while(x < 0)       x += cols;
            while(x >= cols)   x -= cols;
            while(y < 0)       y += rows;
            while(y >= rows)   y -= rows;
            return dialatedMap.at<uchar>(y,x);
        }

        if((0 <= x && x < cols) && (0 <= y && y < rows)){
            return dialatedMap.at<uchar>(y,x);
        }

        return 0xFF;
    }

    /** Return the value of the distance map at coordinate (x,y) */
    #ifdef USE_CV_DISTANCE_TRANSFORM
    inline uchar distanceMapAt(int x, int y){
        int& rows = mapData.rows;
        int& cols = mapData.cols;

        // Wrapping the map
        if(wrapMap){
            while(x < 0)       x += cols;
            while(x >= cols)   x -= cols;
            while(y < 0)       y += rows;
            while(y >= rows)   y -= rows;
            return distanceMap.at<uchar>(y,x);
        }

        if((0 <= x && x < cols) && (0 <= y && y < rows)){
            return distanceMap.at<uchar>(y,x);
        }

        return 0;
    }
    #endif

    /** Update the map. This should be called whenever new laser scan data is available. */
    void updateMap(LaserScanner *scan, float maxLSDist){
        // Cell Laser Scanner Position
        scanPosCell = worldToCell(scan->pose(0),scan->pose(1));
        // cout << "scanPosCell = (" << scanPosCell.x << "," << scanPosCell.y << ")" << endl;

        // Convert polar scan coordinates to laser cartesian coordinates
        const MatrixXd& scanPolar = scan->getScanPolar();
        // Convert laser cartesian coordinates to world cartesian coordinates
        const MatrixXd& scanWorld = scan->getScanCartWorld();

        vector<Point> pointsToFill;

        // For each scanline i
        for(int i=0; i<scan->nScanLines; i++){
            // Convert scan from world cartesian coordinates to cell position
            double const& rho = scanPolar(1,i);
            if(rho < 0.020) continue;
            Point cell = worldToCell(scanWorld(0,i), scanWorld(1,i));

            // Clear cells using bresenham's algorithm
            // cout << "ScanPosCell: (" << scanPosCell.x << "," << scanPosCell.y << "), cell: (" << cell.x << "," << cell.y << ")" << endl;
            vector<Point> pointsToClear = bresenhamPoints(scanPosCell.x, scanPosCell.y, cell.x, cell.y);
            // cout << "Number of Points To Clear = " << pointsToClear.size() << endl;

            for(uint ii=0; ii<pointsToClear.size(); ii++){
                setCell(pointsToClear[ii].x, pointsToClear[ii].y, 0, clearMode);
            }

            if(0.025 < rho && rho <= maxLSDist) pointsToFill.push_back(cell);
        }

        for(uint ii=0; ii<pointsToFill.size(); ii++){
            setCell(pointsToFill[ii].x, pointsToFill[ii].y, 1, fillMode);
        }

        // cout << "scanWorldCell:\n" << scanWorldCell.transpose() << endl;
    }

    /** Compute various transformations, i.e. dialatedMap and distanceMap */
    void transform(){
        dilate( mapData > 0, dialatedMap, strel);

        #ifdef USE_CV_DISTANCE_TRANSFORM
        bitwise_not(dialatedMap, invDialatedMap);
        distanceTransform(invDialatedMap, distanceMap_32F, CV_DIST_C, 5, CV_32F);
        distanceMap_32F.convertTo(distanceMap,dialatedMap.type());
        #endif
   }

    /** Determine next waypoint defined as the furthest point seen by the laserscanner which is
    not occupied in the dialated obstacle map. The search has a preference for scanlines with small angles, which is achieved by
    iterating from the middle and out and only replacing if a longer distance is found. Thus, small angles are only used in case of ties.*/
    Point determineNextWaypointCell(LaserScanner *scan, float maxLSDist=1e7){
        float biggestDistanceInCells = 0;
        Point mostDistantCell;
        int nScanLinesHalf = scan->nScanLines/2;

        // Convert polar scan coordinates to laser cartesian coordinates
        MatrixXd scanPolar = scan->getScanPolar();

        // Limit the range of the scan
        if(maxLSDist < 1e6){
            for(int i=0; i<scanPolar.cols(); i++){
                double& rho = scanPolar(1,i);
                if(rho > maxLSDist) rho = maxLSDist;
            }
        }
        // Convert laser cartesian coordinates to world cartesian coordinates
        MatrixXd scanWorld = scan->polarToWorld(scanPolar);

        // Convert scan from world cartesian coordinates to cell position
        for(int i=0; i<=nScanLinesHalf; i++){
            int k;
            for(int j=0; j<2; j++){
                if(j==0)   k = nScanLinesHalf-i;
                else       k = nScanLinesHalf+i;
                if(k >= 0 && k < scan->nScanLines){
                    double& rho = scanPolar(1,k);
                    double& xs  = scanWorld(0,k);
                    double& ys  = scanWorld(1,k);
                    // float& phi = scanPolar(0,k);

                    if(rho < biggestDistanceInCells*cellSize) continue;
                    Point cell = worldToCell(xs, ys);
                    // int obstdist = distanceMapAt(cell.x,cell.y);

                    // Clear search for furthest available point using Bresenham's algorithm
                    Point closestOccupiedPoint;
                    vector<Point> points = bresenhamPoints(scanPosCell.x, scanPosCell.y, cell.x, cell.y);

                    // cout << "Diff:" <<scanPosCell-cell << endl;
                    for(uint ii=0; ii<points.size(); ii++){
                        Point point = points[ii];
                        float dx = point.x - scanPosCell.x;
                        float dy = point.y - scanPosCell.y;
                        float celldist = sqrt(dx*dx+dy*dy);
                        if(celldist > biggestDistanceInCells && dialatedMapAt(point.x,point.y) == 0){
                            biggestDistanceInCells = celldist;
                            mostDistantCell = point;
                        }
                    }
                }
            }
        }

        return mostDistantCell;
    }

    /** Determine next waypoint defined as the furthest point seen by the laserscanner,
    not occupied in the dialated map, with preference for scanlines with small angles, achieved by
    iterating from the middle and out and only replacing if a longer distance is found. */
    inline Point2f determineNextWaypoint(LaserScanner *scan, float maxLSDist=1e8){
        return cellToWorld(determineNextWaypointCell(scan));
    }

    /** Determine next waypoint defined as the furthest point seen by the laserscanner which is
    not occupied in the dialated obstacle map. The search has a preference for scanlines with small angles, which is achieved by
    iterating from the middle and out and only replacing if a longer distance is found. Thus, small angles are only used in case of ties.*/
    Point determineNextWaypointCellB(LaserScanner *scan, float maxLSDist=1e4){
        float biggestDistanceInCells = 0;
        Point mostDistantCell;
        int nScanLinesHalf = scan->nScanLines/2;

        // Convert polar scan coordinates to laser cartesian coordinates
        MatrixXd scanPolar = scan->getScanPolar();

        // Limit the range of the scan
        if(maxLSDist < 1e3){
            for(int i=0; i<scanPolar.cols(); i++){
                double& rho = scanPolar(1,i);
                if(rho > maxLSDist) rho = maxLSDist;
            }
        }
        // Convert laser cartesian coordinates to world cartesian coordinates
        MatrixXd scanWorld = scan->polarToWorld(scanPolar);

        // Convert scan from world cartesian coordinates to cell position
        for(int i=0; i<=nScanLinesHalf; i++){
            int k;
            for(int j=0; j<2; j++){
                if(j==0)   k = nScanLinesHalf-i;
                else       k = nScanLinesHalf+i;
                if(k >= 0 && k < scan->nScanLines){
                    double& rho = scanPolar(1,k);
                    double& xs  = scanWorld(0,k);
                    double& ys  = scanWorld(1,k);
                    // float& phi = scanPolar(0,k);

                    if(rho <= biggestDistanceInCells*cellSize) continue;
                    Point cell = worldToCell(xs, ys);
                    // int obstdist = distanceMapAt(cell.x,cell.y);

                    // Clear search for furthest available point using Bresenham's algorithm
                    Point closestOccupiedPoint;
                    vector<Point> points = bresenhamPoints(scanPosCell.x, scanPosCell.y, cell.x, cell.y);
                    if(scanPosCell != points[0]) reverse(points.begin(), points.end());

                    #if (0)
                    cout << "Scan:(" << scanPosCell.x << "," << scanPosCell.y << ")";
                    for(int p=0; p<points.size(); p++){
                    	cout << "->(" << points[p].x << "," << points[p].y << ")";
                    }
                    cout << endl;
					#endif

                    // cout << "Diff:" <<scanPosCell-cell << endl;
                    uint ii=1;
                    while(ii<points.size()){
                        Point point = points[ii];
                        if(dialatedMapAt(point.x,point.y) != 0) break;
                        float dx = point.x - scanPosCell.x;
                        float dy = point.y - scanPosCell.y;
                        float celldist = sqrt(dx*dx+dy*dy);
                        if(celldist > biggestDistanceInCells){
                            biggestDistanceInCells = celldist;
                            mostDistantCell = point;
                        }
                        ii++;
                    }
                }
            }
        }

        return mostDistantCell;
    }

    /** Determine next waypoint defined as the furthest point seen by the laserscanner,
    not occupied in the dialated map, with preference for scanlines with small angles, achieved by
    iterating from the middle and out and only replacing if a longer distance is found. */
    inline Point2f determineNextWaypointB(LaserScanner *scan, float maxLSDist=1e8){
        return cellToWorld(determineNextWaypointCellB(scan));
    }

    /** This function will find the minimal cost path from the starting cell to the target cell.
    The function uses the A* search algorithm with tie-breaker and an additional cost related to
    the obstacle distance implemented as an obstacle repulsive potential. */
    vector<MapNode *> findpath(int xStart, int yStart, int xTarget, int yTarget, unsigned long maxIter = 10000){
        // vector<MapNode *> path;
        astar.clear();
        for (uint y = 0; y < mapData.rows; y++) {
            for (uint x = 0; x < mapData.cols; x++) {
                MapNode *node = astar.mapAt(x,y);
                if(dialatedMapAt(x,y) > 0) node->type = NODE_TYPE_OBSTACLE;
                else if(mapAt(x,y) == -1)  node->type = NODE_TYPE_UNKNOWN;
                else node->type = NODE_TYPE_ZERO;

                #ifdef USE_CV_DISTANCE_TRANSFORM
                    node->obstdist = distanceMapAt(x,y);
                #else
                    node->obstdist = -1; // -1 means that the distance is still unknown, and thus it will be calculated by astar during node expansion.
                #endif
            }
        }
        return astar.findpath(xStart, yStart, xTarget, yTarget, maxIter);
    }

    /** Wrap a value into the [-pi,pi) range */
    inline float wrapToPi(float angle){
        float TWO_PI = 2*M_PI;
        while(angle > M_PI)     angle -= TWO_PI;
        while(angle <= -M_PI)   angle += TWO_PI;
        return angle;
    }

    /** Simplify the path by removing points along straight line segments, keeping only the corner points. */
    inline vector<MapNode *> simplifyPath(vector<MapNode *> path){
        const bool DEBUG = false;

        vector<MapNode *> newpath;
        path = astar.simplifyPath(path);
        //return path;

        newpath.push_back(path.front());
        for(int i=1; i<(path.size()-1); i++){
            MapNode * P0 = path[i-1];
            MapNode * P1 = path[i];
            MapNode * P2 = path[i+1];
            int dx01 = P1->x - P0->x;
            int dy01 = P1->y - P0->y;
            int dx12 = P2->x - P1->x;
            int dy12 = P2->y - P1->y;
            if(abs(dx12) == 1 && abs(dy12) == 1)  continue;
            if(dx01 == dx12 && dy01 == dy12) continue;
            newpath.push_back(P1);
        }
        newpath.push_back(path.back());

        if(DEBUG){
            cout << "newpath2:--";
            for(int i=0; i<newpath.size(); i++){
                cout << "->(" << newpath[i]->x << "," << newpath[i]->y << ")";
            }
            cout << endl;
        }

        return newpath;
    }

    /** Convert a path consisting of a vector of MapNode pointers to a vector of world coordinate waypoints */
    vector<Point2f> pathToWorld(vector<MapNode *> const& path){
        vector<Point2f> waypoints;
        for(int i=0; i<path.size(); i++){
            waypoints.push_back(cellToWorld(path[i]->x,path[i]->y));
        }

        /*
        cout << "wayPoints:";
        for(int i=0; i<waypoints.size(); i++){
            cout << "->(" << waypoints[i].x << "," << waypoints[i].y << ")";
        }
        cout << endl;
        */
        return waypoints;
    }

    inline vector<Point2f> findpathAsWaypoints(int const&  xStart, int const& yStart, int const& xTarget, int const& yTarget){
        return pathToWorld(simplifyPath(findpath(xStart, yStart, xTarget, yTarget)));
    }

};

#endif


// This file is part of snark, a generic and flexible library for robotics research
// Copyright (c) 2011 The University of Sydney
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
// 3. Neither the name of the University of Sydney nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
// GRANTED BY THIS LICENSE.  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT
// HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
// BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
// OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
// IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <cmath>
#include <Eigen/Eigen>
#include "covariance.h"

namespace snark{

squared_exponential_covariance::squared_exponential_covariance( double length_scale
                                                              , double signal_variance
                                                              , double data_variance )
    : signal_variance_( signal_variance )
    , data_variance_( data_variance )
    , factor_( -1.0 / ( 2.0 * std::sqrt( length_scale ) ) )
    , self_covariance_( signal_variance + data_variance )
{
}

double squared_exponential_covariance::covariance( const Eigen::VectorXd& v, const Eigen::VectorXd& w ) const
{
    const Eigen::VectorXd& diff = v - w;
    return signal_variance_ * std::exp( factor_ * diff.dot( diff ) );
}

double squared_exponential_covariance::self_covariance() const { return self_covariance_; }

} // namespace snark{ { namespace Robotics {