// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_SFUNCTOR_INCLUDE
#define MTL_SFUNCTOR_INCLUDE

#include <cmath>
#include <boost/numeric/mtl/concept/std_concept.hpp>
#include <boost/numeric/mtl/concept/magnitude.hpp>
#include <boost/numeric/mtl/concept/static_functor.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl { namespace sfunctor {

template <typename Value1, typename Value2>
struct plus
{
    typedef const Value1&                                 first_argument_type;
    typedef const Value2&                                 second_argument_type;
    typedef typename Addable<Value1, Value2>::result_type result_type;

    static inline result_type apply(const Value1& v1, const Value2& v2)
    {
	return v1 + v2;
    }

    result_type operator() (const Value1& v1, const Value2& v2) const
    {
	vampir_trace<23> tracer;
	return v1 + v2;
    }
};

template <typename Value1, typename Value2>
struct minus
{
    typedef const Value1&                                 first_argument_type;
    typedef const Value2&                                 second_argument_type;
    typedef typename Subtractable<Value1, Value2>::result_type result_type;

    static inline result_type apply(const Value1& v1, const Value2& v2)
    {
	return v1 - v2;
    }

    result_type operator() (const Value1& v1, const Value2& v2) const
    {
	vampir_trace<24> tracer;
	return v1 - v2;
    }
};

template <typename Value1, typename Value2>
struct times
{
    typedef const Value1&                                 first_argument_type;
    typedef const Value2&                                 second_argument_type;
    typedef typename Multiplicable<Value1, Value2>::result_type result_type;

    static inline result_type apply(const Value1& v1, const Value2& v2)
    {
	return v1 * v2;
    }

    result_type operator() (const Value1& v1, const Value2& v2) const
    {
	vampir_trace<25> tracer;
	return v1 * v2;
    }
};

template <typename Value1, typename Value2>
struct divide
{
    typedef const Value1&                                 first_argument_type;
    typedef const Value2&                                 second_argument_type;
    typedef typename Divisible<Value1, Value2>::result_type result_type;

    static inline result_type apply(const Value1& v1, const Value2& v2)
    {
	return v1 / v2;
    }

    result_type operator() (const Value1& v1, const Value2& v2) const
    {
	vampir_trace<26> tracer;
	return v1 / v2;
    }
};

template <typename Value1, typename Value2>
struct assign
{
    typedef Value1&                                       first_argument_type;
    typedef const Value2&                                 second_argument_type;
    typedef Value1&                                       result_type;

    static inline result_type apply(Value1& v1, const Value2& v2)
    {
	return v1= v2;
    }

    result_type operator() (Value1& v1, const Value2& v2) const
    {
	vampir_trace<27> tracer;
	return v1= v2;
    }
};

template <typename Value1, typename Value2>
struct plus_assign
{
    typedef Value1&                                       first_argument_type;
    typedef const Value2&                                 second_argument_type;
    typedef Value1&                                       result_type;

    static inline result_type apply(Value1& v1, const Value2& v2)
    {
	return v1+= v2;
    }

    result_type operator() (Value1& v1, const Value2& v2) const
    {
	vampir_trace<28> tracer;
	return v1+= v2;
    }
};

template <typename Value1, typename Value2>
struct minus_assign
{
    typedef Value1&                                       first_argument_type;
    typedef const Value2&                                 second_argument_type;
    typedef Value1&                                       result_type;

    static inline result_type apply(Value1& v1, const Value2& v2)
    {
	return v1-= v2;
    }

    result_type operator() (Value1& v1, const Value2& v2) const
    {
	vampir_trace<29> tracer;
	return v1-= v2;
    }
};

template <typename Value1, typename Value2>
struct times_assign
{
    typedef Value1&                                       first_argument_type;
    typedef const Value2&                                 second_argument_type;
    typedef Value1&                                       result_type;

    static inline result_type apply(Value1& v1, const Value2& v2)
    {
	return v1*= v2;
    }

    result_type operator() (Value1& v1, const Value2& v2) const
    {
	vampir_trace<30> tracer;
	return v1*= v2;
    }
};

template <typename Value1, typename Value2>
struct divide_assign
{
    typedef Value1&                                       first_argument_type;
    typedef const Value2&                                 second_argument_type;
    typedef Value1&                                       result_type;

    static inline result_type apply(Value1& v1, const Value2& v2)
    {
	return v1/= v2;
    }

    result_type operator() (Value1& v1, const Value2& v2) const
    {
	vampir_trace<31> tracer;
	return v1/= v2;
    }
};

// Might be helpful for surplus functor arguments
template <typename Value>
struct identity
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
	return v;
    }

    result_type operator() (const Value& v) const
    {
	vampir_trace<32> tracer;
	return v;
    }
};

template <typename Value>
struct abs
{
    typedef const Value&                                  argument_type;
    typedef typename Magnitude<Value>::type               result_type;

    static inline result_type apply(const Value& v)
    {
	using std::abs;
	return abs(v);
    }

    result_type operator() (const Value& v)
    {
	vampir_trace<33> tracer;
	return apply(v);
    }
};

template <typename Value>
struct sqrt
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
	using std::sqrt;
	return sqrt(v);
    }

    result_type operator() (const Value& v)
    {
	vampir_trace<34> tracer;
	return apply(v);
    }
};

template <typename Value>
struct square
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
	return v * v;
    }

    result_type operator() (const Value& v)
    {
	vampir_trace<35> tracer;
	return apply(v);
    }
};

template <typename Value>
struct negate
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v) { return -v;  }
    result_type operator() (const Value& v) const
    {
	vampir_trace<36> tracer;
	return -v;
    }
};

/// Compose functors \p F and \p G, i.e. compute f(g(x)).
/** Functors must be models of StaticUnaryFunctor,
    StaticUnaryFunctor<G>::result_type must be convertible to
    StaticUnaryFunctor<F>::argument_type.
    Under these conditions compose<F, G> will be a model of StaticUnaryFunctor.
**/
template <typename F, typename G>
struct compose
{
    typedef typename StaticUnaryFunctor<G>::argument_type argument_type;
    typedef typename StaticUnaryFunctor<F>::result_type   result_type;

    static inline result_type apply(argument_type x)
    {
	return F::apply(G::apply(x));
    }

    result_type operator()(argument_type x)
    {
	vampir_trace<37> tracer;
	return apply(x);
    }
};

/// Compose functors \p F and \p G with G in F's first argument, i.e. compute f(g(x), y).
/** F/G must be models of StaticBinaryFunctor/StaticUnaryFunctor,
    StaticUnaryFunctor<G>::result_type must be convertible to
    StaticBinaryFunctor<F>::first_argument_type.
    Under these conditions compose_first<F, G> will be a model of StaticBinaryFunctor.
**/
template <typename F, typename G>
struct compose_first
{
    typedef typename StaticUnaryFunctor<G>::argument_type         first_argument_type;
    typedef typename StaticBinaryFunctor<F>::second_argument_type second_argument_type;
    typedef typename StaticBinaryFunctor<F>::result_type          result_type;

    static inline result_type apply(first_argument_type x, second_argument_type y)
    {
	return F::apply(G::apply(x), y);
    }

    result_type operator()(first_argument_type x, second_argument_type y)
    {
	vampir_trace<38> tracer;
	return apply(x, y);
    }
};

/// Compose functors \p F and \p G with G in F's second argument, i.e. compute f(x, g(y)).
/** F/G must be models of StaticBinaryFunctor/StaticUnaryFunctor,
    StaticUnaryFunctor<G>::result_type must be convertible to
    StaticBinaryFunctor<F>::second_argument_type.
    Under these conditions compose_second<F, G> will be a model of StaticBinaryFunctor.
**/
template <typename F, typename G>
struct compose_second
{
    typedef typename StaticBinaryFunctor<F>::first_argument_type  first_argument_type;
    typedef typename StaticUnaryFunctor<G>::argument_type         second_argument_type;
    typedef typename StaticBinaryFunctor<F>::result_type          result_type;

    static inline result_type apply(first_argument_type x, second_argument_type y)
    {
	return F::apply(x, G::apply(y));
    }

    result_type operator()(first_argument_type x, second_argument_type y)
    {
	vampir_trace<39> tracer;
	return apply(x, y);
    }
};

/// Compose functors \p F, \p G, and \p H with G/H in F's first/second argument, i.e. compute f(g(x), h(y)).
/** F/G must be models of StaticBinaryFunctor/StaticUnaryFunctor,
    StaticUnaryFunctor<G>::result_type must be convertible to
    StaticBinaryFunctor<F>::first_argument_type and
    StaticUnaryFunctor<H>::result_type must be convertible to
    StaticBinaryFunctor<F>::second_argument_type.
    Under these conditions compose_both<F, G, H> will be a model of StaticBinaryFunctor.
**/
template <typename F, typename G, typename H>
struct compose_both
{
    typedef typename StaticUnaryFunctor<G>::argument_type         first_argument_type;
    typedef typename StaticUnaryFunctor<H>::argument_type         second_argument_type;
    typedef typename StaticBinaryFunctor<F>::result_type          result_type;

    static inline result_type apply(first_argument_type x, second_argument_type y)
    {
	return F::apply(G::apply(x), H::apply(y));
    }

    result_type operator()(first_argument_type x, second_argument_type y)
    {
	vampir_trace<40> tracer;
	return apply(x, y);
    }
};

/// Compose unary functor \p F with binary functor \p G, i.e. compute f(g(x, y)).
/** F/G must be models of StaticUnaryFunctor/StaticBinaryFunctor,
    StaticBinaryFunctor<G>::result_type must be convertible to
    StaticUnaryFunctor<F>::argument_type.
    Under these conditions compose_binary<F, G> will be a model of StaticBinaryFunctor.
**/
template <typename F, typename G>
struct compose_binary
{
    typedef typename StaticBinaryFunctor<G>::first_argument_type  first_argument_type;
    typedef typename StaticBinaryFunctor<G>::second_argument_type second_argument_type;
    typedef typename StaticUnaryFunctor<F>::result_type           result_type;

    static inline result_type apply(first_argument_type x, second_argument_type y)
    {
	return F::apply(G::apply(x, y));
    }

    result_type operator()(first_argument_type x, second_argument_type y)
    {
	vampir_trace<41> tracer;
	return apply(x, y);
    }
};

/// Templatized example of composition, computes l_2 norm in 2D, i.e. sqrt(abs(x*x + y*y))
template <typename T>
struct l_2_2D
  : public compose_binary<sqrt<typename abs<T>::result_type>,
			  compose_binary<abs<T>,
					 compose_both<plus<T, T>,
						      square<T>,
						      square<T>  >
                                        >
                         >
{};

}} // namespace mtl::sfunctor

#endif // MTL_SFUNCTOR_INCLUDE


#ifndef NWNBASE_H
#define NWNBASE_H

#include <array>

#include <boost/bimap.hpp>
#include <boost/assign.hpp>
#include <boost/serialization/nvp.hpp>

namespace Nwn {

enum class AblScore : int {
    Str,
    Dex,
    Con,
    Int,
    Wis,
    Cha
};

static const std::array<std::string, 6> ablAbbrevs = {
    "STR", "DEX", "CON", "INT", "WIS", "CHA"
};

AblScore indexToAbl( int i );

int getAblMod( int score );

enum class Alignment : int {
    LawfulGood = 1,
    NeutralGood = 2,
    ChaoticGood = 4,
    LawfulNeutral = 8,
    Neutral = 16,
    ChaoticNeutral = 32,
    LawfulEvil = 64,
    NeutralEvil = 128,
    ChaoticEvil = 256
};

static const std::array<Alignment, 9> allAlignments = {
    Alignment::LawfulGood,
    Alignment::NeutralGood,
    Alignment::ChaoticGood,
    Alignment::LawfulNeutral,
    Alignment::Neutral,
    Alignment::ChaoticNeutral,
    Alignment::LawfulEvil,
    Alignment::NeutralEvil,
    Alignment::ChaoticEvil
};

/// Returns order as in allAlignments
Alignment indexToAlignment( size_t index );

/// Returns order as in allAlignments
size_t alignmentToIndex( Alignment aln );

using AlignmentBimap = boost::bimap< Alignment, std::string >;

static const AlignmentBimap alignmentStrings = boost::assign::list_of< AlignmentBimap::relation >
    ( Alignment::LawfulGood, "Lawful Good" )
    ( Alignment::NeutralGood, "Neutral Good" )
    ( Alignment::ChaoticGood, "Chaotic Good" )
    ( Alignment::LawfulNeutral, "Lawful Neutral" )
    ( Alignment::Neutral, "Neutral" )
    ( Alignment::ChaoticNeutral, "Chaotic Neutral" )
    ( Alignment::LawfulEvil, "Lawful Evil" )
    ( Alignment::NeutralEvil, "Neutral Evil" )
    ( Alignment::ChaoticEvil, "Chaotic Evil" )
    ;

const std::string& alignmentToStr( Alignment aln );
Alignment strToAlignment( const std::string& txt );

enum class Dice : int {
    d4 = 4,
    d6 = 6,
    d8 = 8,
    d10 = 10,
    d12 = 12,
    d20 = 20
};

/// Integer equivalent of a dice
int diceToInt( Dice d );

/// Integer to enum
Dice intToDice( int i );

struct SavingThrows
{
    template<class Archive>
    void serialize( Archive & ar, const unsigned int /* file_version */ ) {
        ar & boost::serialization::make_nvp( "Fort", Fort )
           & boost::serialization::make_nvp( "Ref", Ref )
           & boost::serialization::make_nvp( "Will", Will );
    }

    SavingThrows& operator+=( const SavingThrows& rhs ) {
        this->Fort += rhs.Fort;
        this->Ref += rhs.Ref;
        this->Will += rhs.Will;
        return *this;
    }

    int Fort { 0 };
    int Ref  { 0 };
    int Will { 0 };
};

} // namespace Nwn

#endif // NWNBASE_H


#pragma once

#include <assert.h>

#include <algorithm>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
#include <cmath>
#include <exception>
#include <functional>
#include <iostream>
#include <iterator>
#include <map>
#include <memory>
#include <opencv2/core/types.hpp>
#include <opencv2/opencv.hpp>
#include <regex>
#include <string>
#include <utility>
#include <vector>

#define print_line                                                  \
    {                                                               \
        std::cout << __FILE__ << "  ---  " << __func__ << "  ---  " \
                  << __LINE__ << std::endl;                         \
    }

namespace utils {

inline int round(float v) {
    return int(std::round(v));
}

inline cv::Point2f __add__(const cv::Point2f& a, const std::vector<float>& b) {
    return cv::Point2f{a.x + b.at(0), a.y + b.at(1)};
}

inline cv::Point2i pntf2pnti(const cv::Point2f& pnt) {
    return cv::Point2i{round(pnt.x), round(pnt.y)};
}

inline void setpntf(cv::Point2f& pntf, std::vector<float>& pt) {
    assert(pt.size() >= 2);
    pntf.x = pt.at(0);
    pntf.y = pt.at(1);
}

std::shared_ptr<std::vector<float>> arange(float b, float e, float t) {
    assert(b < e && t > 0);
    std::shared_ptr<std::vector<float>> res{new std::vector<float>{}};
    for (; b < e; b += t) {
        res->push_back(b);
    }
    return res;
}
std::shared_ptr<std::vector<float>> linspace(float b, float e, int cnt = 2) {
    float c = std::max(2, cnt);
    assert(b < e && c >= 2.0f);
    std::shared_ptr<std::vector<float>> res{new std::vector<float>{}};
    float it = b;
    float sp = e - b;
    res->push_back(it);
    while (c > 2) {
        float delta = sp / (c - 1.0f);
        it += delta;
        sp -= delta;
        c -= 1.0f;
        res->push_back(it);
    }
    res->push_back(e);
    return res;
}

inline float eular_distance(const cv::Point2f& a, const cv::Point2f& b) {
    cv::Point2f c = a - b;
    return std::sqrt(c.x * c.x + c.y * c.y);
}

inline cv::Point2f _get_quadratic_bezier_pnt(const cv::Point2f& b,
                                             const cv::Point2f& c1,
                                             const cv::Point2f& c2,
                                             const cv::Point2f& e,
                                             float t) {
    return std::pow((1.0f - t), 3) * b + 3 * std::pow((1.0f - t), 2) * t * c1 +
           3 * std::pow(t, 2) * (1.0f - t) * c2 + std::pow(t, 3) * e;
}
std::shared_ptr<std::vector<cv::Point2i>> quadratic_bezier(
    const cv::Point2f& b,
    const cv::Point2f& c1,
    const cv::Point2f& c2,
    const cv::Point2f& e,
    int cnt = -1) {
    std::shared_ptr<std::vector<cv::Point2i>> res{
        new std::vector<cv::Point2i>{}};

    cnt = cnt <= 0 ? int(eular_distance(b, c1) + eular_distance(c1, c2) +
                         eular_distance(c2, e))
                   : cnt;
    auto ts = linspace(0, 1, cnt);
    for (auto& t : *ts) {
        res->push_back(pntf2pnti(_get_quadratic_bezier_pnt(b, c1, c2, e, t)));
    }

    return res;
}

void _get_paths_from_g(boost::property_tree::ptree& pt,
                       std::vector<boost::property_tree::ptree>& paths) {
    for (auto& el : pt) {
        if (el.first == "g") {
            _get_paths_from_g(el.second, paths);
        } else if (el.first == "path") {
            paths.push_back(el.second);
        }
    }
}

std::pair<int, int> get_size_from_svg(boost::property_tree::ptree& svg) {
    auto res = std::make_pair(0, 0);
    try {
        res.first  = svg.get<int>("<xmlattr>.width");
        res.second = svg.get<int>("<xmlattr>.height");

    } catch (std::exception& e) {
        std::cerr << e.what() << std::endl;
    }

    return res;
}

std::pair<int, int> get_size_from_doc(boost::property_tree::ptree& doc) {
    auto svg = doc.get_child("svg");
    return get_size_from_svg(svg);
}

std::shared_ptr<std::vector<boost::property_tree::ptree>> get_paths_from_svg(
    boost::property_tree::ptree& doc) {
    std::shared_ptr<std::vector<boost::property_tree::ptree>> res{
        new std::vector<boost::property_tree::ptree>{}};
    auto root = doc.get_child("svg");
    auto g    = root.get_child("g");
    _get_paths_from_g(g, *res);
    return res;
}

std::shared_ptr<std::vector<boost::property_tree::ptree>> get_paths_from_svg(
    const std::string& svg_filepath) {
    boost::property_tree::ptree pt;
    boost::property_tree::read_xml(svg_filepath, pt);
    return get_paths_from_svg(pt);
}

std::shared_ptr<std::vector<std::string>> get_pathd_from_doc(
    boost::property_tree::ptree& doc) {
    auto paths = get_paths_from_svg(doc);
    std::shared_ptr<std::vector<std::string>> res{
        new std::vector<std::string>{}};

    for (auto& path : *paths) {
        res->push_back(path.get<std::string>("<xmlattr>.d"));
    }
    return res;
}

inline std::shared_ptr<std::vector<std::string>> split(
    const std::string& s,
    const std::string& p = "[\\s]+") {
    std::regex reg{p};
    std::shared_ptr<std::vector<std::string>> res{new std::vector<std::string>{
        std::sregex_token_iterator{s.begin(), s.end(), reg, -1},
        std::sregex_token_iterator{}}};
    return res;
}

inline std::shared_ptr<std::vector<float>> split_to_floats(
    const std::string& s,
    const std::string& p = ",") {
    print_line;
    std::cout << "split s:" << s << " ;; by:" << p << std::endl;
    std::regex reg{p};
    std::shared_ptr<std::vector<float>> res{new std::vector<float>{}};
    std::vector<std::string> strs{
        std::sregex_token_iterator{s.begin(), s.end(), reg, -1},
        std::sregex_token_iterator{}};
    std::transform(
        strs.begin(), strs.end(), std::back_inserter(*res), [](std::string& s) {
            float res = 0;
            try {
                res = std::stof(s);
            } catch (std::exception& e) {
                std::cerr << e.what() << std::endl;
            }
            return res;
        });
    return res;
}

static std::map<std::string,
                std::function<bool(cv::Point2f&,
                                   std::vector<std::vector<cv::Point2i>>&,
                                   const std::vector<std::string>&,
                                   int&)>>
    __handle_mapper__{
        {"m",
         [](cv::Point2f& prev_pt,
            std::vector<std::vector<cv::Point2i>>& ways,
            const std::vector<std::string>& pathd,
            int& i) -> bool {
             ways.push_back(std::vector<cv::Point2i>{});
             ++i;
             if (i >= pathd.size()) {
                 return false;
             }
             auto pts = split_to_floats(pathd.at(i));
             prev_pt.x += pts->at(0);
             prev_pt.y += pts->at(1);
             ways.back().push_back(prev_pt);
             ++i;
             return true;
         }},
        {"M",
         [](cv::Point2f& prev_pt,
            std::vector<std::vector<cv::Point2i>>& ways,
            const std::vector<std::string>& pathd,
            int& i) -> bool {
             ways.push_back(std::vector<cv::Point2i>{});
             ++i;

             if (i >= pathd.size()) {
                 return false;
             }
             auto pts = split_to_floats(pathd.at(i));
             setpntf(prev_pt, *pts);
             ways.back().push_back(prev_pt);
             ++i;
             return true;
         }},
        {"l",
         [](cv::Point2f& prev_pt,
            std::vector<std::vector<cv::Point2i>>& ways,
            const std::vector<std::string>& pathd,
            int& i) -> bool {
             ++i;
             if (i >= pathd.size()) {
                 return false;
             }
             auto pts = split_to_floats(pathd.at(i));
             prev_pt.x += pts->at(0);
             prev_pt.y += pts->at(1);
             ways.back().push_back(prev_pt);
             ++i;
             return true;
         }},
        {"L",
         [](cv::Point2f& prev_pt,
            std::vector<std::vector<cv::Point2i>>& ways,
            const std::vector<std::string>& pathd,
            int& i) -> bool {
             ++i;
             if (i >= pathd.size()) {
                 return false;
             }
             auto pts = split_to_floats(pathd.at(i));
             setpntf(prev_pt, *pts);
             ways.back().push_back(prev_pt);
             ++i;
             return true;
         }},
        {"h",
         [](cv::Point2f& prev_pt,
            std::vector<std::vector<cv::Point2i>>& ways,
            const std::vector<std::string>& pathd,
            int& i) -> bool {
             ++i;
             if (i >= pathd.size()) {
                 return false;
             }
             auto pts = split_to_floats(pathd.at(i));
             prev_pt.x += pts->at(0);
             ways.back().push_back(prev_pt);
             ++i;
             return true;
         }},
        {"H",
         [](cv::Point2f& prev_pt,
            std::vector<std::vector<cv::Point2i>>& ways,
            const std::vector<std::string>& pathd,
            int& i) -> bool {
             ++i;
             if (i >= pathd.size()) {
                 return false;
             }
             auto pts  = split_to_floats(pathd.at(i));
             prev_pt.x = pts->at(0);
             ways.back().push_back(prev_pt);
             ++i;
             return true;
         }},
        {"v",
         [](cv::Point2f& prev_pt,
            std::vector<std::vector<cv::Point2i>>& ways,
            const std::vector<std::string>& pathd,
            int& i) -> bool {
             ++i;
             if (i >= pathd.size()) {
                 return false;
             }
             auto pts = split_to_floats(pathd.at(i));
             prev_pt.y += pts->at(0);
             ways.back().push_back(prev_pt);
             ++i;
             return true;
         }},
        {"V",
         [](cv::Point2f& prev_pt,
            std::vector<std::vector<cv::Point2i>>& ways,
            const std::vector<std::string>& pathd,
            int& i) -> bool {
             ++i;
             if (i >= pathd.size()) {
                 return false;
             }
             auto pts  = split_to_floats(pathd.at(i));
             prev_pt.y = pts->at(0);
             ways.back().push_back(prev_pt);
             ++i;
             return true;
         }},
        {"c",
         [](cv::Point2f& prev_pt,
            std::vector<std::vector<cv::Point2i>>& ways,
            const std::vector<std::string>& pathd,
            int& i) -> bool {
             cv::Point2f b = prev_pt;

             if (++i >= pathd.size()) {
                 return false;
             }
             cv::Point2f c1 = __add__(prev_pt, *split_to_floats(pathd.at(i)));

             if (++i >= pathd.size()) {
                 return false;
             }
             cv::Point2f c2 = __add__(prev_pt, *split_to_floats(pathd.at(i)));

             if (++i >= pathd.size()) {
                 return false;
             }
             cv::Point2f e = __add__(prev_pt, *split_to_floats(pathd.at(i)));

             auto pts = quadratic_bezier(b, c1, c2, e);
             prev_pt  = pts->back();
             std::copy(
                 pts->begin(), pts->end(), std::back_inserter(ways.back()));

             ++i;
             return true;
         }},
        {"C",
         [](cv::Point2f& prev_pt,
            std::vector<std::vector<cv::Point2i>>& ways,
            const std::vector<std::string>& pathd,
            int& i) -> bool {
             cv::Point2f b = prev_pt;

             if (++i >= pathd.size()) {
                 return false;
             }
             cv::Point2f c1;
             setpntf(c1, *split_to_floats(pathd.at(i)));

             if (++i >= pathd.size()) {
                 return false;
             }
             cv::Point2f c2;
             setpntf(c2, *split_to_floats(pathd.at(i)));

             if (++i >= pathd.size()) {
                 return false;
             }
             cv::Point2f e;
             setpntf(e, *split_to_floats(pathd.at(i)));

             auto pts = quadratic_bezier(b, c1, c2, e);
             prev_pt  = pts->back();
             std::copy(
                 pts->begin(), pts->end(), std::back_inserter(ways.back()));

             ++i;
             return true;
         }},
        {"z",
         [](cv::Point2f& prev_pt,
            std::vector<std::vector<cv::Point2i>>& ways,
            const std::vector<std::string>& pathd,
            int& i) -> bool {
             ways.back().push_back(ways.back().front());
             prev_pt = ways.back().back();
             ++i;
             return true;
         }},
        {"Z",
         [](cv::Point2f& prev_pt,
            std::vector<std::vector<cv::Point2i>>& ways,
            const std::vector<std::string>& pathd,
            int& i) -> bool {
             ways.back().push_back(ways.back().front());
             prev_pt = ways.back().back();
             ++i;
             return true;
         }}};

void get_way_from_pathd(std::vector<std::string>& pathd,
                        std::vector<std::vector<cv::Point2i>>& ways) {
    print_line;

    auto prev_func_it = __handle_mapper__.begin();
    int i             = 0;
    cv::Point2f prev_pt{0, 0};
    while (i < pathd.size()) {
        auto key = pathd.at(i);
        auto fit = __handle_mapper__.find(key);
        if (fit == __handle_mapper__.end()) {
            --i;
            fit = prev_func_it;
        }
        std::cout << "handle in " << fit->first << std::endl;
        if (false == fit->second(prev_pt, ways, pathd, i)) {
            return;
        }
        std::cout << "this i is " << i << std::endl;
        prev_func_it = fit;
    }
}

std::shared_ptr<std::vector<std::vector<cv::Point2i>>> get_ways_form_paths(
    std::vector<boost::property_tree::ptree>& paths) {
    //
    print_line;
    std::shared_ptr<std::vector<std::vector<cv::Point2i>>> res{
        new std::vector<std::vector<cv::Point2i>>{}};
    for (auto& pt : paths) {
        auto d     = pt.get<std::string>("<xmlattr>.d");
        auto pathd = split(d);
        for (auto& s : *pathd) {
            std::cout << s << " ; ";
        }
        std::cout << "%%" << std::endl;
        get_way_from_pathd(*pathd, *res);
    }
    return res;
}

std::shared_ptr<std::vector<std::vector<cv::Point2i>>> get_ways_form_svg(
    boost::property_tree::ptree& doc) {
    auto paths = get_paths_from_svg(doc);
    return get_ways_form_paths(*paths);
}

std::shared_ptr<std::vector<std::vector<cv::Point2i>>> get_ways_form_svg(
    const std::string& svg_filepath) {
    boost::property_tree::ptree pt;
    boost::property_tree::read_xml(svg_filepath, pt);
    return get_ways_form_svg(pt);
}

}  // namespace utils


/*
 * ColorMapFilter.cpp
 *
 *  Created on: Sep 23, 2017
 *      Author: Peter Fankhauser
 *   Institute: ETH Zurich, ANYbotics
 */

#include "grid_map_filters/ColorMapFilter.hpp"

#include <Eigen/Dense>

#include <grid_map_core/grid_map_core.hpp>

using namespace filters;

namespace grid_map {

ColorMapFilter::ColorMapFilter() : min_(0.0), max_(1.0) {}

ColorMapFilter::~ColorMapFilter() = default;

bool ColorMapFilter::configure() {
  if (!FilterBase::getParam(std::string("input_layer"), inputLayer_)) {
    ROS_ERROR("Color map filter did not find parameter `input_layer`.");
    return false;
  }
  ROS_DEBUG("Color map filter input_layer = %s.", inputLayer_.c_str());

  if (!FilterBase::getParam(std::string("output_layer"), outputLayer_)) {
    ROS_ERROR("Color map filter did not find parameter `output_layer`.");
    return false;
  }
  ROS_DEBUG("Color map filter output_layer = %s.", outputLayer_.c_str());

  if (!FilterBase::getParam(std::string("min/value"), min_)) {
    ROS_ERROR("Color map filter did not find parameter `min/value`.");
    return false;
  }
  if (!FilterBase::getParam(std::string("max/value"), max_)) {
    ROS_ERROR("Color map filter did not find parameter `max/value`.");
    return false;
  }

  std::vector<double> minColor;
  if (!FilterBase::getParam(std::string("min/color"), minColor)) {
    ROS_ERROR("Color map filter did not find parameter `min/color`.");
    return false;
  }
  if (minColor.size() != 3) {
    ROS_ERROR("Color map filter parameter `min/color` needs to be of size 3.");
    return false;
  }
  minColor_ << minColor[0], minColor[1], minColor[2];

  std::vector<double> maxColor;
  if (!FilterBase::getParam(std::string("max/color"), maxColor)) {
    ROS_ERROR("Color map filter did not find parameter `max/color`.");
    return false;
  }
  if (maxColor.size() != 3) {
    ROS_ERROR("Color map filter parameter `max/color` needs to be of size 3.");
    return false;
  }
  maxColor_ << maxColor[0], maxColor[1], maxColor[2];

  return true;
}

bool ColorMapFilter::update(const GridMap& mapIn, GridMap& mapOut) {
  mapOut = mapIn;
  const auto& input = mapIn[inputLayer_];
  mapOut.add(outputLayer_);
  auto& output = mapOut[outputLayer_];

  const double range = max_ - min_;
  const Eigen::Vector3f colorRange = maxColor_ - minColor_;

  // For each cell in map.
  for (Eigen::Index i = 0; i < output.size(); ++i) {
    if (!std::isfinite(input(i))) {
      continue;
    }
    const double value = std::min<float>(std::max<float>(input(i), min_), max_);
    const double factor = (value - min_) / range;
    const Eigen::Vector3f color = minColor_ + factor * colorRange;
    colorVectorToValue(color, output(i));
  }

  return true;
}

}  // namespace grid_map


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

#include <boost/dispatch/meta/mpl.hpp>
#include <boost/dispatch/meta/hierarchy_of.hpp>
#include <boost/dispatch/meta/nth_hierarchy.hpp>

#include <boost/mpl/int.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

NT2_TEST_CASE(hierarchy_of_mpl_int_)
{
  using boost::mpl::_;
  using boost::mpl::int_;
  using boost::dispatch::meta::int32_;
  using boost::dispatch::meta::scalar_;
  using boost::dispatch::meta::mpl_integral_;
  using boost::dispatch::meta::nth_hierarchy;
  using boost::dispatch::meta::hierarchy_of;

  int_<4> int4;

  typedef hierarchy_of< int_<4>::value_type, int_<4> >::type base;

  NT2_TEST_EXPR_TYPE( int4
                    , (nth_hierarchy< _, int_<0> >)
                    , mpl_integral_< base >
                    );

  NT2_TEST_EXPR_TYPE( int4
                    , (nth_hierarchy< _, int_<1> >)
                    , mpl_integral_< base::parent >
                    );

  NT2_TEST_EXPR_TYPE( int4
                    , (nth_hierarchy< _, int_<2> >)
                    , mpl_integral_< base::parent::parent >
                    );

  NT2_TEST_EXPR_TYPE( int4
                    , (nth_hierarchy< _, int_<3> >)
                    , mpl_integral_< base::parent::parent::parent >
                    );

  NT2_TEST_EXPR_TYPE( int4
                    , (nth_hierarchy< _, int_<4> >)
                    , mpl_integral_< base::parent::parent::parent::parent >
                    );

  NT2_TEST_EXPR_TYPE( int4
                    , (nth_hierarchy< _, int_<5> >)
                    , mpl_integral_< base::parent::parent::parent::parent::parent >
                    );

  NT2_TEST_EXPR_TYPE( int4
                    , (nth_hierarchy< _, int_<6> >)
                    , mpl_integral_ < base::parent::parent::parent
                                          ::parent::parent::parent
                                    >
                    );

  NT2_TEST_EXPR_TYPE( int4
                    , (nth_hierarchy< _, int_<7> >)
                    , mpl_integral_ < base::parent::parent::parent
                                          ::parent::parent::parent
                                          ::parent
                                    >
                    );

  NT2_TEST_EXPR_TYPE( int4
                    , (nth_hierarchy< _, int_<8> >)
                    , mpl_integral_ < base::parent::parent::parent
                                          ::parent::parent::parent
                                          ::parent::parent
                                    >
                    );

  NT2_TEST_EXPR_TYPE( int4
                    , (nth_hierarchy< _, int_<9> >)
                    , mpl_integral_ < base::parent::parent::parent
                                          ::parent::parent::parent
                                          ::parent::parent::parent
                                    >
                    );

  NT2_TEST_EXPR_TYPE( int4
                    , (nth_hierarchy< _, int_<10> >)
                    , mpl_integral_ < base::parent::parent::parent
                                          ::parent::parent::parent
                                          ::parent::parent::parent
                                          ::parent
                                    >
                    );

  NT2_TEST_EXPR_TYPE( int4
                    , (nth_hierarchy< _, int_<21> >)
                    , (hierarchy_of< int_<4>::value_type, int_<4> >::type)
                    );
}


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

// Copyright (c) 2014-2017, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_TEST_MODULE
#define BOOST_TEST_MODULE test_disjoint_coverage
#endif

// unit test to test disjoint for all geometry combinations

#include <iostream>

#include <boost/test/included/unit_test.hpp>

#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/strategies/strategies.hpp>

#include <boost/geometry/io/wkt/wkt.hpp>
#include <boost/geometry/io/dsv/write.hpp>

#include <boost/geometry/geometries/geometries.hpp>

#include <boost/geometry/algorithms/disjoint.hpp>

#include <from_wkt.hpp>

#ifdef HAVE_TTMATH
#include <boost/geometry/extensions/contrib/ttmath_stub.hpp>
#endif

namespace bg = ::boost::geometry;

//============================================================================

struct test_disjoint
{
    template <typename Geometry1, typename Geometry2>
    static inline void apply(std::string const& case_id,
                             Geometry1 const& geometry1,
                             Geometry2 const& geometry2,
                             bool expected_result)
    {
        bool result = bg::disjoint(geometry1, geometry2);
        BOOST_CHECK_MESSAGE(result == expected_result,
            "case ID: " << case_id << ", G1: " << bg::wkt(geometry1)
            << ", G2: " << bg::wkt(geometry2) << " -> Expected: "
            << expected_result << ", detected: " << result);

        result = bg::disjoint(geometry2, geometry1);
        BOOST_CHECK_MESSAGE(result == expected_result,
            "case ID: " << case_id << ", G1: " << bg::wkt(geometry2)
            << ", G2: " << bg::wkt(geometry1) << " -> Expected: "
            << expected_result << ", detected: " << result);

#ifdef BOOST_GEOMETRY_TEST_DEBUG
        std::cout << "case ID: " << case_id << "; G1 - G2: ";
        std::cout << bg::wkt(geometry1) << " - ";
        std::cout << bg::wkt(geometry2) << std::endl;
        std::cout << std::boolalpha;
        std::cout << "expected/computed result: "
                  << expected_result << " / " << result << std::endl;
        std::cout << std::endl;
        std::cout << std::noboolalpha;
#endif
    }
};

//============================================================================

// pointlike-pointlike geometries
template <typename P>
inline void test_point_point()
{
    typedef test_disjoint tester;

    tester::apply("p-p-01",
                  from_wkt<P>("POINT(0 0)"),
                  from_wkt<P>("POINT(0 0)"),
                  false);

    tester::apply("p-p-02",
                  from_wkt<P>("POINT(0 0)"),
                  from_wkt<P>("POINT(1 1)"),
                  true);
}

template <typename P>
inline void test_point_multipoint()
{
    typedef bg::model::multi_point<P> MP;

    typedef test_disjoint tester;

    tester::apply("p-mp-01",
                  from_wkt<P>("POINT(0 0)"),
                  from_wkt<MP>("MULTIPOINT(0 0,1 1)"),
                  false);

    tester::apply("p-mp-02",
                  from_wkt<P>("POINT(0 0)"),
                  from_wkt<MP>("MULTIPOINT(1 1,2 2)"),
                  true);

    tester::apply("p-mp-03",
                  from_wkt<P>("POINT(0 0)"),
                  from_wkt<MP>("MULTIPOINT()"),
                  true);
}

template <typename P>
inline void test_multipoint_point()
{
    typedef bg::model::multi_point<P> MP;

    typedef test_disjoint tester;

    tester::apply("mp-p-01",
                  from_wkt<MP>("MULTIPOINT(0 0,1 1)"),
                  from_wkt<P>("POINT(0 0)"),
                  false);

    tester::apply("mp-p-02",
                  from_wkt<MP>("MULTIPOINT(1 1,2 2)"),
                  from_wkt<P>("POINT(0 0)"),
                  true);
}

template <typename P>
inline void test_multipoint_multipoint()
{
    typedef bg::model::multi_point<P> MP;

    typedef test_disjoint tester;

    tester::apply("mp-mp-01",
                  from_wkt<MP>("MULTIPOINT(0 0,1 0)"),
                  from_wkt<MP>("MULTIPOINT(0 0,1 1)"),
                  false);

    tester::apply("mp-mp-02",
                  from_wkt<MP>("MULTIPOINT(0 0,1 0)"),
                  from_wkt<MP>("MULTIPOINT(1 1,2 2)"),
                  true);

    tester::apply("mp-mp-03",
                  from_wkt<MP>("MULTIPOINT()"),
                  from_wkt<MP>("MULTIPOINT(1 1,2 2)"),
                  true);

    tester::apply("mp-mp-04",
                  from_wkt<MP>("MULTIPOINT(0 0,1 0)"),
                  from_wkt<MP>("MULTIPOINT()"),
                  true);
}

//============================================================================

template <typename CoordinateType>
inline void test_pointlike_pointlike()
{
    typedef bg::model::point<CoordinateType, 2, bg::cs::cartesian> point_type;

    test_point_point<point_type>();
    test_point_multipoint<point_type>();
    test_multipoint_point<point_type>();
    test_multipoint_multipoint<point_type>();
}

//============================================================================

BOOST_AUTO_TEST_CASE( test_pointlike_pointlike_all )
{
    test_pointlike_pointlike<double>();
    test_pointlike_pointlike<int>();
#ifdef HAVE_TTMATH
    test_pointlike_pointlike<ttmath_big>();
#endif
}


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 4, Eigen::Dynamic> test_int_4_D(const Eigen::Matrix<int, 4, Eigen::Dynamic> & M)
{
	return M;
}
void export_int_4_D()
{
	boost::python::def("test_int_4_D",test_int_4_D);
}


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

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

NT2_TEST_CASE_TPL( modf, NT2_TYPES)
{
  using nt2::modf;
  using nt2::tag::modf_;

  NT2_TEST_TYPE_IS( (typename nt2::meta::call<modf_(T)>::type)
                  , (std::pair<T,T>)
                  );

  {
    T ent;
    T frac;

    modf(T(1.5), frac, ent);
    NT2_TEST_EQUAL(ent, nt2::trunc(T(1.5)));
    NT2_TEST_EQUAL(frac , T(.5));
  }

  {
    T ent;
    T frac;

    frac = modf(T(1.5), ent);
    NT2_TEST_EQUAL(ent, nt2::trunc(T(1.5)));
    NT2_TEST_EQUAL(frac , T(.5));
  }

  {
    T ent;
    T frac;

    boost::fusion::vector_tie(frac,ent) = modf(T(1.5));
    NT2_TEST_EQUAL(ent, nt2::trunc(T(1.5)));
    NT2_TEST_EQUAL(frac , T(.5));
  }

  {
    std::pair<T,T> p;

    p = modf(T(1.5));
    NT2_TEST_EQUAL(p.first  , T(.5));
    NT2_TEST_EQUAL(p.second , nt2::trunc(T(1.5)));
  }
}


#include "acceleration/two_grid/spectral_shape/material_spectral_shapes.hpp"

#include <deal.II/lac/full_matrix.h>

#include "spectral_shape_mock.hpp"
#include "data/cross_sections/tests/cross_sections_mock.hpp"
#include "test_helpers/gmock_wrapper.h"
#include "test_helpers/test_helper_functions.h"
#include "material_spectral_shapes_mock.hpp"

namespace  {

using namespace bart;
using ::testing::NiceMock, ::testing::Return, ::testing::DoDefault, ::testing::_;

class CalculatorTwoGridMaterialSpectralShapes : public ::testing::Test {
 public:
  using CrossSections = NiceMock<data::cross_sections::CrossSectionsMock>;
  using DealiiMatrix = dealii::FullMatrix<double>;
  using SpectralShapeMock = NiceMock<acceleration::two_grid::spectral_shape::SpectralShapeMock>;
  using MaterialSpectralShapes = acceleration::two_grid::spectral_shape::MaterialSpectralShapes;
  // Test object
  std::unique_ptr<MaterialSpectralShapes> test_calculator_{ nullptr };

  // Supporing mock objects and observation pointers
  SpectralShapeMock* spectral_shape_calculator_obs_ptr_{ nullptr };
  std::shared_ptr<CrossSections> cross_sections_ptr_{ std::make_shared<CrossSections>() };

  // Test parameters
  const int n_groups{ test_helpers::RandomInt(5, 10) };
  std::unordered_map<int, std::vector<double>> material_id_mapped_to_spectral_shape_;

  auto SetUp() -> void override;
  auto MakeMatrix(std::vector<double> values) const -> DealiiMatrix;
};

auto CalculatorTwoGridMaterialSpectralShapes::SetUp() -> void {
  const double max_cross_section_value{ 10.0 };
  const double max_spectral_shape_value{ 100 };
  auto spectral_shape_calculator_ptr = std::make_unique<SpectralShapeMock>();
  spectral_shape_calculator_obs_ptr_ = spectral_shape_calculator_ptr.get();

  std::unordered_map<int, DealiiMatrix> sigma_s;
  std::unordered_map<int, std::vector<double>> sigma_t;
  std::unordered_map<int, DealiiMatrix> sigma_t_matrices;
  for (int group = 0; group < n_groups; ++group) {
    sigma_s[group] = MakeMatrix(test_helpers::RandomVector(n_groups*n_groups, 0, max_cross_section_value));
    sigma_t[group] = test_helpers::RandomVector(n_groups, 0, max_cross_section_value);
    DealiiMatrix sigma_t_matrix(n_groups, n_groups);
    for (int i = 0; i < n_groups; ++i)
      sigma_t_matrix(i, i) = sigma_t.at(group).at(i);
    sigma_t_matrices[group] = sigma_t_matrix;
    material_id_mapped_to_spectral_shape_[group] = test_helpers::RandomVector(n_groups, 0, max_spectral_shape_value);
  }

  // set expectations
  ON_CALL(*cross_sections_ptr_, sigma_s()).WillByDefault(Return(sigma_s));
  ON_CALL(*cross_sections_ptr_, sigma_t()).WillByDefault(Return(sigma_t));
  for (int group = 0; group < n_groups; ++group) {
    ON_CALL(*spectral_shape_calculator_obs_ptr_, CalculateSpectralShape(sigma_t_matrices.at(group),
                                                                        sigma_s.at(group)))
        .WillByDefault(Return(material_id_mapped_to_spectral_shape_.at(group)));
  }
  test_calculator_ = std::make_unique<MaterialSpectralShapes>(std::move(spectral_shape_calculator_ptr));
}

auto CalculatorTwoGridMaterialSpectralShapes::MakeMatrix(const std::vector<double> values) const -> DealiiMatrix {
  EXPECT_EQ(values.size(), n_groups * n_groups);
  DealiiMatrix return_matrix(n_groups, n_groups);

  for (int i = 0; i < n_groups; ++i) {
    for (int j = 0; j < n_groups; ++j) {
      return_matrix(i, j) = values.at(i * n_groups + j);
    }
  }

  return return_matrix;
}

TEST_F(CalculatorTwoGridMaterialSpectralShapes, ConstructorAndGetter) {
  ASSERT_NE(test_calculator_->spectral_shape_caluculator_ptr(), nullptr);
  EXPECT_EQ(test_calculator_->spectral_shape_caluculator_ptr(), spectral_shape_calculator_obs_ptr_);
}

TEST_F(CalculatorTwoGridMaterialSpectralShapes, ConstructorThrowsOnNullDependency) {
  EXPECT_ANY_THROW(MaterialSpectralShapes(nullptr));
}

TEST_F(CalculatorTwoGridMaterialSpectralShapes, CalculateSpectralShapes) {
  EXPECT_CALL(*cross_sections_ptr_, sigma_s()).WillOnce(DoDefault());
  EXPECT_CALL(*cross_sections_ptr_, sigma_t()).WillOnce(DoDefault());
  EXPECT_CALL(*spectral_shape_calculator_obs_ptr_, CalculateSpectralShape(_,_))
      .Times(n_groups).WillRepeatedly(DoDefault());

  test_calculator_->CalculateMaterialSpectralShapes(this->cross_sections_ptr_);
  ASSERT_EQ(test_calculator_->material_spectral_shapes().size(), n_groups);
  for (int i = 0; i < n_groups; ++i) {
    EXPECT_EQ(test_calculator_->material_spectral_shapes().at(i), material_id_mapped_to_spectral_shape_.at(i));
  }
}

} // namespace


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    testVectorValues.cpp
 * @author  Richard Roberts
 * @date    Sep 16, 2010
 */

#include <gtsam/base/Testable.h>
#include <gtsam/linear/VectorValues.h>

#include <CppUnitLite/TestHarness.h>

#include <boost/assign/std/vector.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/range/adaptor/map.hpp>

using namespace std;
using namespace boost::assign;
using boost::adaptors::map_keys;
using namespace gtsam;

/* ************************************************************************* */
TEST(VectorValues, basics)
{
  // Tests insert(), size(), dim(), exists(), vector()

  // insert
  VectorValues actual;
  actual.insert(0, (Vector(1) << 1).finished());
  actual.insert(1, Vector2(2, 3));
  actual.insert(5, Vector2(6, 7));
  actual.insert(2, Vector2(4, 5));

  // Check dimensions
  LONGS_EQUAL(4, actual.size());
  LONGS_EQUAL(1, actual.dim(0));
  LONGS_EQUAL(2, actual.dim(1));
  LONGS_EQUAL(2, actual.dim(2));
  LONGS_EQUAL(2, actual.dim(5));

  // Logic
  EXPECT(actual.exists(0));
  EXPECT(actual.exists(1));
  EXPECT(actual.exists(2));
  EXPECT(!actual.exists(3));
  EXPECT(!actual.exists(4));
  EXPECT(actual.exists(5));
  EXPECT(!actual.exists(6));

  // Check values
  EXPECT(assert_equal((Vector(1) << 1).finished(), actual[0]));
  EXPECT(assert_equal(Vector2(2, 3), actual[1]));
  EXPECT(assert_equal(Vector2(4, 5), actual[2]));
  EXPECT(assert_equal(Vector2(6, 7), actual[5]));
  KeyVector keys {0, 1, 2, 5};
  EXPECT(assert_equal((Vector(7) << 1, 2, 3, 4, 5, 6, 7).finished(), actual.vector(keys)));

  // Check exceptions
  CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
  CHECK_EXCEPTION(actual.dim(3), out_of_range);
}

/* ************************************************************************* */
TEST(VectorValues, combine)
{
  VectorValues expected;
  expected.insert(0, (Vector(1) << 1).finished());
  expected.insert(1, Vector2(2, 3));
  expected.insert(5, Vector2(6, 7));
  expected.insert(2, Vector2(4, 5));

  VectorValues first;
  first.insert(0, (Vector(1) << 1).finished());
  first.insert(1, Vector2(2, 3));

  VectorValues second;
  second.insert(5, Vector2(6, 7));
  second.insert(2, Vector2(4, 5));

  VectorValues actual(first, second);

  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST(VectorValues, subvector)
{
  VectorValues init;
  init.insert(10, (Vector(1) << 1).finished());
  init.insert(11, Vector2(2, 3));
  init.insert(12, Vector2(4, 5));
  init.insert(13, Vector2(6, 7));

  KeyVector keys {10, 12, 13};
  Vector expSubVector = (Vector(5) << 1, 4, 5, 6, 7).finished();
  EXPECT(assert_equal(expSubVector, init.vector(keys)));
}

/* ************************************************************************* */
TEST(VectorValues, LinearAlgebra)
{
  VectorValues test1;
  test1.insert(0, (Vector(1) << 1).finished());
  test1.insert(1, Vector2(2, 3));
  test1.insert(5, Vector2(6, 7));
  test1.insert(2, Vector2(4, 5));

  VectorValues test2;
  test2.insert(0, (Vector(1) << 6).finished());
  test2.insert(1, Vector2(1, 6));
  test2.insert(5, Vector2(4, 3));
  test2.insert(2, Vector2(1, 8));

  // Dot product
  double dotExpected = test1.vector().dot(test2.vector());
  double dotActual = test1.dot(test2);
  DOUBLES_EQUAL(dotExpected, dotActual, 1e-10);

  // Norm
  double normExpected = test1.vector().norm();
  double normActual = test1.norm();
  DOUBLES_EQUAL(normExpected, normActual, 1e-10);

  // Squared norm
  double sqNormExpected = test1.vector().norm();
  double sqNormActual = test1.norm();
  DOUBLES_EQUAL(sqNormExpected, sqNormActual, 1e-10);

  // Addition
  Vector sumExpected = test1.vector() + test2.vector();
  VectorValues sumActual = test1 + test2;
  EXPECT(sumActual.hasSameStructure(test1));
  EXPECT(assert_equal(sumExpected, sumActual.vector()));
  Vector sum2Expected = sumActual.vector() + test1.vector();
  VectorValues sum2Actual = sumActual;
  sum2Actual += test1;
  EXPECT(assert_equal(sum2Expected, sum2Actual.vector()));

  // Add to empty
  VectorValues sumActual3;
  sumActual3.addInPlace_(test1);
  sumActual3.addInPlace_(test2);
  EXPECT(assert_equal(sumExpected, sumActual3.vector()));

  // Subtraction
  Vector difExpected = test1.vector() - test2.vector();
  VectorValues difActual = test1 - test2;
  EXPECT(difActual.hasSameStructure(test1));
  EXPECT(assert_equal(difExpected, difActual.vector()));

  // Scaling
  Vector scalExpected = test1.vector() * 5.0;
  VectorValues scalActual = test1;
  scalActual *= 5.0;
  EXPECT(assert_equal(scalExpected, scalActual.vector()));
  VectorValues scal2Actual = 5.0 * test1;
  EXPECT(assert_equal(scalExpected, scal2Actual.vector()));
}

/* ************************************************************************* */
TEST(VectorValues, convert)
{
  Vector x(7);
  x << 1, 2, 3, 4, 5, 6, 7;

  VectorValues expected;
  expected.insert(0, (Vector(1) << 1).finished());
  expected.insert(1, Vector2(2, 3));
  expected.insert(2, Vector2(4, 5));
  expected.insert(5, Vector2(6, 7));

  std::map<Key,size_t> dims;
  dims.insert(make_pair(0,1));
  dims.insert(make_pair(1,2));
  dims.insert(make_pair(2,2));
  dims.insert(make_pair(5,2));
  VectorValues actual(x,dims);
  EXPECT(assert_equal(expected, actual));

  Scatter scatter;
  scatter.emplace_back(0,1);
  scatter.emplace_back(1,2);
  scatter.emplace_back(2,2);
  scatter.emplace_back(5,2);
  VectorValues actual2(x,scatter);
  EXPECT(assert_equal(expected, actual2));

  // Test other direction, note vector() is not guaranteed to give right result
  KeyVector keys {0, 1, 2, 5};
  EXPECT(assert_equal(x, actual.vector(keys)));

  // Test version with dims argument
  EXPECT(assert_equal(x, actual.vector(dims)));
}

/* ************************************************************************* */
TEST(VectorValues, vector_sub)
{
  VectorValues vv;
  vv.insert(0, (Vector(1) << 1).finished());
  vv.insert(1, Vector2(2, 3));
  vv.insert(2, Vector2(4, 5));
  vv.insert(5, Vector2(6, 7));
  vv.insert(7, Vector2(8, 9));

  std::map<Key,size_t> dims;
  dims.insert(make_pair(0,1));
  dims.insert(make_pair(5,2));

  Vector expected(3);
  expected << 1, 6, 7;

  // Test FastVector version
  KeyVector keys {0, 5};
  EXPECT(assert_equal(expected, vv.vector(keys)));

  // Test version with dims argument
  EXPECT(assert_equal(expected, vv.vector(dims)));
}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */


/**
    @file sysid_actuator_gazebo.cpp
    ROS node for collecting data for sysid to determine
    an appropriate scaling for thrust and body angular
    acceleration commands over ActuatorControl messages.
*/
#include <ros/ros.h>
#include <ros/console.h>
#include <mavros_msgs/ParamSet.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/ActuatorControl.h>
#include <mavros_msgs/RCOut.h>
#include <gazebo_msgs/LinkStates.h>
#include <string>
#include <iostream>
#include <algorithm>
#include <vector>
#include <Eigen/Dense>

#include <utils/rotation.hpp>

bool px4_connected; // PX4 connection exists
std::string px4_mode; // PX4 mode
void state_cb(const mavros_msgs::State::ConstPtr& msg) {
    px4_connected = msg->connected;
    px4_mode = msg->mode;
}

unsigned pwm;
void rcout_cb(const mavros_msgs::RCOut::ConstPtr& msg) {
    pwm = msg->channels[0];
}

/**
    Extract the orientation of the control surfaces from Gazebo and
    compute their relative orientation with respect to the body. Each
    quaternion from Gazebo is rotation from world frame to link frame,
    therefore need to convert to get relative rotation.
*/
std::vector<double> gz_ctrl(5, 0.0); // gz_ctrl = [al, ar, e, r, T]
Eigen::Vector4d ctrl_q; // quaternion for control surface
Eigen::Vector4d body_q; // quaternion for body
Eigen::Vector4d rel_q; // quaternion for relative orientation
Eigen::Vector3d axis;
double angle;
Eigen::Vector3d aa;
Eigen::Vector3d rel_om_wrld;
Eigen::Matrix3d R;
std::vector<int> ctrl_map = {4, 5, 6, 7}; // maps to gazebo message indices
void gz_links_cb(const gazebo_msgs::LinkStates::ConstPtr& msg) {
    // Get body orientation relative to world
    body_q(0) = msg->pose[1].orientation.w;
    body_q(1) = msg->pose[1].orientation.x;
    body_q(2) = msg->pose[1].orientation.y;
    body_q(3) = msg->pose[1].orientation.z;
    Rot::invert_quat(body_q); // invert to rotation from body to world

    // Orientations of control surfaces
    for (int i = 0; i < 4; ++i) {
        // Get control surface orientation relative to world
        ctrl_q(0) = msg->pose[ctrl_map[i]].orientation.w;
        ctrl_q(1) = msg->pose[ctrl_map[i]].orientation.x;
        ctrl_q(2) = msg->pose[ctrl_map[i]].orientation.y;
        ctrl_q(3) = msg->pose[ctrl_map[i]].orientation.z;

        // Compute relative orientation
        Rot::compose_quats(body_q, ctrl_q, rel_q); // compose the rotations
        Rot::quat_to_axis(rel_q, aa);
        angle = aa.norm();
        axis = aa.normalized();
        gz_ctrl[i] = angle;
    }

    // relative = prop - body spin speed written in world frame coordinates
    rel_om_wrld(0) = msg->twist[3].angular.x - msg->twist[1].angular.x;
    rel_om_wrld(1) = msg->twist[3].angular.y - msg->twist[1].angular.y;
    rel_om_wrld(2) = msg->twist[3].angular.z - msg->twist[1].angular.z;

    // Convert world to body frame coordinates
    Rot::quat_to_R(body_q, R);
    gz_ctrl[4] = R.row(0)*rel_om_wrld;

}

int main(int argc, char **argv) {
    ros::init(argc, argv, "act_ctrl_sysid_node");
    ros::NodeHandle nh;

    // Get parameters
    double STEP_SIZE = 0.1;
    if (!nh.getParam("/sysid_actuator_ctrl/STEP_SIZE", STEP_SIZE)) {
        ROS_INFO("Using default step size of 0.1");
    }

    double HOLD_TIME = 5.0;
    if (!nh.getParam("/sysid_actuator_ctrl/HOLD_TIME", HOLD_TIME)) {
        ROS_INFO("Using default hold time of 5 seconds");
    }
    HOLD_TIME = std::max(HOLD_TIME, 0.1);

    // Define subscribers
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
                ("mavros/state", 10, state_cb);
    ros::Subscriber rcout_sub = nh.subscribe<mavros_msgs::RCOut>
                ("mavros/rc/out", 10, rcout_cb);
    ros::Subscriber gz_links_sub = nh.subscribe<gazebo_msgs::LinkStates>
                ("gazebo/link_states", 10, gz_links_cb);

    // Define publishers
    ros::Publisher act_cmd_pub = nh.advertise<mavros_msgs::ActuatorControl>
                ("mavros/actuator_control", 10);

    // Define services
    ros::ServiceClient arm_clt = nh.serviceClient<mavros_msgs::CommandBool>
                ("mavros/cmd/arming");
    ros::ServiceClient param_clt = nh.serviceClient<mavros_msgs::ParamSet>
                ("mavros/param/set");

    // Get from user which set of servos to check
    char input = 'z';
    std::string controls = "rpyt";
    while (controls.find(input) == std::string::npos) {
        std::cout << "Select input: roll (r), pitch (p), yaw (y), thrust (t): ";
        std::cin >> input;
    }
    int c = controls.find(input);

    char sign = '.';
    std::string signs = "+-";
    while (signs.find(sign) == std::string::npos) {
        std::cout << "Select direction: positive (+), negative (-): ";
        std::cin >> sign;
    }
    double s;
    if (sign == '+') s = 1.0;
    else s = -1.0;

    // Define rate for the node
    ros::Rate rate(5.0);

    // Turn off auto-disarm before takeoff so sysid can be performed on the ground
    mavros_msgs::ParamSet dsrm_off;
    dsrm_off.request.param_id = "COM_DISARM_PRFLT";
    dsrm_off.request.value.real = -1.;
    param_clt.call(dsrm_off);
    if (dsrm_off.response.success) ROS_INFO("Disabled auto-disarm before takeoff");

    // Automatically arm the vehicle
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;
    arm_clt.call(arm_cmd);
    if (arm_cmd.response.success) ROS_INFO("Vehicle armed");

    // Define the actuator control message
    mavros_msgs::ActuatorControl act_cmd;
    act_cmd.group_mix = mavros_msgs::ActuatorControl::PX4_MIX_FLIGHT_CONTROL;

    while(ros::ok() && !px4_connected) {
        ros::spinOnce();
        rate.sleep();
    }

    int hold_steps = HOLD_TIME/0.1;
    int inc_steps = 1.0/STEP_SIZE;
    int h = 0;
    int i = 0;
    std::cout << "Set PX4 to offboard mode to start sequence" << std::endl;
    while(ros::ok() && i <= inc_steps) {
        ros::spinOnce();

        std::cout << "[|al|, |ar|, |e|, |r|, |T|] = [" << gz_ctrl[0] << " " << gz_ctrl[1]
                << " " << gz_ctrl[2] << " " << gz_ctrl[3] << " " << gz_ctrl[4]
                << "] rad, rad/s" << std::endl;

        act_cmd_pub.publish(act_cmd);

        if (px4_mode == "OFFBOARD") {

            // Switch the level every hold_steps number of steps
            if (h == hold_steps) {
                act_cmd.controls[c] += s*STEP_SIZE;
                h = 0;
                i++;

                // Print the command
                if (i <= inc_steps) {
                    std::cout << "act_cmd.controls[" << input << "] = "
                                << act_cmd.controls[c] << std::endl;
                }
            }

            h++;

        }

        rate.sleep(); // sleep for remaining time
    }
    act_cmd.controls[c] = 0.0;
    act_cmd_pub.publish(act_cmd);
    ROS_INFO("Done with open-loop inputs, shutting down node");
    ros::shutdown();
}


/*
 Copyright (C) 2019 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include "toplevelfixture.hpp"
#include <boost/test/unit_test.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/time/calendars/nullcalendar.hpp>
#include <ql/time/daycounters/actualactual.hpp>
#include <qle/termstructures/blackvariancesurfacestddevs.hpp>

using namespace boost::unit_test_framework;
using namespace QuantLib;
using std::vector;

BOOST_FIXTURE_TEST_SUITE(QuantExtTestSuite, qle::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(BlackVarianceSurfaceStdDevs)

BOOST_AUTO_TEST_CASE(testFlatSurface) {

    BOOST_TEST_MESSAGE("Testing QuantExt::BlackVarianceSurfaceStdDevs");

    SavedSettings backup;
    Settings::instance().evaluationDate() = Date(1, Dec, 2015);
    Date today = Settings::instance().evaluationDate();

    // we setup a flat surface, all at 12%
    // Then we ask it for vol at different tenors and strikes
    Calendar cal = NullCalendar();
    Handle<Quote> spot = Handle<Quote>(boost::make_shared<SimpleQuote>(100));
    vector<Time> times = { 1.0, 2.0, 3.0, 4.0 };
    vector<Real> stdDevs = { -1.0, -0.5, 0, 0.5, 1.0 };
    Volatility flatVol = 0.12;
    Handle<Quote> flatVolQ = Handle<Quote>(boost::make_shared<SimpleQuote>(flatVol));
    vector<vector<Handle<Quote> > > blackVolMatrix(stdDevs.size(), vector<Handle<Quote> >(times.size(), flatVolQ));
    DayCounter dc = ActualActual();
    Handle<YieldTermStructure> forTS(
        boost::make_shared<FlatForward>(today, Handle<Quote>(boost::make_shared<SimpleQuote>(0.02)), ActualActual()));
    Handle<YieldTermStructure> domTS(
        boost::make_shared<FlatForward>(today, Handle<Quote>(boost::make_shared<SimpleQuote>(0.01)), ActualActual()));

    QuantExt::BlackVarianceSurfaceStdDevs surface(cal, spot, times, stdDevs, blackVolMatrix, dc, forTS, domTS);

    // Now get a vol for different times and strikes
    for (Time t = 0.05; t < 5.0; t += 0.1) {
        // spot is 100 so strikes should range from (say) 70 to 150
        for (Real k = 70; k < 150; k += 0.5) {
            Volatility vol = surface.blackVol(t, k);
            // BOOST_TEST_MESSAGE("BlackVarianceSurfaceStdDevs vol for t=" << t << " and k=" << k << " is " << vol);
            BOOST_CHECK_CLOSE(vol, flatVol, 1e-12);
        }
    }
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


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

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-ieee
    This function object returns the input value which have the least absolute value.

    @par Header <boost/simd/function/minmag.hpp>

    @par Example:

      @snippet minmag.cpp minmag

    @par Possible output:

      @snippet minmag.txt minmag

  **/
  Value minmag(Value const& x, Value const& y);
} }
#endif

#include <boost/simd/function/scalar/minmag.hpp>
#include <boost/simd/function/simd/minmag.hpp>

#endif