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

#include "./config.hpp"

#ifdef TEST_STD_INCLUDES
#  include <functional>
#else
#  include <boost/functional/hash/hash.hpp>
#endif

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>

#include <cmath>
#include <boost/limits.hpp>

#include <iostream>

template <class T>
void float_tests(char const* name, T* = 0)
{
    std::cout<<"\n"
        "Testing " BOOST_STRINGIZE(HASH_NAMESPACE) "::hash<"<<name<<">\n"
        "\n";

    HASH_NAMESPACE::hash<T> x1;

    T zero = 0;
    T minus_zero = (T) -1 * zero;

    BOOST_CHECK(zero == minus_zero);
    BOOST_CHECK(x1(zero) == x1(minus_zero));

    BOOST_CHECK(x1(zero) == HASH_NAMESPACE::hash_value(zero));
    BOOST_CHECK(x1(minus_zero) == HASH_NAMESPACE::hash_value(minus_zero));

    using namespace std;

// Doing anything with infinity causes borland to crash.
#if defined(__BORLANDC__)
    std::cout<<"Not running infinity checks on Borland, as it causes it to crash.\n";
#else
    if(std::numeric_limits<T>::has_infinity) {
        T infinity = -log(zero);
        T infinity2 = (T) 1. / zero;
        T infinity3 = (T) -1. / minus_zero;
        T infinity4 = std::numeric_limits<T>::infinity();

        T minus_infinity = log(zero);
        T minus_infinity2 = (T) -1. / zero;
        T minus_infinity3 = (T) 1. / minus_zero;

        BOOST_CHECK(x1(infinity) == HASH_NAMESPACE::hash_value(infinity));
        BOOST_CHECK(x1(minus_infinity)
                == HASH_NAMESPACE::hash_value(minus_infinity));

        BOOST_CHECK(infinity == infinity2);
        BOOST_CHECK(infinity == infinity3);
        BOOST_CHECK(infinity == infinity4);
        BOOST_CHECK(x1(infinity) == x1(infinity2));
        BOOST_CHECK(x1(infinity) == x1(infinity3));
        BOOST_CHECK(x1(infinity) == x1(infinity4));

        BOOST_CHECK(minus_infinity == minus_infinity2);
        BOOST_CHECK(x1(minus_infinity) == x1(minus_infinity2));
        BOOST_CHECK(minus_infinity == minus_infinity3);
        BOOST_CHECK(x1(minus_infinity) == x1(minus_infinity3));

        BOOST_CHECK(infinity != minus_infinity);

        // My hash fails this one, I guess it's not that bad.
        if(x1(infinity) == x1(minus_infinity)) {
            std::cout<<"x1(infinity) == x1(-infinity) == "<<x1(infinity)<<"\n";
        }

        // This should really be 'has_denorm == denorm_present' but some
        // compilers don't have 'denorm_present'. See also a later use.
        if(std::numeric_limits<T>::has_denorm) {
            if(x1(std::numeric_limits<T>::denorm_min()) == x1(infinity)) {
                std::cout<<"x1(denorm_min) == x1(infinity) == "<<x1(infinity)<<"\n";
            }
            if(x1(std::numeric_limits<T>::denorm_min()) == x1(minus_infinity)) {
                std::cout<<"x1(denorm_min) == x1(-infinity) == "<<x1(minus_infinity)<<"\n";
            }
        }
        if(std::numeric_limits<T>::has_quiet_NaN) {
            if(x1(std::numeric_limits<T>::quiet_NaN()) == x1(infinity)) {
                std::cout<<"x1(quiet_NaN) == x1(infinity) == "<<x1(infinity)<<"\n";
            }
            if(x1(std::numeric_limits<T>::quiet_NaN()) == x1(minus_infinity)) {
                std::cout<<"x1(quiet_NaN) == x1(-infinity) == "<<x1(minus_infinity)<<"\n";
            }
        }
    }
#endif

    T max = (std::numeric_limits<T>::max)();
    T half_max = max / 2;
    T quarter_max = max / 4;
    T three_quarter_max = max - quarter_max;

    BOOST_CHECK(x1(max) == HASH_NAMESPACE::hash_value(max));
    BOOST_CHECK(x1(half_max) == HASH_NAMESPACE::hash_value(half_max));
    BOOST_CHECK(x1(quarter_max) == HASH_NAMESPACE::hash_value(quarter_max));
    BOOST_CHECK(x1(three_quarter_max) == HASH_NAMESPACE::hash_value(three_quarter_max));

    // The '!=' tests could legitimately fail, but with my hash it indicates a bug.
    BOOST_CHECK(x1(max) == x1(max));
    BOOST_CHECK(x1(max) != x1(quarter_max));
    BOOST_CHECK(x1(max) != x1(half_max));
    BOOST_CHECK(x1(max) != x1(three_quarter_max));
    BOOST_CHECK(x1(quarter_max) == x1(quarter_max));
    BOOST_CHECK(x1(quarter_max) != x1(half_max));
    BOOST_CHECK(x1(quarter_max) != x1(three_quarter_max));
    BOOST_CHECK(x1(half_max) == x1(half_max));
    BOOST_CHECK(x1(half_max) != x1(three_quarter_max));
    BOOST_CHECK(x1(three_quarter_max) == x1(three_quarter_max));

    T v1 = asin((T) 1);
    T v2 = acos((T) 0);
    if(v1 == v2)
        BOOST_CHECK(x1(v1) == x1(v2));
    BOOST_CHECK(x1(v1) == HASH_NAMESPACE::hash_value(v1));
    BOOST_CHECK(x1(v2) == HASH_NAMESPACE::hash_value(v2));

    BOOST_CHECK(x1(std::numeric_limits<T>::epsilon()) != x1((T) 0));
    BOOST_CHECK(x1(std::numeric_limits<T>::epsilon()) ==
            HASH_NAMESPACE::hash_value(std::numeric_limits<T>::epsilon()));

    // As before.
    if(std::numeric_limits<T>::has_denorm) {
        if(x1(std::numeric_limits<T>::denorm_min()) == x1(zero)) {
            std::cout<<"x1(denorm_min) == x1(zero) == "<<x1(zero)<<"\n";
        }
#if !BOOST_WORKAROUND(__DECCXX_VER,<70190006)
        // The Tru64/CXX standard library prior to 7.1 contains a bug in the
        // specialization of std::numeric_limits::denorm_min() for long
        // doubles which causes this test to fail.
        BOOST_CHECK_MESSAGE(x1(std::numeric_limits<T>::denorm_min()) ==
            HASH_NAMESPACE::hash_value(std::numeric_limits<T>::denorm_min()),
            "x1(std::numeric_limits<T>::denorm_min()) = "
                << x1(std::numeric_limits<T>::denorm_min())
                << "\nhash_value(std::numeric_limits<T>::denorm_min()) = "
                << HASH_NAMESPACE::hash_value(
                    std::numeric_limits<T>::denorm_min())
                << "\nx1(0) = "<<x1(0)<<"\n");
#endif
    }

// NaN also causes borland to crash.
#if !defined(__BORLANDC__)
    if(std::numeric_limits<T>::has_quiet_NaN) {
        if(x1(std::numeric_limits<T>::quiet_NaN()) == x1(1.0)) {
            std::cout<<"x1(quiet_NaN) == x1(1.0) == "<<x1(1.0)<<"\n";
        }
        BOOST_CHECK(x1(std::numeric_limits<T>::quiet_NaN()) ==
            HASH_NAMESPACE::hash_value(std::numeric_limits<T>::quiet_NaN()));
    }
#endif
}

BOOST_AUTO_UNIT_TEST(hash_float_tests)
{
    std::cout<<"Compiler: "<<BOOST_COMPILER<<"\n";
    std::cout<<"Platform: "<<BOOST_PLATFORM<<"\n";
    std::cout<<"Library: "<<BOOST_STDLIB<<"\n\n";

    float_tests("float", (float*) 0);
}

BOOST_AUTO_UNIT_TEST(hash_double_tests)
{
    float_tests("double", (double*) 0);
}

BOOST_AUTO_UNIT_TEST(hash_long_double_tests)
{
    float_tests("long double", (long double*) 0);
}


#define BOOST_TEST_MODULE matrix

#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/mpl/list.hpp>

#include <mla/matrix/SparseCRS.h++>

typedef boost::mpl::list<
	float,
	double
> scalar_list;

template <typename Scalar>
using MatrixType = mla::matrix::SparseCRS<Scalar>;

BOOST_AUTO_TEST_SUITE(test_matrix)

BOOST_AUTO_TEST_CASE_TEMPLATE( Declaration, Scalar, scalar_list )
{
	MatrixType<Scalar> m(3,3);

	BOOST_CHECK_EQUAL(m.rows(), 3);
	BOOST_CHECK_EQUAL(m.columns(), 3);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( Clearing, Scalar, scalar_list )
{
	MatrixType<Scalar> m(3,3);
	m.setZero();

	for(unsigned int i = 0; i < 3; i++)
	{
		for(unsigned int j = 0; j < 3; j++)
		{
			BOOST_CHECK_EQUAL(m.getValue(i, j), 0.0);
		}
	}

}

BOOST_AUTO_TEST_CASE_TEMPLATE( Assigning, Scalar, scalar_list )
{
	MatrixType<Scalar> m(3,3);
	m.setZero();

	m(1,1) = 1.0;

	BOOST_CHECK_EQUAL(m.getValue(1, 1), 1.0);

	BOOST_CHECK_EQUAL(m.getValue(0, 1), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(2, 1), 0.0);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( get_row, Scalar, scalar_list )
{
	MatrixType<Scalar> m(3,3);
	m.setZero();

	m(0,0) = 1.0;
	m(1,1) = 2.0;
	m(2,2) = 3.0;

	for(size_t i = 0; i < m.rows(); i++)
	{
		typename MatrixType<Scalar>::VectorType v = m.getRow(i);
		for(size_t j = 0; j < m.columns(); j++)
		{
			BOOST_CHECK_EQUAL(v.getValue(j), m.getValue(i,j));
		}
	}

}

BOOST_AUTO_TEST_CASE_TEMPLATE( Incrementing_21, Scalar, scalar_list )
{
	MatrixType<Scalar> m(3,3);
	m.setZero();

	m(2,1) += 1.0;

	BOOST_CHECK_EQUAL(m.getValue(0, 0), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(0, 1), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(0, 2), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(1, 0), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(1, 1), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(1, 2), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(2, 0), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(2, 1), 1.0);
	BOOST_CHECK_EQUAL(m.getValue(2, 2), 0.0);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( Incrementing_02, Scalar, scalar_list )
{
	MatrixType<Scalar> m(3,3);
	m.setZero();

	m(0,2) += 1.0;

	BOOST_CHECK_EQUAL(m.getValue(0, 0), (Scalar)0);
	BOOST_CHECK_EQUAL(m.getValue(0, 1), (Scalar)0);
	BOOST_CHECK_EQUAL(m.getValue(0, 2), (Scalar)1);
	BOOST_CHECK_EQUAL(m.getValue(1, 0), (Scalar)0);
	BOOST_CHECK_EQUAL(m.getValue(1, 1), (Scalar)0);
	BOOST_CHECK_EQUAL(m.getValue(1, 2), (Scalar)0);
	BOOST_CHECK_EQUAL(m.getValue(2, 0), (Scalar)0);
	BOOST_CHECK_EQUAL(m.getValue(2, 1), (Scalar)0);
	BOOST_CHECK_EQUAL(m.getValue(2, 2), (Scalar)0);
}

BOOST_AUTO_TEST_SUITE_END()


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/integral.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTANT_ASSERT(int_<1> + int_<3> == int_<4>);

    // Mixed-type operations are supported:
    BOOST_HANA_CONSTANT_ASSERT(size_t<3> * ushort<5> == size_t<15>);
    BOOST_HANA_CONSTANT_ASSERT(size_t<15> == int_<15>);
    //! [main]
}


/*
 * Date:      2020/01/31 10:33
 * Author:    Petra Stefanikova, Petr Vana, Jan Faigl
 */

#include <boost/foreach.hpp>

#include <crl/random.h>
#include <crl/logging.h>
#include <crl/assert.h>
#include <crl/file_utils.h>

#include <crl/gui/shape.h>
#include <crl/gui/shapes.h>
#include <crl/gui/colormap.h>
#include "canvasview_coords.h"

#include <math.h>
#include <cstdio>
#include <limits>

#include "grasp.h"

using namespace grasp;

using namespace crl;
using namespace crl::gui;

bool isFeasible(SPath& p, STargetVector & targets, double Tmax) {
    if (p.length() > Tmax) return false;

    bool used[targets.size()];
    for(auto&a:used) a = false;
    for (int i = 0; i < p.size(); ++i) {
        if (used[p.tv[i]->label]) {
            DEBUG("USED TWICE " << p.tv[i]->label);
            return false;
        } else if ((p.tv[i]->centre - p.cv[i]).length() > p.tv[i]->radius + 1e-10) {
            DEBUG("t:" + crl::string_format<int>(p.tv[i]->label) + " " + crl::string_format<int>(i)
                + " diff:" + crl::string_format<double>((p.tv[i]->centre - p.cv[i]).length()));
            return false;
        } else {
            used[p.tv[i]->label] = true;
        }
    }
    return true;
}

/// - protected method ---------------------------------------------------------
void CGRASP::iterate(int iter) {
    curPath.clear();
    bestPath.clear();
    // initialization of the solution vector
    Coords initLocation = dataset.targets[dataset.startID]->centre;
    Coords finalLocation = dataset.targets[dataset.endID]->centre;

    if (dataset.Tmax <
        (initLocation - finalLocation).length()) {
            drawPath();
            fillResultRecord(iter);
            return;
    }

    curPath.push_back(dataset.targets[dataset.startID], initLocation);
    curPath.push_back(dataset.targets[dataset.endID], finalLocation);

    bestPath = curPath;
    drawPath();

    // construction phase
    while (addLocation()) {
        drawPath(curPath);
    }

    bestPath = curPath;

    // local search phase
    localSearch();

    drawPath();

    // feasibility check
    if (CHECK_FEASIBILITY && !isFeasible(bestPath, dataset.targets, dataset.Tmax)){
        ERROR("IS NOT FEASIBLE!");
        exit(1);
    }

    fillResultRecord(iter);
}

/// - help structure of difference to currPath
struct SDiffPath {
    SPath* path;
    STarget* target;
    Coords coords;
    int i;
    // Removed part [from, to]
    int from = -1;
    int to = -1;

    double len, rew;

    SDiffPath(SPath &curPath, double length, double reward){
        path = &curPath;
        this->len = length;
        this->rew = reward;
    }

    void getLenRewAfterAdding() {
        len -= (path->cv[i-1] - path->cv[i]).length();
        len += (path->cv[i-1] - coords).length();
        len += (coords - path->cv[i]).length();
        rew +=  target->reward;
    }

    void removeFirst(int a) {
        from = to = a;
        if (from == i - 1) { // Removed part starts just before inserted point (i)
            len -= (path->cv[from-1] - path->cv[from]).length();
            len -= (path->cv[from] - coords).length();
            len += (path->cv[from-1] - coords).length();
        }else if (to == i) { // Removed part starts just after inserted point (i)
            len -= (coords - path->cv[from]).length();
            len -= (path->cv[from] - path->cv[from+1]).length();
            len += (coords - path->cv[to+1]).length();
        } else { // General case
            len -= (path->cv[from-1] - path->cv[from]).length();
            len -= (path->cv[from] - path->cv[from+1]).length();
            len += (path->cv[from-1] - path->cv[from+1]).length();
        }
        rew -= path->tv[from]->reward;
    }

    void removeNext() {
        to++;
        if (to == i - 1) { // Removed part ends just before inserted point (i)
            len -= (path->cv[from-1] - path->cv[to]).length();
            len -= (path->cv[to] - coords).length();
            len += (path->cv[from-1] - coords).length();
        } else if (to == i) { // Removed part ends just after the inserted point (i)
            len -= (coords - path->cv[to]).length();
            len -= (path->cv[to] - path->cv[to+1]).length();
            len += (coords - path->cv[to+1]).length();
        } else if (to > i && i > from ) { // Interted point (i) is in the removed part
            len -= (coords - path->cv[to]).length();
            len -= (path->cv[to] - path->cv[to+1]).length();
            len += (coords - path->cv[to+1]).length();
        } else if (from == i) { // Removed part starts just after inserted point (i)
            len -= (coords - path->cv[to]).length();
            len -= (path->cv[to] - path->cv[to+1]).length();
            len += (coords - path->cv[to+1]).length();
        } else { // General case
            len -= (path->cv[from-1] - path->cv[to]).length();
            len -= (path->cv[to] - path->cv[to+1]).length();
            len += (path->cv[from-1] - path->cv[to+1]).length();
        }
        rew -= path->tv[to]->reward;
    }

    void addAtBegining() {
        //INFO("ADD " << from << " " << to)
        if (from == i - 1) { // Removed part starts just before inserted point (i)
            len -= (path->cv[from-1] - coords).length();
            len += (path->cv[from-1] - path->cv[from]).length();
            len += (path->cv[from] - coords).length();
        } else if (to == i - 1) { // Removed part ends just before inserted point (i)
            len -= (path->cv[from-1] - coords).length();
            len += (path->cv[from-1] - path->cv[from]).length();
            len += (path->cv[from] - coords).length();
        } else if (from == i) { // Removed part starts just after inserted point (i)
            len -= (coords - path->cv[to+1]).length();
            len += (coords - path->cv[from]).length();
            len += (path->cv[from] - path->cv[to+1]).length();
        } else if (from < i && i <= to) { // Interted point (i) is in the removed part
            len -= (path->cv[from-1] - coords).length();
            len += (path->cv[from-1] - path->cv[from]).length();
            len += (path->cv[from] - coords).length();
        } else { // General case
            len -= (path->cv[from-1] - path->cv[to+1]).length();
            len += (path->cv[from-1] - path->cv[from]).length();
            len += (path->cv[from] - path->cv[to+1]).length();
        }
        rew += path->tv[from]->reward;
        from++;
    }
};

/// from the help structure SDiffPath create SPath
SPath createPath(SDiffPath &dp, SPath& curPath) {
    SPath new_path;
    int addedIdx = -1;
    int idx = -1;
    for (int i = 0; i < curPath.size(); i++) {
        if (i == dp.i) {
            idx++;
            addedIdx = idx;
            new_path.push_back(dp.target, dp.coords);
        }
        if (dp.from == -1 || i < dp.from || i > dp.to) {
            idx++;
            new_path.push_back(curPath.tv[i], curPath.cv[i]);
        }
    }

    return new_path;
}

/// - updating SDiffPath 'stour' by adding target 'w'
void find_best_position(SPath &p, STarget* w, SDiffPath &stour) {
    int label = -1;
    double minLen = std::numeric_limits<double>::max();

    double length = 0;
    Coords coords;

    for (int i = 1; i < p.size(); ++i) {
        double org_len = (p.cv[i-1] - p.cv[i]).length();
        // lower bounds
        double lb_prolong = - org_len
                            + (p.cv[i-1] - w->centre).length()
                            + (w->centre - p.cv[i]).length()
                            - 2 * w->radius;
        if (lb_prolong > minLen) continue;

        coords = p.getBestWaypoint(i-1, i, w);

        length = - org_len
                    + sqrt(p.cv[i-1].squared_distance(coords))
                    + sqrt(p.cv[i].squared_distance(coords));

        if (length < minLen - 1e-10) {
            label = i;
            minLen = length;
            stour.coords = coords;
        }
    }

    stour.target = w;
    stour.i = label;
    stour.getLenRewAfterAdding();
}

std::vector<SDiffPath> CL;

/// - private method -----------------------------------------------------------
bool CGRASP::addLocation(int blocked) {
    double length0 = curPath.length();
    double reward0 = curPath.reward();
    double best_rew = reward0;

    int l = curPath.size(); // number of targets in a path
    CL.clear();

    // choose unadded vertex from path
    for (int w = 2; w < dataset.targets.size(); ++w) {
        STarget* wTarget = dataset.targets[w];
        if (!curPath.contains(wTarget) && w != blocked) {

            // insertion to the curent path
            SDiffPath stour1(curPath, length0, reward0);
            find_best_position(curPath, wTarget, stour1);

            if (stour1.len <= dataset.Tmax && stour1.rew > reward0 + 1e-10) {
                CL.push_back(stour1);
                if (stour1.rew > best_rew){
                    best_rew = stour1.rew;
                }
            } else { // if it's not feasible, try to remove segment
                SDiffPath stour2 = stour1;
                int a = 1;
                int b = 1;
                stour2.removeFirst(a);

                // try to remove interval/segment to make the path feasible
                for (; a < l-1 ; ++a) {
                    if (a > stour2.to){
                        stour2.removeNext();
                        b++;
                    }

                    if(a > 1){
                        stour2.addAtBegining();
                    }

                    while (b != (l-2) && stour2.len > dataset.Tmax) {
                        b++;
                        stour2.removeNext();
                    }

                    if (stour2.len <= dataset.Tmax && stour2.rew >= CL_RESTRICT * best_rew) {
                        if (stour2.rew > reward0 + 1e-10){
                            CL.push_back(stour2);

                            if (stour2.rew > best_rew) best_rew = stour2.rew;

                        } else if ((fabs(stour2.rew - reward0) < 1e-10 &&
                                    stour2.len < length0 - 1e-10)) {
                            CL.push_back(stour2);

                            if (stour2.rew > best_rew) best_rew = stour2.rew;
                        }
                    }
                }
            }
        }
    }

    // randomly choose from the candidate list CL
    if (!CL.empty()) {
        restrict(best_rew);
        SDiffPath dp = CL[0];
        if (CL.size() > 1) {
            dp = CL[crl::CRandom::next(CL.size() - 1)];
        }

        curPath = createPath(dp, curPath);

        return true;
    }

    return false;
}

/// - private method -----------------------------------------------------------
void CGRASP::localSearch() {
    bool improved = true;

    while (improved) {
        SPath r = bestPath;
        int l = bestPath.size();
        improved = false;

        // try to replace target with some others
        for (int i = 1; i < l - 1; ++i) {
            curPath = r.remove(i);
            twoOpt(curPath);
            if(WAYPOINT_OPT) curPath.optimize();

            while (addLocation(r.tv[i]->label)) {}

            double rewardCur = curPath.reward();
            double rewardBest = bestPath.reward();
            if (rewardCur > rewardBest + 1e-10 ||
                (fabs(rewardCur - rewardBest) < 1e-10 &&
                curPath.length() < bestPath.length() - 1e-10)) {
                bestPath = curPath;
                improved = true;
                drawPath();
            }
        }
    }
}

/// - private method -----------------------------------------------------------
void CGRASP::restrict(double best) {
    std::vector<SDiffPath> RCL;
    for (int i = 0; i < CL.size(); ++i) {
        if (CL[i].rew >= CL_RESTRICT * best) {
            RCL.push_back(CL[i]);
        }
    }
    CL = RCL;
}

/// - private method -----------------------------------------------------------
bool CGRASP::twoOpt(SPath &path) {
    bool changePerformed = true;
    int numTwoOpts = 0;
    while (changePerformed) {
        changePerformed = false;
        int numTargets = path.size();
        double actualLength = path.length();
        double dist[numTargets][numTargets];
        for (int i = 0; i < numTargets - 1; i++) {
            for (int j = i + 1; j < numTargets; j++) {
                dist[i][j] = (path.cv[i] - path.cv[j]).length();
            }
        }

        for (int i = 1; i < numTargets - 2; i++) {
            for (int j = i + 1; j < numTargets - 1; j++) {
                double optLength = actualLength
                                    - dist[i-1][i] - dist[j][j+1]
                                    + dist[i-1][j] + dist[i][j+1];
                if (optLength < actualLength - 1e-10) {
                    path = twoOptSwap(path, i, j);
                    changePerformed = true;
                    numTwoOpts++;
                    break;
                }
            }
            if (changePerformed) break;
        }
    }
    return (numTwoOpts > 0);
}

/// - private method -----------------------------------------------------------
SPath CGRASP::twoOptSwap(SPath &tour, const int &i, const int &j) {
    int size = tour.size();

    SPath newTour;

    // 1. take route[0] to route[i-1] and add them in order to new_route
    for (int c = 0; c <= i - 1; ++c) {
        newTour.push_back(tour.tv[c], tour.cv[c]);
    }

    // 2. take route[i] to route[j] and add them in reverse order to new_route
    int dec = 0;
    for (int c = j; c >= i; --c) {
        newTour.push_back(tour.tv[c], tour.cv[c]);
    }

    // 3. take route[j+1] to end and add them in order to new_route
    for (int c = j + 1; c < size; ++c) {
        newTour.push_back(tour.tv[c], tour.cv[c]);
    }

    return newTour;
}

/*
 *
 * FUNCTIONS OF VENDORS LIBRARY
 *
 */

/// - static method ------------------------------------------------------------
crl::CConfig &CGRASP::getConfig(crl::CConfig &config) {
    // basic properties not included in the crl_algorithm
    Base::getConfig(config);
    config.add<bool>("save-info", "disable/enable save info", true);
    config.add<bool>("save-settings", "disable/enable save settings", true);

    config.add<std::string>("result-path", "file name for the final found path (ring) as sequence of points",
                            "path");
    config.add<std::string>("pic-dir", "relative directory in result directory to store pictures from each iteration");
    config.add<std::string>("pic-ext",
                            "extension of pic, eps, png, pdf, svg (supported by particular gui renderer rendered",
                            "png");
    config.add<bool>("save-pic", "enable/disable saving pictures (after each refine)");
    config.add<bool>("wait-to-click", "enable/disable waiting for click after redrawing new best path", false);
    //
    // Problem specification
    config.add<std::string>("problem", "problem file");
    config.add<double>("budget", "budget for OP", 40);
    config.add<std::string>("method", "specify method name in the result log", "grasp-ceop");
    //
    // GRASP specitication
    config.add<bool>("waypoint-optimization", "enable/disable waypoint optimization in local search phase", false);
    //
    // Gui properties
    config.add<std::string>("draw-shape-targets", "shape of the target", Shape::CITY());
    config.add<std::string>("draw-shape-path", "shape of the path", Shape::RED_LINE());
    config.add<std::string>("draw-shape-path-nodes", "shape of the path nodes", Shape::MAP_VERTEX());
    config.add<std::string>("draw-shape-radius", "shape of the neighborhood radius", Shape::POLYGON_FILL());
    config.add<bool>("draw-targets-reward", "draw colored regions depending on rewards", false);
    config.add<bool>("draw-target-location-reward", "draw colored target locations (or centers for regions) depending on rewards", false);
    config.add<std::string>("draw-targets-reward-palette", "file of rewards palette", "./etc/jet.txt");

    config.add<bool>("draw-path", "enable/Disable drawing ring in the final shoot", true);
    config.add<bool>("draw-path-nodes", "enable/disable drawing path vertices(nodes)", true);
    return config;
}

/// - constructor --------------------------------------------------------------
CGRASP::CGRASP(crl::CConfig &config, SDataset &ds) : Base(config, "TRIAL"),
                                                       SAVE_RESULTS(config.get<bool>("save-results")),
                                                       SAVE_SETTINGS(config.get<bool>("save-settings")),
                                                       SAVE_INFO(config.get<bool>("save-info")),
                                                       SAVE_PIC(config.get<bool>("save-pic")),
                                                       WAYPOINT_OPT(config.get<bool>("waypoint-optimization")) {
    shapeTargets.setShape(config.get<std::string>("draw-shape-targets"));
    shapePath.setShape(config.get<std::string>("draw-shape-path"));
    shapePathNodes.setShape(config.get<std::string>("draw-shape-path-nodes"));
    shapeRadius.setShape(config.get<std::string>("draw-shape-radius"));

    method = config.get<std::string>("method");

    CGRASP::dataset = ds;

    const std::string fname = config.get<std::string>("problem");
    if (name.size() == 0) {
        std::string n = getBasename(fname);
        size_t i = n.rfind(".txt");
        if (i != std::string::npos) {
            name = n.erase(i, 4);
        }
    }
}

/// - destructor ---------------------------------------------------------------
CGRASP::~CGRASP() {
}

/// - public method ------------------------------------------------------------
std::string CGRASP::getVersion(void) {
    return "GRASP-CEOP";
}

/// - public method ------------------------------------------------------------
std::string CGRASP::getRevision(void) {
    return "IRMAS'20";
}

/// - public method ------------------------------------------------------------
void CGRASP::solve(void) {
    crl::CRandom::randomize();
    Base::solve();
}

/// - protected method ---------------------------------------------------------
void CGRASP::load(void) {
    if (canvas) {
        // resize area
        *canvas << canvas::AREA;
        for (STarget * target : dataset.targets) {
	   const double x = target->centre.x;
	   const double y = target->centre.y;
	   const double r = target->radius;
           *canvas << x << y << (x+r) << (y+r) << (x-r) << (y-r);
        }
        *canvas << canvas::END;

        // draw regions
	crl::gui::CColorMap map;
	std::string pallete = config.get<std::string>("draw-targets-reward-palette");
	const bool rt = config.get<bool>("draw-targets-reward");
	const bool rtl = config.get<bool>("draw-target-location-reward");
	if (crl::isFile(pallete) and (rt or rtl)) { // load pallete
	   map.load(pallete);
	   double minReward = std::numeric_limits<double>::max();
	   double maxReward = std::numeric_limits<double>::min();
	   for (STarget * t : dataset.targets) {
	      if (t->reward < minReward) {
		 minReward = t->reward;
	      }
	      if (t->reward > maxReward) {
		 maxReward = t->reward;
	      }
	   }
	   if (minReward == maxReward) {
	      minReward = 0.99 * maxReward;
	   }
	   map.setRange(minReward, maxReward);
	}
	if (rt) {
            *canvas << "regions" << shapeRadius << canvas::ARC;
            for (STarget * target : dataset.targets) {
                if (target->label == dataset.startID || target->label == dataset.endID) continue;
                SColor color = map.getColor(target->reward);
                color.alpha = 0.3;
                *canvas << canvas::FILL_COLOR << color;
                *canvas << target->centre.x << target->centre.y << target->radius << 0.0 << 2 * M_PI;
            }
        } else {
            *canvas << "regions" << shapeRadius << canvas::ARC;
            for (STarget * target : dataset.targets) {
                if (target->label == dataset.startID || target->label == dataset.endID) continue;
                *canvas << target->centre.x << target->centre.y << target->radius << 0.0 << 2 * M_PI;
            }
        }

        // draw points
        *canvas << "targets" << shapeTargets << canvas::POINT;
        for (STarget * target : dataset.targets) {
	   if (rtl) {
	      SColor color = map.getColor(target->reward);
	      *canvas << canvas::FILL_COLOR << color;
	   }
            *canvas << target->centre.x << target->centre.y;
        }
        *canvas << canvas::REDRAW;

   }
    DEBUG("GRASP::load -- done");
}

/// - protected method ---------------------------------------------------------
void CGRASP::initialize(void) {
}

/// - protected method ---------------------------------------------------------
void CGRASP::after_init(void) {
}

/// - protected method ---------------------------------------------------------
void CGRASP::save(void) {
    std::string dir;
    updateResultRecordTimes(); //update timers as load and initilization is outside class
    if (SAVE_SETTINGS) {
        saveSettings(getOutputIterPath(config.get<std::string>("settings"), dir));
    }
    if (SAVE_INFO) {
        saveInfo(getOutputIterPath(config.get<std::string>("info"), dir));
    }
    if (SAVE_RESULTS) {
        std::string file = getOutputIterPath(config.get<std::string>("result-path"), dir);
        assert_io(createDirectory(dir), "Can not create file in the path'" + file + "'");

        const int i = 0;
        std::stringstream ss;
        ss << file << "-" << std::setw(2) << std::setfill('0') << i << ".txt";
        std::ofstream ofs(ss.str().c_str());
        assert_io(not ofs.fail(), "Cannot create path '" + ss.str() + "'");
        ofs << std::setprecision(14);
        for (int i = 0; i < bestPath.size(); ++i) {
            ofs << bestPath.cv[i].x << " " << bestPath.cv[i].y << " " << bestPath.tv[i]->reward << std::endl;
        }
        assert_io(not ofs.fail(), "Error occur during path saving");
        ofs.close();
    }
    if (canvas) { // map must be set
        if (config.get<bool>("draw-path")) {
            drawPath();
        }
        saveCanvas();
    }
}

/// - protected method ---------------------------------------------------------
void CGRASP::release(void) {
}

/// - protected method ---------------------------------------------------------
void CGRASP::defineResultLog(void) {
    static bool resultLogInitialized = false;
    if (!resultLogInitialized) {
        resultLog << result::newcol << "NAME";
        resultLog << result::newcol << "METHOD";
        resultLog << result::newcol << "TRIAL";
        resultLog << result::newcol << "RTIME";
        resultLog << result::newcol << "CTIME";
        resultLog << result::newcol << "UTIME";
        resultLog << result::newcol << "BUDGET";
        resultLog << result::newcol << "RADIUS";
        resultLog << result::newcol << "LENGTH";
        resultLog << result::newcol << "REWARDS";
        resultLogInitialized = true;
    }
}

/// - protected method ---------------------------------------------------------
void CGRASP::fillResultRecord(int trial) {
    resultLog << result::newrec << name << method << trial;
    long t[3] = {0l, 0l, 0l};
    tLoad.addTime(t);
    tInit.addTime(t);
    tSolve.addTime(t);
    tSave.addTime(t);
    resultLog << t[0] << t[1] << t[2];
    DEBUG("time");
    double solutionLength = bestPath.length();
    DEBUG("len");
    double solutionReward = bestPath.reward();
    DEBUG("rew");
    double radius = dataset.targets[2]->radius;
    resultLog
            << dataset.Tmax // budget
            << radius
            << solutionLength
            << solutionReward
            << crl::result::endrec;
    INFO("Trial " << iter << " best solution reward: " << solutionReward << " length: " << solutionLength);
}

/// - private method -----------------------------------------------------------
void CGRASP::drawPath(void) {
    drawPath(bestPath);
}

/// - private method -----------------------------------------------------------
void CGRASP::drawPath(SPath &path) {
    if (canvas) {
        *canvas
               << canvas::CLEAR << "path" << "path"
               << CShape(config.get<std::string>("draw-shape-path"))
               << canvas::LINESTRING
               << path.cv
               << canvas::END;

        if (config.get<bool>("draw-path-nodes")) {
            *canvas
                  << canvas::CLEAR << "nodes" << "nodes"
                  << canvas::POINT << shapePathNodes << path.cv;
        }
        if (config.get<bool>("wait-to-click")) {
            canvas->click();
        }
        canvas->redraw();

    }
}

void printPath(SPath &p) {
    for (int i = 0; i < p.size(); i++) {
        DEBUG(i << " label:" << p.tv[i]->label << " (" << p.cv[i].x << "," << p.cv[i].y << ")");
    }
}


#include <lwr_controllers/minimum_effort_inverse_dynamics.h>
#include <utils/pseudo_inversion.h>
#include <pluginlib/class_list_macros.h>
#include <kdl_parser/kdl_parser.hpp>
#include <Eigen/LU>
#include <math.h>

namespace lwr_controllers
{
	MinimumEffortInverseDynamics::MinimumEffortInverseDynamics() {}
	MinimumEffortInverseDynamics::~MinimumEffortInverseDynamics() {}

	bool MinimumEffortInverseDynamics::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
	{
        KinematicChainControllerBase<hardware_interface::EffortJointInterface>::init(robot, n);

		jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_));
		id_solver_.reset(new KDL::ChainDynParam(kdl_chain_,gravity_));
		fk_pos_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));

		qdot_last_.resize(kdl_chain_.getNrOfJoints());
		tau_.resize(kdl_chain_.getNrOfJoints());
		J_.resize(kdl_chain_.getNrOfJoints());
		J_dot_.resize(kdl_chain_.getNrOfJoints());
		J_last_.resize(kdl_chain_.getNrOfJoints());
		Kp_.resize(kdl_chain_.getNrOfJoints());
		Kd_.resize(kdl_chain_.getNrOfJoints());
		M_.resize(kdl_chain_.getNrOfJoints());
		C_.resize(kdl_chain_.getNrOfJoints());
		G_.resize(kdl_chain_.getNrOfJoints());

		sub_command_ = nh_.subscribe("command", 1, &MinimumEffortInverseDynamics::command, this);

		pub_error_ = nh_.advertise<std_msgs::Float64MultiArray>("error", 1000);
		pub_pose_ = nh_.advertise<std_msgs::Float64MultiArray>("pose", 1000);
		pub_marker_ = nh_.advertise<visualization_msgs::Marker>("marker",1000);

		return true;
	}

	void MinimumEffortInverseDynamics::starting(const ros::Time& time)
	{
		// get joint positions
  		for(int i=0; i < joint_handles_.size(); i++)
  		{
    		joint_msr_states_.q(i) = joint_handles_[i].getPosition();
    		joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity();
    		joint_des_states_.q(i) = joint_msr_states_.q(i);
    		joint_des_states_.qdot(i) = joint_msr_states_.qdot(i);
    		Kp_(i) = 500;
  			Kd_(i) = 2;
    	}

    	I_ = Eigen::Matrix<double,7,7>::Identity(7,7);
    	e_ref_ = Eigen::Matrix<double,6,1>::Zero();

    	first_step_ = 1;
    	cmd_flag_ = 1;
    	step_ = 0;

	}

	void MinimumEffortInverseDynamics::update(const ros::Time& time, const ros::Duration& period)
	{
		// get joint positions
  		for(int i=0; i < joint_handles_.size(); i++)
  		{
    		joint_msr_states_.q(i) = joint_handles_[i].getPosition();
    		joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity();
    		joint_des_states_.q(i) = joint_msr_states_.q(i);
    		joint_des_states_.qdot(i) = joint_msr_states_.qdot(i);
    	}

    	/* TASK 0 (Main)*/

    	double freq_ = 3.0;
    	double amp_ = 0.1;

		//Desired posture (circle figure)
		x_des_.p(0) = 0.3 + amp_*(1 - cos(freq_*period.toSec()*step_));//0.05*sin(freq_*period.toSec()*step_);
    	x_des_.p(1) = 0.3 + amp_*sin(freq_*period.toSec()*step_);//0.05*sin(2*freq_*period.toSec()*step_);
    	x_des_.p(2) = 1.1;
    	x_des_.M = KDL::Rotation(KDL::Rotation::RPY(0,3.1415,0));

    	//velocity
    	x_des_dot_(0) = amp_*freq_*sin(freq_*period.toSec()*step_);//0.05*freq_*cos(freq_*period.toSec()*step_);
    	x_des_dot_(1) = amp_*freq_*cos(freq_*period.toSec()*step_);//0.05*2*freq_*cos(2*freq_*period.toSec()*step_);
    	x_des_dot_(2) = 0;
    	x_des_dot_(3) = 0;
    	x_des_dot_(4) = 0;
    	x_des_dot_(5) = 0;

    	//acc
    	x_des_dotdot_(0) = amp_*freq_*freq_*cos(freq_*period.toSec()*step_);//-0.05*freq_*freq_*sin(freq_*period.toSec()*step_);
    	x_des_dotdot_(1) = -amp_*freq_*freq_*sin(freq_*period.toSec()*step_);//-0.05*2*2*freq_*freq_*sin(freq_*period.toSec()*step_);
    	x_des_dotdot_(2) = 0;
    	x_des_dotdot_(3) = 0;
    	x_des_dotdot_(4) = 0;
    	x_des_dotdot_(5) = 0;

    	step_++;

    	// clearing msgs before publishing
    	msg_err_.data.clear();
    	msg_pose_.data.clear();

    	if (cmd_flag_)
    	{
    		// resetting N and tau(t=0) for the highest priority task
    		N_trans_ = I_;
    		SetToZero(tau_);

    		// computing Inertia, Coriolis and Gravity matrices
		    id_solver_->JntToMass(joint_msr_states_.q, M_);
		    id_solver_->JntToCoriolis(joint_msr_states_.q, joint_msr_states_.qdot, C_);
		    id_solver_->JntToGravity(joint_msr_states_.q, G_);

		    // computing the inverse of M_ now, since it will be used often
		    pseudo_inverse(M_.data,M_inv_,false); //M_inv_ = M_.data.inverse();

	    	// computing Jacobian J(q)
	    	jnt_to_jac_solver_->JntToJac(joint_msr_states_.q,J_);

	    	// using the first step to compute jacobian of the tasks
	    	if (first_step_)
	    	{
	    		J_last_ = J_;
	    		first_step_ = 0;
	    		return;
	    	}

	    	// computing the derivative of Jacobian J_dot(q) through numerical differentiation
	    	J_dot_.data = (J_.data - J_last_.data)/period.toSec();

	    	// computing forward kinematics
	    	fk_pos_solver_->JntToCart(joint_msr_states_.q,x_);

	    	// pushing x to the pose msg
	    	for (int i = 0; i < 3; i++)
	    		msg_pose_.data.push_back(x_.p(i));

	    	// setting marker parameters
	    	set_marker(x_,msg_id_);

	    	// computing end-effector position/orientation error w.r.t. desired frame
	    	x_err_ = diff(x_,x_des_);

	    	x_dot_ = J_.data*joint_msr_states_.qdot.data;

	    	// setting error reference
	    	for(int i = 0; i < e_ref_.size(); i++)
		    {
	    		// e = x_des_dotdot + Kd*(x_des_dot - x_dot) + Kp*(x_des - x)
	    		e_ref_(i) =  x_des_dotdot_(i) + Kd_(i)*(x_des_dot_(i) - x_dot_(i)) + Kp_(i)*x_err_(i);
    			msg_err_.data.push_back(e_ref_(i));
	    	}

    		// computing b = J*M^-1*(c+g) - J_dot*q_dot
    		b_ = J_.data*M_inv_*(C_.data + G_.data) - J_dot_.data*joint_msr_states_.qdot.data;

	    	// computing omega = J*M^-1*N^T*J
	    	omega_ = J_.data*M_inv_*N_trans_*J_.data.transpose();

	    	// computing lambda = omega^-1
	    	pseudo_inverse(omega_,lambda_);

	    	// computing nullspace
	    	N_trans_ = N_trans_ - J_.data.transpose()*lambda_*J_.data*M_inv_;

	    	// finally, computing the torque tau
	    	tau_.data = J_.data.transpose()*lambda_*(e_ref_ + b_) + N_trans_*G_.data;

	    	// saving J_ of the last iteration
	    	J_last_ = J_;
    	}

    	// set controls for joints
    	for (int i = 0; i < joint_handles_.size(); i++)
    	{
    		if(cmd_flag_)
    			joint_handles_[i].setCommand(tau_(i));
    		else
    			joint_handles_[i].setCommand(PIDs_[i].computeCommand(joint_des_states_.q(i) - joint_msr_states_.q(i),period));
    	}

    	// publishing markers for visualization in rviz
    	pub_marker_.publish(msg_marker_);
    	msg_id_++;

	    // publishing error for all tasks as an array of ntasks*6
	    pub_error_.publish(msg_err_);
	    // publishing pose for each task (links) as an array of ntasks*2
	    pub_pose_.publish(msg_pose_);
	    ros::spinOnce();

	}

	void MinimumEffortInverseDynamics::command(const lwr_controllers::MultiPriorityTask::ConstPtr &msg)
	{
		first_step_ = 1;
		cmd_flag_ = 1;
	}

	void MinimumEffortInverseDynamics::set_marker(KDL::Frame x, int id)
	{
				msg_marker_.header.frame_id = "world";
				msg_marker_.header.stamp = ros::Time();
				msg_marker_.ns = "end_effector";
				msg_marker_.id = id;
				msg_marker_.type = visualization_msgs::Marker::SPHERE;
				msg_marker_.action = visualization_msgs::Marker::ADD;
				msg_marker_.pose.position.x = x.p(0);
				msg_marker_.pose.position.y = x.p(1);
				msg_marker_.pose.position.z = x.p(2);
				msg_marker_.pose.orientation.x = 0.0;
				msg_marker_.pose.orientation.y = 0.0;
				msg_marker_.pose.orientation.z = 0.0;
				msg_marker_.pose.orientation.w = 1.0;
				msg_marker_.scale.x = 0.005;
				msg_marker_.scale.y = 0.005;
				msg_marker_.scale.z = 0.005;
				msg_marker_.color.a = 1.0;
				msg_marker_.color.r = 0.0;
				msg_marker_.color.g = 1.0;
				msg_marker_.color.b = 0.0;
	}
}

PLUGINLIB_EXPORT_CLASS(lwr_controllers::MinimumEffortInverseDynamics, controller_interface::ControllerBase)


// Copyright 2019 AES WBC Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <stdarg.h>

#include <NTL/mat_GF2.h>

#include "aes_private.h"

namespace {

void err_quit(const char *fmt, ...) {
  va_list ap;
  char buf[1024];

  va_start(ap, fmt);
  vsprintf(buf, fmt, ap);
  strcat(buf, "\n");
  fputs(buf, stderr);
  fflush(stderr);
  va_end(ap);

  exit(1);
}

void read_key(const char *in, uint8_t* key, size_t size) {
  if (strlen(in) != size << 1)
    err_quit("Invalid key (should be a valid %d-bits hexadecimal string)",
        (size == 16) ? 128 : ((size == 24) ? 192 : 256));
  for (size_t i = 0; i < size; i++) {
    sscanf(in + i * 2, "%2hhx", key + i);
  }
}

template<typename T>
inline NTL::vec_GF2 from_scalar(T in);

template<>
inline NTL::vec_GF2 from_scalar(uint8_t in) {
  NTL::vec_GF2 result;
  result.SetLength(8);
  for (int i = 0; i < 8; i++) {
    result[7 - i] = ((in >> i) & 1);
  }
  return result;
}

template<>
inline NTL::vec_GF2 from_scalar(uint32_t in) {
  NTL::vec_GF2 result;
  result.SetLength(32);
  for (int i = 0; i < 32; i++) {
    result[31 - i] = ((in >> i) & 1);
  }
  return result;
}

template<typename T>
inline T to_scalar(const NTL::vec_GF2& in);

template<>
inline uint8_t to_scalar(const NTL::vec_GF2& in) {
  uint8_t result = 0;
  for (int i = 0; i < 2; i++) {
    long i0 = NTL::rep(in[i*4+0]), i1 = NTL::rep(in[i*4+1]),
         i2 = NTL::rep(in[i*4+2]), i3 = NTL::rep(in[i*4+3]);
    result = (result << 4) | (i0 << 3) | (i1 << 2) | (i2 << 1) | (i3 << 0);
  }
  return result;
}

template<>
inline uint32_t to_scalar(const NTL::vec_GF2& in) {
  uint32_t result = 0;
  for (int i = 0; i < 8; i++) {
    long i0 = NTL::rep(in[i*4+0]), i1 = NTL::rep(in[i*4+1]),
         i2 = NTL::rep(in[i*4+2]), i3 = NTL::rep(in[i*4+3]);
    result = (result << 4) | (i0 << 3) | (i1 << 2) | (i2 << 1) | (i3 << 0);
  }
  return result;
}

template<typename T>
inline T mul(const NTL::mat_GF2& mat, T x) {
  return to_scalar<T>(mat * from_scalar<T>(x));
}

NTL::mat_GF2 GenerateGF2RandomMatrix(int dimension) {
  NTL::mat_GF2 mat(NTL::INIT_SIZE, dimension, dimension);
  for (int i = 0; i < dimension; i++) {
    for (int j = 0; j < dimension; j++) {
      mat[i][j] = NTL::random_GF2();
    }
  }
  return mat;
}

NTL::mat_GF2 GenerateRandomGF2InvertibleMatrix(int dimension) {
  for (;;) {
    NTL::mat_GF2 result = GenerateGF2RandomMatrix(dimension);
    if (NTL::determinant(result) != 0)
      return result;
  }
}

// Calculate the T-boxes, which is a combination of the AddRoundKeyAfterShift
// and the SubBytes functions.
void CalculateTboxes(const uint32_t roundKey[],
    uint8_t Tboxes[][16][256], int Nr) {
  for (int r = 0; r < Nr; r++) {
    for (int x = 0; x < 256; x++) {
      uint8_t state[16] = {
        (uint8_t)x, (uint8_t)x, (uint8_t)x, (uint8_t)x,
        (uint8_t)x, (uint8_t)x, (uint8_t)x, (uint8_t)x,
        (uint8_t)x, (uint8_t)x, (uint8_t)x, (uint8_t)x,
        (uint8_t)x, (uint8_t)x, (uint8_t)x, (uint8_t)x
      };
      AddRoundKeyAfterShift(state, &roundKey[r*4]);
      SubBytes(state);
      if (r == Nr-1) {
        AddRoundKey(state, &roundKey[4*Nr]);
      }
      for (int i = 0; i < 16; i++) {
        Tboxes[r][i][x] = state[i];
      }
    }
  }
}

void CalculateTy(uint8_t Ty[4][256][4]) {
  for (int x = 0; x < 256; x++) {
    Ty[0][x][0] = gf_mul[x][0];
    Ty[0][x][1] = gf_mul[x][1];
    Ty[0][x][2] = x;
    Ty[0][x][3] = x;

    Ty[1][x][0] = x;
    Ty[1][x][1] = gf_mul[x][0];
    Ty[1][x][2] = gf_mul[x][1];
    Ty[1][x][3] = x;

    Ty[2][x][0] = x;
    Ty[2][x][1] = x;
    Ty[2][x][2] = gf_mul[x][0];
    Ty[2][x][3] = gf_mul[x][1];

    Ty[3][x][0] = gf_mul[x][1];
    Ty[3][x][1] = x;
    Ty[3][x][2] = x;
    Ty[3][x][3] = gf_mul[x][0];
  }
}

void CalculateTyBoxes(uint32_t roundKey[],
    uint32_t Tyboxes[][16][256], uint8_t TboxesLast[16][256],
    uint32_t MBL[][16][256], bool enableL, bool enableMB, int Nr) {
  uint8_t Tboxes[Nr][16][256];
  uint8_t Ty[4][256][4];

  CalculateTboxes(roundKey, Tboxes, Nr);
  CalculateTy(Ty);

  for (int r = 0; r < Nr-1; r++) {
    for (int x = 0; x < 256; x++) {
      for (int j = 0; j < 4; j++) {
        for (int i = 0; i < 4; i++) {
          uint32_t v0 = Ty[0][Tboxes[r][j*4 + i][x]][i],
                   v1 = Ty[1][Tboxes[r][j*4 + i][x]][i],
                   v2 = Ty[2][Tboxes[r][j*4 + i][x]][i],
                   v3 = Ty[3][Tboxes[r][j*4 + i][x]][i];
          Tyboxes[r][j*4 + i][x] = (v0 << 24) | (v1 << 16) | (v2 << 8) | v3;
          MBL[r][j*4 + i][x] = x << ((3 - i) << 3);
        }
      }
    }
  }

  for (int x = 0; x < 256; x++) {
    for (int i = 0; i < 16; i++) {
      TboxesLast[i][x] = Tboxes[Nr-1][i][x];
    }
  }

  if (enableMB) {
    NTL::mat_GF2 MB[Nr-1][4];
    for (int r = 0; r < Nr-1; r++) {
      for (int i = 0; i < 4; i++) {
        MB[r][i] = GenerateRandomGF2InvertibleMatrix(32);
      }
    }

    // When applying MB and inv(MB), the operation is quite easy; there is no
    // need to safeguard the existing table, as it is a simple substitution.
    for (int r = 0; r < Nr-1; r++) {
      for (int x = 0; x < 256; x++) {
        for (int i = 0; i < 16; i++) {
          Tyboxes[r][i][x] = mul<uint32_t>(MB[r][i >> 2], Tyboxes[r][i][x]);
          MBL[r][i][x] = mul<uint32_t>(NTL::inv(MB[r][i >> 2]), MBL[r][i][x]);
        }
      }
    }
  }

  if (enableL) {
    NTL::mat_GF2 L[Nr-1][16];
    for (int r = 0; r < Nr-1; r++) {
      for (int i = 0; i < 16; i++) {
        L[r][i] = GenerateRandomGF2InvertibleMatrix(8);
      }
    }

    // When applying L and inv(L), things get a little tricky. As it involves
    // non-linear substitutions, the original table has to be copied before
    // being updated.
    for (int r = 0; r < Nr-1; r++) {

      if (r > 0) {
        // Rounds 1 to Nr-1 are reversed here.
        for (int i = 0; i < 16; i++) {
          uint32_t oldTyboxes[256];
          for (int x = 0; x < 256; x++)
            oldTyboxes[x] = Tyboxes[r][i][x];
          for (int x = 0; x < 256; x++)
            Tyboxes[r][i][x] = oldTyboxes[mul<uint8_t>(NTL::inv(L[r-1][i]), x)];
        }
      }

      // Apply the L transformation at each round.
      for (int j = 0; j < 4; ++j) {
        for (int x = 0; x < 256; x++) {
          uint32_t out0 = MBL[r][j*4 + 0][x];
          uint32_t out1 = MBL[r][j*4 + 1][x];
          uint32_t out2 = MBL[r][j*4 + 2][x];
          uint32_t out3 = MBL[r][j*4 + 3][x];

          MBL[r][j*4 + 0][x] = (mul<uint8_t>(L[r][InvShiftRowsTab[j*4 + 0]], out0 >> 24) << 24)
                             | (mul<uint8_t>(L[r][InvShiftRowsTab[j*4 + 1]], out0 >> 16) << 16)
                             | (mul<uint8_t>(L[r][InvShiftRowsTab[j*4 + 2]], out0 >>  8) <<  8)
                             | (mul<uint8_t>(L[r][InvShiftRowsTab[j*4 + 3]], out0 >>  0) <<  0);

          MBL[r][j*4 + 1][x] = (mul<uint8_t>(L[r][InvShiftRowsTab[j*4 + 0]], out1 >> 24) << 24)
                             | (mul<uint8_t>(L[r][InvShiftRowsTab[j*4 + 1]], out1 >> 16) << 16)
                             | (mul<uint8_t>(L[r][InvShiftRowsTab[j*4 + 2]], out1 >>  8) <<  8)
                             | (mul<uint8_t>(L[r][InvShiftRowsTab[j*4 + 3]], out1 >>  0) <<  0);

          MBL[r][j*4 + 2][x] = (mul<uint8_t>(L[r][InvShiftRowsTab[j*4 + 0]], out2 >> 24) << 24)
                             | (mul<uint8_t>(L[r][InvShiftRowsTab[j*4 + 1]], out2 >> 16) << 16)
                             | (mul<uint8_t>(L[r][InvShiftRowsTab[j*4 + 2]], out2 >>  8) <<  8)
                             | (mul<uint8_t>(L[r][InvShiftRowsTab[j*4 + 3]], out2 >>  0) <<  0);

          MBL[r][j*4 + 3][x] = (mul<uint8_t>(L[r][InvShiftRowsTab[j*4 + 0]], out3 >> 24) << 24)
                             | (mul<uint8_t>(L[r][InvShiftRowsTab[j*4 + 1]], out3 >> 16) << 16)
                             | (mul<uint8_t>(L[r][InvShiftRowsTab[j*4 + 2]], out3 >>  8) <<  8)
                             | (mul<uint8_t>(L[r][InvShiftRowsTab[j*4 + 3]], out3 >>  0) <<  0);
        }
      }
    }

    // The last and final round 9 is reversed here.
    for (int i = 0; i < 16; i++) {
      uint8_t oldTboxesLast[256];
      for (int x = 0; x < 256; x++)
        oldTboxesLast[x] = TboxesLast[i][x];
      for (int x = 0; x < 256; x++)
        TboxesLast[i][x] = oldTboxesLast[mul<uint8_t>(NTL::inv(L[Nr-2][i]), x)];
    }
  }
}

void GenerateXorTable(FILE* out, int Nr) {
  uint8_t Xor[Nr-1][96][16][16];
  for (int r = 0; r < Nr-1; r++)
    for (int n = 0; n < 96; n++)
      for (int i = 0; i < 16; i++)
        for (int j = 0; j < 16; j++)
          Xor[r][n][i][j] = i ^ j;

  fprintf(out, "constexpr uint8_t Xor[%d][96][16][16] = {\n", Nr-1);
  for (int r = 0; r < Nr-1; r++) {
    fprintf(out, "  {\n");
    for (int n = 0; n < 96; n++) {
      fprintf(out, "    {\n");
      for (int i = 0; i < 16; i++) {
        fprintf(out, "      { ");
        for (int j = 0; j < 16; j++)
          fprintf(out, "0x%02x, ", Xor[r][n][i][j]);
        fprintf(out, "},\n");
      }
      fprintf(out, "    },\n");
    }
    fprintf(out, "  },\n");
  }
  fprintf(out, "};\n\n");
}

void GenerateEncryptingTables(FILE* out, uint32_t* roundKey, int Nr) {
  uint32_t Tyboxes[Nr-1][16][256];
  uint8_t TboxesLast[16][256];
  uint32_t MBL[Nr-1][16][256];

  CalculateTyBoxes(roundKey, Tyboxes, TboxesLast, MBL, true, true, Nr);

  fprintf(out, "constexpr uint32_t Tyboxes[%d][16][256] = {\n", Nr-1);
  for (int r = 0; r < Nr-1; r++) {
    fprintf(out, "  {\n");
    for (int i = 0; i < 16; i++) {
      fprintf(out, "    {\n");
      for (int x = 0; x < 256; x++) {
        if ((x % 8) == 0) {
          fprintf(out, "      ");
        }
        fprintf(out, "0x%08x,", Tyboxes[r][i][x]);
        if (x > 0 && (x % 8) == 7) {
          fprintf(out, "\n");
        } else {
          fprintf(out, " ");
        }
      }
      fprintf(out, "    },\n");
    }
    fprintf(out, "  },\n");
  }
  fprintf(out, "};\n\n");

  fprintf(out, "constexpr uint8_t TboxesLast[16][256] = {\n");
  for (int i = 0; i < 16; i++) {
    fprintf(out, "  {\n");
    for (int x = 0; x < 256; x++) {
      if (x % 16 == 0) {
        fprintf(out, "    ");
      }
      fprintf(out, "0x%02x, ", TboxesLast[i][x]);
      if (x % 16 == 15) {
        fprintf(out, "\n");
      }
    }
    fprintf(out, "  },\n");
  }
  fprintf(out, "};\n\n");

  fprintf(out, "constexpr uint32_t MBL[%d][16][256] = {\n", Nr-1);
  for (int r = 0; r < Nr-1; r++) {
    fprintf(out, "  {\n");
    for (int i = 0; i < 16; i++) {
      fprintf(out, "    {\n");
      for (int x = 0; x < 256; x++) {
        if ((x % 8) == 0) {
          fprintf(out, "      ");
        }
        fprintf(out, "0x%08x,", MBL[r][i][x]);
        if (x > 0 && (x % 8) == 7) {
          fprintf(out, "\n");
        } else {
          fprintf(out, " ");
        }
      }
      fprintf(out, "    },\n");
    }
    fprintf(out, "  },\n");
  }
  fprintf(out, "};\n\n");
}

void GenerateTables(const char* hexKey, int Nk, int Nr) {
  uint8_t key[Nk*4];
  uint32_t roundKey[(Nr+1)*4];

  read_key(hexKey, key, Nk*4);
  ExpandKeys(key, roundKey, Nk, Nr);

  FILE* out = fopen("aes_whitebox_tables.cc", "w");

  fprintf(out,
      "// This file is generated, do not edit.\n"
      "\n"
      "namespace {\n"
      "\n"
      "constexpr int Nr = %d;\n"
      "\n", Nr);

  GenerateXorTable(out, Nr);
  GenerateEncryptingTables(out, roundKey, Nr);

  fprintf(out, "}  // namespace");

  fflush(out);
  fclose(out);
}

void syntax() {
  err_quit("Syntax: aes_whitebox_gen <aes128|aes192|aes256> <hex-key>");
}

}  // namespace

int main(int argc, char* argv[]) {
  int Nk = 0, Nr = 0;

  if (argc != 3) {
    syntax();
  } else if (strcmp(argv[1], "aes128") == 0) {
    Nk = 4, Nr = 10;
  } else if (strcmp(argv[1], "aes192") == 0) {
    Nk = 6, Nr = 12;
  } else if (strcmp(argv[1], "aes256") == 0) {
    Nk = 8, Nr = 14;
  } else if (strcmp(argv[1], "aes512") == 0) {
    Nk = 16, Nr = 22;
  } else if (strcmp(argv[1], "aes1024") == 0) {
    Nk = 32, Nr = 38;
  } else if (strcmp(argv[1], "aes2048") == 0) {
    Nk = 64, Nr = 70;
  } else if (strcmp(argv[1], "aes4096") == 0) {
    Nk = 128, Nr = 134;
  } else {
    syntax();
  }

  GenerateTables(argv[2], Nk, Nr);
  return 0;
}


#include <doctest/doctest.h>
#include "tensor.h"
#include "units.h"
#include "core.h"
#include <Eigen/Geometry>
#include <nlohmann/json.hpp>

namespace Faunus {

Tensor::Tensor() { base::setZero(); }

Tensor::Tensor(double xx, double xy, double xz, double yy, double yz, double zz) {
    (*this) << xx, xy, xz, xy, yy, yz, xz, yz, zz;
}

void Tensor::rotate(const Tensor::base &rotation_matrix) {
    (*this) = rotation_matrix * (*this) * rotation_matrix.transpose();
}

void Tensor::eye() { *this = base::Identity(3, 3); }

void to_json(nlohmann::json &j, const Tensor &t) { j = {t(0, 0), t(0, 1), t(0, 2), t(1, 1), t(1, 2), t(2, 2)}; }

void from_json(const nlohmann::json &j, Tensor &t) {
    if (j.size() == 6 and j.is_array())
        t = Tensor(j[0], j[1], j[2], j[3], j[4], j[5]);
    else
        throw std::runtime_error("tensor expects array with exactly six coefficients");
}

TEST_SUITE_BEGIN("Tensor");

TEST_CASE("[Faunus] Tensor") {
    using doctest::Approx;
    using namespace nlohmann;
    Tensor Q1 = Tensor(1, 2, 3, 4, 5, 6);
    Tensor Q2 = json(Q1);        // Q1 --> json --> Q2
    CHECK(json(Q1) == json(Q2)); // Q1 --> json == json <-- Q2 ?
    CHECK(Q2 == Tensor(1, 2, 3, 4, 5, 6));

    auto m = Eigen::AngleAxisd(pc::pi / 2, Point(0, 1, 0)).toRotationMatrix();
    Q1.rotate(m);
    CHECK(Q1(0, 0) == Approx(6));
    CHECK(Q1(0, 1) == Approx(5));
    CHECK(Q1(0, 2) == Approx(-3));
    CHECK(Q1(1, 1) == Approx(4));
    CHECK(Q1(1, 2) == Approx(-2));
    CHECK(Q1(2, 2) == Approx(1));
}

TEST_SUITE_END();

} // namespace Faunus


/****************************************************************************
 * Copyright (c) 2017-2021 by the ArborX authors                            *
 * All rights reserved.                                                     *
 *                                                                          *
 * This file is part of the ArborX library. ArborX is                       *
 * distributed under a BSD 3-clause license. For the licensing terms see    *
 * the LICENSE file in the top-level directory.                             *
 *                                                                          *
 * SPDX-License-Identifier: BSD-3-Clause                                    *
 ****************************************************************************/
#include <ArborX_Box.hpp>
#include <ArborX_Ray.hpp>

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE(intersects_box)
{
  using ArborX::Box;
  using ArborX::Point;
  using ArborX::Experimental::Ray;

  constexpr Box unit_box{{0, 0, 0}, {1, 1, 1}};

  // origin is within the box
  BOOST_TEST(intersects(Ray{{.5, .5, .5}, {1, 0, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{.5, .5, .5}, {0, 1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{.5, .5, .5}, {0, 0, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{.5, .5, .5}, {1, 1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{.5, .5, .5}, {1, 1, 1}}, unit_box));

  // origin is outside the box
  // hit the center of the face
  BOOST_TEST(intersects(Ray{{-1, .5, .5}, {1, 0, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{-1, .5, .5}, {-1, 0, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{-1, .5, .5}, {0, 1, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{2, .5, .5}, {-1, 0, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{2, .5, .5}, {1, 0, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{2, .5, .5}, {0, 1, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{-1, 1.5, .5}, {1, -1, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{-1, 1.5, .5}, {1, 0, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{-1, 1.5, 1.5}, {1, -1, -1}}, unit_box));
  BOOST_TEST(!intersects(Ray{{-1, 1.5, 1.5}, {1, 0, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{-1, 1.5, 1.5}, {1, -1, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{-1, 1.5, 1.5}, {1, 0, -1}}, unit_box));
  BOOST_TEST(intersects(Ray{{2, -.5, .5}, {-1, 1, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{2, -.5, .5}, {-1, 0, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{2, -.5, 1.5}, {-1, 1, -1}}, unit_box));
  BOOST_TEST(!intersects(Ray{{2, -.5, 1.5}, {-1, 0, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{2, -.5, 1.5}, {-1, 1, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{2, -.5, 1.5}, {-1, 0, -1}}, unit_box));

  // hit the center of an edge
  BOOST_TEST(intersects(Ray{{-1, 0, .5}, {1, 0, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{-1, 0, .5}, {-1, 0, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{-1, 0, .5}, {0, 1, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{2, 0, .5}, {-1, 0, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{2, 0, .5}, {1, 0, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{2, 0, .5}, {0, 1, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{-1, -1, .5}, {1, 1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{-1, 1, .5}, {1, -1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{-1, -2, .5}, {1, 2, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{2, 2, .5}, {-1, -1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{2, -1, .5}, {-1, 1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{2, 1, .5}, {-1, -1, 0}}, unit_box));

  // hit a corner
  BOOST_TEST(intersects(Ray{{-0.5, 1.5, 1.5}, {1, -1, -1}}, unit_box));
  BOOST_TEST(!intersects(Ray{{-1, 1, 1}, {-1, 0, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{-1, 1, 1}, {0, 1, 1}}, unit_box));
  BOOST_TEST(!intersects(Ray{{2, 1, 1}, {1, 0, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{2, 1, 1}, {0, 1, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{-1, -1, -1}, {1, 1, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{2, 2, 2}, {-1, -1, -1}}, unit_box));

  BOOST_TEST(!intersects(Ray{{1, 2, 3}, {4, 5, 6}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 2, 3}, {-1, -2, -3}}, unit_box));

  // origin is on the box (no 0*inf).
  BOOST_TEST(intersects(Ray{{0, 0, 0}, {1, 1, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{0, 0, 0}, {-1, -1, -1}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 1, 1}, {-1, -1, -1}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 1, 1}, {1, 1, 1}}, unit_box));

  BOOST_TEST(intersects(Ray{{1, .5, .5}, {-1, 0, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, .5, .5}, {1, 0, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{.5, 1, .5}, {0, -1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{.5, 1, .5}, {0, 1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{.5, .5, 1}, {0, 0, -1}}, unit_box));
  BOOST_TEST(intersects(Ray{{.5, .5, 1}, {0, 0, 1}}, unit_box));

  BOOST_TEST(intersects(Ray{{0, 0, .5}, {1, 1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{0, 0, .5}, {-1, -1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 1, .5}, {-1, -1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 1, .5}, {1, 1, 0}}, unit_box));

  BOOST_TEST(intersects(Ray{{0, 0.5, 0}, {-1, -2, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{0.5, 0, 0.5}, {2, 0, -1}}, unit_box));

  // origin is on the box (with 0*inf).
  BOOST_TEST(intersects(Ray{{0, 0, 0}, {0, 1, -1}}, unit_box));
  BOOST_TEST(intersects(Ray{{0, 0, 0}, {-1, 0, -1}}, unit_box));
  BOOST_TEST(intersects(Ray{{0, 0, 0}, {-1, 1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{0, 0, 0}, {0, 0, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{0, 0, 0}, {-1, 0, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{0, 0, 0}, {0, -1, 0}}, unit_box));

  BOOST_TEST(intersects(Ray{{1, 1, 1}, {0, -1, -1}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 1, 1}, {1, 0, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 1, 1}, {-1, -1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 1, 1}, {1, 0, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 1, 1}, {0, 1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 1, 1}, {0, 0, -1}}, unit_box));

  BOOST_TEST(intersects(Ray{{0, 1, 1}, {0, 0, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 0, 1}, {1, 0, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 1, 0}, {0, 1, 0}}, unit_box));

  BOOST_TEST(intersects(Ray{{0, 0, 1}, {-1, 0, -1}}, unit_box));
  BOOST_TEST(intersects(Ray{{0, 0, 1}, {0, 0, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 0, 0}, {0, 1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 0, 0}, {-1, 0, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{0, 1, 0}, {0, 0, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{0, 1, 0}, {0, 0, -1}}, unit_box));

  // more cases with 0*inf:
  BOOST_TEST(intersects(Ray{{0, 1.5, 1.5}, {0, -1, -1}}, unit_box));
  BOOST_TEST(!intersects(Ray{{0, 1.5, 1.5}, {0, 1, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{1.5, 0, 1.5}, {-1, 0, -1}}, unit_box));
  BOOST_TEST(!intersects(Ray{{1.5, 0, 1.5}, {1, 0, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{1.5, 1.5, 0}, {-1, -1, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{1.5, 1.5, 0}, {1, 1, 0}}, unit_box));

  BOOST_TEST(intersects(Ray{{1, 1.5, 1.5}, {0, -1, -1}}, unit_box));
  BOOST_TEST(!intersects(Ray{{1, 1.5, 1.5}, {0, 1, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{1.5, 1, 1.5}, {-1, 0, -1}}, unit_box));
  BOOST_TEST(!intersects(Ray{{1.5, 1, 1.5}, {1, 0, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{1.5, 1.5, 1}, {-1, -1, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{1.5, 1.5, 1}, {1, 1, 0}}, unit_box));

  BOOST_TEST(intersects(Ray{{1.5, 0, 0}, {-1, 0, 2}}, unit_box));
  BOOST_TEST(intersects(Ray{{1.5, 0, 0}, {-1, 2, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{1.5, 0, 0}, {-1, 0, 2.1}}, unit_box));
  BOOST_TEST(!intersects(Ray{{1.5, 0, 0}, {-1, 2.1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{1.5, 1, 1}, {-1, 0, -2}}, unit_box));
  BOOST_TEST(intersects(Ray{{1.5, 1, 1}, {-1, -2, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{1.5, 1, 1}, {-1, 0, -2.1}}, unit_box));
  BOOST_TEST(!intersects(Ray{{1.5, 1, 1}, {-1, -2.1, 0}}, unit_box));

  BOOST_TEST(intersects(Ray{{0, 1.5, 0}, {0, -1, 2}}, unit_box));
  BOOST_TEST(intersects(Ray{{0, 1.5, 0}, {2, -1, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{0, 1.5, 0}, {0, -1, 2.1}}, unit_box));
  BOOST_TEST(!intersects(Ray{{0, 1.5, 0}, {2.1, -1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 1.5, 1}, {0, -1, -2}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 1.5, 1}, {-2, -1, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{1, 1.5, 1}, {0, -1, -2.1}}, unit_box));
  BOOST_TEST(!intersects(Ray{{1, 1.5, 1}, {-2.1, -1, 0}}, unit_box));
}

#define ARBORX_TEST_RAY_BOX_INTERSECTION(ray, box, t0_ref, t1_ref)             \
  do                                                                           \
  {                                                                            \
    float t0;                                                                  \
    float t1;                                                                  \
    BOOST_TEST(ArborX::Experimental::intersection(ray, box, t0, t1));          \
    BOOST_TEST(t0 == t0_ref);                                                  \
    BOOST_TEST(t1 == t1_ref);                                                  \
  } while (false)

#define ARBORX_TEST_RAY_BOX_NO_INTERSECTION(ray, box)                          \
  do                                                                           \
  {                                                                            \
    float t0;                                                                  \
    float t1;                                                                  \
    constexpr auto inf = KokkosExt::ArithmeticTraits::infinity<float>::value;  \
    BOOST_TEST(!ArborX::Experimental::intersection(ray, box, t0, t1));         \
    BOOST_TEST((t0 == inf || t1 == -inf));                                     \
  } while (false)

BOOST_AUTO_TEST_CASE(ray_box_intersection, *boost::unit_test::tolerance(1e-6f))
{
  using ArborX::Box;
  using ArborX::Experimental::Ray;

  constexpr Box unit_box{{0, 0, 0}, {1, 1, 1}};

  auto const sqrtf_5 = std::sqrt(5.f);
  auto const sqrtf_3 = std::sqrt(3.f);
  auto const sqrtf_2 = std::sqrt(2.f);

  // clang-format off
  // origin is within the box
  ARBORX_TEST_RAY_BOX_INTERSECTION((Ray{{.5, .5, .5}, {1, 0, 0}}), unit_box, -.5f, .5f);
  ARBORX_TEST_RAY_BOX_INTERSECTION((Ray{{.5, .5, .5}, {0, 1, 0}}), unit_box, -.5f, .5f);
  ARBORX_TEST_RAY_BOX_INTERSECTION((Ray{{.5, .5, .5}, {0, 0, 1}}), unit_box, -.5f, .5f);
  ARBORX_TEST_RAY_BOX_INTERSECTION((Ray{{.5, .5, .5}, {1, 1, 0}}), unit_box, -.5f*sqrtf_2, .5f*sqrtf_2);
  ARBORX_TEST_RAY_BOX_INTERSECTION((Ray{{.5, .5, .5}, {1, 1, 1}}), unit_box, -.5f*sqrtf_3, .5f*sqrtf_3);

  // origin is outside the box
  // hit the center of the face
  ARBORX_TEST_RAY_BOX_INTERSECTION((Ray{{-1, .5, .5}, {1, 0, 0}}), unit_box, 1.f, 2.f);
  ARBORX_TEST_RAY_BOX_INTERSECTION((Ray{{-1, .5, .5}, {-1, 0, 0}}), unit_box, -2.f, -1.f);
  ARBORX_TEST_RAY_BOX_NO_INTERSECTION((Ray{{-1, .5, .5}, {0, 1, 1}}), unit_box);
  ARBORX_TEST_RAY_BOX_NO_INTERSECTION((Ray{{-1, .5, .5}, {0, 1, 1}}), unit_box);
  ARBORX_TEST_RAY_BOX_INTERSECTION((Ray{{2, .5, .5}, {-1, 0, 0}}), unit_box, 1.f, 2.f);
  ARBORX_TEST_RAY_BOX_INTERSECTION((Ray{{2, .5, .5}, {1, 0, 0}}), unit_box, -2.f, -1.f);
  ARBORX_TEST_RAY_BOX_NO_INTERSECTION((Ray{{2, .5, .5}, {0, 1, 1}}), unit_box);
  ARBORX_TEST_RAY_BOX_INTERSECTION((Ray{{-1, 1.5, .5}, {1, -1, 0}}), unit_box, sqrtf_2, 1.5f*sqrtf_2);
  ARBORX_TEST_RAY_BOX_NO_INTERSECTION((Ray{{-1, 1.5, .5}, {1, 0, 0}}), unit_box);
  ARBORX_TEST_RAY_BOX_INTERSECTION((Ray{{-1, 1.5, 1.5}, {1, -1, -1}}), unit_box, sqrtf_3, 1.5f*sqrtf_3);
  ARBORX_TEST_RAY_BOX_NO_INTERSECTION((Ray{{-1, 1.5, 1.5}, {1, 0, 0}}), unit_box);
  ARBORX_TEST_RAY_BOX_NO_INTERSECTION((Ray{{-1, 1.5, 1.5}, {1, -1, 0}}), unit_box);
  ARBORX_TEST_RAY_BOX_NO_INTERSECTION((Ray{{-1, 1.5, 1.5}, {1, 0, -1}}), unit_box);
  ARBORX_TEST_RAY_BOX_INTERSECTION((Ray{{2, -.5, .5}, {-1, 1, 0}}), unit_box, sqrtf_2, 1.5f*sqrtf_2);
  ARBORX_TEST_RAY_BOX_NO_INTERSECTION((Ray{{2, -.5, .5}, {-1, 0, 0}}), unit_box);
  ARBORX_TEST_RAY_BOX_INTERSECTION((Ray{{2, -.5, 1.5}, {-1, 1, -1}}), unit_box, sqrtf_3, 1.5f*sqrtf_3);
  ARBORX_TEST_RAY_BOX_NO_INTERSECTION((Ray{{2, -.5, 1.5}, {-1, 0, 0}}), unit_box);
  ARBORX_TEST_RAY_BOX_NO_INTERSECTION((Ray{{2, -.5, 1.5}, {-1, 1, 0}}), unit_box);
  ARBORX_TEST_RAY_BOX_NO_INTERSECTION((Ray{{2, -.5, 1.5}, {-1, 0, -1}}), unit_box);

  // hit the center of an edge
  ARBORX_TEST_RAY_BOX_INTERSECTION((Ray{{-1, 0, .5}, {1, 0, 0}}), unit_box, 1.f, 2.f);
  ARBORX_TEST_RAY_BOX_INTERSECTION((Ray{{-1, 0, .5}, {-1, 0, 0}}), unit_box, -2.f, -1.f);
  ARBORX_TEST_RAY_BOX_NO_INTERSECTION((Ray{{-1, 0, .5}, {0, 1, 1}}), unit_box);
  ARBORX_TEST_RAY_BOX_INTERSECTION((Ray{{2, 0, .5}, {-1, 0, 0}}), unit_box, 1.f, 2.f);
  ARBORX_TEST_RAY_BOX_INTERSECTION((Ray{{2, 0, .5}, {1, 0, 0}}), unit_box, -2.f, -1.f);
  ARBORX_TEST_RAY_BOX_NO_INTERSECTION((Ray{{2, 0, .5}, {0, 1, 1}}), unit_box);
  ARBORX_TEST_RAY_BOX_INTERSECTION((Ray{{-1, -1, .5}, {1, 1, 0}}), unit_box, sqrtf_2, 2*sqrtf_2);
  ARBORX_TEST_RAY_BOX_INTERSECTION((Ray{{-1, 1, .5}, {1, -1, 0}}), unit_box, sqrtf_2, sqrtf_2);
  ARBORX_TEST_RAY_BOX_INTERSECTION((Ray{{-1, -2, .5}, {1, 2, 0}}), unit_box, sqrtf_5, 1.5f*sqrtf_5);
  ARBORX_TEST_RAY_BOX_INTERSECTION((Ray{{2, 2, .5}, {-1, -1, 0}}), unit_box, sqrtf_2, 2*sqrtf_2);
  ARBORX_TEST_RAY_BOX_INTERSECTION((Ray{{2, -1, .5}, {-1, 1, 0}}), unit_box, sqrtf_2, 2*sqrtf_2);
  ARBORX_TEST_RAY_BOX_INTERSECTION((Ray{{2, 1, .5}, {-1, -1, 0}}), unit_box, sqrtf_2, sqrtf_2);

  // hit a corner
  ARBORX_TEST_RAY_BOX_INTERSECTION((Ray{{-0.5, 1.5, 1.5}, {1, -1, -1}}), unit_box, .5f*sqrtf_3, 1.5f*sqrtf_3);
  ARBORX_TEST_RAY_BOX_INTERSECTION((Ray{{-1, 1, 1}, {-1, 0, 0}}), unit_box,-2.f, -1.f);
  ARBORX_TEST_RAY_BOX_NO_INTERSECTION((Ray{{-1, 1, 1}, {0, 1, 1}}), unit_box);
  ARBORX_TEST_RAY_BOX_INTERSECTION((Ray{{2, 1, 1}, {1, 0, 0}}), unit_box, -2.f, -1.f);
  ARBORX_TEST_RAY_BOX_NO_INTERSECTION((Ray{{2, 1, 1}, {0, 1, 1}}), unit_box);
  ARBORX_TEST_RAY_BOX_INTERSECTION((Ray{{-1, -1, -1}, {1, 1, 1}}), unit_box, sqrtf_3, 2*sqrtf_3);
  ARBORX_TEST_RAY_BOX_INTERSECTION((Ray{{2, 2, 2}, {-1, -1, -1}}), unit_box, sqrtf_3, 2*sqrtf_3);
  // clang-format on
}

#undef ARBORX_TEST_RAY_BOX_INTERSECTION
#undef ARBORX_TEST_RAY_BOX_NO_INTERSECTION

// NOTE until boost 1.70 need to cast both operands when comparing floating
// points
BOOST_AUTO_TEST_CASE(overlap_distance_sphere,
                     *boost::unit_test::tolerance(1e-6f))
{
  using ArborX::Sphere;
  using ArborX::Experimental::Ray;
  constexpr Sphere unit_sphere{{0, 0, 0}, 1};

  auto const sqrtf_3 = std::sqrt(3.f);
  auto const half_sqrtf_2 = std::sqrt(2.f) / 2;
  auto const half_sqrtf_3 = std::sqrt(3.f) / 2;

  // hit center of the sphere
  BOOST_TEST(overlapDistance(Ray{{-2, 0, 0}, {1, 0, 0}}, unit_sphere) == 2.f);
  BOOST_TEST(overlapDistance(Ray{{0, 3, 0}, {0, -1, 0}}, unit_sphere) == 2.f);
  BOOST_TEST(overlapDistance(Ray{{0, 0, -4}, {0, 0, 1}}, unit_sphere) == 2.f);
  BOOST_TEST(overlapDistance(Ray{{-2, -2, -2}, {1, 1, 1}}, unit_sphere) == 2.f);
  // miss it
  BOOST_TEST(overlapDistance(Ray{{-2, -2, -2}, {1, 0, 0}}, unit_sphere) == 0.f);
  BOOST_TEST(overlapDistance(Ray{{1, 2, 3}, {4, 5, 6}}, unit_sphere) == 0.f);
  // half-radius
  BOOST_TEST(overlapDistance(Ray{{-2, 0, 0.5}, {1, 0, 0}}, unit_sphere) ==
             sqrtf_3);
  BOOST_TEST(overlapDistance(Ray{{0, -0.5, 2}, {0, 0, -1}}, unit_sphere) ==
             sqrtf_3);
  // touch surface but no intersection
  BOOST_TEST(overlapDistance(Ray{{1, -2, 0}, {0, 1, 0}}, unit_sphere) == 0.f);
  BOOST_TEST(overlapDistance(Ray{{2, 2, -1}, {-1, -1, 0}}, unit_sphere) == 0.f);

  // behind the origin
  BOOST_TEST(overlapDistance(Ray{{-2, 0, 0}, {-1, 0, 0}}, unit_sphere) == 0.f);
  BOOST_TEST(overlapDistance(Ray{{0, 2, 2}, {0, 1, 1}}, unit_sphere) == 0.f);

  // origin inside
  BOOST_TEST(overlapDistance(Ray{{0.5, 0, 0}, {0, 1, 1}}, unit_sphere) ==
             half_sqrtf_3);
  BOOST_TEST(overlapDistance(Ray{{0, -half_sqrtf_3, 0}, {0, 0, 1}},
                             unit_sphere) == 0.5f);
  BOOST_TEST(overlapDistance(Ray{{0, 0, half_sqrtf_2}, {1, -1, 0}},
                             unit_sphere) == half_sqrtf_2);
  BOOST_TEST(overlapDistance(Ray{{0, 0.6, 0}, {0, 1, 0}}, unit_sphere) == 0.4f);
  BOOST_TEST(overlapDistance(Ray{{0, 0, 0}, {1, 2, 3}}, unit_sphere) == 1.f);

  // origin on surface
  //   tangent
  BOOST_TEST(overlapDistance(Ray{{0, 0, 1}, {1, -1, 0}}, unit_sphere) == 0.f);
  BOOST_TEST(overlapDistance(Ray{{0, 1, 0}, {2, 0, 3}}, unit_sphere) == 0.f);
  BOOST_TEST(
      overlapDistance(Ray{{half_sqrtf_2, half_sqrtf_2, 0}, {1, -1, 0}},
                      unit_sphere) == 0.f,
      boost::test_tools::tolerance(5e-4f)); // inside at machine precision...
  BOOST_TEST(overlapDistance(Ray{{-half_sqrtf_3, 0.5, 0}, {1, 2, 0}},
                             unit_sphere) == 0.f);
  BOOST_TEST(overlapDistance(Ray{{0.5, half_sqrtf_3, 0}, {2, -1, 0}},
                             unit_sphere) == 0.f);
  //   directed at sphere
  BOOST_TEST(overlapDistance(Ray{{1, 0, 0}, {-2, 0, 0}}, unit_sphere) == 2.f);
  BOOST_TEST(overlapDistance(Ray{{1, 0, 0}, {-2, 0, 0}}, unit_sphere) == 2.f);
  BOOST_TEST(overlapDistance(Ray{{0, -half_sqrtf_2, half_sqrtf_2}, {0, 1, -1}},
                             unit_sphere) == 2.f);
  BOOST_TEST(overlapDistance(Ray{{half_sqrtf_3, 0.5, 0}, {-1, 0, 0}},
                             unit_sphere) == sqrtf_3);
  //   directed away
  BOOST_TEST(overlapDistance(Ray{{0, 0, 1}, {1, 1, 1}}, unit_sphere) == 0.f);
  BOOST_TEST(overlapDistance(Ray{{half_sqrtf_3, 0.5, 0}, {1, 0, 0}},
                             unit_sphere) == 0.f);
}

#define ARBORX_TEST_RAY_SPHERE_INTERSECTION(ray, sphere, t0_ref, t1_ref)       \
  do                                                                           \
  {                                                                            \
    float t0;                                                                  \
    float t1;                                                                  \
    BOOST_TEST(ArborX::Experimental::intersection(ray, sphere, t0, t1));       \
    BOOST_TEST(t0 == t0_ref);                                                  \
    BOOST_TEST(t1 == t1_ref);                                                  \
  } while (false)

#define ARBORX_TEST_RAY_SPHERE_NO_INTERSECTION(ray, sphere)                    \
  do                                                                           \
  {                                                                            \
    float t0;                                                                  \
    float t1;                                                                  \
    constexpr auto inf = KokkosExt::ArithmeticTraits::infinity<float>::value;  \
    BOOST_TEST(!ArborX::Experimental::intersection(ray, sphere, t0, t1));      \
    BOOST_TEST((t0 == inf && t1 == -inf));                                     \
  } while (false)

BOOST_AUTO_TEST_CASE(ray_sphere_intersection,
                     *boost::unit_test::tolerance(1e-6f))
{
  using ArborX::Sphere;
  using ArborX::Experimental::Ray;

  constexpr Sphere unit_sphere{{0, 0, 0}, 1};

  auto const sqrtf_3 = std::sqrt(3.f);
  auto const sqrtf_2 = std::sqrt(2.f);

  // clang-format off
  // hit center of the sphere
  ARBORX_TEST_RAY_SPHERE_INTERSECTION((Ray{{-2, 0, 0}, {1, 0, 0}}), unit_sphere, 1.f, 3.f);
  ARBORX_TEST_RAY_SPHERE_INTERSECTION((Ray{{0, 3, 0}, {0, -1, 0}}), unit_sphere, 2.f, 4.f);
  ARBORX_TEST_RAY_SPHERE_INTERSECTION((Ray{{0, 0, -4}, {0, 0, 1}}), unit_sphere, 3.f, 5.f);
  ARBORX_TEST_RAY_SPHERE_INTERSECTION((Ray{{-2, -2, -2}, {1, 1, 1}}), unit_sphere, 2*sqrtf_3-1, 2*sqrtf_3+1);
  ARBORX_TEST_RAY_SPHERE_INTERSECTION((Ray{{1, 0, 0}, {1, 0, 0}}), unit_sphere, -2.f, 0.f);
  ARBORX_TEST_RAY_SPHERE_INTERSECTION((Ray{{0, 3, 0}, {0, 1, 0}}), unit_sphere, -4.f, -2.f);
  ARBORX_TEST_RAY_SPHERE_INTERSECTION((Ray{{0, 0, -4}, {0, 0, -1}}), unit_sphere, -5.f, -3.f);
  ARBORX_TEST_RAY_SPHERE_INTERSECTION((Ray{{2, 2, 2}, {1, 1, 1}}), unit_sphere, -2*sqrtf_3-1, -2*sqrtf_3+1);

  ARBORX_TEST_RAY_SPHERE_NO_INTERSECTION((Ray{{-2, -2, -2}, {1, 0, 0}}), unit_sphere);

  // half-radius
  ARBORX_TEST_RAY_SPHERE_INTERSECTION((Ray{{-2, 0, 0.5}, {1, 0, 0}}), unit_sphere, 2-sqrtf_3/2, 2+sqrtf_3/2);
  ARBORX_TEST_RAY_SPHERE_INTERSECTION((Ray{{0, -0.5, 2}, {0, 0, -1}}), unit_sphere, 2-sqrtf_3/2, 2+sqrtf_3/2);

 // touch surface
 ARBORX_TEST_RAY_SPHERE_INTERSECTION((Ray{{1, -2, 0}, {0, 1, 0}}), unit_sphere, 2.f, 2.f);
 // ARBORX_TEST_RAY_SPHERE_INTERSECTION((Ray{{2, 2, -1}, {-1, -1, 0}}), unit_sphere, 2*sqrtf_2, 2*sqrtf_2);  // machine precision

 // behind the origin
 ARBORX_TEST_RAY_SPHERE_INTERSECTION((Ray{{-2, 0, 0}, {-1, 0, 0}}), unit_sphere, -3.f, -1.f);
 ARBORX_TEST_RAY_SPHERE_INTERSECTION((Ray{{0, 2, 2}, {0, 1, 1}}), unit_sphere, -2*sqrtf_2-1, -2*sqrtf_2+1);
  // clang-format on
}

#undef ARBORX_TEST_RAY_SPHERE_INTERSECTION
#undef ARBORX_TEST_RAY_SPHERE_NO_INTERSECTION

#define STATIC_ASSERT(cond) static_assert(cond, "");
BOOST_AUTO_TEST_CASE(make_euclidean_vector)
{
  using ArborX::Experimental::makeVector;
  using ArborX::Experimental::Vector;
  STATIC_ASSERT((makeVector({0, 0, 0}, {1, 2, 3}) == Vector{1, 2, 3}));
  STATIC_ASSERT((makeVector({1, 2, 3}, {4, 5, 6}) == Vector{3, 3, 3}));
}

BOOST_AUTO_TEST_CASE(dot_product)
{
  using ArborX::Experimental::dotProduct;
  STATIC_ASSERT(dotProduct({1, 0, 0}, {1, 0, 0}) == 1);
  STATIC_ASSERT(dotProduct({1, 0, 0}, {0, 1, 0}) == 0);
  STATIC_ASSERT(dotProduct({1, 0, 0}, {0, 0, 1}) == 0);
  STATIC_ASSERT(dotProduct({1, 1, 1}, {1, 1, 1}) == 3);
}

BOOST_AUTO_TEST_CASE(cross_product)
{
  using ArborX::Experimental::crossProduct;
  using ArborX::Experimental::Vector;
  STATIC_ASSERT((crossProduct({1, 0, 0}, {1, 0, 0}) == Vector{0, 0, 0}));
  STATIC_ASSERT((crossProduct({1, 0, 0}, {0, 1, 0}) == Vector{0, 0, 1}));
  STATIC_ASSERT((crossProduct({1, 0, 0}, {0, 0, 1}) == Vector{0, -1, 0}));
  STATIC_ASSERT((crossProduct({0, 1, 0}, {1, 0, 0}) == Vector{0, 0, -1}));
  STATIC_ASSERT((crossProduct({0, 1, 0}, {0, 1, 0}) == Vector{0, 0, 0}));
  STATIC_ASSERT((crossProduct({0, 1, 0}, {0, 0, 1}) == Vector{1, 0, 0}));
  STATIC_ASSERT((crossProduct({1, 1, 1}, {1, 1, 1}) == Vector{0, 0, 0}));
}
#undef STATIC_ASSERT


// ========================================================================= //
// Filename      :  test_eigen_yaml.cpp
// Creation Date :  2016-6-21
// Created by    :  anfranek
// ========================================================================= //

#include <gtest/gtest.h>

#include <Eigen/Core>
#include <yaml-cpp/yaml.h>

#include "eigen_yaml.h"

/**
 * @brief TEST
 */
TEST(eigen_yaml, node)
{
    Eigen::Matrix2d mat;
    mat << 1, 2, 3, 6;

    YAML::Node node;
    node["mat"] = mat;
    Eigen::Matrix2d res;
    res = node["mat"].as<Eigen::Matrix2d>();

    EXPECT_EQ(mat, res);
}

// this test does not work as yaml-cpp does not guarantee any specific order in the emitted string
//TEST(eigen_yaml, emitter)
//{
//    Eigen::Matrix2d mat;
//    mat << 1, 2, 3, 6;

//    YAML::Node node;
//    node["mat"] = mat;

//    YAML::Emitter out;
//    out << node;

//    EXPECT_STREQ("mat:\n  rows: 2\n  cols: 2\n  data-type: double\n  data: [1, 2, 3, 6]", out.c_str());
//}

TEST(eigen_yaml, load)
{
    YAML::Node node = YAML::Load("mat:\n  rows: 2\n  cols: 2\n  data-type: double\n  data: [1, 2, 3, 6]");

    Eigen::Matrix2d mat;
    mat << 1, 2, 3, 6;

    EXPECT_EQ(mat, node["mat"].as<Eigen::Matrix2d>());
}

TEST(eigen_yaml, MatrixXd)
{
    YAML::Node node = YAML::Load("mat:\n  rows: 2\n  cols: 2\n  data-type: double\n  data: [1, 2, 3, 6]");

    Eigen::MatrixXd mat(2,2);
    mat << 1, 2, 3, 6;

    EXPECT_EQ(mat, node["mat"].as<Eigen::MatrixXd>());
}

TEST(eigen_yaml, Matrix2Xd)
{
    YAML::Node node = YAML::Load("mat:\n  rows: 2\n  cols: 2\n  data-type: double\n  data: [1, 2, 3, 6]");

    Eigen::Matrix2Xd mat(2,2);
    mat << 1, 2, 3, 6;

    EXPECT_EQ(mat, node["mat"].as<Eigen::Matrix2Xd>());
}

TEST(eigen_yaml, MatrixX2d)
{
    YAML::Node node = YAML::Load("mat:\n  rows: 2\n  cols: 2\n  data-type: double\n  data: [1, 2, 3, 6]");

    Eigen::MatrixX2d mat(2,2);
    mat << 1, 2, 3, 6;

    EXPECT_EQ(mat, node["mat"].as<Eigen::MatrixX2d>());
}


#include <iostream>
#include <unordered_map>
#include <cstdlib>

#include "hpyplm.h"
#include "corpus/corpus.h"

#include "cpyp/boost_serializers.h"
#include <boost/serialization/vector.hpp>
#include <boost/archive/binary_iarchive.hpp>

#define kORDER 3

using namespace std;
using namespace cpyp;

int main(int argc, char** argv) {
  if (argc != 3) {
    cerr << argv[0] << " <input.lm> <test.txt>\n\nCompute perplexity of a " << kORDER << "-gram HPYP LM\n";
    return 1;
  }
  MT19937 eng;
  string lm_file = argv[1];
  string test_file = argv[2];

  PYPLM<kORDER> lm;
  //vector<unsigned> ctx(kORDER - 1, kSOS);

  cerr << "Reading LM from " << lm_file << " ...\n";
  ifstream ifile(lm_file.c_str(), ios::in | ios::binary);
  if (!ifile.good()) {
    cerr << "Failed to open " << lm_file << " for reading\n";
    return 1;
  }
  boost::archive::binary_iarchive ia(ifile);
  Dict dict;
  ia & dict;
  ia & lm;
  const unsigned max_iv = dict.max();
  const unsigned kSOS = dict.Convert("<s>");
  const unsigned kEOS = dict.Convert("</s>");
  set<unsigned> tv;
  vector<vector<unsigned> > test;
  ReadFromFile(test_file, &dict, &test, &tv);
  double llh = 0;
  unsigned cnt = 0;
  unsigned oovs = 0;
  vector<unsigned> ctx(kORDER - 1, kSOS);
  for (auto& s : test) {
    ctx.resize(kORDER - 1);
    for (unsigned i = 0; i <= s.size(); ++i) {
      unsigned w = (i < s.size() ? s[i] : kEOS);
      double lp = log(lm.prob(w, ctx)) / log(2);
      if (w >= max_iv) {
        cerr << "**OOV ";
        ++oovs;
        lp = 0;
      }
      cerr << "p(" << dict.Convert(w) << " |";
      for (unsigned j = ctx.size() + 1 - kORDER; j < ctx.size(); ++j)
        cerr << ' ' << dict.Convert(ctx[j]);
      cerr << ") = " << lp << endl;
      ctx.push_back(w);
      llh -= lp;
      cnt++;
    }
  }
  cnt -= oovs;
  cerr << "  Log_10 prob: " << (-llh * log(2) / log(10)) << endl;
  cerr << "        Count: " << cnt << endl;
  cerr << "         OOVs: " << oovs << endl;
  cerr << "Cross-entropy: " << (llh / cnt) << endl;
  cerr << "   Perplexity: " << pow(2, llh / cnt) << endl;
  return 0;
}


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

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

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

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

#include <geometry_test_common.hpp>

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

#include <boost/geometry/strategies/cartesian/cart_intersect.hpp>
#include <boost/geometry/strategies/intersection_result.hpp>

#include <boost/geometry/policies/relate/intersection_points.hpp>
#include <boost/geometry/policies/relate/direction.hpp>
#include <boost/geometry/policies/relate/de9im.hpp>
#include <boost/geometry/policies/relate/tupled.hpp>

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

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

template <typename IntersectionPoints>
static int check(IntersectionPoints const& is,
                std::size_t index, double expected_x, double expected_y)
{
    if (expected_x != -99 && expected_y != -99 && is.count > index)
    {
        double x = bg::get<0>(is.intersections[index]);
        double y = bg::get<1>(is.intersections[index]);

        BOOST_CHECK_CLOSE(x, expected_x, 0.001);
        BOOST_CHECK_CLOSE(y, expected_y, 0.001);
        return 1;
    }
    return 0;
}

template <typename P>
static void test_segment_intersection(std::string const& case_id,
                int x1, int y1, int x2, int y2,
                int x3, int y3, int x4, int y4,
                char expected_how, bool expected_opposite,
                int expected_arrival1, int expected_arrival2,
                int expected_x1, int expected_y1,
                int expected_x2 = -99, int expected_y2 = -99)

{
    typedef typename bg::coordinate_type<P>::type coordinate_type;
    typedef bg::model::referring_segment<const P> segment_type;

    P p1, p2, p3, p4;
    bg::assign_values(p1, x1, y1);
    bg::assign_values(p2, x2, y2);
    bg::assign_values(p3, x3, y3);
    bg::assign_values(p4, x4, y4);

    segment_type s12(p1,p2);
    segment_type s34(p3,p4);

    // Get the intersection point (or two points)
    bg::segment_intersection_points<P> is
        = bg::strategy::intersection::relate_cartesian_segments
        <
            bg::policies::relate::segments_intersection_points
                <
                    segment_type,
                    segment_type,
                    bg::segment_intersection_points<P>
                >
        >::apply(s12, s34);

    // Get just a character for Left/Right/intersects/etc, purpose is more for debugging
    bg::policies::relate::direction_type dir
        = bg::strategy::intersection::relate_cartesian_segments
        <
            bg::policies::relate::segments_direction
                <
                    segment_type,
                    segment_type
                >
        >::apply(s12, s34);

    int expected_count =
        check(is, 0, expected_x1, expected_y1)
        + check(is, 1, expected_x2, expected_y2);

    BOOST_CHECK_EQUAL(is.count, expected_count);
    BOOST_CHECK_EQUAL(dir.how, expected_how);
    BOOST_CHECK_EQUAL(dir.opposite, expected_opposite);
    BOOST_CHECK_EQUAL(dir.arrival[0], expected_arrival1);
    BOOST_CHECK_EQUAL(dir.arrival[1], expected_arrival2);
}

template <typename P>
void test_all()
{
    // Collinear - non opposite

    //       a1---------->a2
    // b1--->b2
    test_segment_intersection<P>("n1",
        2, 0, 6, 0,
        0, 0, 2, 0,
        'a', false, -1, 0,
        2, 0);

    //       a1---------->a2
    //    b1--->b2
    test_segment_intersection<P>("n2",
        2, 0, 6, 0,
        1, 0, 3, 0,
        'c', false, -1, 1,
        2, 0, 3, 0);

    //       a1---------->a2
    //       b1--->b2
    test_segment_intersection<P>("n3",
        2, 0, 6, 0,
        2, 0, 4, 0,
        'c', false, -1, 1,
        2, 0, 4, 0);

    //       a1---------->a2
    //          b1--->b2
    test_segment_intersection<P>("n4",
        2, 0, 6, 0,
        3, 0, 5, 0,
        'c', false, -1, 1,
        3, 0, 5, 0);

    //       a1---------->a2
    //              b1--->b2
    test_segment_intersection<P>("n5",
        2, 0, 6, 0,
        4, 0, 6, 0,
        'c', false, 0, 0,
        4, 0, 6, 0);

    //       a1---------->a2
    //                 b1--->b2
    test_segment_intersection<P>("n6",
        2, 0, 6, 0,
        5, 0, 7, 0,
        'c', false, 1, -1,
        5, 0, 6, 0);

    //       a1---------->a2
    //                    b1--->b2
    test_segment_intersection<P>("n7",
        2, 0, 6, 0,
        6, 0, 8, 0,
        'a', false, 0, -1,
        6, 0);

    // Collinear - opposite
    //       a1---------->a2
    // b2<---b1
    test_segment_intersection<P>("o1",
        2, 0, 6, 0,
        2, 0, 0, 0,
        'f', true, -1, -1,
        2, 0);

    //       a1---------->a2
    //    b2<---b1
    test_segment_intersection<P>("o2",
        2, 0, 6, 0,
        3, 0, 1, 0,
        'c', true, -1, -1,
        2, 0, 3, 0);

    //       a1---------->a2
    //       b2<---b1
    test_segment_intersection<P>("o3",
        2, 0, 6, 0,
        4, 0, 2, 0,
        'c', true, -1, 0,
        2, 0, 4, 0);

    //       a1---------->a2
    //           b2<---b1
    test_segment_intersection<P>("o4",
        2, 0, 6, 0,
        5, 0, 3, 0,
        'c', true, -1, 1,
        3, 0, 5, 0);

    //       a1---------->a2
    //              b2<---b1
    test_segment_intersection<P>("o5",
        2, 0, 6, 0,
        6, 0, 4, 0,
        'c', true, 0, 1,
        4, 0, 6, 0);

    //       a1---------->a2
    //                b2<---b1
    test_segment_intersection<P>("o6",
        2, 0, 6, 0,
        7, 0, 5, 0,
        'c', true, 1, 1,
        5, 0, 6, 0);

    //       a1---------->a2
    //                    b2<---b1
    test_segment_intersection<P>("o7",
        2, 0, 6, 0,
        8, 0, 6, 0,
        't', true, 0, 0,
        6, 0);

    //   a1---------->a2
    //   b1---------->b2
    test_segment_intersection<P>("e1",
        2, 0, 6, 0,
        2, 0, 6, 0,
        'e', false, 0, 0,
        2, 0, 6, 0);

    //   a1---------->a2
    //   b2<----------b1
    test_segment_intersection<P>("e1",
        2, 0, 6, 0,
        6, 0, 2, 0,
        'e', true, 0, 0,
        2, 0, 6, 0);

    //   a1---------->a2
    //   b2<----------b1
    test_segment_intersection<P>("case_recursive_boxes_1",
        10, 7, 10, 6,
        10, 10, 10, 9,
        'd', false, 0, 0,
        -1, -1, -1, -1);

}

int test_main(int, char* [])
{
    test_all<bg::model::point<double, 2, bg::cs::cartesian> >();
    return 0;
}