/*
 * Copyright (c) 2009-2021, Albertas Vyšniauskas
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
 *     * Neither the name of the software author nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 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 <boost/test/unit_test.hpp>
#include "math/Matrix.h"
#include <iostream>
namespace math {
static std::ostream &operator<<(std::ostream &stream, const math::Matrix3d &matrix) {
	for (size_t i = 0; i < math::Matrix3d::Size * math::Matrix3d::Size; i++) {
		if (i != 0)
			stream << ", ";
		stream << matrix.flatData[i];
	}
	return stream;
}
static std::ostream &operator<<(std::ostream &stream, const math::Vector3d &vector) {
	for (size_t i = 0; i < math::Vector3d::Size; i++) {
		if (i != 0)
			stream << ", ";
		stream << vector.data[i];
	}
	return stream;
}
}
BOOST_AUTO_TEST_SUITE(matrix)
const static math::Matrix3d zero = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
const static math::Matrix3d identity = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 };
BOOST_AUTO_TEST_CASE(determinant) {
	BOOST_CHECK_EQUAL(zero.determinant(), 0.0);
	BOOST_CHECK_EQUAL(identity.determinant(), 1.0);
	BOOST_CHECK_EQUAL(math::Matrix3d(1.0, 2.0, 3.0, 0.0, 1.0, 4.0, 5.0, 6.0, 0.0).determinant(), 1.0);
}
BOOST_AUTO_TEST_CASE(multiplication) {
	BOOST_CHECK_EQUAL(identity * zero, zero);
	BOOST_CHECK_EQUAL(zero * identity, zero);
	BOOST_CHECK_EQUAL(identity * identity, identity);
}
BOOST_AUTO_TEST_CASE(inverse) {
	BOOST_CHECK_EQUAL(*identity.inverse(), identity);
	BOOST_CHECK_EQUAL(*math::Matrix3d(1.0, 2.0, 3.0, 0.0, 1.0, 4.0, 5.0, 6.0, 0.0).inverse(), math::Matrix3d(-24.0, 18.0, 5.0, 20.0, -15.0, -4.0, -5.0, 4.0, 1.0));
}
BOOST_AUTO_TEST_CASE(vectorMultiplication) {
	math::Vector3d unit = { 1.0, 1.0, 1.0 };
	math::Matrix3d translate = { 1.0, 0.0, 1.0, 0.0, 1.0, 1.0, 0.0, 0.0, 1.0 };
	BOOST_CHECK_EQUAL(translate * unit, math::Vector3d(2.0, 2.0, 1.0));
	BOOST_CHECK_EQUAL(unit * translate, math::Vector3d(1.0, 1.0, 3.0));
}
BOOST_AUTO_TEST_SUITE_END()


// Copyright (c) 2015
// Author: Chrono Law
#include <std.hpp>
using namespace std;

#define BOOST_RATIO_EXTENSIONS
#include <boost/ratio.hpp>
using namespace boost;

//////////////////////////////////////////

void case1()
{
    typedef ratio<1, 2> half;

    assert(half::num == 1);
    assert(half::den == 2);

    auto v = half::value();
    cout << v << endl;
    assert(v * 2 == 1);

    half frac;
    assert(frac().numerator() == frac.num);

    typedef ratio<2, 4> two_fourth;
    cout << two_fourth()() << endl;
    assert(half::value() == two_fourth::value());

    typedef ratio<12> dozen;
    assert(2* dozen()() == 24);
}

//////////////////////////////////////////
typedef ratio<1, 2> half;
typedef ratio<1, 4> quater;

typedef ratio<12, 1> dozen;
typedef ratio<kilo::num*10, 1> cn_wan;

void case2()
{
    assert(kilo::num < kibi::num);
    cout << kilo::num << endl;
    cout << kibi::num << endl;

    assert((quater())()*2 == half()());
    assert((mega())() == cn_wan()()*100);
}

//////////////////////////////////////////
boost::intmax_t operator"" _kb(unsigned long long n)
{
    return n * boost::kibi::num;
}

boost::intmax_t operator"" _gb(unsigned long long n)
{
    return n * boost::gibi::num;
}

void case3()
{
    auto x = 2_gb;
    auto y = 10_kb;

    assert(x = 2 * 100 * y);
}

//////////////////////////////////////////
template<typename R>
using string_out = ratio_string<R, char>;

void case4()
{
    cout << string_out<kilo>::prefix() << endl;
    cout << string_out<kilo>::symbol() << endl;

    cout << string_out<nano>::prefix() << endl;
    cout << string_out<nano>::symbol() << endl;

    cout << string_out<ratio<22, 7>>::prefix() << endl;
}

//////////////////////////////////////////

int main()
{
    case1();
    case2();
    case3();
    case4();
}


#include "catch.hpp"
#include "ada_star_search.h"
#include "simple_point.h"
#include "EuclideanDistanceFunctor.h"
#include <boost/graph/random.hpp>
#include <boost/graph/circle_layout.hpp>
#include <boost/random.hpp>
#include <boost/graph/astar_search.hpp>
#include <algorithm>
#include <cmath>

using namespace boost;
using namespace ada_star;
typedef simple_point<float> pos;
typedef adjacency_list<vecS, vecS, bidirectionalS, pos> G;
typedef graph_traits<G>::vertex_descriptor V;

TEST_CASE("[ADA] Path tests", "[full]") {

    // Generate graph
    random::mt11213b generator(0);
    G g;
    const int N = 10000;
    const float radius = 10.0f;
    V prev = add_vertex({ 0.0f, radius }, g);
    for (int i = 1; i < N; ++i) {
        float theta = (float)i * 2 * M_PI / N;
        pos p = { sin(theta) * radius, cos(theta) * radius };
        V next = add_vertex(p, g);
        add_edge(prev, next, g);
        prev = next;
    }
    for (int i = 0; i < 1000; ++i) {
        V u = random_vertex(g, generator);
        V v = random_vertex(g, generator);
        add_edge(u, v, g);
    }
    V source = random_vertex(g, generator);
    V destination = random_vertex(g, generator);

    // Initialise helpers
    EuclideanDistanceFunctor<G, float> weight_map(g);

    // Build optimal solution
    std::map<V, V> pred_map_map;
    associative_property_map<std::map<V, V>> pred_map(pred_map_map);

    std::vector<V> desired;
    BENCHMARK("Baseline A*") {
        astar_search(
            g,
            source,
            [&weight_map, &destination](V v){
                return weight_map[{ v, destination }];
            },
            predecessor_map(pred_map).
            weight_map(weight_map)
        );

        V current = destination;
        while (current != source) {
            desired.push_back(current);
            current = pred_map[current];
        }
        desired.push_back(current);
        std::reverse(desired.begin(), desired.end());
    }

    float desiredLength = 0.0f;
    for (int i = 0; i + 1 < desired.size(); ++i) {
        desiredLength += get(weight_map, { desired[i], desired[i + 1] });
    }

    SECTION("Optimal") {
        map_property_map<V, float> g_map = make_g(source, destination);

        std::vector<V> solution;
        BENCHMARK("Optimal path") {
            ada_star_search(
                g,
                source,
                destination,
                _weight_map=weight_map,
                _g=g_map
            );

            V current = source;
            while (current != destination) {
                solution.push_back(current);
                current = ada_star_next_step(
                    g,
                    current,
                    weight_map,
                    g_map
                );
            }
            solution.push_back(current);
        }

        REQUIRE(solution.size() > 0);

        float solutionLength = 0.0f;
        for (int i = 0; i + 1 < solution.size(); ++i) {
            solutionLength += get(weight_map, { solution[i], solution[i + 1] });
        }

        REQUIRE(solutionLength == desiredLength);
    }

    SECTION("Suboptimal (ε = 1.5)") {
        map_property_map<V, float> g_map = make_g(source, destination);

        std::vector<V> solution;
        BENCHMARK("Suboptimal path") {
            ada_star_search(
                g,
                source,
                destination,
                _weight_map=weight_map,
                _g=g_map,
                _suboptimality=1.5f
            );

            V current = source;
            while (current != destination) {
                solution.push_back(current);
                current = ada_star_next_step(
                    g,
                    current,
                    weight_map,
                    g_map
                );
            }
            solution.push_back(current);
        }

        REQUIRE(solution.size() > 0);

        float solutionLength = 0.0f;
        for (int i = 0; i + 1 < solution.size(); ++i) {
            solutionLength += get(weight_map, { solution[i], solution[i + 1] });
        }

        REQUIRE(desiredLength <= solutionLength);
        REQUIRE(solutionLength <= 1.5f * desiredLength);
    }

}


//
//  Copyright (C) Toon Knapen 2003
//
// 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_NUMERIC_BINDINGS_BLAS_BLAS3_OVERLOADS_HPP
#define BOOST_NUMERIC_BINDINGS_BLAS_BLAS3_OVERLOADS_HPP

#include <boost/numeric/bindings/blas/blas.h>
#include <boost/numeric/bindings/traits/type_traits.hpp>

namespace boost { namespace numeric { namespace bindings { namespace blas { namespace detail {

  using namespace boost::numeric::bindings::traits ;

  inline
  void gemm( char TRANSA, char TRANSB, const integer_t& m, const integer_t& n, const integer_t& k, const float    & alpha, const float    * a_ptr, const integer_t& lda, const float    * b_ptr, const integer_t& ldb, const float    & beta, float    * c_ptr, const integer_t& ldc ) { BLAS_SGEMM( &TRANSA, &TRANSB, &m, &n, &k,          ( &alpha ),          ( a_ptr ), &lda,          ( b_ptr ), &ldb,          ( &beta ),          ( c_ptr ), &ldc ) ; }
  inline
  void gemm( char TRANSA, char TRANSB, const integer_t& m, const integer_t& n, const integer_t& k, const double   & alpha, const double   * a_ptr, const integer_t& lda, const double   * b_ptr, const integer_t& ldb, const double   & beta, double   * c_ptr, const integer_t& ldc ) { BLAS_DGEMM( &TRANSA, &TRANSB, &m, &n, &k,          ( &alpha ),          ( a_ptr ), &lda,          ( b_ptr ), &ldb,          ( &beta ),          ( c_ptr ), &ldc ) ; }
  inline
  void gemm( char TRANSA, char TRANSB, const integer_t& m, const integer_t& n, const integer_t& k, const complex_f& alpha, const complex_f* a_ptr, const integer_t& lda, const complex_f* b_ptr, const integer_t& ldb, const complex_f& beta, complex_f* c_ptr, const integer_t& ldc ) { BLAS_CGEMM( &TRANSA, &TRANSB, &m, &n, &k, complex_ptr( &alpha ), complex_ptr( a_ptr ), &lda, complex_ptr( b_ptr ), &ldb, complex_ptr( &beta ), complex_ptr( c_ptr ), &ldc ) ; }
  inline
  void gemm( char TRANSA, char TRANSB, const integer_t& m, const integer_t& n, const integer_t& k, const complex_d& alpha, const complex_d* a_ptr, const integer_t& lda, const complex_d* b_ptr, const integer_t& ldb, const complex_d& beta, complex_d* c_ptr, const integer_t& ldc ) { BLAS_ZGEMM( &TRANSA, &TRANSB, &m, &n, &k, complex_ptr( &alpha ), complex_ptr( a_ptr ), &lda, complex_ptr( b_ptr ), &ldb, complex_ptr( &beta ), complex_ptr( c_ptr ), &ldc ) ; }

  //
  // SYRK
  //
  inline
  void syrk( char uplo, char trans, const integer_t& n, const integer_t& k, const float& alpha,
             const float* a_ptr, const integer_t lda, const float& beta, float* c_ptr,
             const integer_t& ldc)
  {
     BLAS_SSYRK( &uplo, &trans, &n, &k, &alpha, a_ptr, &lda, &beta, c_ptr, &ldc);
  }

  inline
  void syrk( char uplo, char trans, const integer_t& n, const integer_t& k, const double& alpha,
             const double* a_ptr, const integer_t lda, const double& beta, double* c_ptr,
             const integer_t& ldc)
  {
     BLAS_DSYRK( &uplo, &trans, &n, &k, &alpha, a_ptr, &lda, &beta, c_ptr, &ldc);
  }

  inline
  void syrk( char uplo, char trans, const integer_t& n, const integer_t& k, const complex_f& alpha,
             const complex_f* a_ptr, const integer_t lda, const complex_f& beta, complex_f* c_ptr,
             const integer_t& ldc)
  {
     BLAS_CSYRK( &uplo, &trans, &n, &k, complex_ptr( &alpha ), complex_ptr( a_ptr ),
                 &lda, complex_ptr( &beta ), complex_ptr( c_ptr ), &ldc);
  }

  inline
  void syrk( char uplo, char trans, const integer_t& n, const integer_t& k, const complex_d& alpha,
             const complex_d* a_ptr, const integer_t lda, const complex_d& beta, complex_d* c_ptr,
             const integer_t& ldc)
  {
     BLAS_ZSYRK( &uplo, &trans, &n, &k, complex_ptr( &alpha ), complex_ptr( a_ptr ),
                 &lda, complex_ptr( &beta ), complex_ptr( c_ptr ), &ldc);
  }

  //
  // HERK
  //
  inline
  void herk( char uplo, char trans, const integer_t& n, const integer_t& k, const float& alpha,
             const float* a_ptr, const integer_t lda, const float& beta, float* c_ptr,
             const integer_t& ldc)
  {
     BLAS_SSYRK( &uplo, &trans, &n, &k, &alpha, a_ptr, &lda, &beta, c_ptr, &ldc);
  }

  inline
  void herk( char uplo, char trans, const integer_t& n, const integer_t& k, const double& alpha,
             const double* a_ptr, const integer_t lda, const double& beta, double* c_ptr,
             const integer_t& ldc)
  {
     BLAS_DSYRK( &uplo, &trans, &n, &k, &alpha, a_ptr, &lda, &beta, c_ptr, &ldc);
  }

  inline
  void herk( char uplo, char trans, const integer_t& n, const integer_t& k, const float& alpha,
             const complex_f* a_ptr, const integer_t lda, const float& beta, complex_f* c_ptr,
             const integer_t& ldc)
  {
     BLAS_CHERK( &uplo, &trans, &n, &k, &alpha, complex_ptr( a_ptr ),
                 &lda, &beta, complex_ptr( c_ptr ), &ldc);
  }

  inline
  void herk( char uplo, char trans, const integer_t& n, const integer_t& k, const double& alpha,
             const complex_d* a_ptr, const integer_t lda, const double& beta, complex_d* c_ptr,
             const integer_t& ldc)
  {
     BLAS_ZHERK( &uplo, &trans, &n, &k, &alpha, complex_ptr( a_ptr ),
                 &lda, &beta, complex_ptr( c_ptr ), &ldc);
  }

  //
  // trsm
  //
  inline
  void trsm( char side, char uplo, char transa, char diag, integer_t m, integer_t n,
             float const& alpha, float const* a_ptr, integer_t lda,
             float* b_ptr, integer_t ldb )
  {
     BLAS_STRSM( &side, &uplo, &transa, &diag, &m, &n, &alpha, a_ptr, &lda, b_ptr, &ldb ) ;
  }

  inline
  void trsm( char side, char uplo, char transa, char diag, integer_t m, integer_t n,
             double const& alpha, double const* a_ptr, integer_t lda,
             double* b_ptr, integer_t ldb )
  {
     BLAS_DTRSM( &side, &uplo, &transa, &diag, &m, &n, &alpha, a_ptr, &lda, b_ptr, &ldb ) ;
  }

  inline
  void trsm( char side, char uplo, char transa, char diag, integer_t m, integer_t n,
             complex_f const& alpha, complex_f const* a_ptr, integer_t lda,
             complex_f* b_ptr, integer_t ldb )
  {
     BLAS_CTRSM( &side, &uplo, &transa, &diag, &m, &n, complex_ptr( &alpha ), complex_ptr( a_ptr ), &lda, complex_ptr( b_ptr ), &ldb ) ;
  }

  inline
  void trsm( char side, char uplo, char transa, char diag, integer_t m, integer_t n,
             complex_d const& alpha, complex_d const* a_ptr, integer_t lda,
             complex_d* b_ptr, integer_t ldb )
  {
     BLAS_ZTRSM( &side, &uplo, &transa, &diag, &m, &n, complex_ptr( &alpha ), complex_ptr( a_ptr ), &lda, complex_ptr( b_ptr ), &ldb ) ;
  }

}}}}}

#endif // BOOST_NUMERIC_BINDINGS_BLAS_BLAS3_OVERLOADS_HPP


// -*- coding: utf-8 -*-
#include <algorithm>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>
#include <boost/property_map/property_map.hpp>
#include <doctest/doctest.h>
#include <ellcpp/cutting_plane.hpp>
#include <ellcpp/ell.hpp>
// #include <ellcpp/ell1d.hpp>
#include <ellcpp/oracles/optscaling_oracle.hpp> // import optscaling
#include <py2cpp/nx2bgl.hpp>
#include <utility> // for std::pair
#include <xtensor/xarray.hpp>

namespace boost
{

enum edge_id_tag_t
{
    id_tag
}; // a unique #
BOOST_INSTALL_PROPERTY(edge, id_tag);

} // namespace boost

using Arr = xt::xarray<double, xt::layout_type::row_major>;

using graph_t =
    boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS,
        boost::no_property, boost::property<boost::edge_id_tag_t, size_t>>;
using Vertex = typename boost::graph_traits<graph_t>::vertex_descriptor;
using edge_t = typename boost::graph_traits<graph_t>::edge_iterator;

/*!
 * @brief Create a test case1 object
 *
 * @return xn::grAdaptor<graph_t>
 */
static xn::grAdaptor<graph_t> create_test_case1()
{
    using Edge = std::pair<int, int>;
    const auto num_nodes = 5;
    enum nodes
    {
        A,
        B,
        C,
        D,
        E
    };
    // char name[] = "ABCDE";
    Edge edge_array[] = {
        Edge(A, B), Edge(B, C), Edge(C, D), Edge(D, E), Edge(E, A)};
    size_t indices[] = {0, 1, 2, 3, 4};
    auto num_arcs = sizeof(edge_array) / sizeof(Edge);
    auto g =
        graph_t(edge_array, edge_array + num_arcs, indices, num_nodes);
    return xn::grAdaptor<graph_t>(std::move(g));
}

TEST_CASE("Test Optimal Scaling (two varaibles, boost)")
{
    using EdgeIndexMap =
        typename boost::property_map<graph_t, boost::edge_id_tag_t>::type;
    using IterMap =
        boost::iterator_property_map<double*, EdgeIndexMap, double, double&>;

    auto G = create_test_case1();

    double elem[] = {1.2, 2.3, 3.4, -4.5, 5.6};
    const auto num_of_nodes = sizeof(elem) / sizeof(double);

    double cost[num_of_nodes];
    for (auto i = 0; i != num_of_nodes; ++i)
    {
        cost[i] = std::log(std::abs(elem[i]));
    }
    auto edge_id = boost::get(boost::id_tag, G);
    auto cost_pa = IterMap {cost, edge_id};

    auto get_cost = [&](const auto& e) -> double
    { return boost::get(cost_pa, e); };

    const auto [cmin, cmax] =
        std::minmax_element(std::begin(cost), std::end(cost));
    // auto cmin = *std::min_element(cost, cost + num_of_nodes);
    const auto x0 = Arr {*cmax, *cmin};
    auto t1 = *cmax - *cmin;
    auto E = ell {1.5 * t1, x0};
    auto dist = std::vector<double>(G.number_of_nodes(), 0.);

    auto P = optscaling_oracle<decltype(G), std::vector<double>,
        decltype(get_cost)> {G, dist, get_cost};
    auto t = 1.e100; // std::numeric_limits<double>::max()
    const auto [x, ell_info] = cutting_plane_dc(P, E, t);

    CHECK(x[1] >= x[0]);
    CHECK(ell_info.feasible);
    CHECK(ell_info.num_iters <= 27);
}


#include <algorithm>
#include <iostream>
#include <vector>

#include <boost/range/counting_range.hpp>

struct Entity {
	std::int64_t health;
	std::int64_t damage;
	std::int64_t armor;
};

struct Item {
	std::int64_t price;
	std::int64_t dmg_bonus;
	std::int64_t ac_bonus;
};

template<typename T>
auto factorial(T n) -> T {
    if(n > 1) {
        return n * factorial(n - 1);
    }
    return 1;
}

template<typename N, typename K>
auto binomial_coefficient(N n, K k) {
	return factorial(n) / (factorial(k) * factorial(n-k));
}

// NOTE: this function is adapted from https://stackoverflow.com/a/9430993/699211
// It works by creating a "selection array", where we place k selectors, then
// we create all permutations of these selectors, and add the corresponding set
// member if it is selected in in the current permutation of the selectors.
auto get_rings() {

	auto rings = std::vector<Item>{{}, {}, {25,1,0}, {50,2,0}, {100,3,0}, {20,0,1}, {40,0,2}, {80,0,3}};

	// a combination of n rings with k elements in each combination
	const auto n = rings.size();
	const auto k = 2;

	// the binomial coefficient gives us the number of all combinations of size k from n elements
	auto ring_combos = std::vector<std::vector<Item>>(binomial_coefficient(n, k));

	enum class Bool { False, True };

	auto selector = std::vector<Bool>(n);

	std::fill(selector.begin(), selector.begin() + k, Bool::True);

	do {

		for(const auto i : boost::counting_range({}, n)) {

			if(selector[i] == Bool::True) {

				// the vectors containing the ring combinations must not exceed k elements
				const auto fillable = std::find_if(ring_combos.begin(), ring_combos.end(), [k] (const auto combo) {
					return combo.size() < k;
				});

				// ring_combos is sized to contain all the combinations of length k,
				// so we are guaranteed that fillable never points to end()
				fillable->push_back(rings[i]);
			}
		}

	} while(std::prev_permutation(selector.begin(), selector.end()));

	return ring_combos;
}

int main() {

	const auto weapons = std::vector<Item>{{8,4,0}, {10,5,0}, {25,6,0}, {40,7,0}, {74,8,0}};
	const auto armors = std::vector<Item>{{}, {13,0,1}, {31,0,2}, {53,0,3}, {75,0,4}, {102,0,5}};
	const auto rings = get_rings();

	auto min_gold = std::numeric_limits<decltype(Item::price)>::max();

	const auto boss = Entity{103, 9, 2};
	Entity hero; // no need to initialize, the innermost loop provides the values

	// cartesian product of all the equipment vectors
	for(const auto& weapon : weapons) {

		for(const auto& armor : armors) {

			for(const auto& ring : rings) {

				const auto& ring1 = ring.front();
				const auto& ring2 = ring.back();

				hero = {
					100,
					(weapon.dmg_bonus + ring1.dmg_bonus + ring2.dmg_bonus),
					(armor.ac_bonus + ring1.ac_bonus + ring2.ac_bonus)
				};

				const auto hero_dmg = (hero.damage - boss.armor);
				const auto boss_dmg = (boss.damage - hero.armor);

				const auto hero_real_dmg = (hero_dmg > 0) ? hero_dmg : 1;
				const auto boss_real_dmg = (boss_dmg > 0) ? boss_dmg : 1;

				const auto hero_health = hero.health;
				const auto boss_health = boss.health;

				// fight sequence is always the same, so we can just calculate how many rounds they both last
				const auto hero_rounds = ((hero_health / boss_real_dmg) + ((hero_health % boss_real_dmg) > 0));
				const auto boss_rounds = ((boss_health / hero_real_dmg) + ((boss_health % hero_real_dmg) > 0));

				// since the hero always attacks first in any given round,
				// equal rounds means the hero always wins
				if(hero_rounds >= boss_rounds) {
					min_gold = std::min(min_gold, (weapon.price + armor.price + ring1.price + ring2.price));
				}
			}
		}
	}

	std::cout << min_gold << std::endl;

	return 0;
}


#pragma once

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

#include "scp_planner.hpp"
#include <mission.hpp>
#include <param.hpp>

#define _USE_MATH_DEFINES
#include <cmath>
#include "matplotlibcpp.h"
namespace plt = matplotlibcpp;

namespace SwarmPlanning {
    class SCPPlotter {
    public:
        SCPPlotter(std::shared_ptr<SCPPlanner> _SCPPlanner_obj,
                   Mission _mission,
                   Param _param)
                : SCPPlanner_obj(_SCPPlanner_obj),
                  mission(_mission),
                  param(_param) {
            N = round(SCPPlanner_obj->msgs_traj_info.data[0]);
            K = round(SCPPlanner_obj->msgs_traj_info.data[1]);
            h = SCPPlanner_obj->msgs_traj_info.data[2];
            T = 34;
            outdim = 3;

            p_curr.resize(N);

            build_mapping_mtx();
            std::vector<double> data = SCPPlanner_obj->msgs_traj_input.data;
            u = Eigen::Map<Eigen::MatrixXd>(data.data(), outdim * N * K, 1);
            p = P * u + p_start;
            v = V * u;
            a = A * u;
        }

        void plot() {
            update_traj(0.1);
            plot_distance_between_quad();
            ROS_INFO_STREAM("TrajPlotter: total length=" << trajectory_length_sum());
        }

    private:
        std::shared_ptr<SCPPlanner> SCPPlanner_obj;
        SwarmPlanning::Mission mission;
        SwarmPlanning::Param param;

        int N, K, outdim;
        double h, T;
        Eigen::MatrixXd P, V, A, J, p_start, p_goal, u, p, v, a;
        std::vector<double> t;
        std::vector<Eigen::MatrixXd> p_curr;

        void build_mapping_mtx() {
            P = Eigen::MatrixXd::Zero(outdim * N * K, outdim * N * K); // position matrix p = Pu + p_start
            V = Eigen::MatrixXd::Zero(outdim * N * K, outdim * N * K); // velocity matrix v = Vu, assume v_start = 0
            A = Eigen::MatrixXd::Identity(outdim * N * K, outdim * N * K); // accelation matrix a = Au
            J = Eigen::MatrixXd::Zero(outdim * N * K, outdim * N * K); // accelation matrix a = Au

            p_start = Eigen::MatrixXd::Zero(outdim * N * K, 1);
            p_goal = Eigen::MatrixXd::Zero(outdim * N, 1);

            for (int dim = 0; dim < outdim; dim++) {
                for (int qi = 0; qi < N; qi++) {
                    int offset = dim * N * K + qi * K;
                    for (int k = 0; k < K; k++) {
                        for (int j = 0; j < k; j++) {
                            P(offset + k, offset + j) = 0.5 * h * h * (2 * (k - j) - 1);
                            V(offset + k, offset + j) = h;
                        }
                        if (k != 0) {
                            J(offset + k, offset + k) = 1 / h;
                            J(offset + k, offset + k - 1) = -1 / h;
                        }

                        p_start(offset + k, 0) = mission.startState[qi][dim];
                    }
                    p_goal(dim * N + qi, 0) = mission.goalState[qi][dim];
                }
            }
        }

//    void plot_quad_dynamics() {
//        plt::figure_size(1500, 1000);
//
//        // Plot Quad Velocity
//        plt::subplot(3, 2, 1);
//        for(int qi = 0; qi < qn; qi++) {
//            plt::named_plot("agent" + std::to_string(qi) , t, quad_state[qi][3]);
//        }
//        plt::title("velocity -x axis");
//
//        plt::subplot(3, 2, 3);
//        for(int qi = 0; qi < qn; qi++) {
//            plt::named_plot("agent" + std::to_string(qi) , t, quad_state[qi][4]);
//        }
//        plt::title("velocity -y axis");
//
//        plt::subplot(3, 2, 5);
//        for(int qi = 0; qi < qn; qi++) {
//            plt::named_plot("agent" + std::to_string(qi) , t, quad_state[qi][5]);
//        }
//        plt::title("velocity -z axis");
//
//        // Plot Quad Acceleration
//        plt::subplot(3, 2, 2);
//        for(int qi = 0; qi < qn; qi++) {
//            plt::named_plot("agent" + std::to_string(qi) , t, quad_state[qi][6]);
//        }
//        plt::title("acceleration -x axis");
//
//        plt::subplot(3, 2, 4);
//        for(int qi = 0; qi < qn; qi++) {
//            plt::named_plot("agent" + std::to_string(qi) , t, quad_state[qi][7]);
//        }
//        plt::title("acceleration -y axis");
//
//        plt::subplot(3, 2, 6);
//        for(int qi = 0; qi < qn; qi++) {
//            plt::named_plot("agent" + std::to_string(qi) , t, quad_state[qi][8]);
//        }
//        plt::title("acceleration -z axis");
//
//        plt::legend();
//        plt::show(false);
//    }

//    void update_traj(double current_time) {
//        int k = floor(current_time / h);
//        if (k >= K - 1) {
//            return;
//        }
//
//        for (int qi = 0; qi < N; qi++) {
//            p_curr[qi] = Eigen::MatrixXd::Zero(outdim, 1);
//            Eigen::MatrixXd p_0 = Eigen::MatrixXd::Zero(outdim, 1);
//            Eigen::MatrixXd p_1 = Eigen::MatrixXd::Zero(outdim, 1);
//            for (int dim = 0; dim < outdim; dim++) {
//                p_0(dim, 0) = p(dim * N * K + qi * K + k, 0);
//                p_1(dim, 0) = p(dim * N * K + qi * K + k + 1, 0);
//            }
//            p_curr[qi] = p_0 + (current_time - k * h) / h * (p_1 - p_0);
//        }
//    }

        void update_traj(double dt) {
            t.resize(floor(T / dt));
            for (int i = 0; i < t.size(); i++) {
                t[i] = i * dt;
            }

            int k;
            Eigen::MatrixXd p_0, p_1;
            for (int qi = 0; qi < N; qi++) {
                p_curr[qi] = Eigen::MatrixXd::Zero(outdim, t.size());
                for (int i = 0; i < t.size(); i++) {
                    p_0 = Eigen::MatrixXd::Zero(outdim, 1);
                    p_1 = Eigen::MatrixXd::Zero(outdim, 1);
                    k = floor(t[i] / h);

                    for (int dim = 0; dim < outdim; dim++) {
                        p_0(dim, 0) = p(dim * N * K + qi * K + k, 0);
                        p_1(dim, 0) = p(dim * N * K + qi * K + k + 1, 0);
                    }
                    p_curr[qi].block(0, i, outdim, 1) = p_0 + (t[i] - k * h) / h * (p_1 - p_0);
                }
            }
        }

        double trajectory_length_sum() {
            double length_sum = 0;
            for (int i = 0; i < t.size() - 1; i++) {
                for (int qi = 0; qi < N; qi++) {
                    length_sum += sqrt(pow(p_curr[qi](0, i + 1) - p_curr[qi](0, i), 2) +
                                       pow(p_curr[qi](1, i + 1) - p_curr[qi](1, i), 2) +
                                       pow(p_curr[qi](2, i + 1) - p_curr[qi](2, i), 2));
                }
            }
            return length_sum;
        }

        void plot_distance_between_quad() {
            plt::figure(1);
            plt::figure_size(1500, 1000);
            std::vector<double> max_dist, min_dist;
            double max_dist_, min_dist_, dist, global_min_dist;

            max_dist.resize(t.size());
            min_dist.resize(t.size());

            for (int i = 0; i < t.size(); i++) {
                max_dist[i] = mission.quad_size[0] + mission.quad_size[0];
            }
            plt::plot(t, max_dist);

            global_min_dist = SP_INFINITY;
            for (int i = 0; i < t.size(); i++) {
                max_dist_ = 0;
                min_dist_ = SP_INFINITY;
                for (int qi = 0; qi < N; qi++) {
                    for (int qj = qi + 1; qj < N; qj++) {
                        dist = sqrt(pow(p_curr[qi](0, i) - p_curr[qj](0, i), 2) +
                                    pow(p_curr[qi](1, i) - p_curr[qj](1, i), 2) +
                                    pow(p_curr[qi](2, i) - p_curr[qj](2, i), 2));
                        if (dist > max_dist_) {
                            max_dist_ = dist;
                        }
                        if (dist < min_dist_) {
                            min_dist_ = dist;
                        }
                        if (dist < global_min_dist) {
                            global_min_dist = dist;
                        }
                    }
                }
                max_dist[i] = max_dist_;
                min_dist[i] = min_dist_;
            }
            plt::plot(t, max_dist);
            plt::plot(t, min_dist);

            ROS_INFO_STREAM("global min_dist: " << global_min_dist);

            plt::title("Ellipsoidal Distance between Quadrotor");

            plt::show(false);
        }
    };
}


#include <iostream>
#include <Eigen/Dense>
#include <vector>
#include <random>
#include <crtdbg.h>

#if _DEBUG
#define new new(_NORMAL_BLOCK, __FILE__, __LINE__)
#endif

#include "hCriterion.h"
#include "hLayer.h"
#include "hOptimizer.h"

using namespace std;

int main() {
	Linear L1(2, 4), L2(4, 1);
	ReLU R1;
	Sigmoid Sig1;
	Serial Net({ &L1, &R1, &L2, &Sig1 });
	CELoss criterion(CELoss::CELossType::sigmoid);
	SGD optim(1e-2, 1e-4);

	Eigen::MatrixXd X(4, 2);
	Eigen::MatrixXd Y(4, 1);
	X << 0, 0,
		0, 1,
		1, 0,
		1, 1;

	Y << 0, 1, 1, 0;

	for (int i = 0; i < 10000; i++) {
		auto Pred = Net.forward(X);

		auto cost = criterion.forward(Pred, Y);

		if ((i + 1) % 10 == 0) cout << "epoch " << i + 1 << " : " << cost << "\n";

		auto Diff = criterion.backward();
		Net.backward(Diff);

		optim.step(&Net);
	}

	cout << Net.forward(X) << "\n";

	cout << Net << "\n";

	return 0;
}


#define BOOST_TEST_MODULE "test_bond_length_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/forcefield/local/BondLengthInteraction.hpp>
#include <mjolnir/forcefield/local/HarmonicPotential.hpp>
#include <mjolnir/util/make_unique.hpp>

#include <random>

BOOST_AUTO_TEST_CASE(BondLength_calc_force)
{
    using traits_type      = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type        = traits_type::real_type;
    using coord_type       = traits_type::coordinate_type;
    using boundary_type    = traits_type::boundary_type;
    using system_type      = mjolnir::System<traits_type>;
    using potential_type    = mjolnir::HarmonicPotential<real_type>;
    using interaction_type = mjolnir::BondLengthInteraction<traits_type, potential_type>;

    constexpr real_type tol = 1e-8;

    auto normalize = [](const coord_type& v){return v / mjolnir::math::length(v);};

    const real_type k(100.);
    const real_type native(2.0);

    potential_type    potential(k, native);
    interaction_type interaction("none", {{ {{0,1}}, potential}});

    system_type sys(2, boundary_type{});

    sys.at(0).mass = 1.0;
    sys.at(1).mass = 1.0;
    sys.at(0).rmass = 1.0;
    sys.at(1).rmass = 1.0;

    sys.at(0).position = coord_type(0,0,0);
    sys.at(1).position = coord_type(0,0,0);
    sys.at(0).velocity = coord_type(0,0,0);
    sys.at(1).velocity = coord_type(0,0,0);
    sys.at(0).force    = coord_type(0,0,0);
    sys.at(1).force    = coord_type(0,0,0);

    sys.at(0).name  = "X";
    sys.at(1).name  = "X";
    sys.at(0).group = "NONE";
    sys.at(1).group = "NONE";

    const real_type dr = 1e-3;
    real_type dist = 1e0;
    for(int i = 0; i < 2000; ++i)
    {
        sys[0].position = coord_type(0,0,0);
        sys[1].position = coord_type(0,0,0);
        sys[0].force    = coord_type(0,0,0);
        sys[1].force    = coord_type(0,0,0);
        sys[1].position[0] = dist;

        const real_type deriv = potential.derivative(dist);
        const real_type coef  = std::abs(deriv);

        interaction.calc_force(sys);

        const real_type force_strength1 = mjolnir::math::length(sys[0].force);
        const real_type force_strength2 = mjolnir::math::length(sys[1].force);

        // direction
        if(i == 1000) // most stable point
        {
            BOOST_TEST(force_strength1 == 0.0, boost::test_tools::tolerance(tol));
            BOOST_TEST(force_strength2 == 0.0, boost::test_tools::tolerance(tol));
        }
        else if(i < 1000) // repulsive
        {
            BOOST_TEST(coef == force_strength1, boost::test_tools::tolerance(tol));
            BOOST_TEST(coef == force_strength2, boost::test_tools::tolerance(tol));

            const real_type dir1 = mjolnir::math::dot_product(
                normalize(sys[0].force), normalize(sys[0].position - sys[1].position));
            const real_type dir2 = mjolnir::math::dot_product(
                normalize(sys[1].force), normalize(sys[1].position - sys[0].position));

            BOOST_TEST(dir1 == 1.0, boost::test_tools::tolerance(tol));
            BOOST_TEST(dir2 == 1.0, boost::test_tools::tolerance(tol));
        }
        else if(i > 1000) // attractive
        {
            BOOST_TEST(coef == force_strength1, boost::test_tools::tolerance(tol));
            BOOST_TEST(coef == force_strength2, boost::test_tools::tolerance(tol));

            const real_type dir1 = mjolnir::math::dot_product(
                normalize(sys[0].force), normalize(sys[1].position - sys[0].position));
            const real_type dir2 = mjolnir::math::dot_product(
                normalize(sys[1].force), normalize(sys[0].position - sys[1].position));

            BOOST_TEST(dir1 == 1e0, boost::test_tools::tolerance(tol));
            BOOST_TEST(dir2 == 1e0, boost::test_tools::tolerance(tol));
        }
        BOOST_TEST(mjolnir::math::length(sys[0].force + sys[1].force) == 0.0,
                   boost::test_tools::tolerance(tol));

        dist += dr;
    }
}

BOOST_AUTO_TEST_CASE(BondLength_numerical_difference)
{
    using traits_type      = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type        = traits_type::real_type;
    using coord_type       = traits_type::coordinate_type;
    using boundary_type    = traits_type::boundary_type;
    using system_type      = mjolnir::System<traits_type>;
    using potential_type    = mjolnir::HarmonicPotential<real_type>;
    using interaction_type = mjolnir::BondLengthInteraction<traits_type, potential_type>;

    const real_type k(100.0);
    const real_type native(std::sqrt(3.0));

    potential_type   potential(k, native);
    interaction_type interaction("none", {{ {{0,1}}, potential}});

    std::mt19937 mt(123456789);
    std::uniform_real_distribution<real_type> uni(-1.0, 1.0);

    for(std::size_t i = 0; i < 1000; ++i)
    {
        system_type sys(2, boundary_type{});

        sys.at(0).mass  = 1.0;
        sys.at(1).mass  = 1.0;
        sys.at(0).rmass = 1.0;
        sys.at(1).rmass = 1.0;

        sys.at(0).position = coord_type( 0.0 + 0.01 * uni(mt), 0.0 + 0.01 * uni(mt), 0.0 + 0.01 * uni(mt));
        sys.at(1).position = coord_type( 1.0 + 0.01 * uni(mt), 1.0 + 0.01 * uni(mt), 1.0 + 0.01 * uni(mt));
        sys.at(0).velocity = coord_type( 0.0, 0.0, 0.0);
        sys.at(1).velocity = coord_type( 0.0, 0.0, 0.0);
        sys.at(0).force    = coord_type( 0.0, 0.0, 0.0);
        sys.at(1).force    = coord_type( 0.0, 0.0, 0.0);

        sys.at(0).name  = "X";
        sys.at(1).name  = "X";
        sys.at(0).group = "TEST";
        sys.at(1).group = "TEST";

        const auto init = sys;

        constexpr real_type tol = 1e-4;
        constexpr real_type dr  = 1e-5;
        for(std::size_t idx=0; idx<2; ++idx)
        {
            {
                // ----------------------------------------------------------------
                // reset positions
                sys = init;

                // calc U(x-dx)
                const auto E0 = interaction.calc_energy(sys);

                mjolnir::math::X(sys.position(idx)) += dr;

                // calc F(x)
                interaction.calc_force(sys);

                mjolnir::math::X(sys.position(idx)) += dr;

                // calc U(x+dx)
                const auto E1 = interaction.calc_energy(sys);

                // central difference
                const auto dE = (E1 - E0) * 0.5;

                BOOST_TEST(-dE == dr * mjolnir::math::X(sys.force(idx)),
                           boost::test_tools::tolerance(tol));
            }
            {
                // ----------------------------------------------------------------
                // reset positions
                sys = init;

                // calc U(x-dx)
                const auto E0 = interaction.calc_energy(sys);

                mjolnir::math::Y(sys.position(idx)) += dr;

                // calc F(x)
                interaction.calc_force(sys);

                mjolnir::math::Y(sys.position(idx)) += dr;

                // calc U(x+dx)
                const auto E1 = interaction.calc_energy(sys);

                // central difference
                const auto dE = (E1 - E0) * 0.5;

                BOOST_TEST(-dE == dr * mjolnir::math::Y(sys.force(idx)),
                           boost::test_tools::tolerance(tol));
            }
            {
                // ----------------------------------------------------------------
                // reset positions
                sys = init;

                // calc U(x-dx)
                const auto E0 = interaction.calc_energy(sys);

                mjolnir::math::Z(sys.position(idx)) += dr;

                // calc F(x)
                interaction.calc_force(sys);

                mjolnir::math::Z(sys.position(idx)) += dr;

                // calc U(x+dx)
                const auto E1 = interaction.calc_energy(sys);

                // central difference
                const auto dE = (E1 - E0) * 0.5;

                BOOST_TEST(-dE == dr * mjolnir::math::Z(sys.force(idx)),
                           boost::test_tools::tolerance(tol));
            }
        }
    }
}

BOOST_AUTO_TEST_CASE(BondLength_calc_force_and_energy)
{
    using traits_type      = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type        = traits_type::real_type;
    using coord_type       = traits_type::coordinate_type;
    using boundary_type    = traits_type::boundary_type;
    using system_type      = mjolnir::System<traits_type>;
    using potential_type    = mjolnir::HarmonicPotential<real_type>;
    using interaction_type = mjolnir::BondLengthInteraction<traits_type, potential_type>;

    const real_type k(100.0);
    const real_type native(std::sqrt(3.0));

    potential_type   potential(k, native);
    interaction_type interaction("none", {{ {{0,1}}, potential}});

    std::mt19937 mt(123456789);
    std::uniform_real_distribution<real_type> uni(-1.0, 1.0);

    for(std::size_t i = 0; i < 1000; ++i)
    {
        system_type sys(2, boundary_type{});

        sys.at(0).mass  = 1.0;
        sys.at(1).mass  = 1.0;
        sys.at(0).rmass = 1.0;
        sys.at(1).rmass = 1.0;

        sys.at(0).position = coord_type( 0.0 + 0.01 * uni(mt), 0.0 + 0.01 * uni(mt), 0.0 + 0.01 * uni(mt));
        sys.at(1).position = coord_type( 1.0 + 0.01 * uni(mt), 1.0 + 0.01 * uni(mt), 1.0 + 0.01 * uni(mt));
        sys.at(0).velocity = coord_type( 0.0, 0.0, 0.0);
        sys.at(1).velocity = coord_type( 0.0, 0.0, 0.0);
        sys.at(0).force    = coord_type( 0.0, 0.0, 0.0);
        sys.at(1).force    = coord_type( 0.0, 0.0, 0.0);

        sys.at(0).name  = "X";
        sys.at(1).name  = "X";
        sys.at(0).group = "TEST";
        sys.at(1).group = "TEST";

        constexpr real_type tol = 1e-4;
        auto ref_sys = sys;

        const auto energy = interaction.calc_force_and_energy(sys);
        const auto ref_energy = interaction.calc_energy(ref_sys);
        interaction.calc_force(ref_sys);
        BOOST_TEST(ref_energy == energy, boost::test_tools::tolerance(tol));

        for(std::size_t idx=0; idx<sys.size(); ++idx)
        {
            BOOST_TEST(mjolnir::math::X(sys.force(idx)) == mjolnir::math::X(ref_sys.force(idx)), boost::test_tools::tolerance(tol));
            BOOST_TEST(mjolnir::math::Y(sys.force(idx)) == mjolnir::math::Y(ref_sys.force(idx)), boost::test_tools::tolerance(tol));
            BOOST_TEST(mjolnir::math::Z(sys.force(idx)) == mjolnir::math::Z(ref_sys.force(idx)), boost::test_tools::tolerance(tol));
        }
    }
}


/**
 * math lib test
 * @author Tobias Weber <tweber@ill.fr>
 * @date 20-aug-20
 * @license GPLv3, see 'LICENSE' file
 *
 * ----------------------------------------------------------------------------
 * tlibs
 * Copyright (C) 2017-2021  Tobias WEBER (Institut Laue-Langevin (ILL),
 *                          Grenoble, France).
 * Copyright (C) 2015-2017  Tobias WEBER (Technische Universitaet Muenchen
 *                          (TUM), Garching, Germany).
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, version 3 of the License.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 * ----------------------------------------------------------------------------
 */

#define BOOST_TEST_MODULE Stat1
#include <boost/test/included/unit_test.hpp>
namespace test = boost::unit_test;
namespace testtools = boost::test_tools;

#include <iostream>
#include <vector>
#include <boost/math/quaternion.hpp>

#include "libs/maths.h"
using namespace tl2_ops;

using t_types = std::tuple<long double, double, float>;
BOOST_AUTO_TEST_CASE_TEMPLATE(test_equals, t_real, t_types)
{
	using t_vec = tl2::vec<t_real, std::vector>;
	using t_mat = tl2::mat<t_real, std::vector>;

	std::vector<t_vec> vecs = {{
		tl2::create<t_vec>({-1, 60}),
		tl2::create<t_vec>({20, 5}),
		tl2::create<t_vec>({-3, 40}),
		tl2::create<t_vec>({40, 3}),
		tl2::create<t_vec>({-5, 20}),
		tl2::create<t_vec>({60, 1}),
	}};

	auto [cov, cor] = tl2::covariance<t_mat, t_vec>(vecs);
	std::cout << "cov = " << cov << std::endl;
	std::cout << "cor = " << cor << std::endl;

	//BOOST_TEST(tl2::equals(vec, vec2, 1e-5));
}


//          Copyright Christopher Kormanyos 2021.
// 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)

#include <boost/math/bindings/e_float.hpp>
#include <boost/math/concepts/real_type_concept.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/multiprecision/number.hpp>

namespace local
{
  using big_float_type = boost::multiprecision::number<boost::math::ef::e_float,
                                                       boost::multiprecision::et_off>;
}

template <class T>
void test_extra(T)
{
   T t = 1;
   t   = abs(t);
   t   = abs(t * t);

   t = fabs(t);
   t = fabs(t * t);

   t = sqrt(t);
   t = sqrt(t * t);

   t = floor(t);
   t = floor(t * t);

   t = ceil(t);
   t = ceil(t * t);

   t = trunc(t);
   t = trunc(t * t);

   t = round(t);
   t = round(t * t);

   t = exp(t);
   t = exp(t * t);

   t = log(t);
   t = log(t * t);

   t = log10(t);
   t = log10(t * t);

   t = cos(t);
   t = cos(t * t);

   t = sin(t);
   t = sin(t * t);

   t = tan(t);
   t = tan(t * t);

   t = asin(t);
   t = asin(t * t);

   t = atan(t);
   t = atan(t * t);

   t = acos(t);
   t = acos(t * t);

   t = cosh(t);
   t = cosh(t * t);

   t = sinh(t);
   t = sinh(t * t);

   t = tanh(t);
   t = tanh(t * t);

   double dval = 2;
   t           = pow(t, t);
   t           = pow(t, t * t);
   t           = pow(t, dval);
   t           = pow(t * t, t);
   t           = pow(t * t, t * t);
   t           = pow(t * t, dval);
   t           = pow(dval, t);
   t           = pow(dval, t * t);

   t = atan2(t, t);
   t = atan2(t, t * t);
   t = atan2(t, dval);
   t = atan2(t * t, t);
   t = atan2(t * t, t * t);
   t = atan2(t * t, dval);
   t = atan2(dval, t);
   t = atan2(dval, t * t);

   t = fmod(t, t);
   t = fmod(t, t * t);
   t = fmod(t, dval);
   t = fmod(t * t, t);
   t = fmod(t * t, t * t);
   t = fmod(t * t, dval);
   t = fmod(dval, t);
   t = fmod(dval, t * t);

   typedef typename T::backend_type             backend_type;
   typedef typename backend_type::exponent_type exp_type;
   exp_type                                     e = 0;
   int                                          i = 0;

   t = ldexp(t, i);
   t = ldexp(t * t, i);
   t = ldexp(t, e);
   t = ldexp(t * t, e);

   t = frexp(t, &i);
   t = frexp(t * t, &i);
   t = frexp(t, &e);
   t = frexp(t * t, &e);

   t = scalbn(t, i);
   t = scalbn(t * t, i);
   t = scalbn(t, e);
   t = scalbn(t * t, e);

   t = logb(t);
   t = logb(t * t);
   e = ilogb(t);
   e = ilogb(t * t);
}

void foo()
{
  test_extra(local::big_float_type());
}

bool test_boost_real_concept()
{
  foo();

  BOOST_CONCEPT_ASSERT((boost::math::concepts::RealTypeConcept<local::big_float_type>));

  return true;
}


//
//  Copyright Toon Knapen, Karl Meerbergen
//
// 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)
//

#include "../../blas/test/random.hpp"

#include <boost/numeric/bindings/lapack/gees.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <iostream>
#include <limits>

namespace ublas = boost::numeric::ublas;
namespace lapack = boost::numeric::bindings::lapack;

// Randomize a matrix
template <typename M>
void randomize(M& m) {
   typedef typename M::size_type  size_type ;
   typedef typename M::value_type value_type ;

   size_type size1 = m.size1() ;
   size_type size2 = m.size2() ;

   for (size_type i=0; i<size2; ++i) {
      for (size_type j=0; j<size1; ++j) {
         m(j,i) = random_value< value_type >() ;
      }
   }
} // randomize()

template <typename T, typename W>
int do_memory_type(int n, W workspace) {
   typedef typename boost::numeric::bindings::traits::type_traits<T>::real_type real_type ;
   typedef std::complex< real_type >                                            complex_type ;

   typedef ublas::matrix<T, ublas::column_major> matrix_type ;
   typedef ublas::vector<complex_type>           vector_type ;
   double safety_factor (1.5);

   // Set matrix
   matrix_type a( n, n );
   matrix_type z( n, n );
   vector_type e1( n );
   vector_type e2( n );

   randomize( a );
   matrix_type a2( a );

   // Compute Schur decomposition.
   lapack::gees( a, e1, z, workspace ) ;

   // Check Schur factorization
   if (norm_frobenius( prod( a2, z ) - prod( z, a ) )
           >= safety_factor*10.0* norm_frobenius( a2 ) * std::numeric_limits< real_type >::epsilon() ) return 255 ;

   lapack::gees( a2, e2, workspace ) ;
   if (norm_2( e1 - e2 ) > safety_factor*norm_2( e1 ) * std::numeric_limits< real_type >::epsilon()) return 255 ;

   if (norm_frobenius( a2 - a )
           >= safety_factor*10.0* norm_frobenius( a2 ) * std::numeric_limits< real_type >::epsilon() ) return 255 ;

   return 0 ;
} // do_value_type()

template <typename T>
struct Workspace {
   typedef ublas::vector<T>                         array_type ;
   typedef lapack::detail::workspace1< array_type > type ;

   Workspace(size_t n)
   : work_( 3*n )
   {}

   type operator() () {
      return lapack::workspace(work_) ;
   }

   array_type work_ ;
};

template <typename T>
struct Workspace< std::complex<T> > {
   typedef ublas::vector<T>                                                 real_array_type ;
   typedef ublas::vector< std::complex<T> >                                 complex_array_type ;
   typedef lapack::detail::workspace2< complex_array_type,real_array_type > type ;

   Workspace(size_t n)
   : work_( 2*n )
   , rwork_( n )
   {}

   type operator() () {
      return lapack::workspace(work_, rwork_) ;
   }

   complex_array_type work_ ;
   real_array_type    rwork_ ;
};

template <typename T>
int do_value_type() {
   const int n = 8 ;

   if (do_memory_type<T,lapack::optimal_workspace>( n, lapack::optimal_workspace() ) ) return 255 ;
   if (do_memory_type<T,lapack::minimal_workspace>( n, lapack::minimal_workspace() ) ) return 255 ;

   Workspace<T> work( n );
   do_memory_type<T,typename Workspace<T>::type >( n, work() );
   return 0;
} // do_value_type()

int main() {
   // Run tests for different value_types
   if (do_value_type<float>()) return 255;
   if (do_value_type<double>()) return 255;
   if (do_value_type< std::complex<float> >()) return 255;
   if (do_value_type< std::complex<double> >()) return 255;

   std::cout << "Regression test succeeded\n" ;
   return 0;
}


/**
 *
 * Core Libraries.
 * Sophus Interpolations & other missing bits.
 *
 * Copyright (c) Robert Lukierski 2016. All rights reserved.
 * Author: Robert Lukierski.
 *
 */

#ifndef VISIONCORE_SOPHUS_MISSINGBITS_HPP
#define VISIONCORE_SOPHUS_MISSINGBITS_HPP

#include <VisionCore/Platform.hpp>

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

#include <sophus/so2.hpp>
#include <sophus/so3.hpp>
#include <sophus/se2.hpp>
#include <sophus/se3.hpp>
#include <sophus/rxso3.hpp>
#include <sophus/sim3.hpp>

/**
 * NOTE: Don't use <sophus/interpolate.hpp> for SE groups. It's wrong, see page 41:
 * https://www.cvl.isy.liu.se/education/graduate/geometry-for-computer-vision-2014/geometry2014/lecture7.pdf
 */

namespace Sophus
{

// SO2
template<typename T>
EIGEN_DEVICE_FUNC static inline Sophus::SO2<T> interpolateLinear(const Sophus::SO2<T>& t0, const Sophus::SO2<T>& t1, T ratio)
{
    return Sophus::SO2<T>(t0 * Sophus::SO2<T>::exp(ratio * ( t0.inverse() * t1 ).log() ));
}

// SE2
template<typename T>
EIGEN_DEVICE_FUNC static inline Sophus::SE2<T> interpolateLinear(const Sophus::SE2<T>& t0, const Sophus::SE2<T>& t1, T ratio)
{
    return Sophus::SE2<T>(t0.so2() * Sophus::SO2<T>::exp(ratio * ( t0.so2().inverse() * t1.so2() ).log() ),
                          t0.translation() + ratio * (t1.translation() - t0.translation()));
}

// SO3
template<typename T>
EIGEN_DEVICE_FUNC static inline Sophus::SO3<T> interpolateLinear(const Sophus::SO3<T>& t0, const Sophus::SO3<T>& t1, T ratio)
{
    return Sophus::SO3<T>(t0 * Sophus::SO3<T>::exp(ratio * ( t0.inverse() * t1 ).log() ));
}

// SE3
template<typename T>
EIGEN_DEVICE_FUNC static inline Sophus::SE3<T> interpolateLinear(const Sophus::SE3<T>& t0, const Sophus::SE3<T>& t1, T ratio)
{
    return Sophus::SE3<T>(t0.so3() * Sophus::SO3<T>::exp(ratio * ( t0.so3().inverse() * t1.so3() ).log() ),
                          t0.translation() + ratio * (t1.translation() - t0.translation()));
}

template<typename T>
struct B4SplineGenerator
{

    EIGEN_DEVICE_FUNC static inline void get(const T& u, T& b1, T& b2, T& b3)
    {
        const T u2 = u*u;
        const T u3 = u*u*u;

        b1 = (u3 - T(3.0) * u2 + T(3.0) * u + T(5.0)) / T(6.0);
        b2 = (T(-2.0) * u3 + T(3.0) * u2 + T(3.0) * u + T(1.0)) / T(6.0);
        b3 = u3 / T(6.0);
    }

#if 0
    EIGEN_DEVICE_FUNC static inline void get(const T& u, T& b1, T& b2, T& b3)
    {
        Eigen::Matrix<T,4,1> v(T(1.0),u,u*u,u*u*u);
        Eigen::Matrix<T,4,4> m;
        m << T(6.0) , T(0.0) , T(0.0) , T(0.0),
             T(5.0) , T(3.0) , T(-3.0), T(1.0),
             T(1.0) , T(3.0) , T(3.0), T(-2.0),
             T(0.0) , T(0.0) , T(0.0) , T(1.0);

        const Eigen::Matrix<T,4,1> uvec = (m * v) / T(6.0);
        b1 = uvec(1);
        b2 = uvec(2);
        b3 = uvec(3);
    }
#endif
};

// B4-Spline SO2
template<typename T>
EIGEN_DEVICE_FUNC static inline Sophus::SO2<T> interpolateB4Spline(const Sophus::SO2<T>& tm1,
                                                                   const Sophus::SO2<T>& t0,
                                                                   const Sophus::SO2<T>& t1,
                                                                   const Sophus::SO2<T>& t2,
                                                                   const T& u)
{
    T b1, b2, b3;

    B4SplineGenerator<T>::get(u,b1,b2,b3);

    return tm1 * Sophus::SO2<T>::exp(b1 * ( tm1.inverse() * t0 ).log() +
                                          b2 * ( t0.inverse()  * t1 ).log() +
                                          b3 * ( t1.inverse()  * t2 ).log() );
}

// B4-Spline SE2
template<typename T>
EIGEN_DEVICE_FUNC static inline Sophus::SE2<T> interpolateB4Spline(const Sophus::SE2<T>& tm1,
                                                                   const Sophus::SE2<T>& t0,
                                                                   const Sophus::SE2<T>& t1,
                                                                   const Sophus::SE2<T>& t2,
                                                                   const T& u)
{
    T b1, b2, b3;

    B4SplineGenerator<T>::get(u,b1,b2,b3);

    const Sophus::SO2<T> rot_final = tm1.so2() * Sophus::SO2<T>::exp(b1 * ( tm1.so2().inverse() * t0.so2() ).log() +
                                                                               b2 * ( t0.so2().inverse()  * t1.so2() ).log() +
                                                                               b3 * ( t1.so2().inverse()  * t2.so2() ).log() );

    const typename Sophus::SE2<T>::Point tr_final = tm1.translation() + (b1 * (t0.translation() - tm1.translation())) +
                                                                             (b2 * (t1.translation() - t0.translation())) +
                                                                             (b3 * (t2.translation() - t1.translation()));

    return Sophus::SE2<T>(rot_final, tr_final);
}

// B4-Spline SO3
template<typename T>
EIGEN_DEVICE_FUNC static inline Sophus::SO3<T> interpolateB4Spline(const Sophus::SO3<T>& tm1,
                                                                   const Sophus::SO3<T>& t0,
                                                                   const Sophus::SO3<T>& t1,
                                                                   const Sophus::SO3<T>& t2,
                                                                   const T& u)
{
    T b1, b2, b3;

    B4SplineGenerator<T>::get(u,b1,b2,b3);

    return tm1 * Sophus::SO3<T>::exp(b1 * ( tm1.inverse() * t0 ).log() +
                                          b2 * ( t0.inverse()  * t1 ).log() +
                                          b3 * ( t1.inverse()  * t2 ).log() );
}

// B4-Spline SE3
template<typename T>
EIGEN_DEVICE_FUNC static inline Sophus::SE3<T> interpolateB4Spline(const Sophus::SE3<T>& tm1,
                                                                   const Sophus::SE3<T>& t0,
                                                                   const Sophus::SE3<T>& t1,
                                                                   const Sophus::SE3<T>& t2,
                                                                   const T& u)
{
    T b1, b2, b3;

    B4SplineGenerator<T>::get(u,b1,b2,b3);

    const Sophus::SO3<T> rot_final = tm1.so3() * Sophus::SO3<T>::exp(b1 * ( tm1.so3().inverse() * t0.so3() ).log() +
                                                                               b2 * ( t0.so3().inverse()  * t1.so3() ).log() +
                                                                               b3 * ( t1.so3().inverse()  * t2.so3() ).log() );

    const typename Sophus::SE3<T>::Point tr_final = tm1.translation() + (b1 * (t0.translation() - tm1.translation())) +
                                                                             (b2 * (t1.translation() - t0.translation())) +
                                                                             (b3 * (t2.translation() - t1.translation()));

    return Sophus::SE3<T>(rot_final, tr_final);
}

// let's put ostreams here
template<typename Derived>
inline std::ostream& operator<<(std::ostream& os, const SO2Base<Derived>& p)
{
    os << "(" << p.log() << ")";
    return os;
}

template<typename Derived>
inline std::ostream& operator<<(std::ostream& os, const SO3Base<Derived>& p)
{
    os << "(" << p.unit_quaternion().x() << "," << p.unit_quaternion().y() << ","
       << p.unit_quaternion().z() << "|" << p.unit_quaternion().w() << ")";
    return os;
}

template<typename Derived>
inline std::ostream& operator<<(std::ostream& os, const SE2Base<Derived>& p)
{
    os << "[t = " << p.translation()(0) << "," << p.translation()(1) << " | r = " << p.so2() << ")";
    return os;
}

template<typename Derived>
inline std::ostream& operator<<(std::ostream& os, const SE3Base<Derived>& p)
{
    os << "[t = " << p.translation()(0) << "," << p.translation()(1) << "," << p.translation()(2) << " | r = " << p.so3() << ")";
    return os;
}

#ifdef VISIONCORE_ENABLE_CEREAL

/**
 * SO2
 */
template<typename Archive, typename Derived>
void load(Archive & archive, SO2Base<Derived>& m, std::uint32_t const version)
{
    typename SO2Base<Derived>::Point cplx;
    archive(cplx);
    m.setComplex(cplx);
}

template<typename Archive, typename Derived>
void save(Archive & archive, SO2Base<Derived> const & m, std::uint32_t const version)
{
    archive(m.unit_complex());
}

/**
 * SO3
 */
template<typename Archive, typename Derived>
void load(Archive & archive, SO3Base<Derived>& m, std::uint32_t const version)
{
    Eigen::Quaternion<typename SO3Base<Derived>::Scalar> quaternion;
    archive(cereal::make_nvp("Quaternion", quaternion));
    m.setQuaternion(quaternion);
}

template<typename Archive, typename Derived>
void save(Archive & archive, SO3Base<Derived> const & m, std::uint32_t const version)
{
    archive(cereal::make_nvp("Quaternion", m.unit_quaternion()));
}

/**
 * SE2
 */
template<typename Archive, typename Derived>
void load(Archive & archive, SE2Base<Derived>& m, std::uint32_t const version)
{
    archive(cereal::make_nvp("Translation", m.translation()));
    archive(cereal::make_nvp("Rotation", m.so2()));
}

template<typename Archive, typename Derived>
void save(Archive & archive, SE2Base<Derived> const & m, std::uint32_t const version)
{
    archive(cereal::make_nvp("Translation", m.translation()));
    archive(cereal::make_nvp("Rotation", m.so2()));
}

/**
 * SE3
 */
template<typename Archive, typename Derived>
void load(Archive & archive, SE3Base<Derived>& m, std::uint32_t const version)
{
    archive(cereal::make_nvp("Translation", m.translation()));
    archive(cereal::make_nvp("Rotation", m.so3()));
}

template<typename Archive, typename Derived>
void save(Archive & archive, SE3Base<Derived> const & m, std::uint32_t const version)
{
    archive(cereal::make_nvp("Translation", m.translation()));
    archive(cereal::make_nvp("Rotation", m.so3()));
}

/**
 * RxSO3
 */
template<typename Archive, typename Derived>
void load(Archive & archive, RxSO3Base<Derived>& m, std::uint32_t const version)
{
    archive(cereal::make_nvp("Quaternion", m.quaternion()));
}

template<typename Archive, typename Derived>
void save(Archive & archive, RxSO3Base<Derived> const & m, std::uint32_t const version)
{
    archive(cereal::make_nvp("Quaternion", m.quaternion()));
}

/**
 * Sim3
 */
template<typename Archive, typename Derived>
void load(Archive & archive, Sim3Base<Derived>& m, std::uint32_t const version)
{
    archive(cereal::make_nvp("Translation", m.translation()));
    archive(cereal::make_nvp("Rotation", m.rxso3()));
}

template<typename Archive, typename Derived>
void save(Archive & archive, Sim3Base<Derived> const & m, std::uint32_t const version)
{
    archive(cereal::make_nvp("Translation", m.translation()));
    archive(cereal::make_nvp("Rotation", m.rxso3()));
}

#endif // VISIONCORE_ENABLE_CEREAL

/**
 * Stuff that Hauke removed.
 *
 * \param alpha1 rotation around x-axis
 * \param alpha2 rotation around y-axis
 * \param alpha3 rotation around z-axis
 *
 * Since rotations in 3D do not commute, the order of the individual rotations
 * matter. Here, the following convention is used. We calculate a SO3 member
 * corresponding to the rotation matrix \f$ R \f$ such
 * that \f$ R=\exp\left(\begin{array}{c}\alpha_1\\ 0\\ 0\end{array}\right)
 *    \cdot   \exp\left(\begin{array}{c}0\\ \alpha_2\\ 0\end{array}\right)
 *    \cdot   \exp\left(\begin{array}{c}0\\ 0\\ \alpha_3\end{array}\right)\f$.
 */
template<typename T>
EIGEN_DEVICE_FUNC static inline SO3<T> fromEulerAngles(const T alpha1, const T alpha2, const T alpha3)
{
    typedef typename SO3<T>::Tangent Tangent;
    const static T zero = static_cast<T>(0);

    return SO3<T>((SO3<T>::exp(Tangent(alpha1, zero, zero)) *
                   SO3<T>::exp(Tangent(zero, alpha2, zero)) *
                   SO3<T>::exp(Tangent(zero, zero, alpha3)))
                 );
}

}

#endif // VISIONCORE_SOPHUS_MISSINGBITS_HPP


//
// Dynamics of a simple car.
// State space is 4-d [pos_x, pos_y, vel, angle]
// Control is 2-d [acceleration, delta-angle]
//

# pragma once

#include <ilqr/ilqr_taylor_expansions.hh> // Definition of dynamics function.

#include <Eigen/Dense>

#include <cstdint>

namespace simulators
{
namespace simplecar
{

enum State
{
    POS_X = 0,
    POS_Y,
    VEL,
    ANG
};

enum Control
{
    CNTRL_ACC = 0,
    CNTRL_DELTA_ANG,
};

// State is [position, velocity, Steering Angle]
constexpr int STATE_DIM = 5;

// Control is [Acceleration, Delta-Steering Angle]
constexpr int CONTROL_DIM = 2;

ilqr::DynamicsFunc make_discrete_dynamics_func(const double dt);

Eigen::VectorXd discrete_dynamics(const Eigen::VectorXd& xt,
                                  const Eigen::VectorXd& ut,
                                  const double dt);
} // namespace pendulum
} // namespace simulators


/*
 * Copyright 2018 James Dyer
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 *
 * This is being developed for the TANGO Project: http://tango-project.eu
 */

#include <sstream>
#include <NTL/RR.h>
#include <jsoncpp/json/json.h>
#include "GACDEncrypter.h"

GACDEncrypter::GACDEncrypter(int lambda) {
	k = NTL::RandomPrime_ZZ(lambda, 20);
	NTL::RR kreal = to_RR(k);
	minr = RoundToZZ(sqrt(sqrt(power(kreal,3)))); // minr = k^{3/4}
	maxr = k-minr;
}

NTL::ZZ GACDEncrypter::encrypt(NTL::ZZ& plaintext){
	NTL::ZZ r = rng.nextBigInteger(minr, maxr);
	return plaintext*k+r;
}

std::string GACDEncrypter::writeSecretsToJSON(){
	Json::Value root;
	std::stringstream keyStr;
	keyStr << k;
	root["key"] = keyStr.str();
	Json::FastWriter writer;
	return writer.write(root);
}

int GACDEncrypter::getMinimumKeyLength(int bits){
	return ceil(8.0*bits/3);
}


// This file is part of PoseEstimation.
// Copyright (c) 2021, Eijiro Shibusawa <phd_kimberlite@yahoo.co.jp>
// 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.

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

#ifndef SEVEN_POINT_HPP_
#define SEVEN_POINT_HPP_

#include <Eigen/Dense>

#include <vector>

class SevenPointTest;

namespace SevenPoint
{

template <typename FloatType>
class SevenPoint
{
	friend ::SevenPointTest;
private:
	typedef Eigen::Matrix<std::complex<FloatType>, 3, 1> CMatrix_3x1;
	typedef Eigen::Matrix<FloatType, 3, 3> Matrix_3x3;
	typedef Eigen::Matrix<FloatType, 7, 9> Matrix_7x9;
	typedef Eigen::Matrix<FloatType, 9, 9> Matrix_9x9;
	typedef Eigen::Matrix<FloatType, 9, Eigen::Dynamic> Matrix_9xD;

public:
	SevenPoint() :
		m_F1(m_F)
		, m_F2(m_F1 + 9)
	{
	}

	virtual ~SevenPoint()
	{
	}

	bool getMatrix(const FloatType *pts1, const FloatType *pts2, int &nSolutions, std::vector<FloatType> &Fs)
	{
		getConstraints(pts1, pts2);
		bool ret = getRoot();
		if (!ret)
		{
			return false;
		}

		nSolutions = m_mFvec.cols();
		Fs.resize(9 * nSolutions);
		for (int i = 0; i < nSolutions; i++)
		{
			for (int j = 0; j < 9; j++)
			{
				Fs[9 * i + j] = m_mFvec(j, i);
			}
		}

		return true;
	}

private:
	// 1st order polynomial multiplication
	// (ax + b) * (cx + d) = (acx^2 + (ad + bc)x + bd)
	// p1 = [x 1]
	// p2 = [x 1]
	// p3 = [x^2 x 1]
	static inline void polynomial_multiplication1(const FloatType *p1, const FloatType *p2, FloatType *p3)
	{
		p3[0] = (p1[0])*(p2[0]);					// x^2
		p3[1] = (p1[0])*(p2[1]) + (p1[1])*(p2[0]);	// x
		p3[2] = (p1[1])*(p2[1]);					// 1
	}

	// 2nd order polynomial multiplication
	// (ax + b) * (cx^2 + dx + e) = (acx^3 + (ad + bc)x^2 + ... + be)
	// p1 = [x 1]
	// p3 = [x^2 x 1]
	// p3 = [x^3 x^2 x 1]
	static inline void polynomial_multiplication2(const FloatType *p1, const FloatType *p2, FloatType *p3)
	{
		p3[0] = (p1[0])*(p2[0]);					// x^3
		p3[1] = (p1[0])*(p2[1]) + (p1[1])*(p2[0]);	// x^2
		p3[2] = (p1[0])*(p2[2]) + (p1[1])*(p2[1]);	// x
		p3[3] = (p1[1])*(p2[2]);					// 1
	}

	static inline void getLinearConstraints(const FloatType *pts1, const FloatType *pts2, FloatType *F1, FloatType *F2)
	{
		Matrix_7x9 mC;
		for (unsigned int i = 0; i < 7; i++){
			const FloatType *p1 = &(pts1[2*i]), *p2 = &(pts2[2*i]);
			mC(i, 0) = (p2[0])*(p1[0]);
			mC(i, 1) = (p2[0])*(p1[1]);
			mC(i, 2) = (p2[0]);
			mC(i, 3) = (p2[1])*(p1[0]);
			mC(i, 4) = (p2[1])*(p1[1]);
			mC(i, 5) = (p2[1]);
			mC(i, 6) = (p1[0]);
			mC(i, 7) = (p1[1]);
			mC(i, 8) = 1;
		}

		Eigen::JacobiSVD<Matrix_7x9> svd(mC, Eigen::ComputeFullV);
		Matrix_9x9 mV = svd.matrixV();
		for (int i = 0; i < 9; i++)
		{
			F1[i] = mV(i, 7);
			F2[i] = mV(i, 8);
		}
	}
	static inline void getDeterminantConstraints(const FloatType *F1, const FloatType *F2, FloatType *C)
	{
		C[0] = C[1] = C[2] = C[3] = 0;
		FloatType p00[] = {F1[0], F2[0]}; // (0, 0)
		FloatType p01[] = {F1[1], F2[1]}; // (0, 1)
		FloatType p02[] = {F1[2], F2[2]}; // (0, 2)
		FloatType p10[] = {F1[3], F2[3]}; // (1, 0)
		FloatType p11[] = {F1[4], F2[4]}; // (1, 1)
		FloatType p12[] = {F1[5], F2[5]}; // (1, 2)
		FloatType p20[] = {F1[6], F2[6]}; // (2, 0)
		FloatType p21[] = {F1[7], F2[7]}; // (2, 1)
		FloatType p22[] = {F1[8], F2[8]}; // (2, 2)

		FloatType c1[3], c2[3], c3[4];
		polynomial_multiplication1(p11, p22, c1);
		polynomial_multiplication1(p12, p21, c2);
		c1[0] -= c2[0]; c1[1] -= c2[1]; c1[2] -= c2[2];
		polynomial_multiplication2(p00, c1, c3);
		C[0] += c3[0]; C[1] += c3[1]; C[2] += c3[2]; C[3] += c3[3];

		polynomial_multiplication1(p01, p22, c1);
		polynomial_multiplication1(p02, p21, c2);
		c1[0] -= c2[0]; c1[1] -= c2[1]; c1[2] -= c2[2];
		polynomial_multiplication2(p10, c1, c3);
		C[0] -= c3[0]; C[1] -= c3[1]; C[2] -= c3[2]; C[3] -= c3[3];

		polynomial_multiplication1(p01, p12, c1);
		polynomial_multiplication1(p02, p11, c2);
		c1[0] -= c2[0]; c1[1] -= c2[1]; c1[2] -= c2[2];
		polynomial_multiplication2(p20, c1, c3);
		C[0] += c3[0]; C[1] += c3[1]; C[2] += c3[2]; C[3] += c3[3];
	}

protected:
	static const size_t m_minSet; // minimum data number for parameter esimation
	static const bool m_acceptArbitraryNSet; // if true, this model accepts arbitrary data number for parameter esimation

private:
	void getConstraints(const FloatType *pts1, const FloatType *pts2)
	{
		getLinearConstraints(pts1, pts2, m_F1, m_F2);
		getDeterminantConstraints(m_F1, m_F2, m_C);
	}

	bool getRoot()
	{
		// construct the companion matrix
		Matrix_3x3 mC;
		mC.setZero();
		mC(1, 0) = mC(2, 1) = 1;
		mC(0, 2) = -m_C[3] / m_C[0];
		mC(1, 2) = -m_C[2] / m_C[0];
		mC(2, 2) = -m_C[1] / m_C[0];
		Eigen::EigenSolver<Matrix_3x3> eig(mC, false);
		CMatrix_3x1 mSols = eig.eigenvalues();

		std::vector<int> idx(0);
		for(int j = 0; j < 3; j++)
		{
			if(mSols[j].imag() == 0)
			{
				idx.push_back(j);
			}
		}

		if (idx.empty())
		{
			return false; // 3rd oder polynomial has at least one real root!
		}

		m_mFvec = Matrix_9xD(9, idx.size());
		for (size_t i = 0; i < idx.size(); i++)
		{
			for (int j = 0; j < 9; j++)
			{
				m_mFvec(j, i) = (mSols[i].real()) * (m_F1[j]) + (m_F2[j]);
			}
		}
		return true;
	}

	FloatType m_F[9*2]; // orthogonal basis of null space of linear constarints
	FloatType m_C[4]; 	// deteminant constraints
	FloatType *m_F1, *m_F2; // pointer to F
	Matrix_9xD m_mFvec; // Essential matrix solutions
};
template <typename FloatType>
const size_t SevenPoint<FloatType>::m_minSet = 7;
template <typename FloatType>
const bool SevenPoint<FloatType>::m_acceptArbitraryNSet = false;

}

#endif // SEVEN_POINT_HPP_


#ifndef MLT_DEFS_HPP
#define MLT_DEFS_HPP

#include <Eigen/Core>

#ifdef MLT_VERBOSE
#define MLT_LOG(log) cout << log;
#else
#define MLT_LOG(log)
#endif

#define MLT_LOG_LINE(log) MLT_LOG(log << endl);

namespace mlt {
	using namespace std;
	using namespace Eigen;

	using VectorXdRef = const Ref<const VectorXd>&;
	using MatrixXdRef = const Ref<const MatrixXd>&;

	using VectorXiRef = const Ref<const VectorXi>&;
	using MatrixXiRef = const Ref<const MatrixXi>&;

	using Features = MatrixXdRef;
}
#endif


//
// harmonic.cpp
//
// Copyright (c) 1996-1998 Takashi Kanai
//
// This software is released under the MIT License.
// http://opensource.org/licenses/mit-license.php
//

#include "StdAfx.h"

#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif

#include <math.h>
#include "smd.h"
#include "harmonic.h"
#include "ppdedge.h"
#include "hgppd.h"
// #include "linbcg.h"

#define KAPPA 1.0

#include <vector>
#include <Eigen/Sparse>
#include <Eigen/OrderingMethods>

/*******************************************************************************
  STEP 2: harmonic maps (Related functions are in "harmonic.c".)
*******************************************************************************/

void harmonic_initialize_vector_2( HGfc *hgfc, Splp *lp,
                                   Eigen::VectorXd& bx, Eigen::VectorXd& by )
{
  int    i;
  HGvt   *vt;
  Vec2d  *uvbprm;
  Sple   *le, *tle, *sle;
  Splv   *lv, *tlv, *slv, *elv;
  double sum, esum, val;
  Vec2d  sub;

  /* initialize vector */
  for ( vt = hgfc->shgvt; vt != (HGvt *) NULL; vt = vt->nxt )
    {
      bx[vt->sid] = 0.0;
      by[vt->sid] = 0.0;
    //   emat->bx[vt->sid] = emat->xx[vt->sid] = 0.0;
    // emat->by[vt->sid] = emat->yy[vt->sid] = 0.0;
    }

  uvbprm = calc_uvbprm( hgfc->hgcn );

  /* calculate boundary values */

  lv = lp->splv;
  le = lp->sple;

  i = 0;
  while ( i < hgfc->hgcn ) {

    slv = lv;
    sle = le;
    sum = 0.0;

    while ( 1 ) {
      sum += le->ed->length;
      lv = lv->nxt;
      le = le->nxt;
      if ( lv == (Splv *) NULL ) {
        elv = lp->splv;
        break;
      }
      if ( lv->vt->sp_type == SP_VERTEX_HVERTEX ) {
        elv = lv;
        break;
      }
    }

    esum = 0.0;
    tlv = slv;
    tle = sle;
    sub.x = uvbprm[i+1].x - uvbprm[i].x;
    sub.y = uvbprm[i+1].y - uvbprm[i].y;
    while ( 1 ) {

      val = esum / sum;
      bx[ tlv->hgvt->sid ] = uvbprm[i].x + sub.x * val;
      by[ tlv->hgvt->sid ] = uvbprm[i].y + sub.y * val;
      // emat->bx[ tlv->hgvt->sid ] = uvbprm[i].x + sub.x * val;
      // emat->by[ tlv->hgvt->sid ] = uvbprm[i].y + sub.y * val;

      esum += tle->ed->length;

      tlv = tlv->nxt;
      tle = tle->nxt;

      if ( (tlv == elv) || (tlv == (Splv *) NULL) ) break;
    }

    ++i;
  }

  free(uvbprm);
}

// harmonic mapping functions with Eigen

void hgfcharmonic_Eigen( HGfc *hgfc, Splp *lp )
{
  int vn = initialize_hgfc( hgfc );

  // alternates harmonic_calc_kappa function
  double* kappa = (double*) malloc( hgfc->hgen * sizeof(double) );
  for ( HGed* ed = hgfc->shged; ed != (HGed*) NULL; ed = ed->nxt )
    {
      kappa[ed->sid] = 1.0;
    }

  // vt->vval initialization
  for ( HGvt* vt = hgfc->shgvt; vt != (HGvt *) NULL; vt = vt->nxt )
    {
      vt->vval = 0.0;
    }

  // setup val and vval
  for ( HGvt* vt = hgfc->shgvt; vt != (HGvt *) NULL; vt = vt->nxt ) {
    for ( HGvted* ve = vt->shgve; ve != (HGvted *) NULL; ve = ve->nxt )
      {
        HGed* ed = ve->ed;
        // sv
        int id = ed->sv->sid;
        if (vt->sid != id)
          {
            HGvtvt* vv = find_sort_hgvtvt( vt, id );
            vv->val -= kappa[ed->sid];
          }
        else
          {
            vt->vval += kappa[ed->sid];
          }
        // ev
        id = ed->ev->sid;
        if (vt->sid != id)
          {
            HGvtvt* vv = find_sort_hgvtvt( vt, id );
            vv->val -= kappa[ed->sid];
          }
        else
          {
            vt->vval += kappa[ed->sid];
          }
      }
  }

  free(kappa);

  // boundary points are fixed
  for ( HGvt* vt = hgfc->shgvt; vt != (HGvt *) NULL; vt = vt->nxt )
    {
      if ( vt->vt->sp_type != SP_VERTEX_NORMAL )
        {
          vt->vval = 1.0;
          free_hgppdvertexvertex( vt );
        }
    }

  // setup matrix A
  // build up sparse matrix A

  int n_vt = hgfc->hgvn;
  Eigen::SparseMatrix<double> spmat( n_vt, n_vt );

  for ( HGvt* vt = hgfc->shgvt; vt != (HGvt*) NULL; vt = vt->nxt )
    {
      int i = vt->sid;
      spmat.insert( i, i ) = vt->vval;
      if ( vt->vt->sp_type == SP_VERTEX_NORMAL )
        {
          for ( HGvtvt* vv = vt->shgvv; vv != (HGvtvt*) NULL; vv = vv->nxt )
            {
              int j = vv->id;
              spmat.insert( i, j ) = vv->val;
            }
        }
    }
  spmat.makeCompressed();

  // setup vector b
  Eigen::VectorXd bx(n_vt), by(n_vt);
  harmonic_initialize_vector_2( hgfc, lp, bx, by );

  // solve x and y
  Eigen::VectorXd xx(n_vt), xy(n_vt);
  Eigen::BiCGSTAB<Eigen::SparseMatrix<double> > solver(spmat);
  xx = solver.solve(bx);
  xy = solver.solve(by);

  // store x to uvw
  for ( HGvt* vt = hgfc->shgvt; vt != (HGvt *) NULL; vt = vt->nxt )
    {
      vt->uvw.x = xx[vt->sid];
      vt->uvw.y = xy[vt->sid];
    }

  exit_hgfc( hgfc );
}

void hppdharmonic( HPpd *hppd )
{
  HFace *hf;

  /* calc edge length */
  edge_length( hppd->ppd1 );
  edge_length( hppd->ppd2 );
  for ( hf = hppd->shfc; hf != (HFace *) NULL; hf = hf->nxt )
    {
      hgfcharmonic_Eigen( hf->hgfc1, hf->hloop->lp1 );
      hgfcharmonic_Eigen( hf->hgfc2, hf->hloop->lp2 );
      // hgfcharmonic( hf->hgfc1, hf->hloop->lp1 );
      // hgfcharmonic( hf->hgfc2, hf->hloop->lp2 );
    }
}

#if 0
/* harmonic mapping functions */
void hgfcharmonic( HGfc *hgfc, Splp *lp )
{
  int    iter;
  int    vn;
  double rsq;
  Semat  *emat;

  vn = initialize_hgfc( hgfc );
  emat = create_harmonic_emat( hgfc );
  harmonic_initialize_vector( hgfc, lp, emat );

  linbcg( emat, emat->bx, emat->xx, 1, SMDZEROEPS, 1000, &iter, &rsq );
  linbcg( emat, emat->by, emat->yy, 1, SMDZEROEPS, 1000, &iter, &rsq );
  solvec_hgfc( hgfc, emat );

  free_emat( emat );
  exit_hgfc( hgfc );
}
#endif

int initialize_hgfc( HGfc *hgfc )
{
  int  i;
  int  cnt = 0;
  HGvt *hv;
  HGed *he;

  /* id */

  for (i = 0, hv = hgfc->shgvt; hv != (HGvt *) NULL; hv = hv->nxt, ++i) {
    hv->sid  = i;
    if ( hv->vt->sp_type == SP_VERTEX_NORMAL ) {
      ++cnt;
    }
  }
  /* , and create hgvt->hged links */
  for (i = 0, he = hgfc->shged; he != (HGed *) NULL; he = he->nxt, ++i) {
    he->sid  = i;
    create_hgvted( he->sv, he );
    create_hgvted( he->ev, he );
  }
  return cnt;
}

#if 0
Semat *create_harmonic_emat( HGfc *hgfc )
{
  int    i, vn;
  int    id;
  double *kappa;
  Semat  *emat;
  HGvt   *vt;
  HGed   *ed;
  HGvted *ve;
  HGvtvt *vv;

  emat = init_emat( hgfc->hgvn );

  kappa = harmonic_calc_kappa( hgfc );

  for ( vt = hgfc->shgvt; vt != (HGvt *) NULL; vt = vt->nxt ) {
    vt->vval = 0.0;
  }

  for ( vt = hgfc->shgvt; vt != (HGvt *) NULL; vt = vt->nxt ) {
    for ( ve = vt->shgve; ve != (HGvted *) NULL; ve = ve->nxt ) {

      ed = ve->ed;
      /* sv */
      id = ed->sv->sid;
      if (vt->sid != id) {
        vv = find_sort_hgvtvt( vt, id );
        vv->val -= kappa[ed->sid];
      } else {
        vt->vval += kappa[ed->sid];
      }
      /* ev */
      id = ed->ev->sid;
      if (vt->sid != id) {
        vv = find_sort_hgvtvt( vt, id );
        vv->val -= kappa[ed->sid];
      } else {
        vt->vval += kappa[ed->sid];
      }
    }
  }

  /* Boundary Points are fixed. */
  for ( vt = hgfc->shgvt; vt != (HGvt *) NULL; vt = vt->nxt ) {

    if ( vt->vt->sp_type != SP_VERTEX_NORMAL ) {
      vt->vval = 1.0;
      free_hgppdvertexvertex( vt );
    }

  }

  vn = 0;
  for ( vt = hgfc->shgvt; vt != (HGvt *) NULL; vt = vt->nxt ) {
    vn += vt->hgvvn;
  }

  emat->num = vn + hgfc->hgvn + 1;
  emat->sa  = (double *) malloc(emat->num * sizeof(double));
  for (i = 0; i < emat->num; ++i) emat->sa[i] = 0.0;
  emat->ija = (int *) malloc(emat->num * sizeof(int));

  dsprsin( hgfc, emat );

  free(kappa);

  FreeHGppdVertexVertex( hgfc );

  return emat;
}

double *harmonic_calc_kappa( HGfc *hgfc )
{
  double *kappa;
  HGed   *ed;

  /* initialize length, area, kappa */
  kappa   = (double *) malloc( hgfc->hgen * sizeof(double));

  for ( ed = hgfc->shged; ed != (HGed *) NULL; ed = ed->nxt ) {
    kappa[ed->sid] = 1.0;
  }
  return kappa;
}

void harmonic_initialize_vector( HGfc *hgfc, Splp *lp, Semat *emat )
{
  int    i;
  HGvt   *vt;
  Vec2d  *uvbprm;
  Sple   *le, *tle, *sle;
  Splv   *lv, *tlv, *slv, *elv;
  double sum, esum, val;
  Vec2d  sub;

  /* initialize vector */
  for ( vt = hgfc->shgvt; vt != (HGvt *) NULL; vt = vt->nxt ) {

    emat->bx[vt->sid] = emat->xx[vt->sid] = 0.0;
    emat->by[vt->sid] = emat->yy[vt->sid] = 0.0;

  }

  uvbprm = calc_uvbprm( hgfc->hgcn );

  /* calculate boundary values */

  lv = lp->splv;
  le = lp->sple;

  i = 0;
  while ( i < hgfc->hgcn ) {

    slv = lv;
    sle = le;
    sum = 0.0;

    while ( 1 ) {
      sum += le->ed->length;
      lv = lv->nxt;
      le = le->nxt;
      if ( lv == (Splv *) NULL ) {
        elv = lp->splv;
        break;
      }
      if ( lv->vt->sp_type == SP_VERTEX_HVERTEX ) {
        elv = lv;
        break;
      }
    }

    esum = 0.0;
    tlv = slv;
    tle = sle;
    sub.x = uvbprm[i+1].x - uvbprm[i].x;
    sub.y = uvbprm[i+1].y - uvbprm[i].y;
    while ( 1 ) {

      val = esum / sum;
      emat->bx[ tlv->hgvt->sid ] = uvbprm[i].x + sub.x * val;
      emat->by[ tlv->hgvt->sid ] = uvbprm[i].y + sub.y * val;

      esum += tle->ed->length;

      tlv = tlv->nxt;
      tle = tle->nxt;

      if ( (tlv == elv) || (tlv == (Splv *) NULL) ) break;
    }

    ++i;
  }

  free(uvbprm);
}
#endif

Vec2d *calc_uvbprm( int cn )
{
  int i;
  double angle;
  Vec2d *uvbprm;

  uvbprm = (Vec2d *) malloc( (cn+1) * sizeof(Vec2d) );

  for ( i = 0; i <= cn; ++i ) {
    angle = 2 * (double) i * SMDPI / (double) cn;
    uvbprm[i].x = cos( angle );
    uvbprm[i].y = sin( angle );
/*     display("uvbprm %g %g\n", uvbprm[i].x, uvbprm[i].y); */
  }
  return uvbprm;
}

#if 0
void solvec_hgfc( HGfc *hgfc, Semat *emat )
{
  HGvt *vt;

  for ( vt = hgfc->shgvt; vt != (HGvt *) NULL; vt = vt->nxt ) {
    vt->uvw.x = emat->xx[vt->sid];
    vt->uvw.y = emat->yy[vt->sid];
  }
}
#endif

void exit_hgfc( HGfc *hgfc )
{
  HGvt *hv;
  HGed *he;

  for ( hv = hgfc->shgvt; hv != (HGvt *) NULL; hv = hv->nxt ) {
    hv->sid  = SMDNULL;
    hv->vval = 0.0;
    free_hgppdvertexedge( hv );
  }

  /* create hgfc surface */

  for ( he = hgfc->shged; he != (HGed *) NULL; he = he->nxt ) {
    he->sid  = SMDNULL;
  }

}

#if 0
/* Semat functions */
Semat *init_emat( int num )
{
  Semat *emat;

  emat = (Semat *) malloc(sizeof(Semat));
  emat->rnum = num;
  emat->cnum = num;
  /* constant vectors initialize */
  emat->bx = (double *) malloc(num * sizeof(double));
  emat->by = (double *) malloc(num * sizeof(double));
  /* solution vectors initialize */
  emat->xx = (double *) malloc(num * sizeof(double));
  emat->yy = (double *) malloc(num * sizeof(double));

  return emat;
}

void free_emat(Semat *emat)
{
  free(emat->sa);
  free(emat->ija);
  free(emat->bx);
  free(emat->by);
  free(emat->xx);
  free(emat->yy);
  free(emat);
}
#endif

/*
void printemat(Semat *emat)
{
  int i;

  for (i = 0; i < emat->cnum; ++i) {
    display("%d (b) %f %f\n", i, emat->bx[i], emat->by[i]);
    display("%d (x) %f %f\n", i, emat->xx[i], emat->yy[i]);
  }
  for (i = 0; i < emat->num; ++i)
    display("(%d) (sa) %f (ija) %d\n", i, emat->sa[i], emat->ija[i]);
}
*/


/**
 * Authors:
 *      Andre Potes (andre.potes@tecnico.ulisboa.pt)
 *      Marcelo Jacinto (marcelo.jacinto@tecnico.ulisboa.pt)
 * Maintained by:
 *      Andre Potes (andre.potes@tecnico.ulisboa.pt)
 *      Marcelo Jacinto (marcelo.jacinto@tecnico.ulisboa.pt)
 * Last Update: 17/12/2021
 * License: MIT
 * File: test_math.cpp
 * Brief: Tests all the functions declared in math.hpp
 */
#include "dsor_utils/math.hpp"
#include <Eigen/Dense>
#include <gtest/gtest.h>

/* ========================== Test unit for sign function =============================== */

/**
 * @brief Test function: inline int sign(T v)
 * only for positive values
 */
TEST(TestSuite, SignPositive) {

    // test positive numbers
    ASSERT_EQ( DSOR::sign<int>(10) , 1);
    ASSERT_EQ( DSOR::sign<double>( (double)20.0 ) , 1 );
    ASSERT_EQ( DSOR::sign<float>( (float)15.0 ) , 1 );
}

/**
 * @brief Test function: inline int sign(T v)
 * only for zero
 */
TEST(TestSuite, SignNegative) {
    // test negative numbers
    ASSERT_EQ( DSOR::sign<int>(-10) , -1);
    ASSERT_EQ( DSOR::sign<double>( (double)-20.0 ) , -1 );
    ASSERT_EQ( DSOR::sign<float>( (float)-15.0 ) , -1 );
}

/**
 * @brief Test function: inline int sign(T v)
 * only for positive values
 */
TEST(TestSuite, SignZero) {

    // test zero
    ASSERT_EQ( DSOR::sign<int>(0) , 0);
    ASSERT_EQ( DSOR::sign<double>( (double)0.0 ) , 0 );
    ASSERT_EQ( DSOR::sign<float>( (float)0.0 ) , 0 );
}

/* ============================================================================================ */

/* ========================== Test unit for saturation function =============================== */

/**
 * @brief Test function: inline T saturation(T value, T min, T max)
 * only for positive saturated values
 */
TEST(TestSuite, SaturatedPositive) {

    // test saturated positive value
    ASSERT_EQ( DSOR::saturation<int>(15, -12, 10) , 10);
    ASSERT_EQ( DSOR::saturation<double>( (double)15.0, (double)-12.0, (double)10.0) , (double)10.0);
    ASSERT_EQ( DSOR::saturation<float>( (float)15.0, (float)-12.0, (float)10.0) , (float)10.0);

}

/**
 * @brief Test function: inline T saturation(T value, T min, T max)
 * only for negative saturated values
 */
TEST(TestSuite, SaturatedNegative) {
    // test saturated negative value
    ASSERT_EQ( DSOR::saturation<int>(-15, -12, 10) , -12);
    ASSERT_EQ( DSOR::saturation<double>( (double)-15.0, (double)-12.0, (double)10.0) , (double)-12.0);
    ASSERT_EQ( DSOR::saturation<float>( (float)-15.0, (float)-12.0, (float)10.0) , (float)-12.0);
}

/**
 * @brief Test function: inline T saturation(T value, T min, T max)
 * only for not saturated values
 */
TEST(TestSuite, NotSatured) {
    // test a value that is not saturated
    ASSERT_EQ( DSOR::saturation<int>(5, -10, 10) , 5);
    ASSERT_EQ( DSOR::saturation<double>( (double)5.0, (double)-12.0, (double)10.0) , (double)5.0);
    ASSERT_EQ( DSOR::saturation<float>( (float)5.0, (float)-12.0, (float)10.0) , (float)5.0);
}

int main(int argc, char **argv) {
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}