#include "catch.hpp"

#include <boost/filesystem.hpp>

#include <cpp-utils/adjacentGraph.hpp>
#include <cpp-utils/functional.hpp>
#include <pathfinding-utils/types.hpp>

#include <pathfinding-utils/GridMap.hpp>
#include <pathfinding-utils/MovingAIGridMapReader.hpp>
#include <pathfinding-utils/GridMapGraphConverter.hpp>

#include <compressed-path-database/CpdManager.hpp>

#include "CpdHeuristic.hpp"
#include "CpdSearch.hpp"
#include "CpdHeuristic.hpp"
#include "CpdFocalSearch.hpp"
#include "CountCpdSearchListener.hpp"

using namespace pathfinding::search;
using namespace pathfinding::search::listeners;
using namespace pathfinding::maps;

using namespace cpp_utils::graphs;
using namespace compressed_path_database;

using State = GraphState<std::string,  xyLoc, cost_t, cpd_search_generated_e>;
using StatePerturbated = GraphState<std::string,  xyLoc, PerturbatedCost, cpd_search_generated_e>;

SCENARIO("test CpdHeuristic") {

    GIVEN("a gridmap") {

        MovingAIGridMapReader reader{
            '.', 100, color_t::WHITE,
            'T', 150, color_t::GREEN,
            '@', cost_t::INFTY, color_t::BLACK
        };
        GridMap gridMap = reader.load(boost::filesystem::path{"square03.map"});
        GridMapGraphConverter converter{GridBranching::EIGHT_CONNECTED};
        AdjacentGraph<std::string, xyLoc, cost_t> graph{*converter.toGraph(gridMap)};

        /*
         *  01234
         * 0.....
         * 1...@@
         * 2..@@.
         * 3...@@
         * 4.....
         *
         */

        CpdManager<std::string, xyLoc> cpdManager{boost::filesystem::path{"./square03.cpd"}, graph};
        const IImmutableGraph<std::string, xyLoc, cost_t>& reorderedGraph = cpdManager.getReorderedGraph();
        // IT'S REALLY IMPORTANT THAT WE QUERY NOT ON "graph" BUT ON "reorderedGraph"!!!!!
        // THIS because the CPD uses those indices, not the ones from "graph"!!!
        std::function<PerturbatedCost(const cost_t&)> costFunction = [&](const cost_t& c) { return PerturbatedCost{c, false};};
        std::unique_ptr<IImmutableGraph<std::string, xyLoc, PerturbatedCost>> perturbatedGraph = std::unique_ptr<IImmutableGraph<std::string, xyLoc, PerturbatedCost>>(reorderedGraph.mapEdges(costFunction));
        CpdHeuristic<State, std::string, xyLoc> h{
            cpdManager,
            *perturbatedGraph
        };

        REQUIRE(h.isAdmissible());
        REQUIRE(h.isConsistent());

        WHEN("start=goal") {
            xyLoc startLoc{0,0};
            xyLoc goalLoc{0,0};
            State start{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc), cpd_search_generated_e::INPUT};
            State goal{0, reorderedGraph, reorderedGraph.idOfVertex(goalLoc), cpd_search_generated_e::INPUT};
            REQUIRE(h.getHeuristic(start, &goal) == 0);
        }

        WHEN("start above goal") {
            xyLoc startLoc{0,0};
            xyLoc goalLoc{0,1};
            State start{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc), cpd_search_generated_e::INPUT};
            State goal{0, reorderedGraph, reorderedGraph.idOfVertex(goalLoc), cpd_search_generated_e::INPUT};
            REQUIRE(h.getHeuristic(start, &goal) == 100);
            REQUIRE(h.getCachedElementsNumber() == 2);
        }

        WHEN("start diagonal goal") {
            xyLoc startLoc{0,0};
            xyLoc goalLoc{1,1};
            State start{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc), cpd_search_generated_e::INPUT};
            State goal{0, reorderedGraph, reorderedGraph.idOfVertex(goalLoc), cpd_search_generated_e::INPUT};
            REQUIRE(h.getHeuristic(start, &goal) == 141);
            REQUIRE(h.getCachedElementsNumber() == 2);
        }

        WHEN("start distance from goal of 2") {
            xyLoc startLoc{0,0};
            xyLoc goalLoc{2,0};
            State start{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc), cpd_search_generated_e::INPUT};
            State goal{0, reorderedGraph, reorderedGraph.idOfVertex(goalLoc), cpd_search_generated_e::INPUT};
            REQUIRE(h.getHeuristic(start, &goal) == 200);
            REQUIRE(h.getCachedElementsNumber() == 3);
        }

        WHEN("start diangonal distance from goal of 2") {
            xyLoc startLoc{0,2};
            xyLoc goalLoc{2,4};
            State start{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc), cpd_search_generated_e::INPUT};
            State goal{0, reorderedGraph, reorderedGraph.idOfVertex(goalLoc), cpd_search_generated_e::INPUT};
            REQUIRE(h.getHeuristic(start, &goal) == 282);
            REQUIRE(h.getCachedElementsNumber() == 3);
        }

        WHEN("start far from goal") {
            xyLoc startLoc{0,0};
            xyLoc goalLoc{4,4};
            State start{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc), cpd_search_generated_e::INPUT};
            State goal{0, reorderedGraph, reorderedGraph.idOfVertex(goalLoc), cpd_search_generated_e::INPUT};
            REQUIRE(h.getHeuristic(start, &goal) == (100*4 + 141*2));
            REQUIRE(h.getCachedElementsNumber() == 7);
        }

        WHEN("reusing heuristic and paths are not overlapping (except goal") {
            xyLoc startLoc1{0,0};
            xyLoc startLoc2{4,0};
            xyLoc goalLoc{2,0};
            State start1{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc1), cpd_search_generated_e::INPUT};
            State start2{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc2), cpd_search_generated_e::INPUT};
            State goal{0, reorderedGraph, reorderedGraph.idOfVertex(goalLoc), cpd_search_generated_e::INPUT};
            REQUIRE(h.getHeuristic(start1, &goal) == (100*2));
            REQUIRE(h.getHeuristic(start2, &goal) == (100*2));

            REQUIRE(h.getCachedElementsNumber() == 5);
        }

        //TODO coniotnue
        WHEN("reusing heuristic and paths are overlapping in the middle") {
            xyLoc startLoc1{0,0};
            xyLoc startLoc2{2,1};
            xyLoc goalLoc{4,0};
            State start1{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc1), cpd_search_generated_e::INPUT};
            State start2{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc2), cpd_search_generated_e::INPUT};
            State goal{0, reorderedGraph, reorderedGraph.idOfVertex(goalLoc), cpd_search_generated_e::INPUT};
            REQUIRE(h.getHeuristic(start1, &goal) == (100*4));
            REQUIRE(h.getHeuristic(start2, &goal) == (100*3));
            REQUIRE(h.getCachedElementsNumber() == 6);
        }

        WHEN("reusing heuristic and one path is subset of the other") {
            xyLoc startLoc1{0,0};
            xyLoc startLoc2{2,0};
            xyLoc goalLoc{4,0};
            State start1{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc1), cpd_search_generated_e::INPUT};
            State start2{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc2), cpd_search_generated_e::INPUT};
            State goal{0, reorderedGraph, reorderedGraph.idOfVertex(goalLoc), cpd_search_generated_e::INPUT};
            REQUIRE(h.getHeuristic(start1, &goal) == (100*4));
            REQUIRE(h.getHeuristic(start2, &goal) == (100*2));
            REQUIRE(h.getCachedElementsNumber() == 5);
        }

        WHEN("performing cleanup") {
            xyLoc startLoc{0,0};
            xyLoc goalLoc{4,4};
            State start{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc), cpd_search_generated_e::INPUT};
            State goal{0, reorderedGraph, reorderedGraph.idOfVertex(goalLoc), cpd_search_generated_e::INPUT};
            h.getHeuristic(start, &goal);

            REQUIRE(h.getCachedElementsNumber() == 7);
            h.cleanup();
            REQUIRE(h.getCachedElementsNumber() == 0);
        }

        WHEN("goal unreachable") {
            xyLoc startLoc{0,0};
            xyLoc goalLoc{4,2};
            State start{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc), cpd_search_generated_e::INPUT};
            State goal{0, reorderedGraph, reorderedGraph.idOfVertex(goalLoc), cpd_search_generated_e::INPUT};
            REQUIRE(h.getHeuristic(start, &goal) == cost_t::INFTY);
            REQUIRE(h.getCachedElementsNumber() == 0);
        }
    }

}

SCENARIO("test GraphState supporting concepts") {

    GIVEN("a gridmap") {

        MovingAIGridMapReader reader{
            '.', 100, color_t::WHITE,
            'T', 150, color_t::GREEN,
            '@', cost_t::INFTY, color_t::BLACK
        };
        GridMap gridMap = reader.load("square03.map");
        GridMapGraphConverter converter{GridBranching::EIGHT_CONNECTED};
        std::unique_ptr<IImmutableGraph<std::string, xyLoc, cost_t>> graph = converter.toGraph(gridMap);

        /*
         *  01234
         * 0.....
         * 1...@@
         * 2..@@.
         * 3...@@
         * 4.....
         *
         */

        GraphStateSupplier<std::string, xyLoc, cost_t, cpd_search_generated_e> supplier{*graph};

        WHEN("test GraphStateSupplier") {

            THEN("state not yet generated") {
                State& s = supplier.getState(graph->idOfVertex(xyLoc{1,3}), cpd_search_generated_e::INPUT);
                REQUIRE(s.getPosition() == graph->idOfVertex(xyLoc{1,3}));
                REQUIRE(s.getId() == graph->idOfVertex(xyLoc{1,3}));
            }

            THEN("state already generated") {
                auto& s = supplier.getState(graph->idOfVertex(xyLoc{1,3}), cpd_search_generated_e::INPUT);
                auto& s2 = supplier.getState(graph->idOfVertex(xyLoc{1,3}),cpd_search_generated_e::INPUT);
                REQUIRE(s.getPosition() == graph->idOfVertex(xyLoc{1,3}));
                REQUIRE(s.getId() == graph->idOfVertex(xyLoc{1,3}));
                REQUIRE(s2.getPosition() == graph->idOfVertex(xyLoc{1,3}));
                REQUIRE(s2.getId() == graph->idOfVertex(xyLoc{1,3}));
                REQUIRE(&s == &s2);
            }
        }

        cpp_utils::function_t<cost_t, cost_t> costFunction = [&](auto c) { return c;};

        StandardStateExpander<State, std::string, xyLoc, cost_t, cpd_search_generated_e> expander{*graph, costFunction};

        WHEN("test GraphStateExpander") {
            State s = supplier.getState(graph->idOfVertex(xyLoc{1,3}), cpd_search_generated_e::INPUT);
            cpp_utils::vectorplus<std::pair<State&, cost_t>> successors = expander.getSuccessors(s, supplier);
            info("successors: ", successors);
            REQUIRE(successors.size() == 7);

            IImmutableGraph<std::string, xyLoc, cost_t>* originalGraph = graph.get();

            //moves
            REQUIRE(successors
                .select([&, originalGraph](std::pair<State, cost_t> p) {
                    return originalGraph->getVertex(p.first.getPosition()) == xyLoc{2,3};
                }).size() == 1);
            REQUIRE(successors
                .select([&, originalGraph](std::pair<State, cost_t> p) {
                    return originalGraph->getVertex(p.first.getPosition()) == xyLoc{0,3};
                }).size() == 1);
            REQUIRE(successors
                .select([&, originalGraph](std::pair<State, cost_t> p) {
                    return originalGraph->getVertex(p.first.getPosition()) == xyLoc{1,2};
                }).size() == 1);
            REQUIRE(successors
                .select([&, originalGraph](std::pair<State, cost_t> p) {
                    return originalGraph->getVertex(p.first.getPosition()) == xyLoc{1,4};
                }).size() == 1);

            REQUIRE(successors
                .select([&, originalGraph](std::pair<State, cost_t> p) {
                    return originalGraph->getVertex(p.first.getPosition()) == xyLoc{0,2};
                }).size() == 1);
            REQUIRE(successors
                .select([&, originalGraph](std::pair<State, cost_t> p) {
                    return originalGraph->getVertex(p.first.getPosition()) == xyLoc{0,4};
                }).size() == 1);
            REQUIRE(successors
                .select([&, originalGraph](std::pair<State, cost_t> p) {
                    return originalGraph->getVertex(p.first.getPosition()) == xyLoc{2,4};
                }).size() == 1);
        }

        WHEN("testing GraphStateGoalChecker") {
            StandardLocationGoalChecker<State> goalChecker{};

            auto n1 = supplier.getState(graph->idOfVertex(xyLoc{1,3}));
            auto n2 = supplier.getState(graph->idOfVertex(xyLoc{2,3}));
            auto n3 = supplier.getState(graph->idOfVertex(xyLoc{4,4}));
            auto n4 = supplier.getState(graph->idOfVertex(xyLoc{4,4}));
            info("werty2");
            auto goal = supplier.getState(graph->idOfVertex(xyLoc{4,4}));
            info("werty1");

            //location and time do not match
            info("n1");
            REQUIRE_FALSE(goalChecker.isGoal(n1, &goal));

            //timestamp match
            info("n2");
            REQUIRE_FALSE(goalChecker.isGoal(n2, &goal));

            //location match
            info("n3");
            REQUIRE(goalChecker.isGoal(n3, &goal));

            //everything match
            info("n4");
            REQUIRE(goalChecker.isGoal(n4, &goal));
        }

    }

}

SCENARIO("test CpdSearch with optimality bound") {

    GIVEN("a gridmap") {

        // CREATE GRAPH WHERE WE WANT TO OPERATE

        MovingAIGridMapReader reader{
            '.', 100, color_t::WHITE,
            'T', 150, color_t::GREEN,
            '@', cost_t::INFTY, color_t::BLACK
        };
        GridMap gridMap = reader.load(boost::filesystem::path{"square03.map"});
        GridMapGraphConverter converter{GridBranching::EIGHT_CONNECTED};
        AdjacentGraph<std::string, xyLoc, cost_t> graph{*converter.toGraph(gridMap)};

        /*
         *  01234
         * 0.XX..
         * 1...@@
         * 2..@@.
         * 3...@@
         * 4..XX.
         *
         */

        // INCLUDE THE CPD

        CpdManager<std::string, xyLoc> cpdManager{boost::filesystem::path{"./square03.cpd"}, graph};
        const IImmutableGraph<std::string, xyLoc, cost_t>& g = cpdManager.getReorderedGraph();

        // CREATE THE TIME GRAPH WITH PERTURBATIONS

        auto perturbatedGraphTmp = std::unique_ptr<IImmutableGraph<std::string, xyLoc, PerturbatedCost>>(cpdManager.getReorderedGraph().mapEdges<PerturbatedCost>([&](cost_t c) { return PerturbatedCost{c, false}; }));
        AdjacentGraph<std::string, xyLoc, PerturbatedCost> perturbatedGraph{*perturbatedGraphTmp};

        perturbatedGraph.changeWeightUndirectedEdge(perturbatedGraph.idOfVertex(xyLoc{1, 0}), perturbatedGraph.idOfVertex(xyLoc{2, 0}), PerturbatedCost{150, true});
        perturbatedGraph.changeWeightUndirectedEdge(perturbatedGraph.idOfVertex(xyLoc{2, 4}), perturbatedGraph.idOfVertex(xyLoc{3, 4}), PerturbatedCost{200, true});

        // USE THE FACTORY TO PROVIDE CpdSearch

        CpdSearchFactory factory{};
        auto factory_output = factory.get(cpdManager, perturbatedGraph, 1);
        CountCpdSearchListener<std::string, xyLoc, StatePerturbated> listener{};
        factory_output->search.setListener(listener);

        REQUIRE(g.haveSameVertices(perturbatedGraph));

        WHEN("start == goal") {
            xyLoc startLoc{0,0};
            xyLoc goalLoc{0,0};
            nodeid_t startId = g.idOfVertex(startLoc);
            nodeid_t goalId = g.idOfVertex(goalLoc);
            auto& start = factory_output->stateSupplier.getState(startId);
            auto& goal = factory_output->stateSupplier.getState(goalId);
            auto solution = factory_output->search.search(start, goal, false, false);
            REQUIRE(
                solution->map<std::tuple<xyLoc>>([&](auto x) {
                    return x.getPayload();
                }) == vectorplus<std::tuple<xyLoc>>::make(std::make_tuple(xyLoc{0,0})
            ));
            REQUIRE(solution->getCost() == 0);
            REQUIRE(listener.getNodeExpanded() == 0);
        }

        WHEN("start is diagonal goal (no perturbations)") {
            xyLoc startLoc{0,0};
            xyLoc goalLoc{1,1};
            nodeid_t startId = g.idOfVertex(startLoc);
            nodeid_t goalId = g.idOfVertex(goalLoc);
            auto& start = factory_output->stateSupplier.getState(startId);
            auto& goal = factory_output->stateSupplier.getState(goalId);
            auto solution = factory_output->search.search(start, goal, false, true);
            REQUIRE(
                solution->map<std::tuple<xyLoc>>([&](auto& x) {
                    return x.getPayload();
                }) == vectorplus<std::tuple<xyLoc>>::make(std::make_tuple(xyLoc{0,0}), std::make_tuple(xyLoc{1,1})
            ));
            REQUIRE(solution->getCost() == 141);
            REQUIRE(listener.getNodeExpanded() == 1);
        }

        WHEN("start is above goal (no perturbations)") {
            xyLoc startLoc{0,0};
            xyLoc goalLoc{0,1};
            nodeid_t startId = g.idOfVertex(startLoc);
            nodeid_t goalId = g.idOfVertex(goalLoc);
            auto& start = factory_output->stateSupplier.getState(startId);
            auto& goal = factory_output->stateSupplier.getState(goalId);
            auto solution = factory_output->search.search(start, goal, false, true);
            REQUIRE(
                solution->map<std::tuple<xyLoc>>([&](auto& x) {
                    return x.getPayload();
                }) == vectorplus<std::tuple<xyLoc>>::make(std::make_tuple(xyLoc{0,0}), std::make_tuple(xyLoc{0,1})
            ));
            REQUIRE(solution->getCost() == 100);
            REQUIRE(listener.getNodeExpanded() == 1);
        }

        WHEN("start to goal via perturbated edge") {
            xyLoc startLoc{0,0};
            xyLoc goalLoc{4,0};
            nodeid_t startId = g.idOfVertex(startLoc);
            nodeid_t goalId = g.idOfVertex(goalLoc);
            auto& start = factory_output->stateSupplier.getState(startId);
            auto& goal = factory_output->stateSupplier.getState(goalId);
            auto solution = factory_output->search.search(start, goal, false, true);
            REQUIRE(
                solution->map<std::tuple<xyLoc>>([&](auto& x) {
                    return x.getPayload();
                }) == vectorplus<std::tuple<xyLoc>>::make(
                    std::make_tuple(xyLoc{0,0}),
                    std::make_tuple(xyLoc{1,0}),
                    std::make_tuple(xyLoc{2,0}),
                    std::make_tuple(xyLoc{3,0}),
                    std::make_tuple(xyLoc{4,0})
            ));
            REQUIRE(solution->getCost() == 450);
        }

        WHEN("early termination (no perturbation)") {
            xyLoc startLoc{0,0};
            xyLoc goalLoc{0,4};
            nodeid_t startId = g.idOfVertex(startLoc);
            nodeid_t goalId = g.idOfVertex(goalLoc);
            auto& start = factory_output->stateSupplier.getState(startId);
            auto& goal = factory_output->stateSupplier.getState(goalId);
            auto solution = factory_output->search.search(start, goal, false, true);
            REQUIRE(
                solution->map<std::tuple<xyLoc>>([&](auto& x) {
                    return x.getPayload();
                }) == vectorplus<std::tuple<xyLoc>>::make(
                    std::make_tuple(xyLoc{0,0}),
                    std::make_tuple(xyLoc{0,1}),
                    std::make_tuple(xyLoc{0,2}),
                    std::make_tuple(xyLoc{0,3}),
                    std::make_tuple(xyLoc{0,4})
            ));
            REQUIRE(solution->getCost() == 400);
        }

        WHEN("early termination (perturbation near the end)") {
            xyLoc startLoc{0,0};
            xyLoc goalLoc{4,4};
            nodeid_t startId = g.idOfVertex(startLoc);
            nodeid_t goalId = g.idOfVertex(goalLoc);
            auto& start = factory_output->stateSupplier.getState(startId);
            auto& goal = factory_output->stateSupplier.getState(goalId);
            auto solution = factory_output->search.search(start, goal, false, true);
            REQUIRE(
                solution->map<std::tuple<xyLoc>>([&](auto& x) {
                    return x.getPayload();
                }) == vectorplus<std::tuple<xyLoc>>::make(
                    std::make_tuple(xyLoc{0,0}),
                    std::make_tuple(xyLoc{0,1}),
                    std::make_tuple(xyLoc{0,2}),
                    std::make_tuple(xyLoc{1,3}),
                    std::make_tuple(xyLoc{2,4}),
                    std::make_tuple(xyLoc{3,4}),
                    std::make_tuple(xyLoc{4,4})
            ));
            REQUIRE(solution->getCost() == (2*141 + 3*100 + 1*200));
        }

        WHEN("early termination (perturbation near the start)") {
            xyLoc startLoc{4,4};
            xyLoc goalLoc{0,0};
            nodeid_t startId = g.idOfVertex(startLoc);
            nodeid_t goalId = g.idOfVertex(goalLoc);
            auto& start = factory_output->stateSupplier.getState(startId);
            auto& goal = factory_output->stateSupplier.getState(goalId);
            auto solution = factory_output->search.search(start, goal, false, true);
            REQUIRE(
                solution->map<std::tuple<xyLoc>>([&](auto& x) {
                    return x.getPayload();
                }) == vectorplus<std::tuple<xyLoc>>::make(
                    std::make_tuple(xyLoc{4,4}),
                    std::make_tuple(xyLoc{3,4}),
                    std::make_tuple(xyLoc{2,4}),
                    std::make_tuple(xyLoc{1,3}),
                    std::make_tuple(xyLoc{0,2}),
                    std::make_tuple(xyLoc{0,1}),
                    std::make_tuple(xyLoc{0,0})
            ));
            REQUIRE(solution->getCost() == (2*141 + 3*100 + 1*200));
        }

        delete factory_output;
    }
}

SCENARIO("test CpdSearch for suboptimality solutions") {

    GIVEN("a gridmap") {

        // CREATE GRAPH WHERE WE WANT TO OPERATE

        MovingAIGridMapReader reader{
            '.', 100, color_t::WHITE,
            'T', 150, color_t::GREEN,
            '@', cost_t::INFTY, color_t::BLACK
        };
        GridMap gridMap = reader.load(boost::filesystem::path{"square03.map"});
        GridMapGraphConverter converter{GridBranching::EIGHT_CONNECTED};
        AdjacentGraph<std::string, xyLoc, cost_t> graph{*converter.toGraph(gridMap)};

        /*
         *  01234
         * 0.XX..
         * 1...@@
         * 2..@@.
         * 3...@@
         * 4..XX.
         *
         */

        // INCLUDE THE CPD

        CpdManager<std::string, xyLoc> cpdManager{boost::filesystem::path{"./square03.cpd"}, graph};
        const IImmutableGraph<std::string, xyLoc, cost_t>& g = cpdManager.getReorderedGraph();

        // CREATE THE TIME GRAPH WITH PERTURBATIONS

        auto perturbatedGraphTmp = std::unique_ptr<IImmutableGraph<std::string, xyLoc, PerturbatedCost>>(cpdManager.getReorderedGraph().mapEdges<PerturbatedCost>([&](cost_t c) { return PerturbatedCost{c, false}; }));
        AdjacentGraph<std::string, xyLoc, PerturbatedCost> perturbatedGraph{*perturbatedGraphTmp};

        perturbatedGraph.changeWeightUndirectedEdge(perturbatedGraph.idOfVertex(xyLoc{1, 0}), perturbatedGraph.idOfVertex(xyLoc{2, 0}), PerturbatedCost{500, true});
        perturbatedGraph.changeWeightUndirectedEdge(perturbatedGraph.idOfVertex(xyLoc{1, 1}), perturbatedGraph.idOfVertex(xyLoc{2, 0}), PerturbatedCost{500, true});
        perturbatedGraph.changeWeightUndirectedEdge(perturbatedGraph.idOfVertex(xyLoc{2, 4}), perturbatedGraph.idOfVertex(xyLoc{3, 4}), PerturbatedCost{200, true});

        // USE THE FACTORY TO PROVIDE TimeCpdSearch

        CpdSearchFactory factory{};
        auto factory_output = factory.get(cpdManager, perturbatedGraph, 5);

        REQUIRE(g.haveSameVertices(perturbatedGraph));

        WHEN("start == goal") {
            xyLoc startLoc{0,0};
            xyLoc goalLoc{0,0};
            nodeid_t startId = g.idOfVertex(startLoc);
            nodeid_t goalId = g.idOfVertex(goalLoc);
            auto start = factory_output->stateSupplier.getState(startId);
            auto goal = factory_output->stateSupplier.getState(goalId);
            auto solution = factory_output->search.search(start, goal, false, false);
            REQUIRE(
                solution->map<std::tuple<xyLoc>>([&](auto x) {
                    return x.getPayload();
                }) == vectorplus<std::tuple<xyLoc>>::make(std::make_tuple(xyLoc{0,0})
            ));
            REQUIRE(solution->getCost() == 0);
        }

        WHEN("start is diagonal goal (no perturbations)") {
            xyLoc startLoc{0,0};
            xyLoc goalLoc{1,1};
            nodeid_t startId = g.idOfVertex(startLoc);
            nodeid_t goalId = g.idOfVertex(goalLoc);
            auto start = factory_output->stateSupplier.getState(startId);
            auto goal = factory_output->stateSupplier.getState(goalId);
            auto solution = factory_output->search.search(start, goal, false, true);
            REQUIRE(
                solution->map<std::tuple<xyLoc>>([&](auto x) {
                    return x.getPayload();
                }) == vectorplus<std::tuple<xyLoc>>::make(std::make_tuple(xyLoc{0,0}), std::make_tuple(xyLoc{1,1})
            ));
            REQUIRE(solution->getCost() == 141);
        }

        WHEN("start is above goal (no perturbations)") {
            xyLoc startLoc{0,0};
            xyLoc goalLoc{0,1};
            nodeid_t startId = g.idOfVertex(startLoc);
            nodeid_t goalId = g.idOfVertex(goalLoc);
            auto start = factory_output->stateSupplier.getState(startId);
            auto goal = factory_output->stateSupplier.getState(goalId);
            auto solution = factory_output->search.search(start, goal, false, true);
            REQUIRE(
                solution->map<std::tuple<xyLoc>>([&](auto x) {
                    return x.getPayload();
                }) == vectorplus<std::tuple<xyLoc>>::make(std::make_tuple(xyLoc{0,0}), std::make_tuple(xyLoc{0,1})
            ));
            REQUIRE(solution->getCost() == 100);
        }

        WHEN("start to goal via perturbated edge") {
            xyLoc startLoc{0,0};
            xyLoc goalLoc{4,0};
            nodeid_t startId = g.idOfVertex(startLoc);
            nodeid_t goalId = g.idOfVertex(goalLoc);
            auto start = factory_output->stateSupplier.getState(startId);
            auto goal = factory_output->stateSupplier.getState(goalId);
            auto solution = factory_output->search.search(start, goal, false, true);
            REQUIRE(
                solution->map<std::tuple<xyLoc>>([&](auto x) {
                    return x.getPayload();
                }) == vectorplus<std::tuple<xyLoc>>::make(
                    std::make_tuple(xyLoc{0,0}),
                    std::make_tuple(xyLoc{1,0}),
                    std::make_tuple(xyLoc{2,0}),
                    std::make_tuple(xyLoc{3,0}),
                    std::make_tuple(xyLoc{4,0})
            ));
            REQUIRE(solution->getCost() == (3*100 + 1*500));
        }

        WHEN("early termination (perturbation near the end)") {
            xyLoc startLoc{0,0};
            xyLoc goalLoc{4,4};
            nodeid_t startId = g.idOfVertex(startLoc);
            nodeid_t goalId = g.idOfVertex(goalLoc);
            auto start = factory_output->stateSupplier.getState(startId);
            auto goal = factory_output->stateSupplier.getState(goalId);
            auto solution = factory_output->search.search(start, goal, false, true);
            REQUIRE(
                solution->map<std::tuple<xyLoc>>([&](auto x) {
                    return x.getPayload();
                }) == vectorplus<std::tuple<xyLoc>>::make(
                    std::make_tuple(xyLoc{0,0}),
                    std::make_tuple(xyLoc{0,1}),
                    std::make_tuple(xyLoc{0,2}),
                    std::make_tuple(xyLoc{1,3}),
                    std::make_tuple(xyLoc{2,4}),
                    std::make_tuple(xyLoc{3,4}),
                    std::make_tuple(xyLoc{4,4})
            ));
            REQUIRE(solution->getCost() == (2*141 + 3*100 + 1*200));
        }

        WHEN("early termination (perturbation near the start)") {
            xyLoc startLoc{4,4};
            xyLoc goalLoc{0,0};
            nodeid_t startId = g.idOfVertex(startLoc);
            nodeid_t goalId = g.idOfVertex(goalLoc);
            auto start = factory_output->stateSupplier.getState(startId);
            auto goal = factory_output->stateSupplier.getState(goalId);
            auto solution = factory_output->search.search(start, goal, false, true);
            REQUIRE(
                solution->map<std::tuple<xyLoc>>([&](auto x) {
                    return x.getPayload();
                }) == vectorplus<std::tuple<xyLoc>>::make(
                    std::make_tuple(xyLoc{4,4}),
                    std::make_tuple(xyLoc{3,4}),
                    std::make_tuple(xyLoc{2,4}),
                    std::make_tuple(xyLoc{1,3}),
                    std::make_tuple(xyLoc{0,2}),
                    std::make_tuple(xyLoc{0,1}),
                    std::make_tuple(xyLoc{0,0})
            ));
            REQUIRE(solution->getCost() == (2*141 + 3*100 + 1*200));
        }

        WHEN("testing bound") {
            xyLoc startLoc{0,0};
            xyLoc goalLoc{2,0};
            nodeid_t startId = g.idOfVertex(startLoc);
            nodeid_t goalId = g.idOfVertex(goalLoc);
            auto start = factory_output->stateSupplier.getState(startId);
            auto goal = factory_output->stateSupplier.getState(goalId);
            auto solution = factory_output->search.search(start, goal, false, true);
            REQUIRE(
                solution->map<std::tuple<xyLoc>>([&](auto x) {
                    return x.getPayload();
                }) == vectorplus<std::tuple<xyLoc>>::make(
                    std::make_tuple(xyLoc{0,0}),
                    std::make_tuple(xyLoc{1,0}),
                    std::make_tuple(xyLoc{2,0})
            ));
            REQUIRE(solution->getCost() == (1*100 + 1*500));
        }

        delete factory_output;
    }
}

SCENARIO("test CPdFocalHeuristic") {

    GIVEN("a gridmap") {

        MovingAIGridMapReader reader{
            '.', 100, color_t::WHITE,
            'T', 150, color_t::GREEN,
            '@', cost_t::INFTY, color_t::BLACK
        };
        GridMap gridMap = reader.load(boost::filesystem::path{"square03.map"});
        GridMapGraphConverter converter{GridBranching::EIGHT_CONNECTED};
        AdjacentGraph<std::string, xyLoc, cost_t> graph{*converter.toGraph(gridMap)};

        /*
         *  01234
         * 0.....
         * 1...@@
         * 2..@@.
         * 3...@@
         * 4.....
         *
         */

        CpdManager<std::string, xyLoc> cpdManager{boost::filesystem::path{"./square03.cpd"}, graph};
        const IImmutableGraph<std::string, xyLoc, cost_t>& reorderedGraph = cpdManager.getReorderedGraph();
        // IT'S REALLY IMPORTANT THAT WE QUERY NOT ON "graph" BUT ON "reorderedGraph"!!!!!
        // THIS because the CPD uses those indices, not the ones from "graph"!!!
        auto perturbatedGraphTmp = std::unique_ptr<IImmutableGraph<std::string, xyLoc, PerturbatedCost>>(cpdManager.getReorderedGraph().mapEdges<PerturbatedCost>([&](cost_t c) { return PerturbatedCost{c, false}; }));
        AdjacentGraph<std::string, xyLoc, PerturbatedCost> perturbatedGraph{*perturbatedGraphTmp};

        perturbatedGraph.changeWeightUndirectedEdge(perturbatedGraph.idOfVertex(xyLoc{0,3}), perturbatedGraph.idOfVertex(xyLoc{0,4}), PerturbatedCost{200, true});

        CpdFocalHeuristic<State, std::string, xyLoc> h{
            cpdManager,
            perturbatedGraph
        };

        REQUIRE(h.isAdmissible());
        REQUIRE(h.isConsistent());

        WHEN("start=goal") {
            xyLoc startLoc{0,0};
            xyLoc goalLoc{0,0};
            State start{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc), cpd_search_generated_e::INPUT};
            State goal{0, reorderedGraph, reorderedGraph.idOfVertex(goalLoc), cpd_search_generated_e::INPUT};
            REQUIRE(h.getHeuristic(start, &goal) == 0);
            REQUIRE(h.getLastEarliestPerturbationSourceIdCost() == 0);
            REQUIRE(h.getLastEarliestNodeBeforePerturbation() == reorderedGraph.idOfVertex(goalLoc));
            REQUIRE(h.getPerturbatedSourceId(reorderedGraph.idOfVertex(goalLoc)) == reorderedGraph.idOfVertex(goalLoc));
            REQUIRE(h.getPerturbatedPathCost(reorderedGraph.idOfVertex(goalLoc)) == 0);
        }

        WHEN("start above goal") {
            xyLoc startLoc{0,0};
            xyLoc goalLoc{0,1};
            State start{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc), cpd_search_generated_e::INPUT};
            State goal{0, reorderedGraph, reorderedGraph.idOfVertex(goalLoc), cpd_search_generated_e::INPUT};
            REQUIRE(h.getHeuristic(start, &goal) == 100);
            REQUIRE(h.getCachedElementsNumber() == 2);
            REQUIRE(h.getLastEarliestPerturbationSourceIdCost() == (1*100));
            REQUIRE(h.getLastEarliestNodeBeforePerturbation() == reorderedGraph.idOfVertex(goalLoc));
            REQUIRE(h.getPerturbatedSourceId(reorderedGraph.idOfVertex(startLoc)) == reorderedGraph.idOfVertex(goalLoc));
            REQUIRE(h.getPerturbatedPathCost(reorderedGraph.idOfVertex(startLoc)) == (1*100));
            REQUIRE(h.getPerturbatedSourceId(reorderedGraph.idOfVertex(goalLoc)) == reorderedGraph.idOfVertex(goalLoc));
            REQUIRE(h.getPerturbatedPathCost(reorderedGraph.idOfVertex(goalLoc)) == (0*100));
        }

        WHEN("start diagonal goal") {
            xyLoc startLoc{0,0};
            xyLoc goalLoc{1,1};
            State start{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc), cpd_search_generated_e::INPUT};
            State goal{0, reorderedGraph, reorderedGraph.idOfVertex(goalLoc), cpd_search_generated_e::INPUT};
            REQUIRE(h.getHeuristic(start, &goal) == 141);
            REQUIRE(h.getCachedElementsNumber() == 2);
            REQUIRE(h.getLastEarliestPerturbationSourceIdCost() == (1*141));
            REQUIRE(h.getLastEarliestNodeBeforePerturbation() == reorderedGraph.idOfVertex(goalLoc));
            REQUIRE(h.getPerturbatedSourceId(reorderedGraph.idOfVertex(startLoc)) == reorderedGraph.idOfVertex(goalLoc));
            REQUIRE(h.getPerturbatedPathCost(reorderedGraph.idOfVertex(startLoc)) == (1*141));
            REQUIRE(h.getPerturbatedSourceId(reorderedGraph.idOfVertex(goalLoc)) == reorderedGraph.idOfVertex(goalLoc));
            REQUIRE(h.getPerturbatedPathCost(reorderedGraph.idOfVertex(goalLoc)) == (0*141));
        }

        WHEN("start above goal with perturbation") {
            xyLoc startLoc{0,3};
            xyLoc goalLoc{0,4};
            State start{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc), cpd_search_generated_e::INPUT};
            State goal{0, reorderedGraph, reorderedGraph.idOfVertex(goalLoc), cpd_search_generated_e::INPUT};
            REQUIRE(h.getHeuristic(start, &goal) == 100);
            REQUIRE(h.getCachedElementsNumber() == 2);
            REQUIRE(h.getLastEarliestPerturbationSourceIdCost() == 0);
            REQUIRE(h.getLastEarliestNodeBeforePerturbation() == reorderedGraph.idOfVertex(startLoc));
            REQUIRE(h.getPerturbatedSourceId(reorderedGraph.idOfVertex(startLoc)) == reorderedGraph.idOfVertex(startLoc));
            REQUIRE(h.getPerturbatedPathCost(reorderedGraph.idOfVertex(startLoc)) == (0*100));
            REQUIRE(h.getPerturbatedSourceId(reorderedGraph.idOfVertex(goalLoc)) == reorderedGraph.idOfVertex(goalLoc));
            REQUIRE(h.getPerturbatedPathCost(reorderedGraph.idOfVertex(goalLoc)) == (0*100));
        }

        WHEN("start distance from goal of 2") {
            xyLoc startLoc{0,0};
            xyLoc goalLoc{2,0};
            State start{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc), cpd_search_generated_e::INPUT};
            State goal{0, reorderedGraph, reorderedGraph.idOfVertex(goalLoc), cpd_search_generated_e::INPUT};
            REQUIRE(h.getHeuristic(start, &goal) == 200);
            REQUIRE(h.getCachedElementsNumber() == 3);
            REQUIRE(h.getLastEarliestPerturbationSourceIdCost() == (2*100));
            REQUIRE(h.getLastEarliestNodeBeforePerturbation() == reorderedGraph.idOfVertex(goalLoc));
            REQUIRE(h.getPerturbatedSourceId(reorderedGraph.idOfVertex(startLoc)) == reorderedGraph.idOfVertex(goalLoc));
            REQUIRE(h.getPerturbatedPathCost(reorderedGraph.idOfVertex(startLoc)) == (2*100));
            REQUIRE(h.getPerturbatedSourceId(reorderedGraph.idOfVertex(goalLoc)) == reorderedGraph.idOfVertex(goalLoc));
            REQUIRE(h.getPerturbatedPathCost(reorderedGraph.idOfVertex(goalLoc)) == (0*100));
        }

        WHEN("start diangonal distance from goal of 2") {
            xyLoc startLoc{0,2};
            xyLoc goalLoc{2,4};
            State start{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc), cpd_search_generated_e::INPUT};
            State goal{0, reorderedGraph, reorderedGraph.idOfVertex(goalLoc), cpd_search_generated_e::INPUT};
            REQUIRE(h.getHeuristic(start, &goal) == 282);
            REQUIRE(h.getCachedElementsNumber() == 3);
            REQUIRE(h.getLastEarliestPerturbationSourceIdCost() == (2*141));
            REQUIRE(h.getLastEarliestNodeBeforePerturbation() == reorderedGraph.idOfVertex(goalLoc));
            REQUIRE(h.getPerturbatedSourceId(reorderedGraph.idOfVertex(startLoc)) == reorderedGraph.idOfVertex(goalLoc));
            REQUIRE(h.getPerturbatedPathCost(reorderedGraph.idOfVertex(startLoc)) == (2*141));
            REQUIRE(h.getPerturbatedSourceId(reorderedGraph.idOfVertex(goalLoc)) == reorderedGraph.idOfVertex(goalLoc));
            REQUIRE(h.getPerturbatedPathCost(reorderedGraph.idOfVertex(goalLoc)) == (0*141));
        }

        WHEN("start distance from goal with perturbation") {
            xyLoc startLoc{0,4};
            xyLoc goalLoc{0,1};
            State start{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc), cpd_search_generated_e::INPUT};
            State goal{0, reorderedGraph, reorderedGraph.idOfVertex(goalLoc), cpd_search_generated_e::INPUT};
            REQUIRE(h.getHeuristic(start, &goal) == (3*100));
            REQUIRE(h.getCachedElementsNumber() == 4);
            REQUIRE(h.getLastEarliestPerturbationSourceIdCost() == (0));
            REQUIRE(h.getLastEarliestNodeBeforePerturbation() == reorderedGraph.idOfVertex(startLoc));
            REQUIRE(h.getPerturbatedSourceId(reorderedGraph.idOfVertex(startLoc)) == reorderedGraph.idOfVertex(startLoc));
            REQUIRE(h.getPerturbatedPathCost(reorderedGraph.idOfVertex(startLoc)) == (0*100));
            REQUIRE(h.getPerturbatedSourceId(reorderedGraph.idOfVertex(xyLoc{0,3})) == reorderedGraph.idOfVertex(goalLoc));
            REQUIRE(h.getPerturbatedPathCost(reorderedGraph.idOfVertex(xyLoc{0,3})) == (2*100));
            REQUIRE(h.getPerturbatedSourceId(reorderedGraph.idOfVertex(xyLoc{0,2})) == reorderedGraph.idOfVertex(goalLoc));
            REQUIRE(h.getPerturbatedPathCost(reorderedGraph.idOfVertex(xyLoc{0,2})) == (1*100));
            REQUIRE(h.getPerturbatedSourceId(reorderedGraph.idOfVertex(goalLoc)) == reorderedGraph.idOfVertex(goalLoc));
            REQUIRE(h.getPerturbatedPathCost(reorderedGraph.idOfVertex(goalLoc)) == (0*100));
        }

        WHEN("start distance from goal with perturbation") {
            xyLoc startLoc{0,1};
            xyLoc goalLoc{0,4};
            State start{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc), cpd_search_generated_e::INPUT};
            State goal{0, reorderedGraph, reorderedGraph.idOfVertex(goalLoc), cpd_search_generated_e::INPUT};
            REQUIRE(h.getHeuristic(start, &goal) == (3*100));
            REQUIRE(h.getCachedElementsNumber() == 4);
            REQUIRE(h.getLastEarliestPerturbationSourceIdCost() == (2*100));
            REQUIRE(h.getLastEarliestNodeBeforePerturbation() == reorderedGraph.idOfVertex(xyLoc{0,3}));
            REQUIRE(h.getPerturbatedSourceId(reorderedGraph.idOfVertex(startLoc)) == reorderedGraph.idOfVertex(xyLoc{0,3}));
            REQUIRE(h.getPerturbatedPathCost(reorderedGraph.idOfVertex(startLoc)) == (2*100));
            REQUIRE(h.getPerturbatedSourceId(reorderedGraph.idOfVertex(xyLoc{0,2})) == reorderedGraph.idOfVertex(xyLoc{0,3}));
            REQUIRE(h.getPerturbatedPathCost(reorderedGraph.idOfVertex(xyLoc{0,2})) == (1*100));
            REQUIRE(h.getPerturbatedSourceId(reorderedGraph.idOfVertex(xyLoc{0,3})) == reorderedGraph.idOfVertex(xyLoc{0,3}));
            REQUIRE(h.getPerturbatedPathCost(reorderedGraph.idOfVertex(xyLoc{0,3})) == (0*100));
            REQUIRE(h.getPerturbatedSourceId(reorderedGraph.idOfVertex(goalLoc)) == reorderedGraph.idOfVertex(goalLoc));
            REQUIRE(h.getPerturbatedPathCost(reorderedGraph.idOfVertex(goalLoc)) == (0*100));
        }

        WHEN("start far from goal") {
            xyLoc startLoc{0,0};
            xyLoc goalLoc{4,4};
            State start{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc), cpd_search_generated_e::INPUT};
            State goal{0, reorderedGraph, reorderedGraph.idOfVertex(goalLoc), cpd_search_generated_e::INPUT};
            REQUIRE(h.getHeuristic(start, &goal) == (100*4 + 141*2));
            REQUIRE(h.getCachedElementsNumber() == 7);
        }

        WHEN("reusing heuristic and paths are not overlapping (except goal") {
            xyLoc startLoc1{0,0};
            xyLoc startLoc2{4,0};
            xyLoc goalLoc{2,0};
            State start1{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc1), cpd_search_generated_e::INPUT};
            State start2{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc2), cpd_search_generated_e::INPUT};
            State goal{0, reorderedGraph, reorderedGraph.idOfVertex(goalLoc), cpd_search_generated_e::INPUT};
            REQUIRE(h.getHeuristic(start1, &goal) == (100*2));
            REQUIRE(h.getHeuristic(start2, &goal) == (100*2));

            REQUIRE(h.getCachedElementsNumber() == 5);
        }

        //TODO coniotnue
        WHEN("reusing heuristic and paths are overlapping in the middle") {
            xyLoc startLoc1{0,0};
            xyLoc startLoc2{2,1};
            xyLoc goalLoc{4,0};
            State start1{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc1), cpd_search_generated_e::INPUT};
            State start2{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc2), cpd_search_generated_e::INPUT};
            State goal{0, reorderedGraph, reorderedGraph.idOfVertex(goalLoc), cpd_search_generated_e::INPUT};
            REQUIRE(h.getHeuristic(start1, &goal) == (100*4));
            REQUIRE(h.getHeuristic(start2, &goal) == (100*3));
            REQUIRE(h.getCachedElementsNumber() == 6);
        }

        WHEN("reusing heuristic and one path is subset of the other") {
            xyLoc startLoc1{0,0};
            xyLoc startLoc2{2,0};
            xyLoc goalLoc{4,0};
            State start1{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc1), cpd_search_generated_e::INPUT};
            State start2{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc2), cpd_search_generated_e::INPUT};
            State goal{0, reorderedGraph, reorderedGraph.idOfVertex(goalLoc), cpd_search_generated_e::INPUT};
            REQUIRE(h.getHeuristic(start1, &goal) == (100*4));
            REQUIRE(h.getHeuristic(start2, &goal) == (100*2));
            REQUIRE(h.getCachedElementsNumber() == 5);
        }

        WHEN("performing cleanup") {
            xyLoc startLoc{0,0};
            xyLoc goalLoc{4,4};
            State start{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc), cpd_search_generated_e::INPUT};
            State goal{0, reorderedGraph, reorderedGraph.idOfVertex(goalLoc), cpd_search_generated_e::INPUT};
            h.getHeuristic(start, &goal);

            REQUIRE(h.getCachedElementsNumber() == 7);
            h.cleanup();
            REQUIRE(h.getCachedElementsNumber() == 0);
        }

        WHEN("goal unreachable") {
            xyLoc startLoc{0,0};
            xyLoc goalLoc{4,2};
            State start{0, reorderedGraph, reorderedGraph.idOfVertex(startLoc), cpd_search_generated_e::INPUT};
            State goal{0, reorderedGraph, reorderedGraph.idOfVertex(goalLoc), cpd_search_generated_e::INPUT};
            REQUIRE(h.getHeuristic(start, &goal) == cost_t::INFTY);
            REQUIRE(h.getCachedElementsNumber() == 0);
        }
    }

}

//TODO remove
// SCENARIO("test CpdFocalSearch in actual failed tests") {

//     GIVEN("failed test 1") {

//         // CREATE GRAPH WHERE WE WANT TO OPERATE

//         MovingAIGridMapReader reader{
//             '.', 1000, color_t::WHITE,
//             'T', 1500, color_t::GREEN,
//             'S', 2000, color_t::CYAN,
//             'W', 2500, color_t::BLUE,
//             '@', cost_t::INFTY, color_t::BLACK
//         };
//         GridMap gridMap = reader.load(boost::filesystem::path{"16room_003.map"});
//         GridMapGraphConverter converter{GridBranching::EIGHT_CONNECTED};
//         AdjacentGraph<std::string, xyLoc, cost_t> graph{*converter.toGraph(gridMap)};

//         /*
//          *  01234
//          * 0.XX..
//          * 1...@@
//          * 2..@@.
//          * 3...@@
//          * 4..XX.
//          *
//          */

//         // INCLUDE THE CPD

//         CpdManager<std::string, xyLoc> cpdManager{boost::filesystem::path{"./16room_003.cpd"}, graph};
//         const IImmutableGraph<std::string, xyLoc, cost_t>& g = cpdManager.getReorderedGraph();

//         // CREATE THE TIME GRAPH WITH PERTURBATIONS

//         boost::filesystem::path perturbateEdgesFilename{"./16room_003_01.dat"};
//         FILE* f = fopen(perturbateEdgesFilename.native().c_str(), "rb");
//         if (f == nullptr) {
//             REQUIRE(false);
//         }
//         SetPlus<Edge<PerturbatedCost>> perturbatedEdges{};
//         AdjacentGraph<std::string, xyLoc, PerturbatedCost>* perturbatedGraph = new AdjacentGraph<std::string, xyLoc, PerturbatedCost>{
//             *cpdManager.getReorderedGraph().mapEdges(std::function<PerturbatedCost(const cost_t&)>([&](const cost_t& c) {
//                 return PerturbatedCost{c, false};
//             }))
//         };
//         cpp_utils::serializers::loadFromFile(f, perturbatedEdges);

//         for (auto perturbatedEdge : perturbatedEdges) {
//             perturbatedGraph->changeWeightEdge(
//                 perturbatedEdge.getSourceId(),
//                 perturbatedEdge.getSinkId(),
//                 perturbatedEdge.getPayload()
//             );
//         }
//         fclose(f);

//         // USE THE FACTORY TO PROVIDE CpdFocalSearch

//         CpdFocalSearchFactory factory{};
//         auto factory_output = factory.get(cpdManager, *perturbatedGraph, fractional_number<cost_t>{1., 1e-3}, 1);

//         REQUIRE(g.haveSameVertices(*perturbatedGraph));

//         WHEN("from 267,135 to 267, 13") {
//             critical("testing failing 1");
//             xyLoc startLoc{267, 135};
//             xyLoc goalLoc{267, 136};
//             nodeid_t startId = g.idOfVertex(startLoc);
//             nodeid_t goalId = g.idOfVertex(goalLoc);
//             GraphFocalState<std::string, xyLoc, PerturbatedCost>& start = factory_output->stateSupplier.getState(startId, cpd_search_generated_e::FROM_INPUT);
//             GraphFocalState<std::string, xyLoc, PerturbatedCost>& goal = factory_output->stateSupplier.getState(goalId, cpd_search_generated_e::FROM_INPUT);
//             auto solution = factory_output->search.search(start, goal, false, false);
//             REQUIRE(solution->getCost() == 1000);
//         }
//     }
// }


// Copyright 2016 Matthew Chandler

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

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

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

#include <algorithm>
#include <array>
#include <fstream>
#include <iostream>
#include <limits>
#include <numeric>
#include <string>
#include <unordered_set>
#include <vector>

#include <boost/program_options.hpp>

namespace po = boost::program_options;
const size_t ALPHABET_LEN = 26;

void find_words(const std::array<std::size_t, ALPHABET_LEN> & ltrs,
                const std::vector<std::string> & word_list,
                const std::vector<std::string> & prefix,
                std::unordered_set<std::string> & seen_groups,
                const bool show_partial,
                const bool permutations,
                const bool use_apostrophe,
                const std::size_t start_ltr_count)
{
    if(std::accumulate(ltrs.begin(), ltrs.end(), 0) == 0)
        return;

    std::vector<std::array<std::size_t, ALPHABET_LEN>> new_ltrs;
    std::vector<std::string> new_word_list;
    std::vector<std::vector<std::string>> new_prefixes;

    for(auto & word: word_list)
    {
        std::array<std::size_t, ALPHABET_LEN> word_ltrs(ltrs);
        bool use_word = true;

        for(auto & c: word)
        {
            if(use_apostrophe && c == '\'')
                continue;

            if(word_ltrs[c - 'A'] == 0)
            {
                use_word = false;
                break;
            }

            --word_ltrs[c - 'A'];
        }
        if(use_word)
        {
            auto new_prefix = prefix;
            new_prefix.emplace_back(word);

            if(!permutations)
                std::sort(new_prefix.begin(), new_prefix.end());

            std::size_t ltr_count = 0;
            std::string anagram;
            for(const auto & prefix_word: new_prefix)
            {
                if(use_apostrophe)
                {
                    for(auto & c: prefix_word)
                    {
                        if(c != '\'')
                            ++ltr_count;
                    }
                }
                else
                    ltr_count += prefix_word.size();

                if(!anagram.empty())
                    anagram += " ";
                anagram += prefix_word;
            }

            if(permutations || !seen_groups.count(anagram))
            {
                if(show_partial)
                {
                    if(ltr_count == start_ltr_count)
                        std::cout<<"* "<<anagram<<std::endl;
                    else
                        std::cout<<"  "<<anagram<<std::endl;
                }
                else
                {
                    if(ltr_count == start_ltr_count)
                        std::cout<<anagram<<std::endl;
                }

                new_ltrs.emplace_back(word_ltrs);
                new_word_list.emplace_back(word);
                new_prefixes.emplace_back(new_prefix);

                if(!permutations)
                    seen_groups.emplace(anagram);
            }
        }
    }

    for(std::size_t i = 0; i < new_ltrs.size(); ++i)
        find_words(new_ltrs[i],
                   new_word_list,
                   new_prefixes[i],
                   seen_groups,
                   show_partial,
                   permutations,
                   use_apostrophe,
                   start_ltr_count);
}

std::string generate_usage(char * argv[],
        const po::options_description & optional_desc,
        const po::options_description & positional_desc,
        const po::positional_options_description & pd)
{
    std::string usage = "usage:";

    std::string prog_name(argv[0]);
    auto sep_pos = prog_name.find_last_of("/");
    if(sep_pos != std::string::npos)
        prog_name = prog_name.substr(sep_pos + 1);
    usage += " " + prog_name;

    for(auto & opt: optional_desc.options())
    {
        // strip long option if both forms allowed
        std::string opt_txt = opt->format_name().substr(0, opt->format_name().find_first_of(" ["));

        if(opt->semantic()->max_tokens() > 0)
        {
            std::string format_param;
            if(!opt->format_parameter().empty())
            {
                // strip default value
                format_param = opt->format_parameter().substr(0, opt->format_parameter().find_first_of(" ("));
            }
            else
            {
                format_param = "VALUE";
            }

            // multitoken allowed
            if(opt->semantic()->max_tokens() > 1)
            {
                format_param += " [" + format_param + "...]";
            }
            // zero tokens allowd
            if(opt->semantic()->min_tokens() == 0)
            {
                format_param = "[" + format_param + "]";
            }

            opt_txt += " " + format_param;
        }

        // bracket optional params
        if(!opt->semantic()->is_required())
        {
            opt_txt = "[" + opt_txt + "]";
        }

        usage += " " + opt_txt;
    }

    // add positional args
    std::size_t num_positional = positional_desc.options().size();
    std::string cur_pos = "";
    std::size_t pos_no = 0;
    for(std::size_t i = 0; i < std::min<std::size_t>(10, pd.max_total_count()); ++i)
    {
        auto pos_name = pd.name_for_position(i);
        usage += " " + positional_desc.find(pos_name, false, false, false).format_parameter();

        if(pos_name != cur_pos)
        {
            cur_pos = pos_name;
            if(++pos_no == num_positional && pd.max_total_count() == std::numeric_limits<decltype(pd.max_total_count())>::max())
            {
                usage += "...";
                break;
            }
        }
    }

    return usage;
}

int main(int argc, char * argv[])
{
    const std::string prog_desc = "Anagram generator";
    po::options_description optional_desc("optional arguments");
    po::options_description positional_desc("positional arguments");
    po::options_description all_desc;
    po::positional_options_description pd;

    optional_desc.add_options()
        ("help,h", "Show this help message and exit")
        ("show-partial,p", "Show partial anagrams. Full anagrams will be preceded by an '*'")
        ("permutations,r", "Generate each permutation instead of each combination. Much slower, but uses much less memory")
        ("no-apostrophe,n", "Don't generate words with apostrophes")
        ("small-words,s", "Restrict small (<= 2 letters) words to a predefined set")
        ("dictionary,d", po::value<std::string>()->default_value("/usr/share/dict/words")->value_name("DICTIONARY"),
            "Dictionary file");

    positional_desc.add_options()
        ("text", po::value<std::vector<std::string>>()->value_name("TEXT")->required(),
            "Text to generate anagrams for");

    pd.add("text", -1);

    all_desc.add(optional_desc);
    all_desc.add(positional_desc);

    po::variables_map vm;
    try
    {
        po::store(po::command_line_parser(argc, argv).options(all_desc).positional(pd).run(), vm);

        if(vm.count("help") || vm.count("h"))
        {
            std::cout<<generate_usage(argv, optional_desc, positional_desc, pd)<<"\n"<<prog_desc<<"\n\n";
            std::cout<<positional_desc<<"\n";
            std::cout<<optional_desc<<std::endl;
            return EXIT_SUCCESS;
        }

        po::notify(vm);
    }
    catch(po::error & e)
    {
        std::cerr<<e.what()<<std::endl;
        std::cerr<<generate_usage(argv, optional_desc, positional_desc, pd)<<std::endl;
        return EXIT_FAILURE;
    }

    bool show_partial = vm.count("show-partial") > 0;
    bool permutations = vm.count("permutations") > 0;
    bool use_apostrophe = vm.count("no-apostrophe") == 0;
    bool restrict_small_words = vm.count("small-words") > 0;
    std::string dictionary_filename = vm["dictionary"].as<std::string>();

    // get letter counts
    std::array<std::size_t, ALPHABET_LEN> ltrs;
    std::fill(ltrs.begin(), ltrs.end(), 0);

    std::size_t start_ltr_count = 0;
    for(auto & word: vm["text"].as<std::vector<std::string>>())
    {
        for(auto c: word)
        {
            if(c == '\'')
                continue;

            c = std::toupper(c);
            if(c < 'A' || c > 'Z')
            {
                std::cerr<<"Illegal character in input: '"<<c<<"'"<<std::endl;
                return EXIT_FAILURE;
            }
            ++ltrs[c - 'A'];
            ++start_ltr_count;
        }
    }

    // open dictionary file
    std::ifstream dictionary_file(dictionary_filename);
    try
    {
        dictionary_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); // throw on error OR failure
    }
    catch(std::system_error & e)
    {
        std::cerr<<"Error opening "<<dictionary_filename<<": "<<std::strerror(errno)<<std::endl;
        return EXIT_FAILURE;
    }

    std::vector<std::string> dictionary;
    try
    {
        std::unordered_set<std::string> dictionary_set;
        dictionary_file.exceptions(std::ifstream::badbit); // only throw on error

        std::string word;
        while(std::getline(dictionary_file, word, '\n'))
        {
            bool skip_word = false;
            for(auto &c: word)
            {
                c = std::toupper(c);
                if((!use_apostrophe || c != '\'') && (c < 'A' || c > 'Z'))
                {
                    skip_word = true;
                    break;
                }
            }

            static const std::unordered_set<std::string> legal_small_words
            {
                "A", "I",
                "AH", "AM", "AN", "AS", "AT", "BE", "BY", "DC", "DO",
                "DR", "EX", "GO", "HA", "HE", "HI", "HO", "IF", "II",
                "IN", "IS", "IT", "LA", "LO", "MA", "ME", "MR", "MS",
                "MY", "NO", "OF", "OH", "OK", "ON", "OR", "OW", "OX",
                "PA", "PI", "SO", "ST", "TO",
                "UP", "US", "WE"
            };

            if(!skip_word && (!restrict_small_words || word.size() > 2 || legal_small_words.count(word)))
            {
                dictionary_set.emplace(word);
            }
        }

        // put words into set to remove dupes. now put them into an array
        dictionary.insert(dictionary.end(), dictionary_set.begin(), dictionary_set.end());
        std::sort(dictionary.begin(), dictionary.end());
    }
    catch(std::system_error & e)
    {
        std::cerr<<"Error reading "<<dictionary_filename<<": "<<std::strerror(errno)<<std::endl;
        return EXIT_FAILURE;
    }

    std::unordered_set<std::string> seen_groups;
    find_words(ltrs, dictionary, std::vector<std::string>(), seen_groups, show_partial, permutations, use_apostrophe, start_ltr_count);

    return EXIT_SUCCESS;
}


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/constant/mantissamask.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>

STF_CASE_TPL( "Check mantissamask behavior for integral types"
            , (std::uint8_t)(std::uint16_t)(std::uint32_t)(std::uint64_t)
              (std::int8_t)(std::int16_t)(std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::mantissamask;
  using boost::simd::Mantissamask;

  STF_TYPE_IS(decltype(Mantissamask<T>()), T);
  STF_EQUAL(Mantissamask<T>(), T(0));
  STF_EQUAL(mantissamask( as(T{}) ),T(0));
}

STF_CASE_TPL( "Check mantissamask behavior for double type"
            , (double)
            )
{
  using boost::simd::as;
  using boost::simd::detail::mantissamask;
  using boost::simd::Mantissamask;

  STF_TYPE_IS(decltype(Mantissamask<T>()), T);
  STF_IEEE_EQUAL(Mantissamask<T>(), -2.225073858507200889e-308);
  STF_IEEE_EQUAL(mantissamask( as(T{}) ), -2.225073858507200889e-308);
}
STF_CASE_TPL( "Check mantissamask behavior for float type"
            , (float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::mantissamask;
  using boost::simd::Mantissamask;

  STF_TYPE_IS(decltype(Mantissamask<T>()), T);
  STF_IEEE_EQUAL(Mantissamask<T>(), -1.1754942106924410755e-38f);
  STF_IEEE_EQUAL(mantissamask( as(T{}) ), -1.1754942106924410755e-38f);
}


/* Copyright 2015,2016 Tao Xu
 * Licensed to the Apache Software Foundation (ASF) under one
 * or more contributor license agreements. See the NOTICE file
 * distributed with this work for additional information
 * regarding copyright ownership. The ASF licenses this file
 * to you 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.
 */

#include "Gbm.h"

#include <boost/scoped_array.hpp>
#include <boost/shared_ptr.hpp>
#include <vector>

#include "Concurrency.h"
#include "Config.h"
#include "DataSet.h"
#include "GbmFun.h"
#include "Tree.h"
#include "TreeRegressor.h"
#include <gflags/gflags.h>

namespace boosting {

using namespace std;

Gbm::Gbm(const GbmFun& fun, const DataSet& ds, const Config& cfg)
  : fun_(fun), ds_(ds), cfg_(cfg) {
}

class ParallelEval : public apache::thrift::concurrency::Runnable {
 public:
  ParallelEval(
    CounterMonitor& monitor,
    const int numExamples,
    const int numFeatures,
    const GbmFun& fun,
    const std::unique_ptr<TreeNode<uint16_t>>& weakModel,
    const DataSet& ds,
    const vector<double>& targets,
    boost::scoped_array<double>& F,
    boost::scoped_array<double>& subLoss,
    const int workIdx,
    const int totalWorkers)
    : monitor_(monitor), numExamples_(numExamples),
      numFeatures_(numFeatures), fun_(fun), weakModel_(weakModel),
      ds_(ds), targets_(targets), F_(F),
      subLoss_(subLoss), workIdx_(workIdx),
      totalWorkers_(totalWorkers) {
  }

  void run() {
    //boost::scoped_array<uint16_t> fvec(new uint16_t[numFeatures_]);
    for (int i = 0; i < numExamples_; i++) {
      if (i % totalWorkers_ == workIdx_) {
        //ds_.getFeatureVec(i, fvec);
        //double score = weakModel_->eval(fvec);
        double score = ds_.getPrediction(weakModel_.get(), i);
        F_[i] += score;
        subLoss_[workIdx_] += fun_.getExampleLoss(targets_[i], F_[i]);
      }
    }
    monitor_.decrement();
  }

 private:
  CounterMonitor& monitor_;
  const int numExamples_;
  const int numFeatures_;
  const GbmFun& fun_;
  const std::unique_ptr<TreeNode<uint16_t>>& weakModel_;
  const DataSet& ds_;
  const vector<double> targets_;
  boost::scoped_array<double>& F_;
  boost::scoped_array<double>& subLoss_;
  const int workIdx_;
  const int totalWorkers_;
};

void Gbm::getModel(
  vector<TreeNode<double>*>* model,
  double fimps[]) {

  const int numExamples = ds_.getNumExamples();

  boost::scoped_array<double> F(new double[numExamples]);
  boost::scoped_array<double> y(new double[numExamples]);

  double f0 = fun_.getF0(ds_.targets_);
  for (int i = 0; i < numExamples; i++) {
    F[i] = f0;
  }

  model->push_back(new LeafNode<double>(f0));

  double initLoss = fun_.getInitLoss(ds_.targets_);

  LOG(INFO) << "init avg loss " << initLoss / numExamples;

  for (int it = 0; it < cfg_.getNumTrees(); it++) {

    LOG(INFO) << "------- iteration " << it << " -------";

    fun_.getGradient(ds_.targets_, F, y);
    TreeRegressor regressor(ds_, y, fun_);

    std::unique_ptr<TreeNode<uint16_t>> weakModel(
      regressor.getTree(cfg_.getNumLeaves(), cfg_.getExampleSamplingRate(),
                        cfg_.getFeatureSamplingRate(), fimps));

    weakModel->scale(cfg_.getLearningRate());

    model->push_back(mapTree(weakModel.get()));

    VLOG(1) << toPrettyJson(weakModel->toJson(cfg_));
    double newLoss = 0.0;

    if (FLAGS_num_threads > 1) {
      CounterMonitor monitor(FLAGS_num_threads);
      boost::scoped_array<double> subLoss(new double[FLAGS_num_threads]);
      for (int wid = 0; wid < FLAGS_num_threads; wid++) {
        subLoss[wid] = 0.0;
        Concurrency::threadManager->add(
          boost::shared_ptr<apache::thrift::concurrency::Runnable>(
            new ParallelEval(monitor, numExamples, ds_.numFeatures_,
                             fun_, weakModel,
                             ds_, ds_.targets_, F, subLoss,
                             wid, FLAGS_num_threads)));
      }
      monitor.wait();

      for (int wid = 0; wid < FLAGS_num_threads; wid++) {
        newLoss += subLoss[wid];
      }
    } else {
      //boost::scoped_array<uint16_t> fvec(new uint16_t[ds_.numFeatures_]);
      for (int i = 0; i < numExamples; i++) {
        // ds_.getFeatureVec(i, fvec);
        // double score = weakModel->eval(fvec);
        double score = ds_.getPrediction(weakModel.get(), i);
        F[i] += score;
        newLoss += fun_.getExampleLoss(ds_.targets_[i], F[i]);
      }
    }

    LOG(INFO) << "total avg loss " << newLoss/numExamples
              << " reduction: " << 1.0 - newLoss/initLoss;
  }
}

TreeNode<double>* Gbm::mapTree(const TreeNode<uint16_t>* rt) {
  const PartitionNode<uint16_t>* pnode =
    dynamic_cast<const PartitionNode<uint16_t>*>(rt);
  if (pnode != NULL) {
    int fid = pnode->getFid();
    PartitionNode<double>* newNode = new PartitionNode<double>(
      fid, ds_.features_[fid].transitions[pnode->getFv()]);
    newNode->setVote(pnode->getVote());
    newNode->setLeft(mapTree(pnode->getLeft()));
    newNode->setRight(mapTree(pnode->getRight()));
    return newNode;
  } else {
    const LeafNode<uint16_t>* lfnode =
      dynamic_cast<const LeafNode<uint16_t>*>(rt);
    return new LeafNode<double>(lfnode->getVote());
  }
}

}


//----------------------------------------------------------------------
//	File:			segmentation.cpp
//	Author:			Elena Garces and Adolfo Munoz
//	Last modified:	18/07/2012
//	Description:	funcions that control the clustering
//----------------------------------------------------------------------
// This file is part of Intrinsic Images by Clustering.
//
//    Intrinsic Images by Clustering 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, either version 3 of the License,
//     or (at your option) any later version.
//
//    Intrinsic Images by Clustering 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 Intrinsic Images by Clustering.  If not,
//	  see <http://www.gnu.org/licenses/>.
////----------------------------------------------------------------------

#include "segmentacion.h"
#include "auxiliar.h"

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/symmetric.hpp>

#include <algorithm>
#include <list>
#include <iostream>
#include <fstream>
#include "CImg.h"
using namespace std;
using namespace cimg_library;

/*-------------------------------------------------------------------------------------------------*/
void Segmentacion::updateLabels()
/*-------------------------------------------------------------------------------------------------*/
{

	int width = this->imageMapLabels.width();
	int height = this->imageMapLabels.height();

	vector<Cluster> newClusters;
	vector<int> founds;

	cimg_forXY(imageMapLabels,x,y)
	{

		int comp = *imageMapLabels.data(x,y,0,0);
		if ( comp != BACKG_CLUSTER )
		{
			vector<int>::iterator i = find(founds.begin(), founds.end(), comp);

			if (i == founds.end())
			{
				/* add */
				founds.push_back(comp);
				*imageMapLabels.data(x,y,0,0) = int(founds.size())-1;
				Cluster nuevo(comp, y*width+x);
				newClusters.push_back(nuevo);
			}
			else
			{
				/* already exists, update*/
				int index = std::distance (founds.begin(), i );
				*imageMapLabels.data(x,y,0,0) = index;
				newClusters[index].addPixel(y*width+x);

			}
		}
	}
	setListOfClusters(newClusters);
}

int Segmentacion::mergeSegment(const CImg<double>& image, int pos, int min_size_clus)
{

	int width = image.width();
	int n_changes=0;

	vector<int> pixels = getListOfClusters()[pos].getPixels();

	int idSetCentral = getListOfClusters()[pos].idCluster;

	for (int p = 0; p < (int) pixels.size(); p++)
	{
		int x = pixels[p] % width;	int y = pixels[p] / width;
		int idMostSimilar=-1;
		double diffMostSimilar=10000000.;

		int dist = 1;
		for (int j = y-dist; j <= y+dist; j++) {
			for (int i = x-dist; i <= x+dist; i++) {

				int set = getCluster(i,j);
				if (set >= 0 && set != idSetCentral && getListOfClusters().at(*imageMapLabels.data(i,j,0,0)).size() >= min_size_clus)
				{
					double squaredDiff = 0.0;
					cimg_forC(image,c) squaredDiff += pow(image(i, j,0,c) -  image(x, y,0,c), 2);
					squaredDiff=sqrt(squaredDiff);

					if (squaredDiff < diffMostSimilar)
					{
						diffMostSimilar = squaredDiff;
						idMostSimilar = set;
					}
				}
			}
		}
		if (idMostSimilar >=0)
		{
			n_changes++;
			setCluster(x, y, idMostSimilar);
		}
	}
		return n_changes;
}

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

int Segmentacion::detectSpurious(const CImg<double>& image, int max_size_clus, float percent_external)
/*-------------------------------------------------------------------------------------------------*/
{
	int total_changes=0;

	int width = image.width();
	for (int k = 0; k < (int) listOfClusters.size(); k++)
	{
		if (listOfClusters[k].getPixels().size() < max_size_clus)
		{
			vector<int> pixels = listOfClusters[k].getPixels();
			int votesBoundary = 0;
			for (int p = 0; p < (int) pixels.size(); p++)
			{
				int x = pixels[p] % width;
				int y = pixels[p] / width;
				int idSetCentral = getCluster(x,y); /* id del conjunto del pixel central */

				if ( (getCluster(x-1, y) >= 0 && getCluster(x-1, y) != idSetCentral) ||
					(getCluster(x+1, y) >= 0 && getCluster(x+1, y) != idSetCentral) ||
					(getCluster(x, y+1) >= 0 && getCluster(x, y+1) != idSetCentral) ||
					(getCluster(x, y-1) >= 0 && getCluster(x, y-1) != idSetCentral))
					votesBoundary++;
			}

			if (votesBoundary >= percent_external * pixels.size())
			{
				int changes = mergeSegment(image, k, max_size_clus);
				total_changes+=changes;
			}
		}
	}

	return total_changes;
}

void Segmentacion::mergeSmallClusters(const CImg<double>& image, int max_it,
								   int max_size_clus, float percent_external)
{
	int n_changes = 1; 	int it = 1;
	while (it < max_it && n_changes > 0)
	{
		n_changes = detectSpurious(image, max_size_clus, percent_external);
		#ifdef VERBOSE
		cout << "Clustering: --> \t\tIteration  " << it << " changes " << n_changes << endl;
		#endif
		updateLabels();
		it++;
	}
}

template <typename matrizBoost>
void analysis_gradient_clusters(matrizBoost& matrixRatios, const Segmentacion& seg,
			const CImg<double>& input, const MatteImage& matte)
	{
		int num_sets = int(seg.getListOfClusters().size());
		list<edge> edges;
		ublas::symmetric_matrix<int> edges_counter(num_sets,num_sets);

		for (int i=0; i < num_sets; i++)
			for (int j=i; j < num_sets; j++)
			{
				edges_counter(i,j) = 0;
				matrixRatios(i,j) = 0.0;
			}

		for (int i=0; i < num_sets; i++)
			matrixRatios(i,i) = 0.0;

		buildGraph(edges, input, 2, matte);
		for (list<edge>::const_iterator i=edges.begin(); i != edges.end(); i++)
		{
			int aSeg = seg.getCluster((*i).a);
			int bSeg = seg.getCluster((*i).b);

			if (aSeg != bSeg && aSeg >= 0 && bSeg >= 0)
			{
				int xa = (*i).a % input.width();
				int ya = (*i).a / input.width();
				int xb = (*i).b % input.width();
				int yb = (*i).b / input.width();

				double diffgrad =0.0;
				cimg_forC(input,c)
				{
					diffgrad+= fabs(input(xa, ya,0,c) - input(xb, yb,0,c));
				}
				diffgrad/=input.spectrum();

				if (aSeg <= bSeg)
				{
					edges_counter(aSeg,bSeg)++;
					matrixRatios(aSeg,bSeg)+=diffgrad;
				} else
				{
					edges_counter(bSeg,aSeg)++;
					matrixRatios(bSeg,aSeg)+=diffgrad;
				}
			}
		}

		for (int i=0; i < num_sets; i++)
			for (int j=(i+1); j < num_sets; j++)
			{
				if (matrixRatios(i,j)!=0)
				{
					if (edges_counter(i,j) > 15)
						matrixRatios(i,j)/=edges_counter(i,j);
					else matrixRatios(i,j)=0.0;
				}
			}
	}

bool func_ord_e(const edge &a, const edge &b)
	{ return (a.w < b.w);}

int Segmentacion::mergeSmoothBoundaries(CImg<double> *im_input, const MatteImage &matte, vector<tpPair> &paresR)
{

	int width = im_input->width();
	int height = im_input->height();

	for (unsigned int p=0; p < paresR.size(); p++)
	{
		paresR[p].i= imageMapLabels( paresR[p].i % im_input->width(), paresR[p].i / im_input->width(),0,0);
		paresR[p].j= imageMapLabels( paresR[p].j % im_input->width(), paresR[p].j / im_input->width(),0,0);
	}

	float max_grad = percent_grad_max*im_input->max();

	/* MERGE, ANALYZING GRADIENTS */
	int num_sets = int(getListOfClusters().size());
	ublas::symmetric_matrix<float> matrixRatios(num_sets,num_sets);
	analysis_gradient_clusters(matrixRatios, *(this),*im_input,matte);

	vector<edge> edges;

	int n=0;
	for (int i=0; i < num_sets; i++)
		for (int j=(i+1); j < num_sets; j++)
		{
			if (matrixRatios(i,j) != 0)
			{
				double diff=matrixRatios(i,j);
				if (diff  < max_grad)
					edges.push_back(edge(i,j,diff));
			}
		}

	std::sort(edges.begin(), edges.end(), func_ord_e);

	vector<bool> tratados(getListOfClusters().size(),false);
	vector<bool> referenciado(getListOfClusters().size(),false);

	int n_changes=0;
	for (vector<edge>::const_iterator i=edges.begin(); i != edges.end(); i++)
	{
		int peq, grand;
		if (getListOfClusters()[(*i).a].size() < getListOfClusters()[(*i).b].size())
		{
			peq = (*i).a;
			grand = (*i).b;
		} else  {
			peq = (*i).b;
			grand = (*i).a;
		}

		if (tratados[grand] == false)
		{

			n_changes++;
			// remove the smaller cluster
			for (unsigned int p=0; p < getListOfClusters()[peq].size(); p++)
			{
				setCluster(getListOfClusters()[peq].getPixels()[p], grand);
			}
			tratados[peq]=true;
			listOfClusters[grand].addPixel(listOfClusters[peq].getPixels());

			for (unsigned int i=0; i < paresR.size(); i++)
			{
				if (paresR[i].valid && (paresR[i].i==peq || paresR[i].j == peq))
				{
					paresR[i].valid=false;
				}
			}
		}
	}

	vector<tpPair> paresRnew;
	for (unsigned int i=0; i < paresR.size(); i++)
	{
			if (paresR[i].valid)
			{
				tpPair aux;
				aux.i = getListOfClusters()[paresR[i].i].getPixels()[0];
				aux.j = getListOfClusters()[paresR[i].j].getPixels()[0];
				aux.valid = true;
				paresRnew.push_back(aux);
			}

	}
	paresR = paresRnew;
	updateLabels();

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

	return n_changes;

}

void Segmentacion::updateReflectancePairs(vector<tpPair>& eqReflectance,  CImg<double> *luminance, int MIN_SIZE_CLUS)
{
	vector<tpPair> eqReflectancenew;
	float MIN_VARIATION_L = (float) 0.07*luminance->max();

	if (eqReflectance.size() > 0)
	{
		vector<float> averageLuminance(getListOfClusters().size(), -1);
		int index=0;
		for (unsigned int i=0; i < eqReflectance.size(); i++)
		{
			tpPair aux;
			aux.valid=true;
			aux.i = imageMapLabels(eqReflectance[i].i % width(),eqReflectance[i].i / width(),0,0);
			aux.j = imageMapLabels(eqReflectance[i].j % width(),eqReflectance[i].j / width(),0,0);

			if (aux.i > aux.j) std::swap(aux.i, aux.j);

			if (aux.i != aux.j && getListOfClusters().at(aux.i).size() > MIN_SIZE_CLUS && getListOfClusters().at(aux.j).size() > MIN_SIZE_CLUS)
			{
				averageLuminance[aux.i] = (averageLuminance[aux.i] < 0) ? computeMean(luminance, getListOfClusters().at(aux.i).getPixels()) : averageLuminance[aux.i];
				averageLuminance[aux.j] = (averageLuminance[aux.j] < 0) ? computeMean(luminance, getListOfClusters().at(aux.j).getPixels()) : averageLuminance[aux.j];

				if (averageLuminance[aux.i]  > 0 && averageLuminance[aux.j] > 0)
				{
					double diff = fabs(averageLuminance[aux.i]-averageLuminance[aux.j]);
					if (diff < MIN_VARIATION_L)	eqReflectancenew.push_back(aux);

				}
			}
		}

	}

	eqReflectance=eqReflectancenew;

}

vector<tpPair> Segmentacion::searchPairsRcte(Segmentacion& seg_old)
{

	vector<tpPair> pairsReflectance;
	vector< vector<int> > vec(seg_old.getListOfClusters().size(), vector<int>(0));
	for (unsigned int s = 0; s < getListOfClusters().size(); s++)
	{
		int p = getListOfClusters()[s].getPixels()[0];
		int clus_old=seg_old.getCluster(p);
		if (getListOfClusters()[s].getPixels().size() > 100)
			vec[clus_old].push_back(s);
	}

	for (unsigned int s = 0; s < vec.size(); s++)
	{
		for (unsigned int subS = 0; subS < vec[s].size(); subS++)
		{

			for (unsigned int sS = subS+1; sS < vec[s].size(); sS++)
			{
				int clusa=vec[s][subS];
				int clusb=vec[s][sS];

				tpPair aux;
				aux.i = getListOfClusters()[clusa].getPixels()[0];
				aux.j = getListOfClusters()[clusb].getPixels()[0];

				pairsReflectance.push_back(aux);
			}
		}
	}

	return pairsReflectance;
}

void Segmentacion::scanline(unsigned int coriginal, unsigned int cnew, int x, int y, Segmentacion& sol)
{
	int x2;

	if (getCluster(x,y) != BACKG_CLUSTER)
	{
		for (x2 = x; (getCluster(x2,y)==coriginal);x2++)	sol.setCluster(x2,y,cnew);

		for (x2 = (x - 1); (getCluster(x2,y)==coriginal);x2--) sol.setCluster(x2,y,cnew);

		for (x2 = (x - 1); (sol.getCluster(x2,y)==cnew) && (getCluster(x2,y)==coriginal);x2--)
		{
			if ( (sol.getCluster(x2,y-1)==NON_ASIGN) && (getCluster(x2,y-1)==coriginal) ) scanline(coriginal,cnew,x2,y-1,sol);
			if ( (sol.getCluster(x2,y+1)==NON_ASIGN) && (getCluster(x2,y+1)==coriginal) ) scanline(coriginal,cnew,x2,y+1,sol);
		}
		for (x2 = x; (sol.getCluster(x2,y)==cnew) && (getCluster(x2,y)==coriginal);x2++)
		{
			if ( (sol.getCluster(x2,y-1)==NON_ASIGN) && (getCluster(x2,y-1)==coriginal) ) scanline(coriginal,cnew,x2,y-1,sol);
			if ( (sol.getCluster(x2,y+1)==NON_ASIGN) && (getCluster(x2,y+1)==coriginal) ) scanline(coriginal,cnew,x2,y+1,sol);
		}

		for (x2 = (x - 1); (sol.getCluster(x2,y)==cnew) && (getCluster(x2,y)==coriginal);x2--)
		{
			if ( (sol.getCluster(x2+1,y-1)==NON_ASIGN) && (getCluster(x2+1,y-1)==coriginal) ) scanline(coriginal,cnew,x2+1,y-1,sol);
			if ( (sol.getCluster(x2-1,y-1)==NON_ASIGN) && (getCluster(x2-1,y-1)==coriginal) ) scanline(coriginal,cnew,x2-1,y-1,sol);

			if ( (sol.getCluster(x2+1,y+1)==NON_ASIGN) && (getCluster(x2+1,y+1)==coriginal) ) scanline(coriginal,cnew,x2+1,y+1,sol);
			if ( (sol.getCluster(x2-1,y+1)==NON_ASIGN) && (getCluster(x2-1,y+1)==coriginal) ) scanline(coriginal,cnew,x2-1,y+1,sol);

		}

		for (x2 = x; (sol.getCluster(x2,y)==cnew) && (getCluster(x2,y)==coriginal);x2++)
		{
			if ( (sol.getCluster(x2+1,y-1)==NON_ASIGN) && (getCluster(x2+1,y-1)==coriginal) ) scanline(coriginal,cnew,x2+1,y-1,sol);
			if ( (sol.getCluster(x2-1,y-1)==NON_ASIGN) && (getCluster(x2-1,y-1)==coriginal) ) scanline(coriginal,cnew,x2-1,y-1,sol);

			if ( (sol.getCluster(x2+1,y+1)==NON_ASIGN) && (getCluster(x2+1,y+1)==coriginal) ) scanline(coriginal,cnew,x2+1,y+1,sol);
			if ( (sol.getCluster(x2-1,y+1)==NON_ASIGN) && (getCluster(x2-1,y+1)==coriginal) ) scanline(coriginal,cnew,x2-1,y+1,sol);

		}
	} else sol.setCluster(x,y,BACKG_CLUSTER);
}

Segmentacion Segmentacion::get_separate_noncontiguous_clusters()
{
	Segmentacion sol(width(),height());
	sol.imageMapLabels.fill(NON_ASIGN);
	unsigned int cnew = 0;
	int x=0, y=0;

	while (y<height())
	{
		while ( (sol.getCluster(x,y)!=NON_ASIGN) && (y<height()) )
		{
			x++;
			if (x>=width()) { x = 0; y++;}
		}

		if (y<height())	scanline(getCluster(x,y),cnew,x,y,sol);
		cnew++;
	}
	return sol;
}


// Copyright (C) 2002-2006 Klaas Gadeyne <first dot last at gmail dot com>
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation; either version 2.1 of the License, or
// (at your option) any later version.
//
// 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 Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA.
//
#include "rng.h"

#include <../config.h>
#ifdef __RNGWRAPPER_BOOST__  // BOOST RANDOM LIBRARY
// THE BOOST RANDOM NUMBER GENERATION LIBRARY

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/uniform_real.hpp>

static boost::mt19937 Boost_Rng; // Source of randomness

static boost::uniform_real<double> Uniform_Distribution; // Uniform distribution
static boost::variate_generator<boost::mt19937&,boost::uniform_real<double> > roll(Boost_Rng,Uniform_Distribution);

double BFL::rnorm(const double& mu,const double& sigma)
{
  boost::normal_distribution<double> TestDist(mu,sigma);
  boost::variate_generator <boost::mt19937 &,boost::normal_distribution<double> > TestGen(Boost_Rng,TestDist);
  return TestGen();
}

double BFL::runif()
{
  return roll();
}

double BFL::runif(const double &min, const double& max)
{
  boost::uniform_real<double> Uniform_DistributionMinMax(min,max); // Uniform distribution
  boost::variate_generator<boost::mt19937&,boost::uniform_real<double> > roll(Boost_Rng,Uniform_DistributionMinMax);
  return roll();
}

#endif // __RNGWRAPPER_BOOST__