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

// Copyright (c) 2012-2014 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2017 Adam Wulkiewicz, Lodz, Poland.

// This file was modified by Oracle on 2016.
// Modifications copyright (c) 2016 Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// 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_BUFFER_TURN_IN_PIECE_VISITOR
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_TURN_IN_PIECE_VISITOR

#include <boost/core/ignore_unused.hpp>

#include <boost/range.hpp>

#include <boost/geometry/core/assert.hpp>

#include <boost/geometry/arithmetic/dot_product.hpp>
#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/algorithms/comparable_distance.hpp>
#include <boost/geometry/algorithms/equals.hpp>
#include <boost/geometry/algorithms/expand.hpp>
#include <boost/geometry/algorithms/detail/disjoint/point_box.hpp>
#include <boost/geometry/algorithms/detail/disjoint/box_box.hpp>
#include <boost/geometry/algorithms/detail/overlay/segment_identifier.hpp>
#include <boost/geometry/algorithms/detail/overlay/get_turn_info.hpp>
#include <boost/geometry/policies/compare.hpp>
#include <boost/geometry/strategies/buffer.hpp>
#include <boost/geometry/algorithms/detail/buffer/buffer_policies.hpp>

#if defined(BOOST_GEOMETRY_BUFFER_USE_SIDE_OF_INTERSECTION)
#include <boost/geometry/strategies/cartesian/side_of_intersection.hpp>
#endif

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace buffer
{

struct piece_get_box
{
    template <typename Box, typename Piece>
    static inline void apply(Box& total, Piece const& piece)
    {
        geometry::expand(total, piece.robust_envelope);
    }
};

struct piece_ovelaps_box
{
    template <typename Box, typename Piece>
    static inline bool apply(Box const& box, Piece const& piece)
    {
        if (piece.type == strategy::buffer::buffered_flat_end
            || piece.type == strategy::buffer::buffered_concave)
        {
            // Turns cannot be inside a flat end (though they can be on border)
            // Neither we need to check if they are inside concave helper pieces

            // Skip all pieces not used as soon as possible
            return false;
        }

        return ! geometry::detail::disjoint::disjoint_box_box(box, piece.robust_envelope);
    }
};

struct turn_get_box
{
    template <typename Box, typename Turn>
    static inline void apply(Box& total, Turn const& turn)
    {
        geometry::expand(total, turn.robust_point);
    }
};

struct turn_ovelaps_box
{
    template <typename Box, typename Turn>
    static inline bool apply(Box const& box, Turn const& turn)
    {
        return ! geometry::detail::disjoint::disjoint_point_box(turn.robust_point, box);
    }
};

enum analyse_result
{
    analyse_unknown,
    analyse_continue,
    analyse_disjoint,
    analyse_within,
    analyse_on_original_boundary,
    analyse_on_offsetted
#if ! defined(BOOST_GEOMETRY_BUFFER_USE_SIDE_OF_INTERSECTION)
    , analyse_near_offsetted
#endif
};

template <typename Point>
inline bool in_box(Point const& previous,
        Point const& current, Point const& point)
{
    // Get its box (TODO: this can be prepared-on-demand later)
    typedef geometry::model::box<Point> box_type;
    box_type box;
    geometry::assign_inverse(box);
    geometry::expand(box, previous);
    geometry::expand(box, current);

    return geometry::covered_by(point, box);
}

template <typename Point, typename Turn>
inline analyse_result check_segment(Point const& previous,
        Point const& current, Turn const& turn,
        bool from_monotonic)
{

#if defined(BOOST_GEOMETRY_BUFFER_USE_SIDE_OF_INTERSECTION)
    typedef geometry::model::referring_segment<Point const> segment_type;
    segment_type const p(turn.rob_pi, turn.rob_pj);
    segment_type const q(turn.rob_qi, turn.rob_qj);
    segment_type const r(previous, current);
    int const side = strategy::side::side_of_intersection::apply(p, q, r,
                turn.robust_point);

    if (side == 0)
    {
        return analyse_on_offsetted;
    }
    if (side == -1 && from_monotonic)
    {
        return analyse_within;
    }
    if (side == 1 && from_monotonic)
    {
        return analyse_disjoint;
    }
    return analyse_continue;

#else

    typedef typename strategy::side::services::default_strategy
        <
            typename cs_tag<Point>::type
        >::type side_strategy;
    typedef typename geometry::coordinate_type<Point>::type coordinate_type;

    coordinate_type const twice_area
        = side_strategy::template side_value
            <
                coordinate_type,
                coordinate_type
            >(previous, current, turn.robust_point);

    if (twice_area == 0)
    {
        // Collinear, only on segment if it is covered by its bbox
        if (in_box(previous, current, turn.robust_point))
        {
            return analyse_on_offsetted;
        }
    }
    else if (twice_area < 0)
    {
        // It is in the triangle right-of the segment where the
        // segment is the hypothenusa. Check if it is close
        // (within rounding-area)
        if (twice_area * twice_area < geometry::comparable_distance(previous, current)
            && in_box(previous, current, turn.robust_point))
        {
            return analyse_near_offsetted;
        }
        else if (from_monotonic)
        {
            return analyse_within;
        }
    }
    else if (twice_area > 0 && from_monotonic)
    {
        // Left of segment
        return analyse_disjoint;
    }

    // Not monotonic, on left or right side: continue analysing
    return analyse_continue;
#endif
}

class analyse_turn_wrt_point_piece
{
public :
    template <typename Turn, typename Piece>
    static inline analyse_result apply(Turn const& turn, Piece const& piece)
    {
        typedef typename Piece::section_type section_type;
        typedef typename Turn::robust_point_type point_type;
        typedef typename geometry::coordinate_type<point_type>::type coordinate_type;

#if defined(BOOST_GEOMETRY_BUFFER_USE_SIDE_OF_INTERSECTION)
        typedef geometry::model::referring_segment<point_type const> segment_type;
        segment_type const p(turn.rob_pi, turn.rob_pj);
        segment_type const q(turn.rob_qi, turn.rob_qj);
#else
        typedef strategy::within::winding<point_type> strategy_type;

        typename strategy_type::state_type state;
        strategy_type strategy;
        boost::ignore_unused(strategy);
#endif

        BOOST_GEOMETRY_ASSERT(! piece.sections.empty());

        coordinate_type const point_x = geometry::get<0>(turn.robust_point);

        for (std::size_t s = 0; s < piece.sections.size(); s++)
        {
            section_type const& section = piece.sections[s];
            // If point within horizontal range of monotonic section:
            if (! section.duplicate
                && section.begin_index < section.end_index
                && point_x >= geometry::get<min_corner, 0>(section.bounding_box) - 1
                && point_x <= geometry::get<max_corner, 0>(section.bounding_box) + 1)
            {
                for (signed_size_type i = section.begin_index + 1; i <= section.end_index; i++)
                {
                    point_type const& previous = piece.robust_ring[i - 1];
                    point_type const& current = piece.robust_ring[i];

#if defined(BOOST_GEOMETRY_BUFFER_USE_SIDE_OF_INTERSECTION)

                    // First check if it is in range - if it is not, the
                    // expensive side_of_intersection does not need to be
                    // applied
                    coordinate_type x1 = geometry::get<0>(previous);
                    coordinate_type x2 = geometry::get<0>(current);

                    if (x1 > x2)
                    {
                        std::swap(x1, x2);
                    }

                    if (point_x >= x1 - 1 && point_x <= x2 + 1)
                    {
                        segment_type const r(previous, current);
                        int const side = strategy::side::side_of_intersection::apply(p, q, r,
                                    turn.robust_point);

                        // Sections are monotonic in x-dimension
                        if (side == 1)
                        {
                            // Left on segment
                            return analyse_disjoint;
                        }
                        else if (side == 0)
                        {
                            // Collinear - TODO: check if really on segment
                            return analyse_on_offsetted;
                        }
                    }
#else
                    analyse_result code = check_segment(previous, current, turn, false);
                    if (code != analyse_continue)
                    {
                        return code;
                    }

                    // Get the state (to determine it is within), we don't have
                    // to cover the on-segment case (covered above)
                    strategy.apply(turn.robust_point, previous, current, state);
#endif
                }
            }
        }

#if defined(BOOST_GEOMETRY_BUFFER_USE_SIDE_OF_INTERSECTION)
        // It is nowhere outside, and not on segment, so it is within
        return analyse_within;
#else
        int const code = strategy.result(state);
        if (code == 1)
        {
            return analyse_within;
        }
        else if (code == -1)
        {
            return analyse_disjoint;
        }

        // Should normally not occur - on-segment is covered
        return analyse_unknown;
#endif
    }

};

class analyse_turn_wrt_piece
{
    template <typename Point, typename Turn>
    static inline analyse_result check_helper_segment(Point const& s1,
                Point const& s2, Turn const& turn,
#if defined(BOOST_GEOMETRY_BUFFER_USE_SIDE_OF_INTERSECTION)
                bool , // is on original, to be reused
#else
                bool is_original,
#endif
                Point const& offsetted)
    {
        boost::ignore_unused(offsetted);
#if defined(BOOST_GEOMETRY_BUFFER_USE_SIDE_OF_INTERSECTION)
        typedef geometry::model::referring_segment<Point const> segment_type;
        segment_type const p(turn.rob_pi, turn.rob_pj);
        segment_type const q(turn.rob_qi, turn.rob_qj);
        segment_type const r(s1, s2);
        int const side = strategy::side::side_of_intersection::apply(p, q, r,
                    turn.robust_point);

        if (side == 1)
        {
            // left of segment
            return analyse_disjoint;
        }
        else if (side == 0)
        {
            // If is collinear, either on segment or before/after
            typedef geometry::model::box<Point> box_type;

            box_type box;
            geometry::assign_inverse(box);
            geometry::expand(box, s1);
            geometry::expand(box, s2);

            if (geometry::covered_by(turn.robust_point, box))
            {
                // Points on helper-segments (and not on its corners)
                // are considered as within
                return analyse_within;
            }

            // It is collinear but not on the segment. Because these
            // segments are convex, it is outside
            // Unless the offsetted ring is collinear or concave w.r.t.
            // helper-segment but that scenario is not yet supported
            return analyse_disjoint;
        }

        // right of segment
        return analyse_continue;
#else
        typedef typename strategy::side::services::default_strategy
            <
                typename cs_tag<Point>::type
            >::type side_strategy;

        switch(side_strategy::apply(s1, s2, turn.robust_point))
        {
            case 1 :
                return analyse_disjoint; // left of segment
            case 0 :
                {
                    // If is collinear, either on segment or before/after
                    typedef geometry::model::box<Point> box_type;

                    box_type box;
                    geometry::assign_inverse(box);
                    geometry::expand(box, s1);
                    geometry::expand(box, s2);

                    if (geometry::covered_by(turn.robust_point, box))
                    {
                        // It is on the segment
                        if (! is_original
                            && geometry::comparable_distance(turn.robust_point, offsetted) <= 1)
                        {
                            // It is close to the offsetted-boundary, take
                            // any rounding-issues into account
                            return analyse_near_offsetted;
                        }

                        // Points on helper-segments are considered as within
                        // Points on original boundary are processed differently
                        return is_original
                            ? analyse_on_original_boundary
                            : analyse_within;
                    }

                    // It is collinear but not on the segment. Because these
                    // segments are convex, it is outside
                    // Unless the offsetted ring is collinear or concave w.r.t.
                    // helper-segment but that scenario is not yet supported
                    return analyse_disjoint;
                }
                break;
        }

        // right of segment
        return analyse_continue;
#endif
    }

    template <typename Turn, typename Piece>
    static inline analyse_result check_helper_segments(Turn const& turn, Piece const& piece)
    {
        typedef typename Turn::robust_point_type point_type;
        geometry::equal_to<point_type> comparator;

        point_type points[4];

        signed_size_type helper_count = static_cast<signed_size_type>(piece.robust_ring.size())
                                            - piece.offsetted_count;
        if (helper_count == 4)
        {
            for (int i = 0; i < 4; i++)
            {
                points[i] = piece.robust_ring[piece.offsetted_count + i];
            }

            //      3--offsetted outline--0
            //      |                     |
            // left |                     | right
            //      |                     |
            //      2===>==original===>===1

        }
        else if (helper_count == 3)
        {
            // Triangular piece, assign points but assign second twice
            for (int i = 0; i < 4; i++)
            {
                int index = i < 2 ? i : i - 1;
                points[i] = piece.robust_ring[piece.offsetted_count + index];
            }
        }
        else
        {
            // Some pieces (e.g. around points) do not have helper segments.
            // Others should have 3 (join) or 4 (side)
            return analyse_continue;
        }

        // First check point-equality
        point_type const& point = turn.robust_point;
        if (comparator(point, points[0]) || comparator(point, points[3]))
        {
            return analyse_on_offsetted;
        }
        if (comparator(point, points[1]))
        {
            // On original, right corner
            return piece.is_flat_end ? analyse_continue : analyse_on_original_boundary;
        }
        if (comparator(point, points[2]))
        {
            // On original, left corner
            return piece.is_flat_start ? analyse_continue : analyse_on_original_boundary;
        }

        // Right side of the piece
        analyse_result result
            = check_helper_segment(points[0], points[1], turn,
                    false, points[0]);
        if (result != analyse_continue)
        {
            return result;
        }

        // Left side of the piece
        result = check_helper_segment(points[2], points[3], turn,
                    false, points[3]);
        if (result != analyse_continue)
        {
            return result;
        }

        if (! comparator(points[1], points[2]))
        {
            // Side of the piece at side of original geometry
            result = check_helper_segment(points[1], points[2], turn,
                        true, point);
            if (result != analyse_continue)
            {
                return result;
            }
        }

        // We are within the \/ or |_| shaped piece, where the top is the
        // offsetted ring.
        if (! geometry::covered_by(point, piece.robust_offsetted_envelope))
        {
            // Not in offsetted-area. This makes a cheap check possible
            typedef typename strategy::side::services::default_strategy
                <
                    typename cs_tag<point_type>::type
                >::type side_strategy;

            switch(side_strategy::apply(points[3], points[0], point))
            {
                case 1 : return analyse_disjoint;
                case -1 : return analyse_within;
                case 0 : return analyse_disjoint;
            }
        }

        return analyse_continue;
    }

    template <typename Turn, typename Piece, typename Compare>
    static inline analyse_result check_monotonic(Turn const& turn, Piece const& piece, Compare const& compare)
    {
        typedef typename Piece::piece_robust_ring_type ring_type;
        typedef typename ring_type::const_iterator it_type;
        it_type end = piece.robust_ring.begin() + piece.offsetted_count;
        it_type it = std::lower_bound(piece.robust_ring.begin(),
                    end,
                    turn.robust_point,
                    compare);

        if (it != end
            && it != piece.robust_ring.begin())
        {
            // iterator points to point larger than point
            // w.r.t. specified direction, and prev points to a point smaller
            // We now know if it is inside/outside
            it_type prev = it - 1;
            return check_segment(*prev, *it, turn, true);
        }
        return analyse_continue;
    }

public :
    template <typename Turn, typename Piece>
    static inline analyse_result apply(Turn const& turn, Piece const& piece)
    {
        typedef typename Turn::robust_point_type point_type;
        analyse_result code = check_helper_segments(turn, piece);
        if (code != analyse_continue)
        {
            return code;
        }

        geometry::equal_to<point_type> comparator;

        if (piece.offsetted_count > 8)
        {
            // If the offset contains some points and is monotonic, we try
            // to avoid walking all points linearly.
            // We try it only once.
            if (piece.is_monotonic_increasing[0])
            {
                code = check_monotonic(turn, piece, geometry::less<point_type, 0>());
                if (code != analyse_continue) return code;
            }
            else if (piece.is_monotonic_increasing[1])
            {
                code = check_monotonic(turn, piece, geometry::less<point_type, 1>());
                if (code != analyse_continue) return code;
            }
            else if (piece.is_monotonic_decreasing[0])
            {
                code = check_monotonic(turn, piece, geometry::greater<point_type, 0>());
                if (code != analyse_continue) return code;
            }
            else if (piece.is_monotonic_decreasing[1])
            {
                code = check_monotonic(turn, piece, geometry::greater<point_type, 1>());
                if (code != analyse_continue) return code;
            }
        }

        // It is small or not monotonic, walk linearly through offset
        // TODO: this will be combined with winding strategy

        for (signed_size_type i = 1; i < piece.offsetted_count; i++)
        {
            point_type const& previous = piece.robust_ring[i - 1];
            point_type const& current = piece.robust_ring[i];

            // The robust ring can contain duplicates
            // (on which any side or side-value would return 0)
            if (! comparator(previous, current))
            {
                code = check_segment(previous, current, turn, false);
                if (code != analyse_continue)
                {
                    return code;
                }
            }
        }

        return analyse_unknown;
    }

};

template <typename Turns, typename Pieces>
class turn_in_piece_visitor
{
    Turns& m_turns; // because partition is currently operating on const input only
    Pieces const& m_pieces; // to check for piece-type

    template <typename Operation, typename Piece>
    inline bool skip(Operation const& op, Piece const& piece) const
    {
        if (op.piece_index == piece.index)
        {
            return true;
        }
        Piece const& pc = m_pieces[op.piece_index];
        if (pc.left_index == piece.index || pc.right_index == piece.index)
        {
            if (pc.type == strategy::buffer::buffered_flat_end)
            {
                // If it is a flat end, don't compare against its neighbor:
                // it will always be located on one of the helper segments
                return true;
            }
            if (pc.type == strategy::buffer::buffered_concave)
            {
                // If it is concave, the same applies: the IP will be
                // located on one of the helper segments
                return true;
            }
        }

        return false;
    }

#if defined(BOOST_GEOMETRY_BUFFER_USE_SIDE_OF_INTERSECTION)
    // NOTE: this function returns a side value in {-1, 0, 1}
    template <typename Turn, typename Piece>
    static inline int turn_in_convex_piece(Turn const& turn,
            Piece const& piece)
    {
        typedef typename Turn::robust_point_type point_type;
        typedef typename Piece::piece_robust_ring_type ring_type;
        typedef geometry::model::referring_segment<point_type const> segment;

        segment const p(turn.rob_pi, turn.rob_pj);
        segment const q(turn.rob_qi, turn.rob_qj);

        typedef typename boost::range_iterator<ring_type const>::type iterator_type;
        iterator_type it = boost::begin(piece.robust_ring);
        iterator_type end = boost::end(piece.robust_ring);

        // A robust ring is always closed, and always clockwise
        for (iterator_type previous = it++; it != end; ++previous, ++it)
        {
            geometry::equal_to<point_type> comparator;
            if (comparator(*previous, *it))
            {
                // Points are the same
                continue;
            }

            segment r(*previous, *it);

            int const side = strategy::side::side_of_intersection::apply(p, q, r,
                        turn.robust_point);

            if (side == 1)
            {
                // IP is left of segment, so it is outside
                return -1; // outside
            }
            else if (side == 0)
            {
                // IP is collinear with segment. TODO: we should analyze this further
                // For now we use the fallback point
                if (in_box(*previous, *it, turn.robust_point))
                {
                    return 0;
                }
                else
                {
                    return -1; // outside
                }
            }
        }
        return 1; // inside
    }
#endif

public:

    inline turn_in_piece_visitor(Turns& turns, Pieces const& pieces)
        : m_turns(turns)
        , m_pieces(pieces)
    {}

    template <typename Turn, typename Piece>
    inline bool apply(Turn const& turn, Piece const& piece, bool first = true)
    {
        boost::ignore_unused_variable_warning(first);

        if (turn.count_within > 0)
        {
            // Already inside - no need to check again
            return true;
        }

        if (piece.type == strategy::buffer::buffered_flat_end
            || piece.type == strategy::buffer::buffered_concave)
        {
            // Turns cannot be located within flat-end or concave pieces
            return true;
        }

        if (! geometry::covered_by(turn.robust_point, piece.robust_envelope))
        {
            // Easy check: if the turn is not in the envelope, we can safely return
            return true;
        }

        if (skip(turn.operations[0], piece) || skip(turn.operations[1], piece))
        {
            return true;
        }

        // TODO: mutable_piece to make some on-demand preparations in analyse
        Turn& mutable_turn = m_turns[turn.turn_index];

        if (piece.type == geometry::strategy::buffer::buffered_point)
        {
            // Optimization for buffer around points: if distance from center
            // is not between min/max radius, the result is clear
            typedef typename default_comparable_distance_result
                <
                    typename Turn::robust_point_type
                >::type distance_type;

            distance_type const cd
                = geometry::comparable_distance(piece.robust_center,
                        turn.robust_point);

            if (cd < piece.robust_min_comparable_radius)
            {
                mutable_turn.count_within++;
                return true;
            }
            if (cd > piece.robust_max_comparable_radius)
            {
                return true;
            }
        }

        analyse_result analyse_code =
            piece.type == geometry::strategy::buffer::buffered_point
                ? analyse_turn_wrt_point_piece::apply(turn, piece)
                : analyse_turn_wrt_piece::apply(turn, piece);

        switch(analyse_code)
        {
            case analyse_disjoint :
                return true;
            case analyse_on_offsetted :
                mutable_turn.count_on_offsetted++; // value is not used anymore
                return true;
            case analyse_on_original_boundary :
                mutable_turn.count_on_original_boundary++;
                return true;
            case analyse_within :
                mutable_turn.count_within++;
                return true;
#if ! defined(BOOST_GEOMETRY_BUFFER_USE_SIDE_OF_INTERSECTION)
            case analyse_near_offsetted :
                mutable_turn.count_within_near_offsetted++;
                return true;
#endif
            default :
                break;
        }

#if defined(BOOST_GEOMETRY_BUFFER_USE_SIDE_OF_INTERSECTION)
        // We don't know (yet)
        int geometry_code = 0;
        if (piece.is_convex)
        {
            geometry_code = turn_in_convex_piece(turn, piece);
        }
        else
        {

            // TODO: this point_in_geometry is a performance-bottleneck here and
            // will be replaced completely by extending analyse_piece functionality
            geometry_code = detail::within::point_in_geometry(turn.robust_point, piece.robust_ring);
        }
#else
        int geometry_code = detail::within::point_in_geometry(turn.robust_point, piece.robust_ring);
#endif

        if (geometry_code == 1)
        {
            mutable_turn.count_within++;
        }

        return true;
    }
};

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

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_TURN_IN_PIECE_VISITOR


/**
 * Copyright (c) 2017 Melown Technologies SE
 *
 * 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.
 *
 * 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 HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <boost/utility/in_place_factory.hpp>

#include <ogr_spatialref.h>
#include <cpl_conv.h>

#include "dbglog/dbglog.hpp"

#include "utility/streams.hpp"

#include "srsdef.hpp"
#include "srs.hpp"
#include "enu.hpp"
#include "csconvertor.hpp"

namespace geo {

Enu::Enu(const math::Point3 &origin, const SrsDefinition &srs)
{
    auto geog(srs.reference());
    auto o(origin);

    if (!geog.IsGeographic()) {
        // extract geographic ref from this srs
        ::OGRSpatialReference tmp;
        if (tmp.CopyGeogCSFrom(&geog) != OGRERR_NONE) {
            LOGTHROW(err1, std::runtime_error)
                << "Could not extract geographic SRS from definition \""
                << srs << "\".";
        }

        // convert origin
        const CsConvertor conv(geog, tmp);
        o = conv(o);
    }

    // construct ENU from geographic system's spheroid and origin
    lon0 = o(0);
    lat0 = o(1);
    h0 = o(2);

    // TODO: check for WGS84 and do not set following
    spheroid = boost::in_place(geog.GetSemiMajor(), geog.GetSemiMinor());
    towgs84.resize(7);
    geog.GetTOWGS84(towgs84.data(), int(towgs84.size()));
}

} // namespace geo


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

#include <nt2/core/functions/inner_fold.hpp>
#include <nt2/core/functions/details/fold_step.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/meta/is_vectorizable.hpp>

#ifndef BOOST_SIMD_NO_SIMD

namespace nt2 { namespace ext
{
  //============================================================================
  // General inner_fold
  //============================================================================
  BOOST_DISPATCH_IMPLEMENT_IF   ( inner_fold_, boost::simd::tag::simd_
                                , (Out)(In)(Neutral)(Bop)(Uop)
                                , ( boost::simd::meta::
                                    is_vectorizable < typename Out::value_type
                                                    , BOOST_SIMD_DEFAULT_EXTENSION
                                                    >
                                  )
                                , ((ast_< Out, nt2::container::domain>))
                                  ((ast_< In, nt2::container::domain>))
                                  (unspecified_<Neutral>)
                                  (unspecified_<Bop>)
                                  (unspecified_<Uop>)
                                )
  {
    typedef void                                                              result_type;
    typedef typename Out::value_type                                           value_type;
    typedef typename In::extent_type                                          extent_type;
    typedef boost::simd::native<value_type,BOOST_SIMD_DEFAULT_EXTENSION>      target_type;

    BOOST_FORCEINLINE result_type
    operator()(Out& out, In& in
              , Neutral const& neutral, Bop const& bop, Uop const& uop
              ) const
    {
      extent_type ext = in.extent();
      std::size_t obound = nt2::numel(boost::fusion::pop_front(ext));
      static const std::size_t N = boost::simd::meta::cardinal_of<target_type>::value;
      std::size_t bound  = boost::fusion::at_c<0>(ext);
      std::size_t nb_vec = (bound/N);
      std::size_t ibound = nb_vec * N;

      for(std::size_t j = 0, k = 0; j != obound; ++j, k+=bound)
      {
        target_type vec_out = details::fold_step(
          neutral(nt2::meta::as_<target_type>()), in, bop, k, nb_vec, N
        );

        value_type s_out = uop( vec_out );
        s_out = details::fold_step(s_out, in, bop, k+ibound, bound-ibound, 1);

        nt2::run(out, j, s_out);
      }
    }
  };
} }

#endif
#endif


// Copyright (c) 2019-2020, CNRS
// Authors: Pierre Fernbach <pierre.fernbach@laas.fr>,

#include <iostream>

#define BOOST_TEST_MODULE StatsTests
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

#include "multicontact-api/scenario/contact-sequence.hpp"
#include "multicontact-api/scenario/fwd.hpp"
#include "curves/fwd.h"
#include <curves/so3_linear.h>
#include <curves/se3_curve.h>
#include <curves/polynomial.h>
#include <curves/bezier_curve.h>
#include <curves/piecewise_curve.h>
#include <curves/exact_cubic.h>
#include <curves/cubic_hermite_spline.h>

/**
 * This unit test try to deserialize the ContactSequences in the examples/previous_versions folder
 * And check that they are compatible with the current version
 */

using namespace multicontact_api::scenario;

const std::string path = TEST_DATA_PATH;
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(api_0) {
  ContactSequence cs;
  cs.loadFromBinary(path + "previous_versions/api_0.cs");
  BOOST_CHECK_EQUAL(cs.size(), 9);
  BOOST_CHECK(cs.haveConsistentContacts());
  BOOST_CHECK(cs.haveTimings());
  BOOST_CHECK(cs.haveCentroidalValues());
  BOOST_CHECK(cs.haveCentroidalTrajectories());
  BOOST_CHECK(cs.haveEffectorsTrajectories(1e-1));
  BOOST_CHECK(cs.haveJointsTrajectories());
  BOOST_CHECK(cs.haveJointsDerivativesTrajectories());
  BOOST_CHECK(cs.haveContactForcesTrajectories());
  BOOST_CHECK(cs.haveZMPtrajectories());
  BOOST_CHECK(cs.haveFriction());
  BOOST_CHECK(!cs.haveContactModelDefined());
}

BOOST_AUTO_TEST_CASE(api_1) {
  ContactSequence cs;
  cs.loadFromBinary(path + "previous_versions/api_1.cs");
  BOOST_CHECK_EQUAL(cs.size(), 9);
  BOOST_CHECK(cs.haveConsistentContacts());
  BOOST_CHECK(cs.haveTimings());
  BOOST_CHECK(cs.haveCentroidalValues());
  BOOST_CHECK(cs.haveCentroidalTrajectories());
  BOOST_CHECK(cs.haveEffectorsTrajectories(1e-1));
  BOOST_CHECK(cs.haveJointsTrajectories());
  BOOST_CHECK(cs.haveJointsDerivativesTrajectories());
  BOOST_CHECK(cs.haveContactForcesTrajectories());
  BOOST_CHECK(cs.haveZMPtrajectories());
  BOOST_CHECK(cs.haveFriction());
  BOOST_CHECK(cs.haveContactModelDefined());
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <atomic>
#include <condition_variable>
#include <map>
#include <mutex>
#include <optional>
#include <variant>

#include <boost/asio/steady_timer.hpp>
#include <boost/multiprecision/cpp_int.hpp>

#include <koinos/block_production/block_producer.hpp>
#include <koinos/contracts/pow/pow.pb.h>

namespace koinos::block_production {

using boost::multiprecision::uint512_t;
using boost::multiprecision::uint256_t;

using worker_group_type = std::pair< uint256_t, uint256_t >;
using hash_count_type   = std::atomic< uint64_t >;
using contract_id_type  = std::string;

class pow_producer : public block_producer
{
public:
   pow_producer(
      crypto::private_key signing_key,
      boost::asio::io_context& main_context,
      boost::asio::io_context& production_context,
      std::shared_ptr< mq::client > rpc_client,
      int64_t production_threshold,
      uint64_t resources_lower_bound,
      uint64_t resources_upper_bound,
      uint64_t max_inclusion_attempts,
      contract_id_type pow_contract_id,
      std::size_t worker_groups
   );
   ~pow_producer();

   virtual void on_block_accept( const protocol::block& b ) override;

protected:
   void commence() override;
   void halt() override;

private:
   std::map< std::size_t, hash_count_type >      _worker_hashrate;
   std::vector< worker_group_type >              _worker_groups;
   std::mutex                                    _cv_mutex;
   std::condition_variable                       _cv;
   boost::asio::steady_timer                     _update_timer;
   uint64_t                                      _last_known_height = 0;
   boost::asio::steady_timer                     _error_timer;
   std::atomic< std::chrono::seconds >           _error_wait_time = std::chrono::seconds( 5 );
   std::atomic< bool >                           _hashing;
   const contract_id_type                        _pow_contract_id;
   const std::size_t                             _num_worker_groups;

   void produce( const boost::system::error_code& ec );
   void display_hashrate( const boost::system::error_code& ec );
   void find_nonce(
      std::size_t worker_index,
      const protocol::block& block,
      uint256_t difficulty,
      uint256_t start,
      uint256_t end,
      std::shared_ptr< std::optional< uint256_t > > nonce,
      std::shared_ptr< std::atomic< bool > > done
   );
   bool target_met( const crypto::multihash& hash, uint256_t target );
   contracts::pow::difficulty_metadata get_difficulty_meta();
   std::string hashrate_to_string( double hashrate );
   std::string compute_network_hashrate( const contracts::pow::difficulty_metadata& meta );
};

} // koinos::block_production


/*******************************************************
 *
 * Copyright (C) 2022, Chen Jianqu, Shanghai University
 *
 * This file is part of detector_desktop.
 *
 * Licensed under the MIT License;
 * you may not use this file except in compliance with the License.
 *******************************************************/
#include "detector_desktop/Arm.h"
#include <iostream>
#include <thread>
#include <Eigen/Core>
#include <opencv2/core/eigen.hpp>
#include <pcl/point_cloud.h>
#include <std_msgs/Header.h>
#include <geometry_msgs/Pose.h>
#include <tf_conversions/tf_eigen.h>

using namespace std;

Arm::Arm(ros::NodeHandle* np_){
    arm_client.reset(new ArmClient(*np_, Config::kKinovaPoseActionAddress, true));
    finger_client.reset(new FingerClient(*np_, Config::kKinovaFingerActionAddress, true));
    transform_listener.reset(new tf::TransformListener);
    transform_broadcaster.reset(new tf::TransformBroadcaster);
    cv::cv2eigen(Config::kCamToArm, Tca.matrix());
    Eigen::AngleAxisd yawAngle(Config::kGraspPoseEuler.at<double>(0, 0), Eigen::Vector3d::UnitZ());
    Eigen::AngleAxisd pitchAngle(Config::kGraspPoseEuler.at<double>(1, 0), Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd rollAngle(Config::kGraspPoseEuler.at<double>(2, 0), Eigen::Vector3d::UnitX());
    grasp_q=yawAngle*pitchAngle*rollAngle; //先绕z旋转yaw,再绕y旋转pitch，最后绕x旋转roll
}

void Arm::GetGraspPose(Instance &inst, geometry_msgs::Pose &pose)
{
    Eigen::Quaterniond q;

    //抓取的位置应该是,根据机械臂的安装位置，x z轴均为中心点，物体的y轴为负的，因此需要+y_half
    auto minPt=inst.minPt;
    auto maxPt=inst.maxPt;
    float y_half_len=std::abs(std::abs(minPt.y)-std::abs(maxPt.y))/2.0f;//半径
    float z_half_len=std::abs(std::abs(minPt.z)-std::abs(maxPt.z))/2.0f;//半径
    Eigen::Vector3d center((minPt.x+maxPt.x)/2,(minPt.y+maxPt.y)/2,(minPt.z+maxPt.z)/2); //点云中心点
    pose.position.x=center.x();
    pose.position.y=center.y();//y_half_len+0.02f;//0.02f为余量
    pose.position.z=center.z()+z_half_len;

    //抓取方向设置:使用预先定义的方向
    pose.orientation.x=grasp_q.x();
    pose.orientation.y=grasp_q.y();
    pose.orientation.z=grasp_q.z();
    pose.orientation.w=grasp_q.w();
}

//发布抓取位置的tf，用于可视化
void Arm::BroadcastGraspPose(const geometry_msgs::Pose &pose)
{
    tf::Transform Ttf;
    tf::Quaternion q;
    q.setX(pose.orientation.x);
    q.setY(pose.orientation.y);
    q.setZ(pose.orientation.z);
    q.setW(pose.orientation.w);
    Ttf.setRotation(q);

    tf::Vector3 v;
    v.setX(pose.position.x);
    v.setY(pose.position.y);
    v.setZ(pose.position.z);
    Ttf.setOrigin(v);

    transform_broadcaster->sendTransform(
            tf::StampedTransform(Ttf, ros::Time::now(),
                                 "j2n6s300_link_base", "grasp_target_frame"));
}

bool Arm::SetFinger(float value){
    kinova_msgs::SetFingersPositionGoal goalFinger;
    goalFinger.fingers.finger1=value;//6120是%90的闭合，0是张到最大的状态
    goalFinger.fingers.finger2=value;
    goalFinger.fingers.finger3=value;
    finger_client->sendGoal(goalFinger);

    if(!finger_client->waitForResult(ros::Duration(5.0))){
        Debugt("fingerClient 超时");
        return false;
    }
    if(finger_client->getState() != actionlib::SimpleClientGoalState::SUCCEEDED){
        Debugt("fingerClient 失败,State:{}",finger_client->getState().toString());
        return false;
    }
    return true;
}

bool Arm::SetArmPose(geometry_msgs::Pose &pose)
{
    //设置目标
    kinova_msgs::ArmPoseGoal goalArm;
    goalArm.pose.header.frame_id=Config::kArmMsgFrame;
    goalArm.pose.pose=pose;
    arm_client->sendGoal(goalArm);

    if(!arm_client->waitForResult(ros::Duration(10.0))){
        Debugt("armClient 超时");
        return false;
    }
    if(arm_client->getState() != actionlib::SimpleClientGoalState::SUCCEEDED){
        Debugt("armClient 失败,State:{}",arm_client->getState().toString());
        return false;
    }
    return true;
}

bool Arm::Run(Instance inst)
{
    if(inst.pc->empty()){
        Debugt("instance未构建");
        return false;
    }

    //抓取点的设置
    geometry_msgs::Pose target_pose,pose;
    GetGraspPose(inst, target_pose);
    Debugt("抓取目标的三维位置：({},{},{})",target_pose.position.x,target_pose.position.y,target_pose.position.z);
    BroadcastGraspPose(target_pose);
    Debugt("Wait Action Server...");
    arm_client->waitForServer();
    finger_client->waitForServer();
    if(!SetFinger(0)) //打开手指
        return false;
    std::this_thread::sleep_for(500ms);//休眠
    pose=target_pose;
    pose.position.z +=0.1;
    if(!SetArmPose(pose)) //先到达目标上方
        return false;
    pose.position.z -=0.1;
    if(!SetArmPose(pose)) //到达目标所在的位置
        return false;
    std::this_thread::sleep_for(500ms);
    if(!SetFinger(Config::kArmFingerValue)) //关闭手指，抓取
        return false;
    std::this_thread::sleep_for(500ms);
    Debugt("已抓取物体 往回运动");
    pose.position.z+=0.1; //往上0.2m
    if(!SetArmPose(pose))
        return false;
    pose.position.x+=0.1; //往左0.1m
    if(!SetArmPose(pose))
        return false;
    Debugt("抓取线程完成");
    return true;
}

//使用future的方式判断程序机械臂是否正在运行
//这种方法虽然不是很直观
bool Arm::TryRunArm(Instance &inst) {
    if(!is_arm_running.valid()){ //第一次运行机械臂或结果已从future中取出
        is_arm_running=std::async(std::launch::async, &Arm::Run, this, inst);//立即启动
        return true;
    }
    else{
        if(is_arm_running.wait_for(std::chrono::milliseconds(1)) == std::future_status::ready){
            Debugt("前一次机械臂执行结果:{}",(is_arm_running.get() ? "成功" : "失败"));
            is_arm_running=std::async(std::launch::async, &Arm::Run, this, inst);
            return true;
        }
        else{
            return false;
        }
    }
}


#ifndef INCLUDE_SWIFT_VIO_POINT_SHARED_DATA_HPP_
#define INCLUDE_SWIFT_VIO_POINT_SHARED_DATA_HPP_

#include <memory>
#include <unordered_map>
#include <Eigen/StdVector>

#include <swift_vio/VectorOperations.hpp>
#include <okvis/FrameTypedefs.hpp>

#include <okvis/Measurements.hpp>
#include <okvis/Parameters.hpp>
#include <okvis/ceres/PoseParameterBlock.hpp>

namespace swift_vio {
// The state info for one keypoint relevant to computing the pose (T_WB) and
// (linear and angular) velocity (v_WB, omega_WB_B) at keypoint observation epoch.
struct StateInfoForOneKeypoint {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    StateInfoForOneKeypoint() {

    }
    StateInfoForOneKeypoint(
        uint64_t _frameId, size_t _camIdx,
        std::shared_ptr<const okvis::ceres::ParameterBlock> T_WB_ptr,
        double _normalizedRow, okvis::Time imageStamp)
        : frameId(_frameId),
          cameraId(_camIdx),
          T_WBj_ptr(T_WB_ptr),
          normalizedRow(_normalizedRow),
          imageTimestamp(imageStamp) {}

    uint64_t frameId;
    size_t cameraId;
    std::shared_ptr<const okvis::ceres::ParameterBlock> T_WBj_ptr;
    std::shared_ptr<const okvis::ceres::ParameterBlock> speedAndBiasPtr;
    // IMU measurements covering the state epoch.
    std::shared_ptr<const okvis::ImuMeasurementDeque> imuMeasurementPtr;
    okvis::Time stateEpoch;
    double normalizedRow; // v / imageHeight - 0.5.
    okvis::Time imageTimestamp; // raw image frame timestamp may be different for cameras in NFrame.
    // linearization points at the state.
    std::shared_ptr<const Eigen::Matrix<double, 6, 1>> positionVelocityPtr;
    // Pose of the body frame in the world frame at the feature observation epoch.
    // It should be computed with IMU propagation for RS cameras.
    okvis::kinematics::Transformation T_WBtij;
    Eigen::Vector3d v_WBtij;
    Eigen::Vector3d omega_Btij;
    okvis::kinematics::Transformation  T_WBtij_fej;
    Eigen::Vector3d v_WBtij_fej;
};

enum class PointSharedDataState {
  Barebones = 0,
  ImuInfoReady = 1,
  NavStateReady = 2,
  NavStateForJacReady = 3,
};

// Data shared by observations of a point landmark in computing Jacobians
// relative to pose (T_WB) and velocity (v_WB) and camera time parameters.
// The data of the class members may be updated in ceres EvaluationCallback.
class PointSharedData {
 public:
  typedef std::vector<StateInfoForOneKeypoint,
                      Eigen::aligned_allocator<StateInfoForOneKeypoint>>
      StateInfoForObservationsType;

  PointSharedData() : status_(PointSharedDataState::Barebones) {}

  void addKeypointObservation(
      const okvis::KeypointIdentifier& kpi,
      std::shared_ptr<const okvis::ceres::ParameterBlock> T_WBj_ptr,
      double normalizedRow, okvis::Time imageTimestamp) {
    stateInfoForObservations_.emplace_back(kpi.frameId, kpi.cameraIndex,
                                           T_WBj_ptr, normalizedRow, imageTimestamp);
  }

  /// @name Setters for data for IMU propagation.
  /// @{
  void setVelocityParameterBlockPtr(
      int index,
      std::shared_ptr<const okvis::ceres::ParameterBlock> speedAndBiasPtr) {
    stateInfoForObservations_[index].speedAndBiasPtr = speedAndBiasPtr;
  }

  void setImuInfo(
      int index, const okvis::Time stateEpoch,
      std::shared_ptr<const okvis::ImuMeasurementDeque> imuMeasurements,
      std::shared_ptr<const Eigen::Matrix<double, 6, 1>> positionVelocityPtr) {
    stateInfoForObservations_[index].stateEpoch = stateEpoch;
    stateInfoForObservations_[index].imuMeasurementPtr = imuMeasurements;
    stateInfoForObservations_[index].positionVelocityPtr = positionVelocityPtr;
  }

  void setImuAugmentedParameterPtrs(
      const std::vector<std::shared_ptr<const okvis::ceres::ParameterBlock>>&
          imuAugmentedParamBlockPtrs,
      const okvis::ImuParameters* imuParams) {
    imuAugmentedParamBlockPtrs_ = imuAugmentedParamBlockPtrs;
    imuParameters_ = imuParams;
  }

  void setCameraTimeParameterPtrs(
      const std::vector<std::shared_ptr<const okvis::ceres::ParameterBlock>>&
          tdParamBlockPtrs,
      const std::vector<std::shared_ptr<const okvis::ceres::ParameterBlock>>&
          trParamBlockPtrs) {
    tdParamBlockPtrs_ = tdParamBlockPtrs;
    trParamBlockPtrs_ = trParamBlockPtrs;
    status_ = PointSharedDataState::ImuInfoReady;
  }
  /// @}

  /// @name functions for IMU propagation.
  /// @{
  /**
   * @brief computePoseAndVelocityAtObservation.
   *     for feature i, estimate $p_B^G(t_{f_i})$, $R_B^G(t_{f_i})$,
   *     $v_B^G(t_{f_i})$, and $\omega_{GB}^B(t_{f_i})$ with the corresponding
   *     states' LATEST ESTIMATES and imu measurements.
   * @warning Call this function after setImuAugmentedParameterPtrs().
   */
  void computePoseAndVelocityAtObservation();

  /**
   * @brief computePoseAndVelocityForJacobians
   * @warning Only call this function after
   * computePoseAndVelocityAtObservation() has finished.
   * @param useLinearizationPoint
   */
  void computePoseAndVelocityForJacobians(bool useLinearizationPoint);
  /// @}

  /**
   * @brief computeSharedJacobians compute Jacobians common to every
   * observation of the landmark.
   * @param cameraObservationModelId
   */
  void computeSharedJacobians(int cameraObservationModelId);

  /// @name Functions for anchors.
  /// @{
  void setAnchors(const std::vector<AnchorFrameIdentifier>& anchorIds) {
    anchorIds_ = anchorIds;
  }

  const std::vector<AnchorFrameIdentifier>& anchorIds() const {
    return anchorIds_;
  }

  /**
   * @brief Get index of observations from the anchor frames in the observation sequence.
   * @warning This only support a monocular camera.
   * @return
   */
  std::vector<int> anchorObservationIds() const;

  okvis::kinematics::Transformation T_WB_mainAnchor() const {
    return stateInfoForObservations_[anchorIds_[0].observationIndex_].T_WBtij;
  }

  okvis::kinematics::Transformation T_WB_mainAnchorForJacobian(bool useFirstEstimate) const {
    if (useFirstEstimate)
      return stateInfoForObservations_[anchorIds_[0].observationIndex_].T_WBtij_fej;
    else
      return T_WB_mainAnchor();
  }

  okvis::kinematics::Transformation T_WB_mainAnchorStateEpoch() const {
    const StateInfoForOneKeypoint& mainAnchorItem =
        stateInfoForObservations_.at(anchorIds_[0].observationIndex_);
    return std::static_pointer_cast<const okvis::ceres::PoseParameterBlock>(
            mainAnchorItem.T_WBj_ptr)
            ->estimate();
  }

  okvis::kinematics::Transformation T_WB_mainAnchorStateEpochForJacobian(
      bool useFirstEstimate) const {
    const StateInfoForOneKeypoint& mainAnchorItem =
        stateInfoForObservations_.at(anchorIds_[0].observationIndex_);
    okvis::kinematics::Transformation T_WB_fej =
        std::static_pointer_cast<const okvis::ceres::PoseParameterBlock>(
            mainAnchorItem.T_WBj_ptr)
            ->estimate();
    if (useFirstEstimate) {
      std::shared_ptr<const Eigen::Matrix<double, 6, 1>>
          posVelFirstEstimatePtr = mainAnchorItem.positionVelocityPtr;
      T_WB_fej = okvis::kinematics::Transformation(
          posVelFirstEstimatePtr->head<3>(), T_WB_fej.q());
    }
    return T_WB_fej;
  }
  /// @}

  /// @name functions for managing the main stateInfo list.
  /// @{
  StateInfoForObservationsType::iterator begin() {
    return stateInfoForObservations_.begin();
  }

  StateInfoForObservationsType::iterator end() {
      return stateInfoForObservations_.end();
  }

  void removeBadObservations(const std::vector<bool>& projectStatus) {
      removeUnsetMatrices<StateInfoForOneKeypoint>(&stateInfoForObservations_, projectStatus);
  }

  /**
   * @brief removeExtraObservations
   * @warning orderedSelectedFrameIds must be a subsets of stateInfoForObservations_
   * @param orderedSelectedFrameIds
   * @param imageNoise2dStdList
   */
  void removeExtraObservations(const std::vector<uint64_t>& orderedSelectedFrameIds,
                               std::vector<double>* imageNoise2dStdList);

  void removeExtraObservationsLegacy(
      const std::vector<uint64_t>& orderedSelectedFrameIds,
      std::vector<double>* imageNoise2dStdList);
  /// @}

  /// @name Getters for frameIds.
  /// @{
  size_t numObservations() const {
      return stateInfoForObservations_.size();
  }

  std::vector<std::pair<uint64_t, size_t>> frameIds() const {
    std::vector<std::pair<uint64_t, size_t>> frameIds;
    frameIds.reserve(stateInfoForObservations_.size());
    for (auto item : stateInfoForObservations_) {
      frameIds.emplace_back(item.frameId, item.cameraId);
    }
    return frameIds;
  }

  uint64_t frameId(int index) const {
    return stateInfoForObservations_[index].frameId;
  }

  uint64_t lastFrameId() const {
    return stateInfoForObservations_.back().frameId;
  }
  /// @}

  /// @name Getters
  /// @{
  double normalizedFeatureTime(int observationIndex) const {
    return normalizedFeatureTime(stateInfoForObservations_[observationIndex]);
  }

  double normalizedFeatureTime(const StateInfoForOneKeypoint& item) const {
    size_t cameraIdx = item.cameraId;
    return tdParamBlockPtrs_[cameraIdx]->parameters()[0] +
           trParamBlockPtrs_[cameraIdx]->parameters()[0] * item.normalizedRow +
        (item.imageTimestamp - item.stateEpoch).toSec();
  }

  size_t cameraIndex(size_t observationIndex) const {
    return stateInfoForObservations_[observationIndex].cameraId;
  }

  double normalizedRow(int index) const {
    return stateInfoForObservations_[index].normalizedRow;
  }

  std::vector<okvis::kinematics::Transformation,
              Eigen::aligned_allocator<okvis::kinematics::Transformation>>
  poseAtObservationList() const {
    std::vector<okvis::kinematics::Transformation,
                Eigen::aligned_allocator<okvis::kinematics::Transformation>>
        T_WBtij_list;
    T_WBtij_list.reserve(stateInfoForObservations_.size());
    for (auto item : stateInfoForObservations_) {
      T_WBtij_list.push_back(item.T_WBtij);
    }
    return T_WBtij_list;
  }

  std::vector<size_t> cameraIndexList() const {
    std::vector<size_t> camIndices;
    camIndices.reserve(stateInfoForObservations_.size());
    for (auto item : stateInfoForObservations_) {
      camIndices.push_back(item.cameraId);
    }
    return camIndices;
  }

  void poseAtObservation(int index, okvis::kinematics::Transformation* T_WBtij) const {
    *T_WBtij = stateInfoForObservations_[index].T_WBtij;
  }

  okvis::kinematics::Transformation T_WBtij(int index) const {
    return stateInfoForObservations_[index].T_WBtij;
  }

  Eigen::Vector3d omega_Btij(int index) const {
    return stateInfoForObservations_[index].omega_Btij;
  }

  Eigen::Vector3d v_WBtij(int index) const {
    return stateInfoForObservations_[index].v_WBtij;
  }

  okvis::kinematics::Transformation T_WBtij_ForJacobian(int index) const {
    return stateInfoForObservations_[index].T_WBtij_fej;
  }

  Eigen::Matrix3d Phi_pq_feature(int observationIndex) const {
    const StateInfoForOneKeypoint& item =
        stateInfoForObservations_[observationIndex];
    double relFeatureTime = normalizedFeatureTime(item);
    Eigen::Vector3d gW(0, 0, -imuParameters_->g);
    Eigen::Vector3d dr =
        -(item.T_WBtij_fej.r() - item.positionVelocityPtr->head<3>() -
          item.positionVelocityPtr->tail<3>() * relFeatureTime -
          0.5 * gW * relFeatureTime * relFeatureTime);
    return okvis::kinematics::crossMx(dr);
  }

  Eigen::Vector3d v_WBtij_ForJacobian(int index) const {
    return stateInfoForObservations_[index].v_WBtij_fej;
  }

  PointSharedDataState status() const {
    return status_;
  }

  double gravityNorm() const {
    return imuParameters_->g;
  }
  /// @}

  /// @name Getters for parameter blocks
  /// @{
  std::shared_ptr<const okvis::ceres::PoseParameterBlock> poseParameterBlockPtr(
      int observationIndex) const;

  std::shared_ptr<const okvis::ceres::ParameterBlock>
  speedAndBiasParameterBlockPtr(int observationIndex) const {
    return stateInfoForObservations_.at(observationIndex).speedAndBiasPtr;
  }

  std::shared_ptr<const okvis::ceres::ParameterBlock>
  cameraTimeDelayParameterBlockPtr(size_t cameraIndex) const {
    return tdParamBlockPtrs_[cameraIndex];
  }

  std::shared_ptr<const okvis::ceres::ParameterBlock>
  frameReadoutTimeParameterBlockPtr(size_t cameraIndex) const {
    return trParamBlockPtrs_[cameraIndex];
  }
  /// @}

 private:
  // The items of stateInfoForObservations_ are added in an ordered manner
  // by sequentially examining the ordered elements of MapPoint.observations.
  std::vector<StateInfoForOneKeypoint,
              Eigen::aligned_allocator<StateInfoForOneKeypoint>>
      stateInfoForObservations_;

  std::vector<AnchorFrameIdentifier> anchorIds_;

  std::vector<std::shared_ptr<const okvis::ceres::ParameterBlock>>
      tdParamBlockPtrs_;
  std::vector<std::shared_ptr<const okvis::ceres::ParameterBlock>>
      trParamBlockPtrs_;
  std::vector<std::shared_ptr<const okvis::ceres::ParameterBlock>>
      imuAugmentedParamBlockPtrs_;
  const okvis::ImuParameters* imuParameters_;

  // The structure of sharedJacobians is determined by an external cameraObservationModelId.
  std::vector<
      Eigen::Matrix<double, -1, -1, Eigen::RowMajor>,
      Eigen::aligned_allocator<Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>>
      sharedJacobians_;
  PointSharedDataState status_;
};
} // namespace swift_vio

#endif // INCLUDE_SWIFT_VIO_POINT_SHARED_DATA_HPP_


#pragma once

#include <scorum/protocol/betting/market.hpp>
#include <scorum/chain/schema/scorum_object_types.hpp>
#include <scorum/protocol/odds.hpp>

#include <boost/multi_index/hashed_index.hpp>
#include <boost/multi_index/composite_key.hpp>

namespace scorum {
namespace chain {

using scorum::protocol::asset;
using scorum::protocol::odds;
using scorum::protocol::wincase_type;
using scorum::protocol::market_type;

enum class pending_bet_kind : uint8_t
{
    live = 0b01,
    non_live = 0b10
};

struct bet_data
{
    uuid_type uuid;
    fc::time_point_sec created;

    account_name_type better;
    wincase_type wincase;
    asset stake = asset(0, SCORUM_SYMBOL);

    class odds odds;
    pending_bet_kind kind = pending_bet_kind::live;
};

class bet_uuid_history_object : public object<bet_uuid_history_object_type, bet_uuid_history_object>
{
public:
    /// @cond DO_NOT_DOCUMENT
    CHAINBASE_DEFAULT_CONSTRUCTOR(bet_uuid_history_object)
    /// @endcond

    id_type id;
    uuid_type uuid;
};

class pending_bet_object : public object<pending_bet_object_type, pending_bet_object>
{
public:
    /// @cond DO_NOT_DOCUMENT
    CHAINBASE_DEFAULT_CONSTRUCTOR(pending_bet_object)
    /// @endcond

    id_type id;
    uuid_type game_uuid;
    market_type market;

    bet_data data;

    // clang-format off
    fc::time_point_sec get_created() const { return data.created; }
    account_name_type get_better() const { return data.better; }
    pending_bet_kind get_kind() const { return data.kind; }
    uuid_type get_uuid() const { return data.uuid; }
    wincase_type get_wincase() const { return data.wincase; }

    // clang-format on
};

class matched_bet_object : public object<matched_bet_object_type, matched_bet_object>
{
public:
    /// @cond DO_NOT_DOCUMENT
    CHAINBASE_DEFAULT_CONSTRUCTOR(matched_bet_object)
    /// @endcond

    id_type id;
    uuid_type game_uuid;
    market_type market;
    fc::time_point_sec created;

    bet_data bet1_data;
    bet_data bet2_data;

    // clang-format off
    uuid_type get_bet1_uuid() const { return bet1_data.uuid; }
    uuid_type get_bet2_uuid() const { return bet2_data.uuid; }
    // clang-format on
};

struct by_uuid;
struct by_game_uuid_kind;
struct by_game_uuid_market;
struct by_game_uuid_created;

struct by_game_uuid_wincase_asc;

using bet_uuid_history_index
    = shared_multi_index_container<bet_uuid_history_object,
                                   indexed_by<ordered_unique<tag<by_id>,
                                                             member<bet_uuid_history_object,
                                                                    bet_uuid_history_id_type,
                                                                    &bet_uuid_history_object::id>>,
                                              hashed_unique<tag<by_uuid>,
                                                            member<bet_uuid_history_object,
                                                                   uuid_type,
                                                                   &bet_uuid_history_object::uuid>>>>;

using pending_bet_index
    = shared_multi_index_container<pending_bet_object,
                                   indexed_by<ordered_unique<tag<by_id>,
                                                             member<pending_bet_object,
                                                                    pending_bet_id_type,
                                                                    &pending_bet_object::id>>,

                                              hashed_unique<tag<by_uuid>,
                                                            const_mem_fun<pending_bet_object,
                                                                          uuid_type,
                                                                          &pending_bet_object::get_uuid>>,

                                              ordered_unique<tag<by_game_uuid_wincase_asc>,
                                                             composite_key<pending_bet_object,
                                                                           member<pending_bet_object,
                                                                                uuid_type,
                                                                                &pending_bet_object::game_uuid>,
                                                                           const_mem_fun<pending_bet_object,
                                                                                wincase_type,
                                                                                &pending_bet_object::
                                                                                get_wincase>,
                                                                           const_mem_fun<pending_bet_object,
                                                                                fc::time_point_sec,
                                                                                &pending_bet_object::
                                                                                get_created>,
                                                                           member<pending_bet_object,
                                                                                pending_bet_id_type,
                                                                                &pending_bet_object::id>>,
                                                             composite_key_compare<std::less<uuid_type>,
                                                                                std::less<wincase_type>,
                                                                                std::less<time_point_sec>,
                                                                                std::less<pending_bet_id_type>>>,

                                              ordered_unique<tag<by_game_uuid_kind>,
                                                             composite_key<pending_bet_object,
                                                                           member<pending_bet_object,
                                                                                uuid_type,
                                                                                &pending_bet_object::game_uuid>,
                                                                           const_mem_fun<pending_bet_object,
                                                                                pending_bet_kind,
                                                                                &pending_bet_object::get_kind>,
                                                                           member<pending_bet_object,
                                                                                pending_bet_id_type,
                                                                                &pending_bet_object::id>>>,

                                              ordered_unique<tag<by_game_uuid_market>,
                                                             composite_key<pending_bet_object,
                                                                           member<pending_bet_object,
                                                                                uuid_type,
                                                                                &pending_bet_object::game_uuid>,
                                                                           member<pending_bet_object,
                                                                                market_type,
                                                                                &pending_bet_object::market>,
                                                                           member<pending_bet_object,
                                                                                pending_bet_id_type,
                                                                                &pending_bet_object::id>>>,

                                              ordered_unique<tag<by_game_uuid_created>,
                                                             composite_key<pending_bet_object,
                                                                           member<pending_bet_object,
                                                                                uuid_type,
                                                                                &pending_bet_object::game_uuid>,
                                                                           const_mem_fun<pending_bet_object,
                                                                                fc::time_point_sec,
                                                                                &pending_bet_object::
                                                                                get_created>,
                                                                           member<pending_bet_object,
                                                                                pending_bet_id_type,
                                                                                &pending_bet_object::id>>>>>;

struct by_bet1_uuid;
struct by_bet2_uuid;

using matched_bet_index
    = shared_multi_index_container<matched_bet_object,
                                   indexed_by<ordered_unique<tag<by_id>,
                                                             member<matched_bet_object,
                                                                    matched_bet_id_type,
                                                                    &matched_bet_object::id>>,

                                              ordered_unique<tag<by_bet1_uuid>,
                                                             composite_key<matched_bet_object,
                                                                           const_mem_fun<matched_bet_object,
                                                                                uuid_type,
                                                                                &matched_bet_object::
                                                                                get_bet1_uuid>,
                                                                           member<matched_bet_object,
                                                                                matched_bet_id_type,
                                                                                &matched_bet_object::id>>>,

                                              ordered_unique<tag<by_bet2_uuid>,
                                                             composite_key<matched_bet_object,
                                                                           const_mem_fun<matched_bet_object,
                                                                                uuid_type,
                                                                                &matched_bet_object::
                                                                                get_bet2_uuid>,
                                                                           member<matched_bet_object,
                                                                                matched_bet_id_type,
                                                                                &matched_bet_object::id>>>,

                                              ordered_unique<tag<by_game_uuid_market>,
                                                             composite_key<matched_bet_object,
                                                                           member<matched_bet_object,
                                                                                uuid_type,
                                                                                &matched_bet_object::game_uuid>,
                                                                           member<matched_bet_object,
                                                                                market_type,
                                                                                &matched_bet_object::market>,
                                                                           member<matched_bet_object,
                                                                                matched_bet_id_type,
                                                                                &matched_bet_object::id>>>,

                                              ordered_unique<tag<by_game_uuid_created>,
                                                             composite_key<matched_bet_object,
                                                                           member<matched_bet_object,
                                                                                uuid_type,
                                                                                &matched_bet_object::game_uuid>,
                                                                           member<matched_bet_object,
                                                                                fc::time_point_sec,
                                                                                &matched_bet_object::created>,
                                                                           member<matched_bet_object,
                                                                                matched_bet_id_type,
                                                                                &matched_bet_object::id>>>>>;
}
}

// clang-format off

FC_REFLECT(scorum::chain::bet_uuid_history_object,
           (id)
           (uuid)
           )

CHAINBASE_SET_INDEX_TYPE(scorum::chain::bet_uuid_history_object, scorum::chain::bet_uuid_history_index)

FC_REFLECT_ENUM(scorum::chain::pending_bet_kind,
                (live)
                (non_live))

FC_REFLECT(scorum::chain::bet_data,
           (uuid)
           (created)
           (better)
           (wincase)
           (stake)
           (odds)
           (kind))

FC_REFLECT(scorum::chain::pending_bet_object,
           (id)
           (game_uuid)
           (market)
           (data)
           )

CHAINBASE_SET_INDEX_TYPE(scorum::chain::pending_bet_object, scorum::chain::pending_bet_index)

FC_REFLECT(scorum::chain::matched_bet_object,
           (id)
           (game_uuid)
           (market)
           (created)
           (bet1_data)
           (bet2_data)
           )

CHAINBASE_SET_INDEX_TYPE(scorum::chain::matched_bet_object, scorum::chain::matched_bet_index)
// clang-format on


// __BEGIN_LICENSE__
//  Copyright (c) 2009-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NGT platform is licensed under the Apache License, Version 2.0 (the
//  "License"); you may not use this file except in compliance with the
//  License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

/// \file nurbs.cc
///

#ifdef _MSC_VER
#pragma warning(disable:4244)
#pragma warning(disable:4267)
#pragma warning(disable:4996)
#endif

#include <boost/program_options.hpp>
namespace po = boost::program_options;

#include <vw/vw.h>
using namespace vw;

#include "SurfaceNURBS.h"

using namespace vw;

int main( int argc, char *argv[] ) {

  std::string input_file_name, output_file_name;
  unsigned int num_iterations;

  po::options_description desc("Options");
  desc.add_options()
    ("help", "Display this help message")
    ("input-file", po::value<std::string>(&input_file_name), "Explicitly specify the input file")
    ("output-file,o", po::value<std::string>(&output_file_name)->default_value("output.png"), "Specify the output file")
    ("iterations,i", po::value<unsigned int>(&num_iterations), "The number of iterations to perform")
    ("holes-only,h", "Only fill areas with no pixels (zero alpha) with interpolated data.")
    ("normalize", "Normalize the output image")
    ("verbose", "Verbose output");
  po::positional_options_description p;
  p.add("input-file", 1);

  po::variables_map vm;
  po::store( po::command_line_parser( argc, argv ).options(desc).positional(p).run(), vm );
  po::notify( vm );

  if( vm.count("help") ) {
    std::cout << desc << std::endl;
    return 1;
  }

  if( vm.count("input-file") != 1 ) {
    std::cout << "Error: Must specify exactly one input file!" << std::endl;
    std::cout << desc << std::endl;
    return 1;
  }

  if( vm.count("verbose") ) {
    set_debug_level(VerboseDebugMessage);
  }

  try {
    ImageView<PixelGrayA<float> > image;
    read_image( image, input_file_name );

    ImageView<PixelGrayA<float> > output;
    output = MBASurfaceNURBS(image, num_iterations);

    if (vm.count("holes-only") ) {
      for (int j = 0; j < image.rows(); ++j) {
        for (int i = 0; i < image.cols(); ++i) {
          if (image(i,j).a() != 0) {
            output(i,j) = image(i,j);
          }
        }
      }
    }

    if( vm.count("normalize") ) {
      output = normalize(output);
    }

    write_image( output_file_name, output );
  }
  catch( Exception& e ) {
    std::cerr << "Error: " << e.what() << std::endl;
  }

  return 0;
}


#include "navigation/algorithm/timetable.hpp"
#include "navigation/route.hpp"
#include "search/stop_to_line.hpp"
#include "timetable/timetable.hpp"

#include "id/line.hpp"
#include "id/stop.hpp"

#include <algorithm>
#include <cstdint>
#include <iterator>
#include <limits>
#include <unordered_map>
#include <utility>
#include <vector>

#include <boost/assert.hpp>

namespace nepomuk
{
namespace navigation
{
namespace algorithm
{

TimeTable::TimeTable(timetable::TimeTable const &time_table, search::StopToLine const &stop_to_line)
    : RoutingAlgorithm(time_table.lines()), time_table(time_table), stop_to_line(stop_to_line)
{
}

boost::optional<Route> TimeTable::
operator()(date::Time const departure, StopID const origin, StopID const destination) const
{
    ReachedStateContainer earliest_arrival;
    StateContainer internal_state;

    add_origin(origin, departure, internal_state, earliest_arrival);

    date::Time upper_bound;
    upper_bound = upper_bound + std::numeric_limits<std::uint32_t>::max();
    StopID reached_destination = destination;
    auto const destination_station = time_table.station(destination);

    auto const check_and_improve_upper_bound = [&](StopID const stop) {
        auto const itr = earliest_arrival.find(stop);
        if (itr != earliest_arrival.end() && itr->second.arrival < upper_bound)
        {
            // std::cout << "Found new upper bound: " << stop << " " << itr->second.arrival
            //          << std::endl;
            upper_bound = itr->second.arrival;
            reached_destination = stop;
        }
    };
    // relax by lines, in order of hops
    auto const &destination_stops = time_table.stops(destination_station);
    std::size_t offset = 0;
    while (offset < internal_state.size())
    {
        // relax all hops
        std::sort(internal_state.begin() + offset,
                  internal_state.end(),
                  [](auto const &lhs, auto const &rhs) { return lhs.arrival < rhs.arrival; });

        std::for_each(
            destination_stops.begin(), destination_stops.end(), check_and_improve_upper_bound);

        auto const new_offset = internal_state.size();
        relax(
            upper_bound, offset, internal_state.size() - offset, internal_state, earliest_arrival);
        offset = new_offset;
    }

    std::for_each(
        destination_stops.begin(), destination_stops.end(), check_and_improve_upper_bound);

    if (!earliest_arrival.count(reached_destination))
        return boost::none;

    return make_route(extract_path(reached_destination, earliest_arrival));
}

boost::optional<Route> TimeTable::operator()(date::Time const departure,
                                             std::vector<ADLeg> const &origins,
                                             std::vector<ADLeg> const &destinations) const
{
    if (origins.empty() || destinations.empty())
        return boost::none;

    ReachedStateContainer earliest_arrival;
    StateContainer internal_state;

    for (auto const &leg : origins)
        add_origin(leg.stop, departure + leg.seconds, internal_state, earliest_arrival);

    std::vector<std::pair<StopID, std::uint32_t>> destination_stations;
    destination_stations.reserve(destinations.size());
    std::transform(destinations.begin(),
                   destinations.end(),
                   std::back_inserter(destination_stations),
                   [this](auto const &adleg) {
                       return std::make_pair(time_table.station(adleg.stop), adleg.seconds);
                   });

    std::sort(destination_stations.begin(), destination_stations.end());
    destination_stations.erase(
        std::unique(destination_stations.begin(),
                    destination_stations.end(),
                    [](auto const &lhs, auto const &rhs) { return lhs.first < rhs.first; }),
        destination_stations.end());

    date::Time upper_bound;
    upper_bound = upper_bound + std::numeric_limits<std::uint32_t>::max();
    StopID reached_destination = destinations.front().stop;
    // make into multiple stations
    auto const check_and_improve_upper_bound = [&](auto const &stop_and_time) {
        auto const itr = earliest_arrival.find(stop_and_time.first);
        if (itr != earliest_arrival.end() &&
            (itr->second.arrival + stop_and_time.second) < upper_bound)
        {
            reached_destination = stop_and_time.first;
        }
    };

    std::size_t offset = 0;
    while (offset < internal_state.size())
    {
        std::sort(internal_state.begin() + offset,
                  internal_state.end(),
                  [](auto const &lhs, auto const &rhs) { return lhs.arrival < rhs.arrival; });

        std::for_each(destination_stations.begin(),
                      destination_stations.end(),
                      check_and_improve_upper_bound);

        auto const new_offset = internal_state.size();
        relax(
            upper_bound, offset, internal_state.size() - offset, internal_state, earliest_arrival);
        offset = new_offset;
    }

    if (!earliest_arrival.count(reached_destination))
        return boost::none;

    return make_route(extract_path(reached_destination, earliest_arrival));
}

void TimeTable::add_origin(StopID const origin,
                           date::Time const departure,
                           StateContainer &internal_state,
                           ReachedStateContainer &reached) const
{
    for (auto start : time_table.stops(time_table.station(origin)))
    {
        if (!reached.count(start) || departure < (reached[start].arrival))
        {
            // std::cout << "[origin] " << start << " at " << departure << std::endl;
            internal_state.push_back({start, departure});
            reached[start] = {departure, start, departure, WALKING_TRANSFER};
        }
    }
}

void TimeTable::relax(date::Time const &upper_bound,
                      std::size_t offset,
                      std::size_t count,
                      StateContainer &internal_state,
                      ReachedStateContainer &earliest_arrival) const
{
    auto const add_if_improved = [&](StopID const stop,
                                     date::Time const time,
                                     StopID const parent,
                                     LineID const line,
                                     date::Time const departure) {
        if (!earliest_arrival.count(stop) || time < earliest_arrival[stop].arrival)
        {
            BOOST_ASSERT(parent != stop);
            earliest_arrival[stop] = {time, parent, departure, line};
            // std::cout << "\t\t[Reach] " << stop << " from " << parent << " at " << time << " via
            // "
            //          << line << std::endl;
            return true;
        }
        else
            return false;
    };

    auto const relax_line = [&](auto const &state, auto const line_id) {
        // std::cout << "-------" << std::endl;
        auto const trip_optional = time_table.line(line_id).get(state.stop_id, state.arrival);
        if (!trip_optional)
            return;

        auto const &trip = *trip_optional;
        auto time = trip.departure;

        auto duration_itr = trip.duration_range.begin();
        for (auto stop_itr = trip.stop_range.begin(); stop_itr != trip.stop_range.end();
             ++stop_itr, ++duration_itr)
        {
            auto const stop_id = *stop_itr;
            // std::cout << "\t\t" << stop_id << " " << time + *duration_itr << std::endl;
            if (add_if_improved(stop_id, time, state.stop_id, line_id, trip.departure))
            {
                auto transfers = time_table.transfers(stop_id);
                // add all transfers
                for (auto transfer : transfers)
                {
                    auto transfer_time = time + std::max<int>(transfer.duration, 60);
                    if (add_if_improved(
                            transfer.stop_id, transfer_time, stop_id, WALKING_TRANSFER, time) ||
                        (transfer.stop_id == stop_id))
                    {
                        // needs to be a transfer line instead of 0
                        internal_state.push_back({transfer.stop_id, time + transfer.duration});
                    }
                }
            }
            time = time + *duration_itr;
        }
    };

    // std::cout << "[Round]" << std::endl;
    while (count--)
    {
        auto const state = internal_state[offset++];

        // std::cout << "\tAt: " << state.stop_id << " " << state.arrival << std::endl;
        // stop when the earliest arrival is known
        if (upper_bound <= state.arrival)
            break; // sorted, all current states will be out of bound

        // get all lines at the given stop
        auto trip_range = stop_to_line(state.stop_id);
        for (auto line : trip_range)
            relax_line(state, line);
    }
}

std::vector<TimeTable::PathEntry>
TimeTable::extract_path(StopID current_stop, ReachedStateContainer const &earliest_arrival) const
{
    std::vector<PathEntry> path;
    auto entry = earliest_arrival.find(current_stop)->second;
    path.push_back({current_stop, entry.line_id, entry.arrival, entry.parent_departure});
    do
    {
        current_stop = entry.parent;
        entry = earliest_arrival.find(current_stop)->second;
        path.push_back({current_stop, entry.line_id, entry.arrival, entry.parent_departure});
    } while (entry.parent != current_stop);

    std::reverse(path.begin(), path.end());
    return path;
}

} // namespace algorithm
} // namespace navigation
} // namespace nepomuk


// ======================================================================
/*!
 * \file
 * \brief Implementation of class TextGen::WindChillFunctionAnalyzer
 */
// ======================================================================
/*!
 * \class TextGen::WindChillFunctionAnalyzer
 *
 * \brief Regular function analysis
 *
 */
// ======================================================================

#include "WindChillFunctionAnalyzer.h"
#include "AnalysisSources.h"
#include "CalculatorFactory.h"
#include "MaskSource.h"
#include "QueryDataIntegrator.h"
#include "Settings.h"
#include "TextGenError.h"
#include "WeatherArea.h"
#include "WeatherPeriod.h"
#include "WeatherResult.h"
#include "WeatherSource.h"
#include "WindChillQueryInfo.h"
#include <boost/shared_ptr.hpp>
#include <macgyver/StringConversion.h>
#include <newbase/NFmiEnumConverter.h>
#include <newbase/NFmiQueryData.h>
#include <sstream>
#include <string>

namespace
{
//! A static instance to avoid construction costs

NFmiEnumConverter converter;
}  // namespace

namespace TextGen
{
// ----------------------------------------------------------------------
/*!
 * \brief Constructor
 *
 * \param theAreaFunction The area function
 * \param theTimeFunction The time function
 * \param theSubTimeFunction The time function for subperiods
 */
// ----------------------------------------------------------------------

WindChillFunctionAnalyzer::WindChillFunctionAnalyzer(const WeatherFunction& theAreaFunction,
                                                     const WeatherFunction& theTimeFunction,
                                                     const WeatherFunction& theSubTimeFunction)
    : itsAreaFunction(theAreaFunction),
      itsTimeFunction(theTimeFunction),
      itsSubTimeFunction(theSubTimeFunction),
      itIsModulo(false),
      itsModulo(-1)
{
}

// ----------------------------------------------------------------------
/*!
 * \brief Set the modulo of the parameter values
 *
 * Throws for 0 or negative modulo
 *
 * \param theModulo The modulo value
 */
// ----------------------------------------------------------------------

void WindChillFunctionAnalyzer::modulo(int theModulo)
{
  if (theModulo <= 0)
    throw TextGenError("Trying to analyze data modulo " + Fmi::to_string(theModulo));
  itsModulo = theModulo;
  itIsModulo = true;
}

// ----------------------------------------------------------------------
/*!
 * \brief Analyze area and time functions
 *
 * Note that theTester argument is associated to both itsAreaCalculator
 * and itsTimeCalculator. Naturally the association is real only
 * for functions which require the tester. At the moment Percentage
 * is the only such function.
 *
 * It is assumed that the space and time functions are never both
 * Percentage, hence atmost one Acceptor is needed for Percentage
 * calculations.
 *
 * \param theSources Analysis sources
 * \param theDataType Analysis data type
 * \param theArea Analysis area
 * \param thePeriods Analysis periods
 * \param theAreaAcceptor The data acceptor in area integration
 * \param theTimeAcceptor The data acceptor in space integration
 * \param theTester The data tester for Percentage calculations
 * \param theDataName The name of the data file
 * \param theParameterName The name of the parameter
 * \return The analysis result
 */
// ----------------------------------------------------------------------

WeatherResult WindChillFunctionAnalyzer::analyze(const AnalysisSources& theSources,
                                                 const WeatherDataType& theDataType,
                                                 const WeatherArea& theArea,
                                                 const WeatherPeriodGenerator& thePeriods,
                                                 const Acceptor& theAreaAcceptor,
                                                 const Acceptor& theTimeAcceptor,
                                                 const Acceptor& theTester,
                                                 const std::string& theDataName,
                                                 const std::string& /* theParameterName */) const
{
  // Establish the data

  const std::string default_forecast = Settings::optional_string("textgen::default_forecast", "");
  const std::string datavar = theDataName + '_' + data_type_name(theDataType);
  const std::string dataname = Settings::optional_string(datavar, default_forecast);

  // Get the data into use

  boost::shared_ptr<WeatherSource> wsource = theSources.getWeatherSource();
  boost::shared_ptr<NFmiQueryData> qd = wsource->data(dataname);

  NFmiFastQueryInfo qi = NFmiFastQueryInfo(qd.get());
  WindChillQueryInfo wi(qi);

  // Try activating the parameter

  /*
  // WindChill parameter does not exists in querydata, but
  // it is composed of wind speed and temperature
  FmiParameterName param = FmiParameterName(converter.ToEnum(theParameterName));
  if(param == kFmiBadParameter)
  throw TextGenError("Parameter "+theParameterName+" is not defined in newbase");

  if(!qi.Param(param))
  throw TextGenError(theParameterName+" is not available in "+dataname);
  */

  boost::shared_ptr<Calculator> spacemod, timemod, subtimemod;

  if (!itIsModulo)
  {
    timemod.reset(CalculatorFactory::create(itsTimeFunction, theTester));
    subtimemod.reset(CalculatorFactory::create(itsSubTimeFunction, theTester));
  }
  else
  {
    timemod.reset(CalculatorFactory::create(itsTimeFunction, theTester, itsModulo));
    subtimemod.reset(CalculatorFactory::create(itsSubTimeFunction, theTester, itsModulo));
  }
  timemod->acceptor(theTimeAcceptor);
  subtimemod->acceptor(theTimeAcceptor);

  if (!theArea.isPoint())
  {
    MaskSource::mask_type mask;

    switch (theArea.type())
    {
      case WeatherArea::Full:
        mask = theSources.getMaskSource()->mask(theArea, dataname, *wsource);
        break;
      case WeatherArea::Land:
        mask = theSources.getLandMaskSource()->mask(theArea, dataname, *wsource);
        break;
      case WeatherArea::Coast:
        mask = theSources.getCoastMaskSource()->mask(theArea, dataname, *wsource);
        break;
      case WeatherArea::Inland:
        mask = theSources.getInlandMaskSource()->mask(theArea, dataname, *wsource);
        break;
      case WeatherArea::Northern:
        mask = theSources.getNorthernMaskSource()->mask(theArea, dataname, *wsource);
        break;
      case WeatherArea::Southern:
        mask = theSources.getSouthernMaskSource()->mask(theArea, dataname, *wsource);
        break;
      case WeatherArea::Eastern:
        mask = theSources.getEasternMaskSource()->mask(theArea, dataname, *wsource);
        break;
      case WeatherArea::Western:
        mask = theSources.getWesternMaskSource()->mask(theArea, dataname, *wsource);
        break;
    }

    // Result

    if (!itIsModulo)
      spacemod.reset(CalculatorFactory::create(itsAreaFunction, theTester));
    else
      spacemod.reset(CalculatorFactory::create(itsAreaFunction, theTester, itsModulo));
    spacemod->acceptor(theAreaAcceptor);

    float result =
        QueryDataIntegrator::Integrate(wi, thePeriods, *subtimemod, *timemod, *mask, *spacemod);
    if (result == kFloatMissing) return WeatherResult(kFloatMissing, 0);

    if (itsAreaFunction != Mean) return WeatherResult(result, 0);

    // Calculate standard deviation for the mean

    if (!itIsModulo)
      spacemod.reset(CalculatorFactory::create(StandardDeviation, theTester));
    else
      spacemod.reset(CalculatorFactory::create(StandardDeviation, theTester, itsModulo));
    spacemod->acceptor(theAreaAcceptor);

    float error =
        QueryDataIntegrator::Integrate(wi, thePeriods, *subtimemod, *timemod, *mask, *spacemod);

    // This would happen if the area covers one point only
    if (error == kFloatMissing) return WeatherResult(result, 0);

    return {result, error};
  }

  if (!(qi.Location(theArea.point())))
  {
    std::ostringstream msg;
    msg << "Could not set desired coordinate (" << theArea.point().X() << ',' << theArea.point().Y()
        << ')';
    if (theArea.isNamed()) msg << " named " << theArea.name();
    msg << " in " << dataname;
    throw TextGenError(msg.str());
  }

  float result = QueryDataIntegrator::Integrate(wi, thePeriods, *subtimemod, *timemod);

  return {result, 0};
}

}  // namespace TextGen

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