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

// Copyright (c) 2007-2014 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2014 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2014 Mateusz Loskot, London, UK.
// Copyright (c) 2013-2014 Adam Wulkiewicz, Lodz, Poland.

// This file was modified by Oracle on 2013-2020.
// Modifications copyright (c) 2013-2020, Oracle and/or its affiliates.

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

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISJOINT_SEGMENT_BOX_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISJOINT_SEGMENT_BOX_HPP

#include <cstddef>

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

#include <boost/geometry/algorithms/detail/assign_indexed_point.hpp>
#include <boost/geometry/algorithms/detail/disjoint/point_box.hpp>
#include <boost/geometry/algorithms/detail/disjoint/box_box.hpp>
#include <boost/geometry/algorithms/detail/envelope/segment.hpp>
#include <boost/geometry/algorithms/detail/normalize.hpp>
#include <boost/geometry/algorithms/dispatch/disjoint.hpp>
#include <boost/geometry/algorithms/envelope.hpp>

#include <boost/geometry/formulas/vertex_longitude.hpp>

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

// Temporary, for envelope_segment_impl
#include <boost/geometry/strategy/spherical/envelope_segment.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace disjoint
{

template <typename CS_Tag>
struct disjoint_segment_box_sphere_or_spheroid
{
    struct disjoint_info
    {
        enum type
        {
            intersect,
            disjoint_no_vertex,
            disjoint_vertex
        };
        disjoint_info(type t) : m_(t){}
        operator type () const {return m_;}
        type m_;
    private :
        //prevent automatic conversion for any other built-in types
        template <typename T>
        operator T () const;
    };

    template
    <
        typename Segment, typename Box,
        typename AzimuthStrategy,
        typename NormalizeStrategy,
        typename DisjointPointBoxStrategy,
        typename DisjointBoxBoxStrategy
    >
    static inline bool apply(Segment const& segment,
                             Box const& box,
                             AzimuthStrategy const& azimuth_strategy,
                             NormalizeStrategy const& normalize_strategy,
                             DisjointPointBoxStrategy const& disjoint_point_box_strategy,
                             DisjointBoxBoxStrategy const& disjoint_box_box_strategy)
    {
        typedef typename point_type<Segment>::type segment_point;
        segment_point vertex;
        return apply(segment, box, vertex,
                     azimuth_strategy,
                     normalize_strategy,
                     disjoint_point_box_strategy,
                     disjoint_box_box_strategy) != disjoint_info::intersect;
    }

    template
    <
        typename Segment, typename Box,
        typename P,
        typename AzimuthStrategy,
        typename NormalizeStrategy,
        typename DisjointPointBoxStrategy,
        typename DisjointBoxBoxStrategy
    >
    static inline disjoint_info apply(Segment const& segment,
                                      Box const& box,
                                      P& vertex,
                                      AzimuthStrategy const& azimuth_strategy,
                                      NormalizeStrategy const& ,
                                      DisjointPointBoxStrategy const& disjoint_point_box_strategy,
                                      DisjointBoxBoxStrategy const& disjoint_box_box_strategy)
    {
        assert_dimension_equal<Segment, Box>();

        typedef typename point_type<Segment>::type segment_point_type;

        segment_point_type p0, p1;
        geometry::detail::assign_point_from_index<0>(segment, p0);
        geometry::detail::assign_point_from_index<1>(segment, p1);

        //vertex not computed here
        disjoint_info disjoint_return_value = disjoint_info::disjoint_no_vertex;

        // Simplest cases first

        // Case 1: if box contains one of segment's endpoints then they are not disjoint
        if ( ! disjoint_point_box(p0, box, disjoint_point_box_strategy)
          || ! disjoint_point_box(p1, box, disjoint_point_box_strategy) )
        {
            return disjoint_info::intersect;
        }

        // Case 2: disjoint if bounding boxes are disjoint

        typedef typename coordinate_type<segment_point_type>::type CT;

        segment_point_type p0_normalized;
        NormalizeStrategy::apply(p0, p0_normalized);
        segment_point_type p1_normalized;
        NormalizeStrategy::apply(p1, p1_normalized);

        CT lon1 = geometry::get_as_radian<0>(p0_normalized);
        CT lat1 = geometry::get_as_radian<1>(p0_normalized);
        CT lon2 = geometry::get_as_radian<0>(p1_normalized);
        CT lat2 = geometry::get_as_radian<1>(p1_normalized);

        if (lon1 > lon2)
        {
            std::swap(lon1, lon2);
            std::swap(lat1, lat2);
        }

        geometry::model::box<segment_point_type> box_seg;

        strategy::envelope::detail::envelope_segment_impl
            <
                CS_Tag
            >::template apply<geometry::radian>(lon1, lat1,
                                                lon2, lat2,
                                                box_seg,
                                                azimuth_strategy);

        if (disjoint_box_box(box, box_seg, disjoint_box_box_strategy))
        {
            return disjoint_return_value;
        }

        // Case 3: test intersection by comparing angles

        CT alp1, a_b0, a_b1, a_b2, a_b3;

        CT b_lon_min = geometry::get_as_radian<geometry::min_corner, 0>(box);
        CT b_lat_min = geometry::get_as_radian<geometry::min_corner, 1>(box);
        CT b_lon_max = geometry::get_as_radian<geometry::max_corner, 0>(box);
        CT b_lat_max = geometry::get_as_radian<geometry::max_corner, 1>(box);

        azimuth_strategy.apply(lon1, lat1, lon2, lat2, alp1);
        azimuth_strategy.apply(lon1, lat1, b_lon_min, b_lat_min, a_b0);
        azimuth_strategy.apply(lon1, lat1, b_lon_max, b_lat_min, a_b1);
        azimuth_strategy.apply(lon1, lat1, b_lon_min, b_lat_max, a_b2);
        azimuth_strategy.apply(lon1, lat1, b_lon_max, b_lat_max, a_b3);

        int s0 = formula::azimuth_side_value(alp1, a_b0);
        int s1 = formula::azimuth_side_value(alp1, a_b1);
        int s2 = formula::azimuth_side_value(alp1, a_b2);
        int s3 = formula::azimuth_side_value(alp1, a_b3);

        if (s0 == 0 || s1 == 0 || s2 == 0 || s3 == 0)
        {
            return disjoint_info::intersect;
        }

        bool s0_positive = s0 > 0;
        bool s1_positive = s1 > 0;
        bool s2_positive = s2 > 0;
        bool s3_positive = s3 > 0;

        bool all_positive = s0_positive && s1_positive && s2_positive && s3_positive;
        bool all_non_positive = !(s0_positive || s1_positive || s2_positive || s3_positive);
        bool vertex_north = lat1 + lat2 > 0;

        if ((all_positive && vertex_north) || (all_non_positive && !vertex_north))
        {
            return disjoint_info::disjoint_no_vertex;
        }

        if (!all_positive && !all_non_positive)
        {
            return disjoint_info::intersect;
        }

        // Case 4: The only intersection case not covered above is when all four
        // points of the box are above (below) the segment in northern (southern)
        // hemisphere. Then we have to compute the vertex of the segment

        CT vertex_lat;

        if ((lat1 < b_lat_min && vertex_north)
                || (lat1 > b_lat_max && !vertex_north))
        {
            CT b_lat_below; //latitude of box closest to equator

            if (vertex_north)
            {
                vertex_lat = geometry::get_as_radian<geometry::max_corner, 1>(box_seg);
                b_lat_below = b_lat_min;
            } else {
                vertex_lat = geometry::get_as_radian<geometry::min_corner, 1>(box_seg);
                b_lat_below = b_lat_max;
            }

            //optimization TODO: computing the spherical longitude should suffice for
            // the majority of cases
            CT vertex_lon = geometry::formula::vertex_longitude<CT, CS_Tag>
                                    ::apply(lon1, lat1,
                                            lon2, lat2,
                                            vertex_lat,
                                            alp1,
                                            azimuth_strategy);

            geometry::set_from_radian<0>(vertex, vertex_lon);
            geometry::set_from_radian<1>(vertex, vertex_lat);
            disjoint_return_value = disjoint_info::disjoint_vertex; //vertex_computed

            // Check if the vertex point is within the band defined by the
            // minimum and maximum longitude of the box; if yes, then return
            // false if the point is above the min latitude of the box; return
            // true in all other cases
            if (vertex_lon >= b_lon_min && vertex_lon <= b_lon_max
                    && std::abs(vertex_lat) > std::abs(b_lat_below))
            {
                return disjoint_info::intersect;
            }
        }

        return disjoint_return_value;
    }
};

struct disjoint_segment_box
{
    template <typename Segment, typename Box, typename Strategy>
    static inline bool apply(Segment const& segment,
                             Box const& box,
                             Strategy const& strategy)
    {
        return strategy.disjoint(segment, box).apply(segment, box);
    }
};

}} // namespace detail::disjoint
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename Segment, typename Box, std::size_t DimensionCount>
struct disjoint<Segment, Box, DimensionCount, segment_tag, box_tag, false>
        : detail::disjoint::disjoint_segment_box
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISJOINT_SEGMENT_BOX_HPP


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

/* Author: Andreas Orthey */

#include <ompl/multilevel/datastructures/PlannerDataVertexAnnotated.h>
#include <ompl/multilevel/datastructures/BundleSpaceGraph.h>
#include <ompl/multilevel/datastructures/src/BundleSpaceGraphGoalVisitor.hpp>

#include <ompl/multilevel/datastructures/graphsampler/RandomVertex.h>
#include <ompl/multilevel/datastructures/graphsampler/RandomDegreeVertex.h>
#include <ompl/multilevel/datastructures/graphsampler/RandomEdge.h>
#include <ompl/multilevel/datastructures/importance/Greedy.h>
#include <ompl/multilevel/datastructures/importance/Exponential.h>
#include <ompl/multilevel/datastructures/importance/Uniform.h>
#include <ompl/multilevel/datastructures/metrics/Geodesic.h>
#include <ompl/multilevel/datastructures/metrics/ShortestPath.h>
#include <ompl/multilevel/datastructures/propagators/Geometric.h>
#include <ompl/multilevel/datastructures/propagators/Dynamic.h>
#include <ompl/multilevel/datastructures/pathrestriction/PathRestriction.h>

#include <ompl/geometric/PathSimplifier.h>
#include <ompl/base/goals/GoalSampleableRegion.h>
#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
#include <ompl/base/objectives/MaximizeMinClearanceObjective.h>
#include <ompl/datastructures/PDF.h>
#include <ompl/tools/config/SelfConfig.h>
#include <ompl/tools/config/MagicConstants.h>
#include <ompl/util/Exception.h>
#include <ompl/control/SpaceInformation.h>

#include <ompl/datastructures/NearestNeighborsSqrtApprox.h>

#include <boost/graph/astar_search.hpp>
#include <boost/graph/incremental_components.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/property_map/vector_property_map.hpp>
#include <boost/property_map/transform_value_property_map.hpp>

#include <boost/foreach.hpp>

#define foreach BOOST_FOREACH

using Configuration = ompl::multilevel::BundleSpaceGraph::Configuration;

using namespace ompl::multilevel;

BundleSpaceGraph::BundleSpaceGraph(
    const ompl::base::SpaceInformationPtr &si,
    BundleSpace *parent_) : BaseT(si, parent_)
{
    setName("BundleSpaceGraph");

    // Functional primitives
    setMetric("geodesic");
    setGraphSampler("randomvertex");
    setImportance("uniform");

    if (hasBaseSpace())
    {
        pathRestriction_ = std::make_shared<PathRestriction>(this);
    }

    if (isDynamic())
    {
        setPropagator("dynamic");
    }
    else
    {
        setPropagator("geometric");
    }

    specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
    specs_.approximateSolutions = false;
    specs_.optimizingPaths = false;

    Planner::declareParam<double>(
        "range",
        this,
        &BundleSpaceGraph::setRange,
        &BundleSpaceGraph::getRange,
        "0.:1.:10000.");

    Planner::declareParam<double>(
        "goal_bias",
        this,
        &BundleSpaceGraph::setGoalBias,
        &BundleSpaceGraph::getGoalBias,
        "0.:.1:1.");

    xRandom_ = new Configuration(getBundle());

    if (!isSetup())
    {
        this->setup();
    }

    if (getFiberDimension() > 0)
    {
        xFiberTmp1_ = getFiber()->allocState();
        xFiberTmp2_ = getFiber()->allocState();
    }
}

BundleSpaceGraph::~BundleSpaceGraph()
{
    deleteConfiguration(xRandom_);
    if (getFiberDimension() > 0)
    {
        getFiber()->freeState(xFiberTmp1_);
        getFiber()->freeState(xFiberTmp2_);
    }
}

void BundleSpaceGraph::setup()
{
    BaseT::setup();

    ompl::tools::SelfConfig sc(getBundle(), getName());
    sc.configurePlannerRange(maxDistance_);

    OMPL_DEBUG("Range distance graph sampling: %f (max extent %f)", maxDistance_, getBundle()->getMaximumExtent());

    if (!nearestDatastructure_)
    {
        if (!isDynamic())
        {
            nearestDatastructure_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Configuration *>(this));
        }
        else
        {
            // for dynamical systems, we use a quasimetric
            //(non-symmetric metric), so we cannot use NN structures like GNAT
            nearestDatastructure_.reset(new NearestNeighborsSqrtApprox<Configuration *>());
        }
        nearestDatastructure_->setDistanceFunction(
            [this](const Configuration *a, const Configuration *b) { return distance(a, b); });
    }

    if (pdef_)
    {
        firstRun_ = true;
        setup_ = true;

        optimizer_ = std::make_shared<ompl::geometric::PathSimplifier>(
            getBundle(), pdef_->getGoal(), getOptimizationObjectivePtr());
        optimizer_->freeStates(false);
    }
    else
    {
        setup_ = false;
    }

}

void BundleSpaceGraph::setFindSectionStrategy(FindSectionType type)
{
    if (pathRestriction_ != nullptr)
    {
        pathRestriction_->setFindSectionStrategy(type);
    }
}

bool BundleSpaceGraph::findSection()
{
    if (hasBaseSpace())
    {
        if (getPathRestriction()->hasFeasibleSection(qStart_, qGoal_))
        {
            if (sameComponent(vStart_, getGoalIndex()))
            {
                hasSolution_ = true;
                return true;
            }
        }
    }
    return false;
}

void BundleSpaceGraph::clear()
{
    BaseT::clear();

    clearVertices();
    pis_.restart();

    graphLength_ = 0;
    bestCost_ = base::Cost(base::dInf);
    setup_ = false;
    vStart_ = 0;
    vGoal_ = 0;
    lengthStartGoalVertexPath_ = base::dInf;
    shortestVertexPath_.clear();

    qStart_ = nullptr;
    qGoal_ = nullptr;

    if (!isDynamic())
    {
        if (solutionPath_ != nullptr)
        {
            std::static_pointer_cast<geometric::PathGeometric>(solutionPath_)->clear();
        }
    }

    numVerticesWhenComputingSolutionPath_ = 0;

    importanceCalculator_->clear();
    graphSampler_->clear();
    if (pathRestriction_ != nullptr)
    {
        pathRestriction_->clear();
    }
}

void BundleSpaceGraph::clearVertices()
{
    if (nearestDatastructure_)
    {
        std::vector<Configuration *> configs;
        nearestDatastructure_->list(configs);
        for (auto &config : configs)
        {
            deleteConfiguration(config);
        }
        nearestDatastructure_->clear();
    }
    graph_.clear();
}

void BundleSpaceGraph::setGoalBias(double goalBias)
{
    goalBias_ = goalBias;
}

double BundleSpaceGraph::getGoalBias() const
{
    return goalBias_;
}

void BundleSpaceGraph::setRange(double maxDistance)
{
    maxDistance_ = maxDistance;
}

double BundleSpaceGraph::getRange() const
{
    return maxDistance_;
}

BundleSpaceGraph::Configuration::Configuration(const ompl::base::SpaceInformationPtr &si) : state(si->allocState())
{
    // Too costly to do dynamic_pointer_cast in every call. Best to create
    // different Configuration classes (dynamic, geometric, etc). Or do inside
    // addconfiguration function.
    //
    //
    // const ompl::control::SpaceInformationPtr siC = std::dynamic_pointer_cast<ompl::control::SpaceInformation>(si);
    // if (siC != nullptr)
    // {
    //     control = siC->allocControl();
    // }
}
BundleSpaceGraph::Configuration::Configuration(const ompl::base::SpaceInformationPtr &si,
                                               const ompl::base::State *state_)
  : state(si->cloneState(state_))
{
    // const ompl::control::SpaceInformationPtr siC = std::dynamic_pointer_cast<ompl::control::SpaceInformation>(si);

    // if (siC != nullptr)
    // {
    //     control = siC->allocControl();
    // }
}

void BundleSpaceGraph::deleteConfiguration(Configuration *q)
{
    if (q != nullptr)
    {
        if (q->state != nullptr)
        {
            getBundle()->freeState(q->state);
        }
        for (unsigned int k = 0; k < q->reachableSet.size(); k++)
        {
            Configuration *qk = q->reachableSet.at(k);
            if (qk->state != nullptr)
            {
                getBundle()->freeState(qk->state);
            }
        }
        if (isDynamic())
        {
            const ompl::control::SpaceInformationPtr siC =
                std::static_pointer_cast<ompl::control::SpaceInformation>(getBundle());
            siC->freeControl(q->control);
        }
        q->reachableSet.clear();

        delete q;
        q = nullptr;
    }
}

double BundleSpaceGraph::getImportance() const
{
    return importanceCalculator_->eval();
}

void BundleSpaceGraph::init()
{
    if (const base::State *state = pis_.nextStart())
    {
        qStart_ = new Configuration(getBundle(), state);
        vStart_ = addConfiguration(qStart_);
        qStart_->isStart = true;
    }

    if (qStart_ == nullptr)
    {
        OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
        throw ompl::Exception("Invalid initial states.");
    }

    // auto *goal = static_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
    // if (goal == nullptr)
    // {
    //     OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
    //     throw ompl::Exception("Unknown goal type");
    // }

    if (const base::State *state = pis_.nextGoal())
    {
        qGoal_ = new Configuration(getBundle(), state);
        qGoal_->isGoal = true;
    }

    if (qGoal_ == nullptr)
    {
        OMPL_ERROR("%s: There are no valid goal states!", getName().c_str());
        throw ompl::Exception("Invalid goal states.");
    }
}

void BundleSpaceGraph::uniteComponents(Vertex m1, Vertex m2)
{
    disjointSets_.union_set(m1, m2);
}

bool BundleSpaceGraph::sameComponent(Vertex m1, Vertex m2)
{
    return boost::same_component(m1, m2, disjointSets_);
}

const BundleSpaceGraph::Configuration *BundleSpaceGraph::nearest(const Configuration *q) const
{
    return nearestDatastructure_->nearest(const_cast<Configuration *>(q));
}

BundleSpaceGraph::Configuration *BundleSpaceGraph::addBundleConfiguration(ompl::base::State *state)
{
    Configuration *x = new Configuration(getBundle(), state);
    addConfiguration(x);
    return x;
}

void BundleSpaceGraph::addBundleEdge(const Configuration *a, const Configuration *b)
{
    //@DEBUG
    // bool feasible = getBundle()->checkMotion(a->state, b->state);
    // if(!feasible)
    // {
    //   std::cout << "Trying to add infeasible bundle edge" << std::endl;
    //   getBundle()->printState(a->state);
    //   if(!getBundle()->isValid(a->state))
    //   {
    //     std::cout << "Infeasible state" << std::endl;
    //   }
    //   getBundle()->printState(b->state);
    //   if(!getBundle()->isValid(b->state))
    //   {
    //     std::cout << "Infeasible state" << std::endl;
    //   }
    //   throw "";
    // }
    //@DEBUG
    addEdge(a->index, b->index);
}

BundleSpaceGraph::Vertex BundleSpaceGraph::addConfiguration(Configuration *q)
{
    // @DEBUG
    // std::vector<double> reals;
    // getBundle()->getStateSpace()->copyToReals(reals, q->state);
    // if(reals != reals)
    // {
    //     getBundle()->printState(q->state);
    //     throw Exception("INVALID");
    // }
    // @DEBUG

    Vertex m = boost::add_vertex(q, graph_);
    graph_[m]->total_connection_attempts = 1;
    graph_[m]->successful_connection_attempts = 0;
    disjointSets_.make_set(m);

    nearestDatastructure_->add(q);
    q->index = m;

    return m;
}

unsigned int BundleSpaceGraph::getNumberOfVertices() const
{
    return num_vertices(graph_);
}

unsigned int BundleSpaceGraph::getNumberOfEdges() const
{
    return num_edges(graph_);
}

const BundleSpaceGraph::Graph &BundleSpaceGraph::getGraph() const
{
    return graph_;
}

BundleSpaceGraph::Graph &BundleSpaceGraph::getGraphNonConst()
{
    return graph_;
}

const BundleSpaceGraph::RoadmapNeighborsPtr &BundleSpaceGraph::getRoadmapNeighborsPtr() const
{
    return nearestDatastructure_;
}

ompl::base::Cost BundleSpaceGraph::costHeuristic(Vertex u, Vertex v) const
{
    return getOptimizationObjectivePtr()->motionCostHeuristic(
        graph_[u]->state, graph_[v]->state);
}

template <template <typename T> class NN>
void BundleSpaceGraph::setNearestNeighbors()
{
    if (nearestDatastructure_ && nearestDatastructure_->size() == 0)
        OMPL_WARN("Calling setNearestNeighbors will clear all states.");
    clear();
    nearestDatastructure_ = std::make_shared<NN<base::State *>>();
    if (!isSetup())
    {
        setup();
    }
}

double BundleSpaceGraph::distance(const Configuration *a, const Configuration *b) const
{
    return metric_->distanceBundle(a, b);
}

bool BundleSpaceGraph::checkMotion(const Configuration *a, const Configuration *b) const
{
    return getBundle()->checkMotion(a->state, b->state);
}

void BundleSpaceGraph::interpolate(const Configuration *a, const Configuration *b, Configuration *dest) const
{
    metric_->interpolateBundle(a, b, dest);
}

Configuration *BundleSpaceGraph::steerTowards(const Configuration *from, const Configuration *to)
{
    Configuration *next = new Configuration(getBundle(), to->state);

    if (!propagator_->steer(from, to, next))
    {
        deleteConfiguration(next);
        return nullptr;
    }
    return next;
}

Configuration *BundleSpaceGraph::steerTowards_Range(const Configuration *from, Configuration *to)
{
    // Configuration *next = new Configuration(getBundle(), to->state);

    double d = distance(from, to);
    if (d > maxDistance_)
    {
        metric_->interpolateBundle(from, to, maxDistance_ / d, to);
    }

    if (!propagator_->steer(from, to, to))
    {
        return nullptr;
    }
    Configuration *next = new Configuration(getBundle(), to->state);
    return next;
}

Configuration *BundleSpaceGraph::extendGraphTowards_Range(const Configuration *from, Configuration *to)
{
    // Configuration *next = new Configuration(getBundle(), to->state);

    if (!isDynamic())
    {
        double d = distance(from, to);
        if (d > maxDistance_)
        {
            metric_->interpolateBundle(from, to, maxDistance_ / d, to);
        }
    }

    if (!propagator_->steer(from, to, to))
    {
        return nullptr;
    }

    Configuration *next = new Configuration(getBundle(), to->state);
    // if (isDynamic())
    // {
    //     const ompl::control::SpaceInformationPtr siC =
    //         std::dynamic_pointer_cast<ompl::control::SpaceInformation>(getBundle());
    //     const control::Control *lastControl =
    //         std::static_pointer_cast<BundleSpacePropagatorDynamic>(propagator_)->getLastControl();
    //     siC->copyControl(next->control, lastControl);
    // }
    addConfiguration(next);
    addBundleEdge(from, next);
    return next;
}

bool BundleSpaceGraph::connect(const Configuration *from, const Configuration *to)
{
    if (!propagator_->steer(from, to, xRandom_))
    {
        return false;
    }

    addBundleEdge(from, to);
    return true;
}

void BundleSpaceGraph::setPropagator(const std::string &sPropagator)
{
    if (sPropagator == "geometric")
    {
        OMPL_DEBUG("Geometric Propagator Selected");
        propagator_ = std::make_shared<BundleSpacePropagatorGeometric>(this);
    }
    else if (sPropagator == "dynamic")
    {
        OMPL_DEBUG("Dynamic Propagator Selected");
        propagator_ = std::make_shared<BundleSpacePropagatorDynamic>(this);
    }
    else
    {
        OMPL_ERROR("Propagator unknown: %s", sPropagator.c_str());
        throw ompl::Exception("Unknown Propagator");
    }
}

void BundleSpaceGraph::setMetric(const std::string &sMetric)
{
    if (sMetric == "geodesic")
    {
        OMPL_DEBUG("Geodesic Metric Selected");
        metric_ = std::make_shared<BundleSpaceMetricGeodesic>(this);
    }
    else if (sMetric == "shortestpath")
    {
        OMPL_DEBUG("ShortestPath Metric Selected");
        metric_ = std::make_shared<BundleSpaceMetricShortestPath>(this);
    }
    else
    {
        OMPL_ERROR("Metric unknown: %s", sMetric.c_str());
        throw ompl::Exception("Unknown Metric");
    }
}

void BundleSpaceGraph::setImportance(const std::string &sImportance)
{
    if (sImportance == "uniform")
    {
        OMPL_DEBUG("Uniform Importance Selected");
        importanceCalculator_ = std::make_shared<BundleSpaceImportanceUniform>(this);
    }
    else if (sImportance == "greedy")
    {
        OMPL_DEBUG("Greedy Importance Selected");
        importanceCalculator_ = std::make_shared<BundleSpaceImportanceGreedy>(this);
    }
    else if (sImportance == "exponential")
    {
        OMPL_DEBUG("Greedy Importance Selected");
        importanceCalculator_ = std::make_shared<BundleSpaceImportanceExponential>(this);
    }
    else
    {
        OMPL_ERROR("Importance calculator unknown: %s", sImportance.c_str());
        throw ompl::Exception("Unknown Importance");
    }
}

void BundleSpaceGraph::setGraphSampler(const std::string &sGraphSampler)
{
    if (sGraphSampler == "randomvertex")
    {
        OMPL_DEBUG("Random Vertex Sampler Selected");
        graphSampler_ = std::make_shared<BundleSpaceGraphSamplerRandomVertex>(this);
    }
    else if (sGraphSampler == "randomedge")
    {
        OMPL_DEBUG("Random Edge Sampler Selected");
        graphSampler_ = std::make_shared<BundleSpaceGraphSamplerRandomEdge>(this);
    }
    else if (sGraphSampler == "randomdegreevertex")
    {
        OMPL_DEBUG("Random Degree Vertex Sampler Selected");
        graphSampler_ = std::make_shared<BundleSpaceGraphSamplerRandomDegreeVertex>(this);
    }
    else
    {
        OMPL_ERROR("Sampler unknown: %s", sGraphSampler.c_str());
        throw ompl::Exception("Unknown Graph Sampler");
    }
}

BundleSpaceGraphSamplerPtr BundleSpaceGraph::getGraphSampler()
{
    return graphSampler_;
}

const std::pair<BundleSpaceGraph::Edge, bool>
BundleSpaceGraph::addEdge(const Vertex a, const Vertex b)
{
    base::Cost weight =
      getOptimizationObjectivePtr()->motionCost(graph_[a]->state, graph_[b]->state);
    EdgeInternalState properties(weight);
    const std::pair<Edge, bool> e = boost::add_edge(a, b, properties, graph_);
    uniteComponents(a, b);
    return e;
}

double BundleSpaceGraph::getGraphLength() const
{
    return graphLength_;
}

BundleSpaceGraph::Vertex BundleSpaceGraph::getStartIndex() const
{
    return vStart_;
}
BundleSpaceGraph::Vertex BundleSpaceGraph::getGoalIndex() const
{
  if(qGoal_)
  {
    return qGoal_->index;
  }else
  {
    std::cout << "NullVertex" << std::endl;
    return nullVertex();
  }
}

BundleSpaceGraph::Vertex BundleSpaceGraph::nullVertex() const
{
    return BGT::null_vertex();
}

void BundleSpaceGraph::setStartIndex(Vertex idx)
{
    vStart_ = idx;
}
void BundleSpaceGraph::setGoalIndex(Vertex idx)
{
    vGoal_ = idx;
}

ompl::base::PathPtr& BundleSpaceGraph::getSolutionPathByReference()
{
  return solutionPath_;
}

bool BundleSpaceGraph::getSolution(ompl::base::PathPtr &solution)
{
    if (hasSolution_)
    {
        if ((solutionPath_ != nullptr) && (getNumberOfVertices() == numVerticesWhenComputingSolutionPath_))
        {
        }
        else
        {
            solutionPath_ = getPath(vStart_, getGoalIndex());
            numVerticesWhenComputingSolutionPath_ = getNumberOfVertices();

            if (!isDynamic() && solutionPath_ != solution && hasTotalSpace())
            {
                // bool optimize = true;
                // int type = getBundle()->getStateSpace()->getType();
                // // if(type == base::STATE_SPACE_DUBINS || type == base::STATE_SPACE_DUBINS_AIRPLANE)
                // if(type == base::STATE_SPACE_DUBINS
                //     || type == base::STATE_SPACE_DUBINS_AIRPLANE)
                // {
                //   optimize = false;
                // }
                // if(!optimize && getBundle()->getStateSpace()->isCompound())
                // {
                //     std::vector<base::StateSpacePtr> Bundle_decomposed;
                //     base::CompoundStateSpace *Bundle_compound =
                //       getBundle()->getStateSpace()->as<base::CompoundStateSpace>();
                //     Bundle_decomposed = Bundle_compound->getSubspaces();
                //     for(unsigned int k = 0; k < Bundle_decomposed.size(); k++)
                //     {
                //       int tk = Bundle_decomposed.at(k)->getType();
                //       if(tk == base::STATE_SPACE_DUBINS || tk == base::STATE_SPACE_DUBINS_AIRPLANE)
                //       {
                //         optimize = false;
                //         break;
                //       }
                //     }
                // }

                // if(optimize)
                // {
                // geometric::PathSimplifier shortcutter(
                //     getBundle(), pdef_->getGoal(), pathRefinementObj_);

                // @NOTE: optimization seems to improve feasibility of sections
                // in low-dim problems (up to 20 dof roughly), but will take too
                // much time for high-dim problems. Reducing vertices seems to
                // be the only optimization not significantly slowing everything
                // down.

                bool valid = false;
                // std::cout << "Optimize" << std::endl;
                for (unsigned int k = 0; k < 3; k++)
                {
                    geometric::PathGeometric &gpath = static_cast<geometric::PathGeometric &>(*solutionPath_);

                    valid = optimizer_->reduceVertices(gpath, 0, 0, 0.1);

                    // valid = optimizer_->simplifyMax(gpath);

                    if (!valid)
                    {
                        // reset solutionPath
                        solutionPath_ = getPath(vStart_, getGoalIndex());
                    }
                    else
                    {
                        break;
                    }
                }
                // std::cout << "Done" << std::endl;

                // geometric::PathGeometric &gpath2 =
                //   static_cast<geometric::PathGeometric &>(*solutionPath_);

                // optimizer_->smoothBSpline(gpath2);

                // gpath2.interpolate();
                // }
            }
        }
        solution = solutionPath_;
        return true;
    }
    else
    {
        return false;
    }
}

ompl::base::PathPtr BundleSpaceGraph::getPath(const Vertex &start, const Vertex &goal)
{
    return getPath(start, goal, graph_);
}

ompl::base::PathPtr BundleSpaceGraph::getPath(const Vertex &start, const Vertex &goal, Graph &graph)
{
    std::vector<Vertex> prev(boost::num_vertices(graph));
    auto weight = boost::make_transform_value_property_map(
        std::mem_fn(&EdgeInternalState::getCost),
        get(boost::edge_bundle, graph));

    try
    {
        boost::astar_search(
            graph,
            start,
            [this, goal](const Vertex v) { return costHeuristic(v, goal); },
            boost::predecessor_map(&prev[0])
            .weight_map(weight)
            .distance_compare(
              [this](EdgeInternalState c1, EdgeInternalState c2)
              {
                return getOptimizationObjectivePtr()->isCostBetterThan(
                    c1.getCost(), c2.getCost());
              })
            .distance_combine(
              [this](EdgeInternalState c1, EdgeInternalState c2)
              {
                return getOptimizationObjectivePtr()->combineCosts(
                    c1.getCost(), c2.getCost());
              })
            .distance_inf(getOptimizationObjectivePtr()->infiniteCost())
            .distance_zero(getOptimizationObjectivePtr()->identityCost())
            .visitor(BundleSpaceGraphGoalVisitor<Vertex>(goal))
        );
    }
    catch (BundleSpaceGraphFoundGoal &)
    {
    }

    auto p(std::make_shared<geometric::PathGeometric>(getBundle()));
    if (prev[goal] == goal)
    {
        return nullptr;
    }

    std::vector<Vertex> vpath;
    for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
    {
        graph[pos]->on_shortest_path = true;
        vpath.push_back(pos);
        p->append(graph[pos]->state);
    }
    graph[start]->on_shortest_path = true;
    vpath.push_back(start);
    p->append(graph[start]->state);

    shortestVertexPath_.clear();
    shortestVertexPath_.insert(shortestVertexPath_.begin(), vpath.rbegin(), vpath.rend());
    p->reverse();

    return p;
}

const PathRestrictionPtr BundleSpaceGraph::getPathRestriction()
{
    if (!hasBaseSpace())
    {
        OMPL_WARN("Tried getting path restriction without base space");
        return nullptr;
    }

    base::PathPtr basePath =
      static_cast<BundleSpaceGraph *>(getBaseBundleSpace())->getSolutionPathByReference();

    pathRestriction_->setBasePath(basePath);

    return pathRestriction_;
}

void BundleSpaceGraph::sampleBundleGoalBias(ompl::base::State *xRandom)
{
    if (hasSolution_)
    {
        // No Goal Biasing if we already found a solution on this bundle space
        sampleBundle(xRandom);
    }
    else
    {
        double s = rng_.uniform01();
        if (s < goalBias_)
        {
            getGoalPtr()->sampleGoal(xRandom);
        }
        else
        {
            sampleBundle(xRandom);
        }
    }
}

void BundleSpaceGraph::sampleFromDatastructure(ompl::base::State *xRandom)
{
    graphSampler_->sample(xRandom);
}

void BundleSpaceGraph::writeToGraphviz(std::string filename) const
{
  std::ofstream f(filename.c_str());
  std::vector<std::string> annotationVec;
  foreach (const Vertex v, boost::vertices(graph_))
  {
      Configuration *qv = graph_[v];
      const base::State *s = qv->state;
      std::ostringstream out;
      getBundle()->printState(s, out);
      annotationVec.push_back(out.str());
  }
  write_graphviz (f, graph_, boost::make_label_writer(&annotationVec[0]));
}

void BundleSpaceGraph::print(std::ostream &out) const
{
    BaseT::print(out);
    out << std::endl
        << " --[BundleSpaceGraph has "
        << getNumberOfVertices() << " vertices and "
        << getNumberOfEdges() << " edges.]"
        << std::endl;
}

void BundleSpaceGraph::printConfiguration(const Configuration *q) const
{
    getBundle()->printState(q->state);
}

void BundleSpaceGraph::getPlannerDataGraph(ompl::base::PlannerData &data, const Graph &graph, const Vertex vStart,
                                           const Vertex vGoal) const
{
    if (boost::num_vertices(graph) <= 0)
        return;

    multilevel::PlannerDataVertexAnnotated pstart(graph[vStart]->state);
    pstart.setLevel(getLevel());
    data.addStartVertex(pstart);

    if (hasSolution_)
    {
        multilevel::PlannerDataVertexAnnotated pgoal(graph[vGoal]->state);
        pgoal.setLevel(getLevel());
        data.addGoalVertex(pgoal);
        if (solutionPath_ != nullptr)
        {
            geometric::PathGeometric &gpath = static_cast<geometric::PathGeometric &>(*solutionPath_);

            std::vector<base::State *> gstates = gpath.getStates();

            multilevel::PlannerDataVertexAnnotated *pLast = &pstart;
            for (unsigned int k = 1; k < gstates.size() - 1; k++)
            {
                multilevel::PlannerDataVertexAnnotated p(gstates.at(k));
                p.setLevel(getLevel());
                data.addVertex(p);
                data.addEdge(*pLast, p);
                pLast = &p;
            }
            data.addEdge(*pLast, pgoal);
        }
    }

    foreach (const Edge e, boost::edges(graph))
    {
        const Vertex v1 = boost::source(e, graph);
        const Vertex v2 = boost::target(e, graph);

        multilevel::PlannerDataVertexAnnotated p1(graph[v1]->state);
        multilevel::PlannerDataVertexAnnotated p2(graph[v2]->state);
        p1.setLevel(getLevel());
        p2.setLevel(getLevel());
        data.addEdge(p1, p2);
    }
    foreach (const Vertex v, boost::vertices(graph))
    {
        multilevel::PlannerDataVertexAnnotated p(graph[v]->state);
        p.setLevel(getLevel());
        data.addVertex(p);
    }
}

void BundleSpaceGraph::getPlannerData(ompl::base::PlannerData &data) const
{
    OMPL_DEBUG("Graph (level %d) has %d/%d vertices/edges",
        getLevel(),
        boost::num_vertices(graph_),
        boost::num_edges(graph_));

    if (bestCost_.value() < ompl::base::dInf)
    {
        OMPL_DEBUG("Best Cost: %.2f", bestCost_.value());
    }
    getPlannerDataGraph(data, graph_, vStart_, getGoalIndex());
}


#include "Fusion.h"

#include <boost/qvm/all.hpp>

#include "qvm_eigen.h"
#include "qvm_cuda.h"

std::vector<const MergedSolutionActor *> getActorPointer(const MergedSolution & solution)
{
	std::vector<const MergedSolutionActor *> actors;
	actors.reserve(solution.actors.size());
	std::transform(solution.actors.begin(), solution.actors.end(), std::back_inserter(actors),
	               [](const MergedSolutionActor & a) { return &a; });
	return actors;
}

bool isInside(const MergedSolutionActor & actor, const sfs::VoxelCluster & volume)
{
	const auto & r = volume.getPreciseBoundingBox();
	const auto & p = actor.position.get();

	return
		p(0) >= r.x1 && p(0) <= r.x2 &&
		p(1) >= r.y1 && p(1) <= r.y2;
}

float dist(const sfs::Voxel * pVoxel, const MergedSolutionActor & a)
{
	auto d = pVoxel->center - boost::qvm::convert_to<float3>(a.position.get());
	d.z = 0;
	return length(d);
}

std::vector<FusedSolutionActor> splitMultiAssignedVolume(const sfs::VoxelCluster & volume,
                                                         const std::vector<MergedSolutionActor> & actors)
{
	std::vector<std::vector<const sfs::Voxel *>> voxelsAssignedToActor(actors.size(),
	                                                                   std::vector<const sfs::Voxel *>{ });

	for (const auto pVoxel : volume.getVoxel())
	{
		const auto closestActorIt = std::min_element(actors.begin(), actors.end(),
		                                             [pVoxel](const MergedSolutionActor & a1,
		                                                      const MergedSolutionActor a2)
		                                             {
			                                             return dist(pVoxel, a1) < dist(pVoxel, a2);
		                                             });

		const auto closestActorIdx = closestActorIt - actors.begin();

		voxelsAssignedToActor[closestActorIdx].push_back(pVoxel);
	}

	std::vector<FusedSolutionActor> ret;
	ret.reserve(actors.size());

	for (size_t idx_actor = 0; idx_actor < actors.size(); ++idx_actor)
	{
		if(voxelsAssignedToActor[idx_actor].empty())
		{
			continue;
		}

		FusedSolutionActor a{ actors[idx_actor], sfs::VoxelCluster(voxelsAssignedToActor[idx_actor]) };
		ret.push_back(a);
	}

	return ret;
}

std::vector<MergedSolutionActor> getActorsByIndices(const std::vector<MergedSolutionActor> & actors,
                                                    const std::vector<size_t> & vec_idx)
{
	std::vector<MergedSolutionActor> ret;
	ret.reserve(vec_idx.size());
	std::transform(vec_idx.begin(), vec_idx.end(), std::back_inserter(ret),
	               [&actors](size_t idx) { return actors[idx]; });
	return ret;
}

void removeTooFarAwayVoxels(const MergedSolutionActor & actor, sfs::VoxelCluster & volume)
{
	auto & voxels = volume.getChangeableVoxel();

	// erase remove leads to really strange results... so lets just build a new vector

	std::vector<const sfs::Voxel *> closeEnoughVoxels;
	closeEnoughVoxels.reserve(voxels.size());

	const auto sqrMaxDist = actor.info.maxSize.get()(0) * actor.info.maxSize.get()(0);

	for (auto v : voxels)
	{
		const auto dx = v->center.x - actor.position.get()(0);
		const auto dy = v->center.y - actor.position.get()(1);
		if (dx * dx + dy * dy < sqrMaxDist)
		{
			closeEnoughVoxels.push_back(v);
		}
	}

	volume = sfs::VoxelCluster(closeEnoughVoxels);
}

FusedSolution fuse(const MergedSolution & solution, std::vector<sfs::VoxelCluster> volumes)
{
	// Currently we assume there are no actors on top of each other

	const auto & actors = solution.actors;

	const auto numActors = actors.size();
	const auto numVolumes = volumes.size();

	std::map<size_t, std::vector<size_t>> volume_assignedActors;
	std::map<size_t, size_t> actor_assignedVolume;

	for (size_t idx_actor = 0; idx_actor < numActors; ++idx_actor)
	{
		const MergedSolutionActor & actor = actors[idx_actor];

		for (size_t idx_volume = 0; idx_volume < numVolumes; ++idx_volume)
		{
			const sfs::VoxelCluster volume = volumes[idx_volume];

			if (isInside(actor, volume))
			{
				const auto [actor_assignedVolume_it, actor_assignedVolume_inserted] =
					actor_assignedVolume.emplace(idx_actor, idx_volume);

				assert(actor_assignedVolume_inserted);

				const auto [volume_assignedActors_it, volume_assignedActors_inserted] =
					volume_assignedActors.emplace(idx_volume, std::vector<size_t>{ });

				volume_assignedActors_it->second.push_back(idx_actor);
				break;
			}
		}
	}

	std::vector<FusedSolutionActor> fusedActors;

	for (const auto & [idx_volume, vec_idx_actor] : volume_assignedActors)
	{
		if (vec_idx_actor.size() == 1)
		{
			FusedSolutionActor a{ actors[vec_idx_actor.front()], volumes[idx_volume] };
			fusedActors.push_back(a);
		}
		else
		{
			std::vector<MergedSolutionActor> actorsForSplitting = getActorsByIndices(actors, vec_idx_actor);

			const auto splitVolumes = splitMultiAssignedVolume(volumes[idx_volume], actorsForSplitting);

			fusedActors.insert(fusedActors.end(), splitVolumes.begin(), splitVolumes.end());
		}
	}

	FusedSolution ret{ solution.framenumber, solution.timestamp, { } };

	for (auto & actor : fusedActors)
	{
		removeTooFarAwayVoxels(actor.actor, actor.volume);
		if(!actor.volume.empty())
		{
			ret.actors.push_back(actor);
		}
	}

	return ret;
}


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

  @copyright 2016 NumScale SAS

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

/*!
  @ingroup group-constant
  @defgroup constant-Six Six (function template)

  Generates the constant @c 6

  @headerref{<boost/simd/constant/six.hpp>}

  @par Description

  1.  @code
      template<typename T> T Six();
      @endcode

  2.  @code
      template<typename T> T Six( boost::simd::as_<T> const& target );
      @endcode

  Generates a value of type @c T that evaluates to 6.

  @par Parameters

  | Name                | Description                                                         |
  |--------------------:|:--------------------------------------------------------------------|
  | **target**          | a [placeholder](@ref type-as) value encapsulating the constant type |

  @par Return Value
  A value of type @c T that evaluates to `T(6)`

  @par Requirements
  - **T** models Value
**/

#include <boost/simd/constant/scalar/six.hpp>
#include <boost/simd/constant/simd/six.hpp>

#endif


// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_STRATEGIES_CLOSEST_POINTS_SPHERICAL_HPP
#define BOOST_GEOMETRY_STRATEGIES_CLOSEST_POINTS_SPHERICAL_HPP

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

#include <boost/geometry/strategies/detail.hpp>
#include <boost/geometry/strategies/distance/detail.hpp>
#include <boost/geometry/strategies/closest_points/services.hpp>

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

#include <boost/geometry/util/type_traits.hpp>

namespace boost { namespace geometry
{

namespace strategies { namespace closest_points
{

template
<
    typename RadiusTypeOrSphere = double,
    typename CalculationType = void
>
class spherical
    : public strategies::distance::spherical<RadiusTypeOrSphere, CalculationType>
{
    using base_t = strategies::distance::spherical<RadiusTypeOrSphere, CalculationType>;

public:
    spherical() = default;

    template <typename RadiusOrSphere>
    explicit spherical(RadiusOrSphere const& radius_or_sphere)
        : base_t(radius_or_sphere)
    {}

    template <typename Geometry1, typename Geometry2>
    auto closest_points(Geometry1 const&, Geometry2 const&,
                        distance::detail::enable_if_ps_t<Geometry1, Geometry2> * = nullptr) const
    {
        return strategy::closest_points::cross_track<CalculationType>(base_t::radius());
    }
};

namespace services
{

template <typename Geometry1, typename Geometry2>
struct default_strategy
<
    Geometry1,
    Geometry2,
    spherical_equatorial_tag,
    spherical_equatorial_tag
>
{
    using type = strategies::closest_points::spherical<>;
};

} // namespace services

}} // namespace strategies::closest_points

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CLOSEST_POINTS_SPHERICAL_HPP


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

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

 * See LICENSE for the license information

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

/**
 * @file NonlinearOptimizer.cpp
 * @brief Convergence functions not dependent on graph types
 * @author Frank Dellaert
 * @date Jul 17, 2010
 */

#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/internal/NonlinearOptimizerState.h>
#include <gtsam/linear/GaussianEliminationTree.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/linear/PCGSolver.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>

#include <gtsam/inference/Ordering.h>

#include <boost/algorithm/string.hpp>
#include <boost/shared_ptr.hpp>

#include <stdexcept>
#include <iostream>
#include <iomanip>

using namespace std;

namespace gtsam {

/* ************************************************************************* */
// NOTE(frank): unique_ptr by-value takes ownership, as discussed in
// http://stackoverflow.com/questions/8114276/
NonlinearOptimizer::NonlinearOptimizer(const NonlinearFactorGraph& graph,
                                       std::unique_ptr<internal::NonlinearOptimizerState> state)
    : graph_(graph), state_(std::move(state)) {}

/* ************************************************************************* */
NonlinearOptimizer::~NonlinearOptimizer() {}

/* ************************************************************************* */
double NonlinearOptimizer::error() const {
  return state_->error;
}

size_t NonlinearOptimizer::iterations() const {
  return state_->iterations;
}

const Values& NonlinearOptimizer::values() const {
  return state_->values;
}

/* ************************************************************************* */
void NonlinearOptimizer::defaultOptimize() {
  const NonlinearOptimizerParams& params = _params();
  double currentError = error();

  // check if we're already close enough
  if (currentError <= params.errorTol) {
    if (params.verbosity >= NonlinearOptimizerParams::ERROR)
      cout << "Exiting, as error = " << currentError << " < " << params.errorTol << endl;
    return;
  }

  // Maybe show output
  if (params.verbosity >= NonlinearOptimizerParams::VALUES)
    values().print("Initial values");
  if (params.verbosity >= NonlinearOptimizerParams::ERROR)
    cout << "Initial error: " << currentError << endl;

  // Return if we already have too many iterations
  if (iterations() >= params.maxIterations) {
    if (params.verbosity >= NonlinearOptimizerParams::TERMINATION) {
      cout << "iterations: " << iterations() << " >? " << params.maxIterations << endl;
    }
    return;
  }

  // Iterative loop
  double newError = currentError; // used to avoid repeated calls to error()
  do {
    // Do next iteration
    currentError = newError;
    iterate();
    tictoc_finishedIteration();

    // Update newError for either printouts or conditional-end checks:
    newError = error();

    // Maybe show output
    if (params.verbosity >= NonlinearOptimizerParams::VALUES)
      values().print("newValues");
    if (params.verbosity >= NonlinearOptimizerParams::ERROR)
      cout << "newError: " << newError << endl;
  } while (iterations() < params.maxIterations &&
           !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, params.errorTol,
                             currentError, newError, params.verbosity) && std::isfinite(currentError));

  // Printing if verbose
  if (params.verbosity >= NonlinearOptimizerParams::TERMINATION) {
    cout << "iterations: " << iterations() << " >? " << params.maxIterations << endl;
    if (iterations() >= params.maxIterations)
      cout << "Terminating because reached maximum iterations" << endl;
  }
}

/* ************************************************************************* */
const Values& NonlinearOptimizer::optimizeSafely() {
  static const Values empty;
  try {
    defaultOptimize();
    return values();
  } catch (...) {
    // uncaught exception, returning empty result
    return empty;
  }
}

/* ************************************************************************* */
VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph& gfg,
                                       const NonlinearOptimizerParams& params) const {
  // solution of linear solver is an update to the linearization point
  VectorValues delta;

  // Check which solver we are using
  if (params.isMultifrontal()) {
    // Multifrontal QR or Cholesky (decided by params.getEliminationFunction())
    if (params.ordering)
      delta = gfg.optimize(*params.ordering, params.getEliminationFunction());
    else
      delta = gfg.optimize(params.getEliminationFunction());
  } else if (params.isSequential()) {
    // Sequential QR or Cholesky (decided by params.getEliminationFunction())
    if (params.ordering)
      delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction(),
                                      boost::none, params.orderingType)->optimize();
    else
      delta = gfg.eliminateSequential(params.getEliminationFunction(), boost::none,
                                      params.orderingType)->optimize();
  } else if (params.isIterative()) {
    // Conjugate Gradient -> needs params.iterativeParams
    if (!params.iterativeParams)
      throw std::runtime_error(
          "NonlinearOptimizer::solve: cg parameter has to be assigned ...");

    if (auto pcg = boost::dynamic_pointer_cast<PCGSolverParameters>(
            params.iterativeParams)) {
      delta = PCGSolver(*pcg).optimize(gfg);
    } else if (auto spcg =
                   boost::dynamic_pointer_cast<SubgraphSolverParameters>(
                       params.iterativeParams)) {
      if (!params.ordering)
        throw std::runtime_error("SubgraphSolver needs an ordering");
      delta = SubgraphSolver(gfg, *spcg, *params.ordering).optimize();
    } else {
      throw std::runtime_error(
          "NonlinearOptimizer::solve: special cg parameter type is not handled in LM solver ...");
    }
  } else {
    throw std::runtime_error("NonlinearOptimizer::solve: Optimization parameter is invalid");
  }

  // return update
  return delta;
}

/* ************************************************************************* */
bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold,
                      double errorThreshold, double currentError, double newError,
                      NonlinearOptimizerParams::Verbosity verbosity) {
  if (verbosity >= NonlinearOptimizerParams::ERROR) {
    if (newError <= errorThreshold)
      cout << "errorThreshold: " << newError << " < " << errorThreshold << endl;
    else
      cout << "errorThreshold: " << newError << " > " << errorThreshold << endl;
  }

  if (newError <= errorThreshold)
    return true;

  // check if diverges
  double absoluteDecrease = currentError - newError;
  if (verbosity >= NonlinearOptimizerParams::ERROR) {
    if (absoluteDecrease <= absoluteErrorTreshold)
      cout << "absoluteDecrease: " << setprecision(12) << absoluteDecrease << " < "
           << absoluteErrorTreshold << endl;
    else
      cout << "absoluteDecrease: " << setprecision(12) << absoluteDecrease
           << " >= " << absoluteErrorTreshold << endl;
  }

  // calculate relative error decrease and update currentError
  double relativeDecrease = absoluteDecrease / currentError;
  if (verbosity >= NonlinearOptimizerParams::ERROR) {
    if (relativeDecrease <= relativeErrorTreshold)
      cout << "relativeDecrease: " << setprecision(12) << relativeDecrease << " < "
           << relativeErrorTreshold << endl;
    else
      cout << "relativeDecrease: " << setprecision(12) << relativeDecrease
           << " >= " << relativeErrorTreshold << endl;
  }
  bool converged = (relativeErrorTreshold && (relativeDecrease <= relativeErrorTreshold)) ||
                   (absoluteDecrease <= absoluteErrorTreshold);
  if (verbosity >= NonlinearOptimizerParams::TERMINATION && converged) {
    if (absoluteDecrease >= 0.0)
      cout << "converged" << endl;
    else
      cout << "Warning:  stopping nonlinear iterations because error increased" << endl;

    cout << "errorThreshold: " << newError << " <? " << errorThreshold << endl;
    cout << "absoluteDecrease: " << setprecision(12) << absoluteDecrease << " <? "
         << absoluteErrorTreshold << endl;
    cout << "relativeDecrease: " << setprecision(12) << relativeDecrease << " <? "
         << relativeErrorTreshold << endl;
  }
  return converged;
}

/* ************************************************************************* */
GTSAM_EXPORT bool checkConvergence(const NonlinearOptimizerParams& params, double currentError,
                                   double newError) {
  return checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, params.errorTol,
                          currentError, newError, params.verbosity);
}
}


// Copyright (c) 2011, Thomas Goyne <plorkyeran@aegisub.org>
//
// Permission to use, copy, modify, and distribute this software for any
// purpose with or without fee is hereby granted, provided that the above
// copyright notice and this permission notice appear in all copies.
//
// THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
// WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
// ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
// WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
// ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
// OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
//
// Aegisub Project http://www.aegisub.org/

/// @file visual_tool_rotatexy.cpp
/// @brief 3D rotation in X/Y axes visual typesetting tool
/// @ingroup visual_ts

#include "config.h"

#include "visual_tool_rotatexy.h"

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

VisualToolRotateXY::VisualToolRotateXY(VideoDisplay *parent, agi::Context *context)
: VisualTool<VisualDraggableFeature>(parent, context)
{
	org = new Feature;
	org->type = DRAG_BIG_TRIANGLE;
	features.push_back(*org);
}

void VisualToolRotateXY::Draw() {
	if (!active_line) return;

	DrawAllFeatures();

	// Transform grid
	gl.SetOrigin(org->pos);
	gl.SetRotation(angle_x, angle_y, angle_z);
	gl.SetShear(fax, fay);

	// Draw grid
	gl.SetLineColour(colour[0], 0.5f, 2);
	gl.SetModeLine();
	float r = colour[0].Red() / 255.f;
	float g = colour[0].Green() / 255.f;
	float b = colour[0].Blue() / 255.f;

	// Number of lines on each side of each axis
	static const int radius = 15;
	// Total number of lines, including center axis line
	static const int line_count = radius * 2 + 1;
	// Distance between each line in pixels
	static const int spacing = 20;
	// Length of each grid line in pixels from axis to one end
	static const int half_line_length = spacing * (radius + 1);
	static const float fade_factor = 0.9f / radius;

	std::vector<float> colors(line_count * 8 * 4);
	for (int i = 0; i < line_count * 8; ++i) {
		colors[i * 4 + 0] = r;
		colors[i * 4 + 1] = g;
		colors[i * 4 + 2] = b;
		colors[i * 4 + 3] = (i + 3) % 4 > 1 ? 0 : (1.f - abs(i / 8 - radius) * fade_factor);
	}

	std::vector<float> points(line_count * 8 * 2);
	for (int i = 0; i < line_count; ++i) {
		int pos = spacing * (i - radius);

		points[i * 16 + 0] = pos;
		points[i * 16 + 1] = half_line_length;

		points[i * 16 + 2] = pos;
		points[i * 16 + 3] = 0;

		points[i * 16 + 4] = pos;
		points[i * 16 + 5] = 0;

		points[i * 16 + 6] = pos;
		points[i * 16 + 7] = -half_line_length;

		points[i * 16 + 8] = half_line_length;
		points[i * 16 + 9] = pos;

		points[i * 16 + 10] = 0;
		points[i * 16 + 11] = pos;

		points[i * 16 + 12] = 0;
		points[i * 16 + 13] = pos;

		points[i * 16 + 14] = -half_line_length;
		points[i * 16 + 15] = pos;
	}

	gl.DrawLines(2, points, 4, colors);

	// Draw vectors
	gl.SetLineColour(colour[3], 1.f, 2);
	float vectors[] = {
		0.f, 0.f, 0.f,
		50.f, 0.f, 0.f,
		0.f, 0.f, 0.f,
		0.f, 50.f, 0.f,
		0.f, 0.f, 0.f,
		0.f, 0.f, 50.f,
	};
	gl.DrawLines(3, vectors, 6);

	// Draw arrow tops
	float arrows[] = {
		60.f,  0.f,  0.f,
		50.f, -3.f, -3.f,
		50.f,  3.f, -3.f,
		50.f,  3.f,  3.f,
		50.f, -3.f,  3.f,
		50.f, -3.f, -3.f,

		 0.f, 60.f,  0.f,
		-3.f, 50.f, -3.f,
		 3.f, 50.f, -3.f,
		 3.f, 50.f,  3.f,
		-3.f, 50.f,  3.f,
		-3.f, 50.f, -3.f,

		 0.f,  0.f, 60.f,
		-3.f, -3.f, 50.f,
		 3.f, -3.f, 50.f,
		 3.f,  3.f, 50.f,
		-3.f,  3.f, 50.f,
		-3.f, -3.f, 50.f,
	};

	gl.DrawLines(3, arrows, 18);

	gl.ResetTransform();
}

bool VisualToolRotateXY::InitializeHold() {
	orig_x = angle_x;
	orig_y = angle_y;

	return true;
}

void VisualToolRotateXY::UpdateHold() {
	Vector2D delta = (mouse_pos - drag_start) * 2;
	if (shift_down)
		delta = delta.SingleAxis();

	angle_x = orig_x - delta.Y();
	angle_y = orig_y + delta.X();

	if (ctrl_down) {
		angle_x = floorf(angle_x / 30.f + .5f) * 30.f;
		angle_y = floorf(angle_y / 30.f + .5f) * 30.f;
	}

	angle_x = fmodf(angle_x + 360.f, 360.f);
	angle_y = fmodf(angle_y + 360.f, 360.f);

	SetSelectedOverride("\\frx", str(boost::format("%.4g") % angle_x));
	SetSelectedOverride("\\fry", str(boost::format("%.4g") % angle_y));
}

void VisualToolRotateXY::UpdateDrag(Feature *feature) {
	SetOverride(active_line, "\\org", ToScriptCoords(feature->pos).PStr());
}

void VisualToolRotateXY::DoRefresh() {
	if (!active_line) return;

	if (!(org->pos = GetLineOrigin(active_line)))
		org->pos = GetLinePosition(active_line);
	org->pos = FromScriptCoords(org->pos);

	GetLineRotation(active_line, angle_x, angle_y, angle_z);
	GetLineShear(active_line, fax, fay);
}


//  Copyright John Maddock 2006.
//  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)
//
// Basic sanity check that header <boost/math/tools/test.hpp>
// #includes all the files that it needs to.
//
#include <boost/math/tools/test.hpp>
#include <boost/array.hpp>
//
// Note this header includes no other headers, this is
// important if this test is to be meaningful:
//
inline void check_result_imp(boost::math::tools::test_result<double>, boost::math::tools::test_result<double>){}

#include "test_compile_result.hpp"

void check()
{
   check_result<float>(boost::math::tools::relative_error<float>(f, f));

   #define A boost::array<boost::array<double, 2>, 2>
   typedef double (*F1)(const boost::array<double, 2>&);
   typedef F1 F2;
   A a;
   F1 f1 = 0;
   F2 f2 = 0;

   check_result<boost::math::tools::test_result<
      boost::math::tools::calculate_result_type<A>::value_type> >
      (boost::math::tools::test<A, F1, F2>(a, f1, f2));

}


/**
 * This program tests the range image likelihood library.
 *
 * You can move around using
 *  w - forward
 *  a - backward
 *  s - left
 *  d - right
 *  q - up
 *  z - down
 *  f - write point cloud file
 *
 * The mouse controls the direction of movement
 *
 *  Esc - quit
 *
 */

#include <Eigen/Dense>
#include <cmath>
#include <iostream>
#include <boost/shared_ptr.hpp>
#ifdef _WIN32
# define WIN32_LEAN_AND_MEAN
# include <windows.h>
#endif
#include <GL/glew.h>

#include <pcl/pcl_config.h>
#ifdef OPENGL_IS_A_FRAMEWORK
# include <OpenGL/gl.h>
# include <OpenGL/glu.h>
#else
# include <GL/gl.h>
# include <GL/glu.h>
#endif
#ifdef GLUT_IS_A_FRAMEWORK
# include <GLUT/glut.h>
#else
# include <GL/glut.h>
#endif

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include "pcl/common/common.h"
#include "pcl/common/transforms.h"

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>

// define the following in order to eliminate the deprecated headers warning
#define VTK_EXCLUDE_STRSTREAM_HEADERS
#include <pcl/io/vtk_lib_io.h>

#include "kinect_sim/camera.h"
#include "kinect_sim/camera_constants.h"
#include "kinect_sim/model.h"
#include "kinect_sim/scene.h"
#include "kinect_sim/range_likelihood.h"

#include <pcl/console/print.h>
#include <pcl/console/parse.h>
#include <pcl/console/time.h>

// RangeImage:
#include <pcl/range_image/range_image_planar.h>

// Pop-up viewer
#include <pcl/visualization/cloud_viewer.h>
#include <boost/thread/thread.hpp>

using namespace Eigen;
using namespace pcl;
using namespace pcl::console;
using namespace pcl::io;
using namespace pcl::simulation;

using namespace std;

uint16_t t_gamma[2048];

Scene::Ptr scene_;
Camera::Ptr camera_;
RangeLikelihood::Ptr range_likelihood_;

int window_width_;
int window_height_;
bool paused_;
bool write_file_;

void printHelp (int argc, char **argv)
{
  print_error ("Syntax is: %s <filename>\n", argv[0]);
  print_info ("acceptable filenames include vtk, obj and ply. ply can support colour\n");
}

void wait ()
{
      std::cout << "Press enter to continue";
      getchar();
      std::cout << "\n\n";
}

void display_score_image(const float* score_buffer)
{
  int npixels = range_likelihood_->getWidth() * range_likelihood_->getHeight();
  uint8_t* score_img = new uint8_t[npixels * 3];

  float min_score = score_buffer[0];
  float max_score = score_buffer[0];
  for (int i=1; i<npixels; i++)
  {
    if (score_buffer[i] < min_score) min_score = score_buffer[i];
    if (score_buffer[i] > max_score) max_score = score_buffer[i];
  }

  for (int i=0; i<npixels; i++)
  {
    float d = (score_buffer[i]-min_score)/(max_score-min_score);
    score_img[3*i+0] = 0;
    score_img[3*i+1] = d*255;
    score_img[3*i+2] = 0;
  }

  glRasterPos2i(-1,-1);
  glDrawPixels(range_likelihood_->getWidth(), range_likelihood_->getHeight(), GL_RGB, GL_UNSIGNED_BYTE, score_img);

  delete [] score_img;
}

void display_depth_image (const float* depth_buffer, int width, int height)
{
  int npixels = width * height;
  uint8_t* depth_img = new uint8_t[npixels * 3];

  float min_depth = depth_buffer[0];
  float max_depth = depth_buffer[0];
  for (int i = 1; i < npixels; ++i)
  {
    if (depth_buffer[i] < min_depth) min_depth = depth_buffer[i];
    if (depth_buffer[i] > max_depth) max_depth = depth_buffer[i];
  }

  for (int i = 0; i < npixels; ++i)
  {
    float zn = 0.7f;
    float zf = 20.0f;
    float d = depth_buffer[i];
    float z = -zf*zn/((zf-zn)*(d - zf/(zf-zn)));
    float b = 0.075f;
    float f = 580.0f;
    uint16_t kd = static_cast<uint16_t>(1090 - b*f/z*8);
    if (kd < 0) kd = 0;
    else if (kd > 2047) kd = 2047;

    int pval = t_gamma[kd];
    int lb = pval & 0xff;
    switch (pval >> 8)
    {
      case 0:
	  depth_img[3*i+0] = 255;
	  depth_img[3*i+1] = 255-lb;
	  depth_img[3*i+2] = 255-lb;
	  break;
      case 1:
	  depth_img[3*i+0] = 255;
	  depth_img[3*i+1] = lb;
	  depth_img[3*i+2] = 0;
	  break;
      case 2:
	  depth_img[3*i+0] = 255-lb;
	  depth_img[3*i+1] = 255;
	  depth_img[3*i+2] = 0;
	  break;
      case 3:
	  depth_img[3*i+0] = 0;
	  depth_img[3*i+1] = 255;
	  depth_img[3*i+2] = lb;
	  break;
      case 4:
	  depth_img[3*i+0] = 0;
	  depth_img[3*i+1] = 255-lb;
	  depth_img[3*i+2] = 255;
	  break;
      case 5:
	  depth_img[3*i+0] = 0;
	  depth_img[3*i+1] = 0;
	  depth_img[3*i+2] = 255-lb;
	  break;
      default:
	  depth_img[3*i+0] = 0;
	  depth_img[3*i+1] = 0;
	  depth_img[3*i+2] = 0;
	  break;
    }
  }

  glRasterPos2i (-1,-1);
  glDrawPixels (width, height,
                GL_RGB, GL_UNSIGNED_BYTE, depth_img);

  delete [] depth_img;
}

/*
void display_depth_image(const float* depth_buffer)
{
  int npixels = range_likelihood_->getWidth() * range_likelihood_->getHeight();
  uint8_t* depth_img = new uint8_t[npixels * 3];

  float min_depth = depth_buffer[0];
  float max_depth = depth_buffer[0];
  for (int i=1; i<npixels; i++)
  {
    if (depth_buffer[i] < min_depth) min_depth = depth_buffer[i];
    if (depth_buffer[i] > max_depth) max_depth = depth_buffer[i];
  }

  for (int i=0; i<npixels; i++)
  {
    float zn = 0.7;
    float zf = 20.0;
    float d = depth_buffer[i];
    float z = -zf*zn/((zf-zn)*(d - zf/(zf-zn)));
    float b = 0.075;
    float f = 580.0;
    uint16_t kd = static_cast<uint16_t>(1090 - b*f/z*8);
    if (kd < 0) kd = 0;
    else if (kd>2047) kd = 2047;

    int pval = t_gamma[kd];
    int lb = pval & 0xff;
    switch (pval>>8) {
      case 0:
	  depth_img[3*i+0] = 255;
	  depth_img[3*i+1] = 255-lb;
	  depth_img[3*i+2] = 255-lb;
	  break;
      case 1:
	  depth_img[3*i+0] = 255;
	  depth_img[3*i+1] = lb;
	  depth_img[3*i+2] = 0;
	  break;
      case 2:
	  depth_img[3*i+0] = 255-lb;
	  depth_img[3*i+1] = 255;
	  depth_img[3*i+2] = 0;
	  break;
      case 3:
	  depth_img[3*i+0] = 0;
	  depth_img[3*i+1] = 255;
	  depth_img[3*i+2] = lb;
	  break;
      case 4:
	  depth_img[3*i+0] = 0;
	  depth_img[3*i+1] = 255-lb;
	  depth_img[3*i+2] = 255;
	  break;
      case 5:
	  depth_img[3*i+0] = 0;
	  depth_img[3*i+1] = 0;
	  depth_img[3*i+2] = 255-lb;
	  break;
      default:
	  depth_img[3*i+0] = 0;
	  depth_img[3*i+1] = 0;
	  depth_img[3*i+2] = 0;
	  break;
    }
  }

  glRasterPos2i(-1,-1);
  glDrawPixels(range_likelihood_->getWidth(), range_likelihood_->getHeight(), GL_RGB, GL_UNSIGNED_BYTE, depth_img);

  delete [] depth_img;
}
*/

boost::shared_ptr<pcl::visualization::PCLVisualizer> simpleVis (pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud)
{
  // --------------------------------------------
  // -----Open 3D viewer and add point cloud-----
  // --------------------------------------------
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
  viewer->setBackgroundColor (0, 0, 0);
  pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
  viewer->addPointCloud<pcl::PointXYZRGB> (cloud, rgb, "sample cloud");
  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
  //viewer->addCoordinateSystem (1.0, "reference");
  viewer->addCoordinateSystem (1.0);
  viewer->initCameraParameters ();
  return (viewer);
}

void display ()
{
  float* reference = new float[range_likelihood_->getRowHeight() * range_likelihood_->getColWidth()];
  const float* depth_buffer = range_likelihood_->getDepthBuffer();
  // Copy one image from our last as a reference.
  for (int i=0, n=0; i<range_likelihood_->getRowHeight(); ++i)
  {
    for (int j=0; j<range_likelihood_->getColWidth(); ++j)
    {
      reference[n++] = depth_buffer[i*range_likelihood_->getWidth() + j];
    }
  }

  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
  std::vector<float> scores;
  int n = range_likelihood_->getRows ()*range_likelihood_->getCols ();
  for (int i = 0; i < n; ++i)
  {
    Camera camera(*camera_);
    camera.move(0.0,i*0.02,0.0);
    //camera.move(0.0,i*0.02,0.0);
    poses.push_back (camera.getPose ());
  }

  range_likelihood_->computeLikelihoods (reference, poses, scores);

  range_likelihood_->computeLikelihoods (reference, poses, scores);
  std::cout << "score: ";
  for (size_t i = 0; i<scores.size (); ++i)
  {
    std::cout << " " << scores[i];
  }
  std::cout << std::endl;

  std::cout << "camera: " << camera_->getX ()
       << " " << camera_->getY ()
       << " " << camera_->getZ ()
       << " " << camera_->getRoll ()
       << " " << camera_->getPitch ()
       << " " << camera_->getYaw ()
       << std::endl;

  delete [] reference;

  glDrawBuffer (GL_BACK);
  glReadBuffer (GL_BACK);

  // Draw the resulting images from the range_likelihood
  glViewport (range_likelihood_->getWidth (), 0, range_likelihood_->getWidth (), range_likelihood_->getHeight ());
  glMatrixMode (GL_PROJECTION);
  glLoadIdentity ();
  glMatrixMode (GL_MODELVIEW);
  glLoadIdentity ();

  // Draw the color image
  glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  glColorMask (true, true, true, true);
  glDisable (GL_DEPTH_TEST);

  glRasterPos2i (-1,-1);
  glDrawPixels (range_likelihood_->getWidth (), range_likelihood_->getHeight (),
                GL_RGB, GL_UNSIGNED_BYTE, range_likelihood_->getColorBuffer ());

  // Draw the depth image
  glViewport (0, 0, range_likelihood_->getWidth (), range_likelihood_->getHeight ());

  glMatrixMode (GL_PROJECTION);
  glLoadIdentity ();
  glMatrixMode (GL_MODELVIEW);
  glLoadIdentity ();
//  display_depth_image (range_likelihood_->getDepthBuffer ());
  display_depth_image (range_likelihood_->getDepthBuffer (),
                       range_likelihood_->getWidth (), range_likelihood_->getHeight ());

  // Draw the score image
  glViewport (0, range_likelihood_->getHeight (),
              range_likelihood_->getWidth (), range_likelihood_->getHeight ());
  glMatrixMode (GL_PROJECTION);
  glLoadIdentity ();
  glMatrixMode (GL_MODELVIEW);
  glLoadIdentity ();
  display_score_image (range_likelihood_->getScoreBuffer ());

  glutSwapBuffers ();

  if (write_file_)
  {
    range_likelihood_->addNoise ();
    pcl::RangeImagePlanar rangeImage;
    range_likelihood_->getRangeImagePlanar (rangeImage);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_out (new pcl::PointCloud<pcl::PointXYZRGB>);

    // Optional argument to save point cloud in global frame:
    // Save camera relative:
    //range_likelihood_->getPointCloud(pc_out);
    // Save in global frame - applying the camera frame:
    //range_likelihood_->getPointCloud(pc_out,true,camera_->pose());
    // Save in local frame
    range_likelihood_->getPointCloud (pc_out,false,camera_->getPose ());
    // TODO: what to do when there are more than one simulated view?

    pcl::PCDWriter writer;
    writer.write ("simulated_range_image.pcd", *pc_out,	false);
    cout << "finished writing file\n";

//     pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
//     viewer.showCloud (pc_out);

  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
  viewer = simpleVis(pc_out);
  while (!viewer->wasStopped ())
  {
    viewer->spinOnce (100);
    boost::this_thread::sleep (boost::posix_time::microseconds (100000));
  }

  // doesnt work:
//    viewer->~PCLVisualizer();
//    viewer.reset();

    cout << "done\n";
    // Problem: vtk and opengl dont seem to play very well together
    // vtk seems to misbehave after a little while and wont keep the window on the screen

    // method1: kill with [x] - but eventually it crashes:
    //while (!viewer.wasStopped ()){
    //}

    // method2: eventually starts ignoring cin and pops up on screen and closes almost
    // immediately
    //  cout << "enter 1 to cont\n";
    //  cin >> pause;
    //  viewer.wasStopped ();

    // method 3: if you interact with the window with keys, the window is not closed properly
    // TODO: use pcl methods as this time stuff is probably not cross playform
//     struct timespec t;
//     t.tv_sec = 100;
//     //t.tv_nsec = (time_t)(20000000); // short sleep
//     t.tv_nsec = (time_t)(0);  // long sleep - normal speed
//     nanosleep (&t, NULL);
    write_file_ = 0;
  }
}

// Handle normal keys
void
on_keyboard (unsigned char key, int x, int y)
{
  double speed = 0.1;

  if (key == 27)
    exit(0);
  else if (key == 'w' || key == 'W')
    camera_->move(speed,0,0);
  else if (key == 's' || key == 'S')
    camera_->move(-speed,0,0);
  else if (key == 'a' || key == 'A')
    camera_->move(0,speed,0);
  else if (key == 'd' || key == 'D')
    camera_->move(0,-speed,0);
  else if (key == 'q' || key == 'Q')
    camera_->move(0,0,speed);
  else if (key == 'z' || key == 'Z')
    camera_->move(0,0,-speed);
  else if (key == 'p' || key == 'P')
    paused_ = !paused_;
  else if (key == 'v' || key == 'V')
    write_file_ = 1;

  // Use glutGetModifiers for modifiers
  // GLUT_ACTIVE_SHIFT, GLUT_ACTIVE_CTRL, GLUT_ACTIVE_ALT
}

// Handle special keys, e.g. F1, F2, ...
void
on_special(int key, int x, int y)
{
  switch (key) {
  case GLUT_KEY_F1:
    break;
  case GLUT_KEY_HOME:
    break;
  }
}

void
on_reshape(int w, int h)
{
  // Window size changed
  window_width_ = w;
  window_height_ = h;
}

void
on_mouse(int button, int state, int x, int y)
{
  // button:
  // GLUT_LEFT_BUTTON
  // GLUT_MIDDLE_BUTTON
  // GLUT_RIGHT_BUTTON
  //
  // state:
  // GLUT_UP
  // GLUT_DOWN
}

void
on_motion(int x, int y)
{
}

void
on_passive_motion(int x, int y)
{
  if (paused_) return;

  double pitch = -(0.5-(double)y/window_height_)*M_PI * 4; // in window coordinates positive y-axis is down
  double yaw =    (0.5-(double)x/window_width_)*M_PI*2 * 4;

  camera_->setPitch (pitch);
  camera_->setYaw (yaw);
}

void on_entry (int state)
{
  // state:
  // GLUT_LEFT
  // GLUT_ENTERED
}

// Read in a 3D model
void load_PolygonMesh_model (char* polygon_file)
{
  pcl::PolygonMesh mesh;	// (new pcl::PolygonMesh);
  //pcl::io::loadPolygonFile("/home/mfallon/data/models/dalet/Darlek_modified_works.obj",mesh);
  pcl::io::loadPolygonFile (polygon_file, mesh);
  pcl::PolygonMesh::Ptr cloud (new pcl::PolygonMesh (mesh));

  // Not sure if PolygonMesh assumes triangles if to
  // TODO: Ask a developer
  PolygonMeshModel::Ptr model = PolygonMeshModel::Ptr (new PolygonMeshModel (GL_POLYGON, cloud));
  scene_->add (model);

  std::cout << "Just read " << polygon_file << std::endl;
  std::cout << mesh.polygons.size () << " polygons and "
	    << mesh.cloud.data.size () << " triangles\n";

}

void
initialize (int argc, char** argv)
{
  const GLubyte* version = glGetString (GL_VERSION);
  std::cout << "OpenGL Version: " << version << std::endl;

  // works well for MIT CSAIL model 3rd floor:
  //camera_->set(4.04454, 44.9377, 1.1, 0.0, 0.0, -2.00352);

  // works well for MIT CSAIL model 2nd floor:
//  camera_->set (27.4503, 37.383, 4.30908, 0.0, 0.0654498, -2.25802);

  // works for small files:
  //camera_->set(-5.0, 0.0, 1.0, 0.0, 0.0, 0.0);
  camera_->set( 1.31762, 0.382931, 1.89533, 0, 0.20944, -9.14989);
  camera_->setPitch(0.20944); // not sure why this is here:
  //camera_->set(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);

  cout << "About to read: " << argv[1] << endl;
  load_PolygonMesh_model (argv[1]);

  paused_ = false;
}

int
main (int argc, char** argv)
{
  int width = kCameraWidth;
  int height = kCameraHeight;

  window_width_ = width * 2;
  window_height_ = height * 2;

  print_info ("Manually generate a simulated RGB-D point cloud using pcl::simulation. For more information, use: %s -h\n", argv[0]);

  if (argc < 2)
  {
    printHelp (argc, argv);
    return (-1);
  }

  int i;
  for (i=0; i<2048; i++)
  {
    float v = i/2048.0;
    v = powf(v, 3)* 6;
    t_gamma[i] = v*6*256;
  }

  glutInit (&argc, argv);
  glutInitDisplayMode (GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB);// was GLUT_RGBA
  glutInitWindowPosition (10, 10);
  glutInitWindowSize (window_width_, window_height_);
  glutCreateWindow ("OpenGL range likelihood");

  GLenum err = glewInit ();
  if (GLEW_OK != err)
  {
    std::cerr << "Error: " << glewGetErrorString (err) << std::endl;
    exit (-1);
  }

  std::cout << "Status: Using GLEW " << glewGetString (GLEW_VERSION) << std::endl;

  if (glewIsSupported ("GL_VERSION_2_0"))
    std::cout << "OpenGL 2.0 supported" << std::endl;
  else
  {
    std::cerr << "Error: OpenGL 2.0 not supported" << std::endl;
    exit(1);
  }
  std::cout << "GL_MAX_VIEWPORTS: " << GL_MAX_VIEWPORTS << std::endl;

  camera_ = Camera::Ptr (new Camera ());
  scene_ = Scene::Ptr (new Scene ());

  //range_likelihood_ = RangeLikelihoodGLSL::Ptr(new RangeLikelihoodGLSL(1, 1, height, width, scene_, 0));

  range_likelihood_ = RangeLikelihood::Ptr (new RangeLikelihood (2, 2, height/2, width/2, scene_));
  // range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(10, 10, 96, 96, scene_));
  // range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(1, 1, 480, 640, scene_));

  // Actually corresponds to default parameters:
  range_likelihood_->setCameraIntrinsicsParameters (kCameraWidth,kCameraHeight, kCameraFX,
            kCameraFY, kCameraCX, kCameraCY);
  range_likelihood_->setComputeOnCPU (false);
  range_likelihood_->setSumOnCPU (true);
  range_likelihood_->setUseColor (true);

  initialize (argc, argv);

  glutReshapeFunc (on_reshape);
  glutDisplayFunc (display);
  glutIdleFunc (display);
  glutKeyboardFunc (on_keyboard);
  glutMouseFunc (on_mouse);
  glutMotionFunc (on_motion);
  glutPassiveMotionFunc (on_passive_motion);
  glutEntryFunc (on_entry);
  glutMainLoop ();

  return 0;
}


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

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

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

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISTANCE_RANGE_TO_SEGMENT_OR_BOX_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISTANCE_RANGE_TO_SEGMENT_OR_BOX_HPP

#include <vector>

#include <boost/range.hpp>

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

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/distance_comparable_to_regular.hpp>
#include <boost/geometry/strategies/tags.hpp>

#include <boost/geometry/algorithms/detail/assign_box_corners.hpp>
#include <boost/geometry/algorithms/detail/assign_indexed_point.hpp>
#include <boost/geometry/algorithms/intersects.hpp>
#include <boost/geometry/algorithms/num_points.hpp>
#include <boost/geometry/algorithms/num_points.hpp>

#include <boost/geometry/algorithms/dispatch/distance.hpp>

#include <boost/geometry/algorithms/detail/distance/point_to_geometry.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace distance
{

template
<
    typename Range,
    typename SegmentOrBox,
    typename Strategy
>
class range_to_segment_or_box
{
private:
    typedef typename point_type<SegmentOrBox>::type segment_or_box_point;
    typedef typename point_type<Range>::type range_point;

    typedef typename strategy::distance::services::comparable_type
        <
            Strategy
        >::type comparable_strategy;

    typedef typename strategy::distance::services::return_type
        <
            comparable_strategy, range_point, segment_or_box_point
        >::type comparable_return_type;

    typedef typename strategy::distance::services::tag
       <
           comparable_strategy
       >::type comparable_strategy_tag;

    typedef dispatch::distance
        <
            segment_or_box_point, Range, comparable_strategy,
            point_tag, typename tag<Range>::type,
            comparable_strategy_tag, false
        > comparable_point_to_range;

    // compute distance of a point to a segment or a box
    template
    <
        typename Point,
        typename SegOrBoxPoints,
        typename ComparableStrategy,
        typename Tag
    >
    struct comparable_distance_point_to_segment_or_box
    {};

    template
    <
        typename Point,
        typename SegmentPoints,
        typename ComparableStrategy
    >
    struct comparable_distance_point_to_segment_or_box
        <
            Point, SegmentPoints, ComparableStrategy, segment_tag
        >
    {
        static inline
        comparable_return_type apply(Point const& point,
                                     SegmentPoints const& segment_points,
                                     ComparableStrategy const& strategy)
        {
            boost::ignore_unused_variable_warning(strategy);
            return strategy.apply(point, segment_points[0], segment_points[1]);
        }
    };

    template
    <
        typename Point,
        typename BoxPoints,
        typename ComparableStrategy
    >
    struct comparable_distance_point_to_segment_or_box
        <
            Point, BoxPoints, ComparableStrategy, box_tag
        >
    {
        static inline
        comparable_return_type apply(Point const& point,
                                     BoxPoints const& box_points,
                                     ComparableStrategy const& strategy)
        {
            return point_to_range
                <
                    Point, BoxPoints, open, ComparableStrategy
                >::apply(point, box_points, strategy);
        }
    };

    // assign the points of a segment or a box to a range
    template
    <
        typename SegOrBox,
        typename PointRange,
        typename Tag = typename tag<SegOrBox>::type
    >
    struct assign_segment_or_box_points
    {};

    template <typename Segment, typename PointRange>
    struct assign_segment_or_box_points<Segment, PointRange, segment_tag>
    {
        static inline void apply(Segment const& segment, PointRange& range)
        {
            detail::assign_point_from_index<0>(segment, range[0]);
            detail::assign_point_from_index<1>(segment, range[1]);
        }
    };

    template <typename Box, typename PointRange>
    struct assign_segment_or_box_points<Box, PointRange, box_tag>
    {
        static inline void apply(Box const& box, PointRange& range)
        {
            detail::assign_box_corners_oriented<true>(box, range);
        }
    };

public:
    typedef typename strategy::distance::services::return_type
        <
            Strategy, range_point, segment_or_box_point
        >::type return_type;

    static inline return_type
    apply(Range const& range, SegmentOrBox const& segment_or_box,
          Strategy const& strategy, bool check_intersection = true)
    {
        if ( check_intersection && geometry::intersects(range, segment_or_box) )
        {
            return 0;
        }

        comparable_strategy cstrategy =
            strategy::distance::services::get_comparable
                <
                    Strategy
                >::apply(strategy);

        // get all points of the segment or the box
        std::vector<segment_or_box_point>
            segment_or_box_points(geometry::num_points(segment_or_box));

        assign_segment_or_box_points
            <

                SegmentOrBox,
                std::vector<segment_or_box_point>
            >::apply(segment_or_box, segment_or_box_points);

        // consider all distances from each endpoint of the segment or box
        // to the range
        typename std::vector<segment_or_box_point>::const_iterator it
            = segment_or_box_points.begin();
        comparable_return_type cd_min =
            comparable_point_to_range::apply(*it, range, cstrategy);

        for (++it; it != segment_or_box_points.end(); ++it)
        {
            comparable_return_type cd =
                comparable_point_to_range::apply(*it, range, cstrategy);
            if ( cd < cd_min )
            {
                cd_min = cd;
            }
        }

        // consider all distances of the points in the range to the
        // segment or box
        typedef typename range_iterator<Range const>::type iterator_type;
        for (iterator_type it = boost::begin(range); it != boost::end(range); ++it)
        {
            comparable_return_type cd =
                comparable_distance_point_to_segment_or_box
                    <
                        typename point_type<Range>::type,
                        std::vector<segment_or_box_point>,
                        comparable_strategy,
                        typename tag<SegmentOrBox>::type
                    >::apply(*it, segment_or_box_points, cstrategy);

            if ( cd < cd_min )
            {
                cd_min = cd;
            }
        }

        return strategy::distance::services::comparable_to_regular
            <
                comparable_strategy, Strategy,
                range_point, segment_or_box_point
            >::apply(cd_min);
    }

    static inline return_type
    apply(SegmentOrBox const& segment_or_box, Range const& range,
          Strategy const& strategy, bool check_intersection = true)
    {
        return apply(range, segment_or_box, strategy, check_intersection);
    }
};

}} // namespace detail::distance
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename Linestring, typename Segment, typename Strategy>
struct distance
    <
        Linestring, Segment, Strategy, linestring_tag, segment_tag,
        strategy_tag_distance_point_segment, false
    >
        : detail::distance::range_to_segment_or_box
            <
                Linestring, Segment, Strategy
            >
{};

template <typename Segment, typename Ring, typename Strategy>
struct distance
    <
        Segment, Ring, Strategy, segment_tag, ring_tag,
        strategy_tag_distance_point_segment, false
    >
    : detail::distance::range_to_segment_or_box
        <
            Ring, Segment, Strategy
        >
{};

template <typename Linestring, typename Box, typename Strategy>
struct distance
    <
        Linestring, Box, Strategy, linestring_tag, box_tag,
        strategy_tag_distance_point_segment, false
    >
        : detail::distance::range_to_segment_or_box
            <
                Linestring, Box, Strategy
            >
{};

template <typename Ring, typename Box, typename Strategy>
struct distance
    <
        Ring, Box, Strategy, ring_tag, box_tag,
        strategy_tag_distance_point_segment, false
    >
    : detail::distance::range_to_segment_or_box
        <
            Ring, Box, Strategy
        >
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISTANCE_RANGE_TO_SEGMENT_OR_BOX_HPP


// Copyright (c) 2021 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE Test_Adam_CompoundScalarHelpers
#include <boost/test/unit_test.hpp>
#include <popart/adam.hpp>
#include <popart/compoundscalarhelper.hpp>
#include <popart/sessionoptions.hpp>

using namespace popart;

namespace {
void validate(Adam adam,
              float expected_lr,
              float expected_gs,
              float expected_eps) {
  AdamLearningRateHelper lr;
  AdamGradientScalingHelper gs;
  AdamEpsHelper eps;

  BOOST_CHECK_EQUAL(lr.val("", adam), expected_lr);
  BOOST_CHECK_EQUAL(gs.val("", adam), expected_gs);
  BOOST_CHECK_EQUAL(eps.val("", adam), expected_eps);
}
} // namespace

BOOST_AUTO_TEST_CASE(TestAdam_Base) {
  Adam adam{{
                {"defaultLearningRate", {0.1f, true}},
                {"defaultEps", {1e-3f, true}},
            },
            AdamMode::Adam,
            WeightDecayMode::Decay,
            DataType::FLOAT,
            DataType::FLOAT,
            DataType::FLOAT};
  SessionOptions opts;
  adam.setFactorsFromOptions(opts);
  validate(adam, 0.1f, 1.0f, 1e-3f);
}

BOOST_AUTO_TEST_CASE(TestAdam_lossScaling) {
  Adam adam{{{"defaultLearningRate", {0.1f, true}},
             {"defaultEps", {1e-3f, true}},
             {"lossScaling", {4.0f, true}}},
            AdamMode::Adam,
            WeightDecayMode::Decay,
            DataType::FLOAT,
            DataType::FLOAT,
            DataType::FLOAT};
  SessionOptions opts;
  adam.setFactorsFromOptions(opts);
  validate(adam, 0.1f, 1.0f / 4.0f, 1e-3f);
}

BOOST_AUTO_TEST_CASE(TestAdam_ReplicaSum) {
  Adam adam{{
                {"defaultLearningRate", {0.1f, true}},
                {"defaultEps", {1e-3f, true}},
            },
            AdamMode::Adam,
            WeightDecayMode::Decay,
            DataType::FLOAT,
            DataType::FLOAT,
            DataType::FLOAT};
  SessionOptions opts;
  opts.enableReplicatedGraphs = true;
  opts.replicatedGraphCount   = 2;
  adam.setFactorsFromOptions(opts);
  validate(adam, 0.1f, 1.0f, 1e-3f);
}

BOOST_AUTO_TEST_CASE(TestAdam_ReplicaMeanPostAndLoss) {
  Adam adam{{
                {"defaultLearningRate", {0.1f, true}},
                {"defaultEps", {1e-3f, true}},
            },
            AdamMode::Adam,
            WeightDecayMode::Decay,
            DataType::FLOAT,
            DataType::FLOAT,
            DataType::FLOAT};
  SessionOptions opts;
  opts.enableReplicatedGraphs                  = true;
  opts.replicatedGraphCount                    = 2;
  opts.accumulationAndReplicationReductionType = ReductionType::Mean;
  opts.meanAccumulationAndReplicationReductionStrategy =
      MeanReductionStrategy::PostAndLoss;
  adam.setFactorsFromOptions(opts);
  validate(adam, 0.1f, 1.0f, 1e-3f);
}

BOOST_AUTO_TEST_CASE(TestAdam_ReplicaMeanPost) {
  Adam adam{{
                {"defaultLearningRate", {0.1f, true}},
                {"defaultEps", {1e-3f, true}},
            },
            AdamMode::Adam,
            WeightDecayMode::Decay,
            DataType::FLOAT,
            DataType::FLOAT,
            DataType::FLOAT};
  SessionOptions opts;
  opts.enableReplicatedGraphs                  = true;
  opts.replicatedGraphCount                    = 2;
  opts.accumulationAndReplicationReductionType = ReductionType::Mean;
  opts.meanAccumulationAndReplicationReductionStrategy =
      MeanReductionStrategy::Post;
  adam.setFactorsFromOptions(opts);
  validate(adam, 0.1f, 1.0f / 2.0f, 1e-3f);
}

BOOST_AUTO_TEST_CASE(TestAdam_ReplicaMeanRunning) {
  Adam adam{{
                {"defaultLearningRate", {0.1f, true}},
                {"defaultEps", {1e-3f, true}},
            },
            AdamMode::Adam,
            WeightDecayMode::Decay,
            DataType::FLOAT,
            DataType::FLOAT,
            DataType::FLOAT};
  SessionOptions opts;
  opts.enableReplicatedGraphs                  = true;
  opts.replicatedGraphCount                    = 2;
  opts.accumulationAndReplicationReductionType = ReductionType::Mean;
  opts.meanAccumulationAndReplicationReductionStrategy =
      MeanReductionStrategy::Running;
  adam.setFactorsFromOptions(opts);
  validate(adam, 0.1f, 1.0f, 1e-3f);
}

BOOST_AUTO_TEST_CASE(TestAdam_AccumSum) {
  Adam adam{{
                {"defaultLearningRate", {0.1f, true}},
                {"defaultEps", {1e-3f, true}},
            },
            AdamMode::Adam,
            WeightDecayMode::Decay,
            DataType::FLOAT,
            DataType::FLOAT,
            DataType::FLOAT};
  SessionOptions opts;
  opts.enableGradientAccumulation = true;
  opts.accumulationFactor         = 4;
  adam.setFactorsFromOptions(opts);
  validate(adam, 0.1f, 1.0f, 1e-3f);
}

BOOST_AUTO_TEST_CASE(TestAdam_AccumMeanPostAndLoss) {
  Adam adam{{
                {"defaultLearningRate", {0.1f, true}},
                {"defaultEps", {1e-3f, true}},
            },
            AdamMode::Adam,
            WeightDecayMode::Decay,
            DataType::FLOAT,
            DataType::FLOAT,
            DataType::FLOAT};
  SessionOptions opts;
  opts.enableGradientAccumulation              = true;
  opts.accumulationFactor                      = 4;
  opts.accumulationAndReplicationReductionType = ReductionType::Mean;
  opts.meanAccumulationAndReplicationReductionStrategy =
      MeanReductionStrategy::PostAndLoss;
  adam.setFactorsFromOptions(opts);
  validate(adam, 0.1f, 1.0f / 4.0f, 1e-3f);
}

BOOST_AUTO_TEST_CASE(TestAdam_AccumMeanPost) {
  Adam adam{{
                {"defaultLearningRate", {0.1f, true}},
                {"defaultEps", {1e-3f, true}},
            },
            AdamMode::Adam,
            WeightDecayMode::Decay,
            DataType::FLOAT,
            DataType::FLOAT,
            DataType::FLOAT};
  SessionOptions opts;
  opts.enableGradientAccumulation              = true;
  opts.accumulationFactor                      = 4;
  opts.accumulationAndReplicationReductionType = ReductionType::Mean;
  opts.meanAccumulationAndReplicationReductionStrategy =
      MeanReductionStrategy::Post;
  adam.setFactorsFromOptions(opts);
  validate(adam, 0.1f, 1.0f / 4.0f, 1e-3f);
}

BOOST_AUTO_TEST_CASE(TestAdam_AccumMeanRunning) {
  Adam adam{{
                {"defaultLearningRate", {0.1f, true}},
                {"defaultEps", {1e-3f, true}},
            },
            AdamMode::Adam,
            WeightDecayMode::Decay,
            DataType::FLOAT,
            DataType::FLOAT,
            DataType::FLOAT};
  SessionOptions opts;
  opts.enableGradientAccumulation              = true;
  opts.accumulationFactor                      = 4;
  opts.accumulationAndReplicationReductionType = ReductionType::Mean;
  opts.meanAccumulationAndReplicationReductionStrategy =
      MeanReductionStrategy::Running;
  adam.setFactorsFromOptions(opts);
  validate(adam, 0.1f, 1.0f, 1e-3f);
}

BOOST_AUTO_TEST_CASE(TestAdam_scaledOptimizerState) {
  Adam adam{std::map<std::string, std::pair<float, bool>>{
                {"defaultLearningRate", {0.1f, true}},
                {"defaultWeightDecay", {0.01f, true}},
                {"defaultEps", {1e-3f, true}},
                {"lossScaling", {4.0f, true}}},
            AdamMode::Adam,
            WeightDecayMode::Decay,
            DataType::FLOAT,
            DataType::FLOAT,
            DataType::FLOAT,
            {},
            true};
  SessionOptions opts;
  adam.setFactorsFromOptions(opts);
  validate(adam, 0.1f, 1.0f, 1e-3f * 4.0f);
}