#ifndef ROUTING_BASE_HPP
#define ROUTING_BASE_HPP

#include "extractor/guidance/turn_instruction.hpp"
#include "engine/internal_route_result.hpp"
#include "engine/search_engine_data.hpp"
#include "util/coordinate_calculation.hpp"
#include "util/typedefs.hpp"

#include <boost/assert.hpp>

#include <cstddef>
#include <cstdint>

#include <algorithm>
#include <iterator>
#include <numeric>
#include <stack>
#include <utility>
#include <vector>

namespace osrm
{
namespace engine
{

namespace routing_algorithms
{

template <class DataFacadeT, class Derived> class BasicRoutingInterface
{
  private:
    using EdgeData = typename DataFacadeT::EdgeData;

  protected:
    DataFacadeT *facade;

  public:
    explicit BasicRoutingInterface(DataFacadeT *facade) : facade(facade) {}
    ~BasicRoutingInterface() {}

    BasicRoutingInterface(const BasicRoutingInterface &) = delete;
    BasicRoutingInterface &operator=(const BasicRoutingInterface &) = delete;

    /*
    min_edge_offset is needed in case we use multiple
    nodes as start/target nodes with different (even negative) offsets.
    In that case the termination criterion is not correct
    anymore.

    Example:
    forward heap: a(-100), b(0),
    reverse heap: c(0), d(100)

    a --- d
      \ /
      / \
    b --- c

    This is equivalent to running a bi-directional Dijkstra on the following graph:

        a --- d
       /  \ /  \
      y    x    z
       \  / \  /
        b --- c

    The graph is constructed by inserting nodes y and z that are connected to the initial nodes
    using edges (y, a) with weight -100, (y, b) with weight 0 and,
    (d, z) with weight 100, (c, z) with weight 0 corresponding.
    Since we are dealing with a graph that contains _negative_ edges,
    we need to add an offset to the termination criterion.
    */
    void RoutingStep(SearchEngineData::QueryHeap &forward_heap,
                     SearchEngineData::QueryHeap &reverse_heap,
                     NodeID &middle_node_id,
                     std::int32_t &upper_bound,
                     std::int32_t min_edge_offset,
                     const bool forward_direction,
                     const bool stalling,
                     const bool force_loop_forward,
                     const bool force_loop_reverse) const
    {
        const NodeID node = forward_heap.DeleteMin();
        const std::int32_t distance = forward_heap.GetKey(node);

        if (reverse_heap.WasInserted(node))
        {
            const std::int32_t new_distance = reverse_heap.GetKey(node) + distance;
            if (new_distance < upper_bound)
            {
                // if loops are forced, they are so at the source
                if (new_distance >= 0 &&
                    (!force_loop_forward || forward_heap.GetData(node).parent != node) &&
                    (!force_loop_reverse || reverse_heap.GetData(node).parent != node))
                {
                    middle_node_id = node;
                    upper_bound = new_distance;
                }
                else
                {
                    // check whether there is a loop present at the node
                    for (const auto edge : facade->GetAdjacentEdgeRange(node))
                    {
                        const EdgeData &data = facade->GetEdgeData(edge);
                        bool forward_directionFlag =
                            (forward_direction ? data.forward : data.backward);
                        if (forward_directionFlag)
                        {
                            const NodeID to = facade->GetTarget(edge);
                            if (to == node)
                            {
                                const EdgeWeight edge_weight = data.distance;
                                const std::int32_t loop_distance = new_distance + edge_weight;
                                if (loop_distance >= 0 && loop_distance < upper_bound)
                                {
                                    middle_node_id = node;
                                    upper_bound = loop_distance;
                                }
                            }
                        }
                    }
                }
            }
        }

        // make sure we don't terminate too early if we initialize the distance
        // for the nodes in the forward heap with the forward/reverse offset
        BOOST_ASSERT(min_edge_offset <= 0);
        if (distance + min_edge_offset > upper_bound)
        {
            forward_heap.DeleteAll();
            return;
        }

        // Stalling
        if (stalling)
        {
            for (const auto edge : facade->GetAdjacentEdgeRange(node))
            {
                const EdgeData &data = facade->GetEdgeData(edge);
                const bool reverse_flag = ((!forward_direction) ? data.forward : data.backward);
                if (reverse_flag)
                {
                    const NodeID to = facade->GetTarget(edge);
                    const EdgeWeight edge_weight = data.distance;

                    BOOST_ASSERT_MSG(edge_weight > 0, "edge_weight invalid");

                    if (forward_heap.WasInserted(to))
                    {
                        if (forward_heap.GetKey(to) + edge_weight < distance)
                        {
                            return;
                        }
                    }
                }
            }
        }

        for (const auto edge : facade->GetAdjacentEdgeRange(node))
        {
            const EdgeData &data = facade->GetEdgeData(edge);
            bool forward_directionFlag = (forward_direction ? data.forward : data.backward);
            if (forward_directionFlag)
            {

                const NodeID to = facade->GetTarget(edge);
                const EdgeWeight edge_weight = data.distance;

                BOOST_ASSERT_MSG(edge_weight > 0, "edge_weight invalid");
                const int to_distance = distance + edge_weight;

                // New Node discovered -> Add to Heap + Node Info Storage
                if (!forward_heap.WasInserted(to))
                {
                    forward_heap.Insert(to, to_distance, node);
                }
                // Found a shorter Path -> Update distance
                else if (to_distance < forward_heap.GetKey(to))
                {
                    // new parent
                    forward_heap.GetData(to).parent = node;
                    forward_heap.DecreaseKey(to, to_distance);
                }
            }
        }
    }

    inline EdgeWeight GetLoopWeight(NodeID node) const
    {
        EdgeWeight loop_weight = INVALID_EDGE_WEIGHT;
        for (auto edge : facade->GetAdjacentEdgeRange(node))
        {
            const auto &data = facade->GetEdgeData(edge);
            if (data.forward)
            {
                const NodeID to = facade->GetTarget(edge);
                if (to == node)
                {
                    loop_weight = std::min(loop_weight, data.distance);
                }
            }
        }
        return loop_weight;
    }

    template <typename RandomIter>
    void UnpackPath(RandomIter packed_path_begin,
                    RandomIter packed_path_end,
                    const PhantomNodes &phantom_node_pair,
                    std::vector<PathData> &unpacked_path) const
    {
        const bool start_traversed_in_reverse =
            (*packed_path_begin != phantom_node_pair.source_phantom.forward_segment_id.id);
        const bool target_traversed_in_reverse =
            (*std::prev(packed_path_end) != phantom_node_pair.target_phantom.forward_segment_id.id);

        BOOST_ASSERT(std::distance(packed_path_begin, packed_path_end) > 0);
        std::stack<std::pair<NodeID, NodeID>> recursion_stack;

        // We have to push the path in reverse order onto the stack because it's LIFO.
        for (auto current = std::prev(packed_path_end); current != packed_path_begin;
             current = std::prev(current))
        {
            recursion_stack.emplace(*std::prev(current), *current);
        }

        BOOST_ASSERT(*packed_path_begin == phantom_node_pair.source_phantom.forward_segment_id.id ||
                     *packed_path_begin == phantom_node_pair.source_phantom.reverse_segment_id.id);
        BOOST_ASSERT(
            *std::prev(packed_path_end) == phantom_node_pair.target_phantom.forward_segment_id.id ||
            *std::prev(packed_path_end) == phantom_node_pair.target_phantom.reverse_segment_id.id);

        std::pair<NodeID, NodeID> edge;
        while (!recursion_stack.empty())
        {
            // edge.first         edge.second
            //     *------------------>*
            //            edge_id
            edge = recursion_stack.top();
            recursion_stack.pop();

            // Contraction might introduce double edges by inserting shortcuts
            // this searching for the smallest upwards edge found by the forward search
            EdgeID smaller_edge_id = SPECIAL_EDGEID;
            EdgeWeight edge_weight = std::numeric_limits<EdgeWeight>::max();
            for (const auto edge_id : facade->GetAdjacentEdgeRange(edge.first))
            {
                const EdgeWeight weight = facade->GetEdgeData(edge_id).distance;
                if ((facade->GetTarget(edge_id) == edge.second) && (weight < edge_weight) &&
                    facade->GetEdgeData(edge_id).forward)
                {
                    smaller_edge_id = edge_id;
                    edge_weight = weight;
                }
            }

            // edge.first         edge.second
            //     *<------------------*
            //            edge_id
            // if we don't find a forward edge, this edge must have been an downwards edge
            // found by the reverse search.
            if (SPECIAL_EDGEID == smaller_edge_id)
            {
                for (const auto edge_id : facade->GetAdjacentEdgeRange(edge.second))
                {
                    const EdgeWeight weight = facade->GetEdgeData(edge_id).distance;
                    if ((facade->GetTarget(edge_id) == edge.first) && (weight < edge_weight) &&
                        facade->GetEdgeData(edge_id).backward)
                    {
                        smaller_edge_id = edge_id;
                        edge_weight = weight;
                    }
                }
            }
            BOOST_ASSERT_MSG(edge_weight != INVALID_EDGE_WEIGHT, "edge id invalid");

            const EdgeData &ed = facade->GetEdgeData(smaller_edge_id);
            if (ed.shortcut)
            { // unpack
                const NodeID middle_node_id = ed.id;
                // again, we need to this in reversed order
                recursion_stack.emplace(middle_node_id, edge.second);
                recursion_stack.emplace(edge.first, middle_node_id);
            }
            else
            {
                BOOST_ASSERT_MSG(!ed.shortcut, "original edge flagged as shortcut");
                unsigned name_index = facade->GetNameIndexFromEdgeID(ed.id);
                const auto turn_instruction = facade->GetTurnInstructionForEdgeID(ed.id);
                const extractor::TravelMode travel_mode =
                    (unpacked_path.empty() && start_traversed_in_reverse)
                        ? phantom_node_pair.source_phantom.backward_travel_mode
                        : facade->GetTravelModeForEdgeID(ed.id);

                std::vector<NodeID> id_vector;
                facade->GetUncompressedGeometry(facade->GetGeometryIndexForEdgeID(ed.id),
                                                id_vector);
                BOOST_ASSERT(id_vector.size() > 0);

                std::vector<EdgeWeight> weight_vector;
                facade->GetUncompressedWeights(facade->GetGeometryIndexForEdgeID(ed.id),
                                               weight_vector);
                BOOST_ASSERT(weight_vector.size() > 0);

                auto total_weight = std::accumulate(weight_vector.begin(), weight_vector.end(), 0);

                BOOST_ASSERT(weight_vector.size() == id_vector.size());
                const bool is_first_segment = unpacked_path.empty();

                const std::size_t start_index =
                    (is_first_segment
                         ? ((start_traversed_in_reverse)
                                ? id_vector.size() -
                                      phantom_node_pair.source_phantom.fwd_segment_position - 1
                                : phantom_node_pair.source_phantom.fwd_segment_position)
                         : 0);
                const std::size_t end_index = id_vector.size();

                BOOST_ASSERT(start_index >= 0);
                BOOST_ASSERT(start_index < end_index);
                for (std::size_t i = start_index; i < end_index; ++i)
                {
                    unpacked_path.push_back(
                        PathData{id_vector[i],
                                 name_index,
                                 weight_vector[i],
                                 extractor::guidance::TurnInstruction::NO_TURN(),
                                 {{0, INVALID_LANEID}, INVALID_LANE_DESCRIPTIONID},
                                 travel_mode,
                                 INVALID_ENTRY_CLASSID});
                }
                BOOST_ASSERT(unpacked_path.size() > 0);
                if (facade->hasLaneData(ed.id))
                    unpacked_path.back().lane_data = facade->GetLaneData(ed.id);

                unpacked_path.back().entry_classid = facade->GetEntryClassID(ed.id);
                unpacked_path.back().turn_instruction = turn_instruction;
                unpacked_path.back().duration_until_turn += (ed.distance - total_weight);
            }
        }
        std::size_t start_index = 0, end_index = 0;
        std::vector<unsigned> id_vector;
        std::vector<EdgeWeight> weight_vector;
        const bool is_local_path = (phantom_node_pair.source_phantom.forward_packed_geometry_id ==
                                    phantom_node_pair.target_phantom.forward_packed_geometry_id) &&
                                   unpacked_path.empty();

        if (target_traversed_in_reverse)
        {
            facade->GetUncompressedGeometry(
                phantom_node_pair.target_phantom.reverse_packed_geometry_id, id_vector);

            facade->GetUncompressedWeights(
                phantom_node_pair.target_phantom.reverse_packed_geometry_id, weight_vector);

            if (is_local_path)
            {
                start_index =
                    id_vector.size() - phantom_node_pair.source_phantom.fwd_segment_position - 1;
            }
            end_index =
                id_vector.size() - phantom_node_pair.target_phantom.fwd_segment_position - 1;
        }
        else
        {
            if (is_local_path)
            {
                start_index = phantom_node_pair.source_phantom.fwd_segment_position;
            }
            end_index = phantom_node_pair.target_phantom.fwd_segment_position;
            facade->GetUncompressedGeometry(
                phantom_node_pair.target_phantom.forward_packed_geometry_id, id_vector);

            facade->GetUncompressedWeights(
                phantom_node_pair.target_phantom.forward_packed_geometry_id, weight_vector);
        }

        // Given the following compressed geometry:
        // U---v---w---x---y---Z
        //    s           t
        // s: fwd_segment 0
        // t: fwd_segment 3
        // -> (U, v), (v, w), (w, x)
        // note that (x, t) is _not_ included but needs to be added later.
        for (std::size_t i = start_index; i != end_index; (start_index < end_index ? ++i : --i))
        {
            BOOST_ASSERT(i < id_vector.size());
            BOOST_ASSERT(phantom_node_pair.target_phantom.forward_travel_mode > 0);
            unpacked_path.push_back(PathData{
                id_vector[i],
                phantom_node_pair.target_phantom.name_id,
                weight_vector[i],
                extractor::guidance::TurnInstruction::NO_TURN(),
                {{0, INVALID_LANEID}, INVALID_LANE_DESCRIPTIONID},
                target_traversed_in_reverse ? phantom_node_pair.target_phantom.backward_travel_mode
                                            : phantom_node_pair.target_phantom.forward_travel_mode,
                INVALID_ENTRY_CLASSID});
        }

        if (unpacked_path.size() > 0)
        {
            const auto source_weight = start_traversed_in_reverse
                                           ? phantom_node_pair.source_phantom.reverse_weight
                                           : phantom_node_pair.source_phantom.forward_weight;
            // The above code will create segments for (v, w), (w,x), (x, y) and (y, Z).
            // However the first segment duration needs to be adjusted to the fact that the source
            // phantom is in the middle of the segment. We do this by subtracting v--s from the
            // duration.

            // Since it's possible duration_until_turn can be less than source_weight here if
            // a negative enough turn penalty is used to modify this edge weight during
            // osrm-contract, we clamp to 0 here so as not to return a negative duration
            // for this segment.

            // TODO this creates a scenario where it's possible the duration from a phantom
            // node to the first turn would be the same as from end to end of a segment,
            // which is obviously incorrect and not ideal...
            unpacked_path.front().duration_until_turn =
                std::max(unpacked_path.front().duration_until_turn - source_weight, 0);
        }

        // there is no equivalent to a node-based node in an edge-expanded graph.
        // two equivalent routes may start (or end) at different node-based edges
        // as they are added with the offset how much "distance" on the edge
        // has already been traversed. Depending on offset one needs to remove
        // the last node.
        if (unpacked_path.size() > 1)
        {
            const std::size_t last_index = unpacked_path.size() - 1;
            const std::size_t second_to_last_index = last_index - 1;

            if (unpacked_path[last_index].turn_via_node ==
                unpacked_path[second_to_last_index].turn_via_node)
            {
                unpacked_path.pop_back();
            }
            BOOST_ASSERT(!unpacked_path.empty());
        }
    }

    void UnpackEdge(const NodeID s, const NodeID t, std::vector<NodeID> &unpacked_path) const
    {
        std::stack<std::pair<NodeID, NodeID>> recursion_stack;
        recursion_stack.emplace(s, t);

        std::pair<NodeID, NodeID> edge;
        while (!recursion_stack.empty())
        {
            edge = recursion_stack.top();
            recursion_stack.pop();

            EdgeID smaller_edge_id = SPECIAL_EDGEID;
            EdgeWeight edge_weight = std::numeric_limits<EdgeWeight>::max();
            for (const auto edge_id : facade->GetAdjacentEdgeRange(edge.first))
            {
                const EdgeWeight weight = facade->GetEdgeData(edge_id).distance;
                if ((facade->GetTarget(edge_id) == edge.second) && (weight < edge_weight) &&
                    facade->GetEdgeData(edge_id).forward)
                {
                    smaller_edge_id = edge_id;
                    edge_weight = weight;
                }
            }

            if (SPECIAL_EDGEID == smaller_edge_id)
            {
                for (const auto edge_id : facade->GetAdjacentEdgeRange(edge.second))
                {
                    const EdgeWeight weight = facade->GetEdgeData(edge_id).distance;
                    if ((facade->GetTarget(edge_id) == edge.first) && (weight < edge_weight) &&
                        facade->GetEdgeData(edge_id).backward)
                    {
                        smaller_edge_id = edge_id;
                        edge_weight = weight;
                    }
                }
            }
            BOOST_ASSERT_MSG(edge_weight != std::numeric_limits<EdgeWeight>::max(),
                             "edge weight invalid");

            const EdgeData &ed = facade->GetEdgeData(smaller_edge_id);
            if (ed.shortcut)
            { // unpack
                const NodeID middle_node_id = ed.id;
                // again, we need to this in reversed order
                recursion_stack.emplace(middle_node_id, edge.second);
                recursion_stack.emplace(edge.first, middle_node_id);
            }
            else
            {
                BOOST_ASSERT_MSG(!ed.shortcut, "edge must be shortcut");
                unpacked_path.emplace_back(edge.first);
            }
        }
        unpacked_path.emplace_back(t);
    }

    void RetrievePackedPathFromHeap(const SearchEngineData::QueryHeap &forward_heap,
                                    const SearchEngineData::QueryHeap &reverse_heap,
                                    const NodeID middle_node_id,
                                    std::vector<NodeID> &packed_path) const
    {
        RetrievePackedPathFromSingleHeap(forward_heap, middle_node_id, packed_path);
        std::reverse(packed_path.begin(), packed_path.end());
        packed_path.emplace_back(middle_node_id);
        RetrievePackedPathFromSingleHeap(reverse_heap, middle_node_id, packed_path);
    }

    void RetrievePackedPathFromSingleHeap(const SearchEngineData::QueryHeap &search_heap,
                                          const NodeID middle_node_id,
                                          std::vector<NodeID> &packed_path) const
    {
        NodeID current_node_id = middle_node_id;
        while (current_node_id != search_heap.GetData(current_node_id).parent)
        {
            current_node_id = search_heap.GetData(current_node_id).parent;
            packed_path.emplace_back(current_node_id);
        }
    }

    // assumes that heaps are already setup correctly.
    // ATTENTION: This only works if no additional offset is supplied next to the Phantom Node
    // Offsets.
    // In case additional offsets are supplied, you might have to force a loop first.
    // A forced loop might be necessary, if source and target are on the same segment.
    // If this is the case and the offsets of the respective direction are larger for the source
    // than the target
    // then a force loop is required (e.g. source_phantom.forward_segment_id ==
    // target_phantom.forward_segment_id
    // && source_phantom.GetForwardWeightPlusOffset() > target_phantom.GetForwardWeightPlusOffset())
    // requires
    // a force loop, if the heaps have been initialized with positive offsets.
    void Search(SearchEngineData::QueryHeap &forward_heap,
                SearchEngineData::QueryHeap &reverse_heap,
                std::int32_t &distance,
                std::vector<NodeID> &packed_leg,
                const bool force_loop_forward,
                const bool force_loop_reverse,
                const int duration_upper_bound = INVALID_EDGE_WEIGHT) const
    {
        NodeID middle = SPECIAL_NODEID;
        distance = duration_upper_bound;

        // get offset to account for offsets on phantom nodes on compressed edges
        const auto min_edge_offset = std::min(0, forward_heap.MinKey());
        BOOST_ASSERT(min_edge_offset <= 0);
        // we only every insert negative offsets for nodes in the forward heap
        BOOST_ASSERT(reverse_heap.MinKey() >= 0);

        // run two-Target Dijkstra routing step.
        const constexpr bool STALLING_ENABLED = true;
        while (0 < (forward_heap.Size() + reverse_heap.Size()))
        {
            if (!forward_heap.Empty())
            {
                RoutingStep(forward_heap,
                            reverse_heap,
                            middle,
                            distance,
                            min_edge_offset,
                            true,
                            STALLING_ENABLED,
                            force_loop_forward,
                            force_loop_reverse);
            }
            if (!reverse_heap.Empty())
            {
                RoutingStep(reverse_heap,
                            forward_heap,
                            middle,
                            distance,
                            min_edge_offset,
                            false,
                            STALLING_ENABLED,
                            force_loop_reverse,
                            force_loop_forward);
            }
        }

        // No path found for both target nodes?
        if (duration_upper_bound <= distance || SPECIAL_NODEID == middle)
        {
            distance = INVALID_EDGE_WEIGHT;
            return;
        }

        // Was a paths over one of the forward/reverse nodes not found?
        BOOST_ASSERT_MSG((SPECIAL_NODEID != middle && INVALID_EDGE_WEIGHT != distance),
                         "no path found");

        // make sure to correctly unpack loops
        if (distance != forward_heap.GetKey(middle) + reverse_heap.GetKey(middle))
        {
            // self loop makes up the full path
            packed_leg.push_back(middle);
            packed_leg.push_back(middle);
        }
        else
        {
            RetrievePackedPathFromHeap(forward_heap, reverse_heap, middle, packed_leg);
        }
    }

    // assumes that heaps are already setup correctly.
    // A forced loop might be necessary, if source and target are on the same segment.
    // If this is the case and the offsets of the respective direction are larger for the source
    // than the target
    // then a force loop is required (e.g. source_phantom.forward_segment_id ==
    // target_phantom.forward_segment_id
    // && source_phantom.GetForwardWeightPlusOffset() > target_phantom.GetForwardWeightPlusOffset())
    // requires
    // a force loop, if the heaps have been initialized with positive offsets.
    void SearchWithCore(SearchEngineData::QueryHeap &forward_heap,
                        SearchEngineData::QueryHeap &reverse_heap,
                        SearchEngineData::QueryHeap &forward_core_heap,
                        SearchEngineData::QueryHeap &reverse_core_heap,
                        int &distance,
                        std::vector<NodeID> &packed_leg,
                        const bool force_loop_forward,
                        const bool force_loop_reverse,
                        int duration_upper_bound = INVALID_EDGE_WEIGHT) const
    {
        NodeID middle = SPECIAL_NODEID;
        distance = duration_upper_bound;

        std::vector<std::pair<NodeID, EdgeWeight>> forward_entry_points;
        std::vector<std::pair<NodeID, EdgeWeight>> reverse_entry_points;

        // get offset to account for offsets on phantom nodes on compressed edges
        const auto min_edge_offset = std::min(0, forward_heap.MinKey());
        // we only every insert negative offsets for nodes in the forward heap
        BOOST_ASSERT(reverse_heap.MinKey() >= 0);

        const constexpr bool STALLING_ENABLED = true;
        // run two-Target Dijkstra routing step.
        while (0 < (forward_heap.Size() + reverse_heap.Size()))
        {
            if (!forward_heap.Empty())
            {
                if (facade->IsCoreNode(forward_heap.Min()))
                {
                    const NodeID node = forward_heap.DeleteMin();
                    const int key = forward_heap.GetKey(node);
                    forward_entry_points.emplace_back(node, key);
                }
                else
                {
                    RoutingStep(forward_heap,
                                reverse_heap,
                                middle,
                                distance,
                                min_edge_offset,
                                true,
                                STALLING_ENABLED,
                                force_loop_forward,
                                force_loop_reverse);
                }
            }
            if (!reverse_heap.Empty())
            {
                if (facade->IsCoreNode(reverse_heap.Min()))
                {
                    const NodeID node = reverse_heap.DeleteMin();
                    const int key = reverse_heap.GetKey(node);
                    reverse_entry_points.emplace_back(node, key);
                }
                else
                {
                    RoutingStep(reverse_heap,
                                forward_heap,
                                middle,
                                distance,
                                min_edge_offset,
                                false,
                                STALLING_ENABLED,
                                force_loop_reverse,
                                force_loop_forward);
                }
            }
        }
        // TODO check if unordered_set might be faster
        // sort by id and increasing by distance
        auto entry_point_comparator = [](const std::pair<NodeID, EdgeWeight> &lhs,
                                         const std::pair<NodeID, EdgeWeight> &rhs) {
            return lhs.first < rhs.first || (lhs.first == rhs.first && lhs.second < rhs.second);
        };
        std::sort(forward_entry_points.begin(), forward_entry_points.end(), entry_point_comparator);
        std::sort(reverse_entry_points.begin(), reverse_entry_points.end(), entry_point_comparator);

        NodeID last_id = SPECIAL_NODEID;
        forward_core_heap.Clear();
        reverse_core_heap.Clear();
        for (const auto &p : forward_entry_points)
        {
            if (p.first == last_id)
            {
                continue;
            }
            forward_core_heap.Insert(p.first, p.second, p.first);
            last_id = p.first;
        }
        last_id = SPECIAL_NODEID;
        for (const auto &p : reverse_entry_points)
        {
            if (p.first == last_id)
            {
                continue;
            }
            reverse_core_heap.Insert(p.first, p.second, p.first);
            last_id = p.first;
        }

        // get offset to account for offsets on phantom nodes on compressed edges
        int min_core_edge_offset = 0;
        if (forward_core_heap.Size() > 0)
        {
            min_core_edge_offset = std::min(min_core_edge_offset, forward_core_heap.MinKey());
        }
        if (reverse_core_heap.Size() > 0 && reverse_core_heap.MinKey() < 0)
        {
            min_core_edge_offset = std::min(min_core_edge_offset, reverse_core_heap.MinKey());
        }
        BOOST_ASSERT(min_core_edge_offset <= 0);

        // run two-target Dijkstra routing step on core with termination criterion
        const constexpr bool STALLING_DISABLED = false;
        while (0 < forward_core_heap.Size() && 0 < reverse_core_heap.Size() &&
               distance > (forward_core_heap.MinKey() + reverse_core_heap.MinKey()))
        {
            RoutingStep(forward_core_heap,
                        reverse_core_heap,
                        middle,
                        distance,
                        min_core_edge_offset,
                        true,
                        STALLING_DISABLED,
                        force_loop_forward,
                        force_loop_reverse);

            RoutingStep(reverse_core_heap,
                        forward_core_heap,
                        middle,
                        distance,
                        min_core_edge_offset,
                        false,
                        STALLING_DISABLED,
                        force_loop_reverse,
                        force_loop_forward);
        }

        // No path found for both target nodes?
        if (duration_upper_bound <= distance || SPECIAL_NODEID == middle)
        {
            distance = INVALID_EDGE_WEIGHT;
            return;
        }

        // Was a paths over one of the forward/reverse nodes not found?
        BOOST_ASSERT_MSG((SPECIAL_NODEID != middle && INVALID_EDGE_WEIGHT != distance),
                         "no path found");

        // we need to unpack sub path from core heaps
        if (facade->IsCoreNode(middle))
        {
            if (distance != forward_core_heap.GetKey(middle) + reverse_core_heap.GetKey(middle))
            {
                // self loop
                BOOST_ASSERT(forward_core_heap.GetData(middle).parent == middle &&
                             reverse_core_heap.GetData(middle).parent == middle);
                packed_leg.push_back(middle);
                packed_leg.push_back(middle);
            }
            else
            {
                std::vector<NodeID> packed_core_leg;
                RetrievePackedPathFromHeap(
                    forward_core_heap, reverse_core_heap, middle, packed_core_leg);
                BOOST_ASSERT(packed_core_leg.size() > 0);
                RetrievePackedPathFromSingleHeap(forward_heap, packed_core_leg.front(), packed_leg);
                std::reverse(packed_leg.begin(), packed_leg.end());
                packed_leg.insert(packed_leg.end(), packed_core_leg.begin(), packed_core_leg.end());
                RetrievePackedPathFromSingleHeap(reverse_heap, packed_core_leg.back(), packed_leg);
            }
        }
        else
        {
            if (distance != forward_heap.GetKey(middle) + reverse_heap.GetKey(middle))
            {
                // self loop
                BOOST_ASSERT(forward_heap.GetData(middle).parent == middle &&
                             reverse_heap.GetData(middle).parent == middle);
                packed_leg.push_back(middle);
                packed_leg.push_back(middle);
            }
            else
            {
                RetrievePackedPathFromHeap(forward_heap, reverse_heap, middle, packed_leg);
            }
        }
    }

    bool NeedsLoopForward(const PhantomNode &source_phantom,
                          const PhantomNode &target_phantom) const
    {
        return source_phantom.forward_segment_id.enabled &&
               target_phantom.forward_segment_id.enabled &&
               source_phantom.forward_segment_id.id == target_phantom.forward_segment_id.id &&
               source_phantom.GetForwardWeightPlusOffset() >
                   target_phantom.GetForwardWeightPlusOffset();
    }

    bool NeedsLoopBackwards(const PhantomNode &source_phantom,
                            const PhantomNode &target_phantom) const
    {
        return source_phantom.reverse_segment_id.enabled &&
               target_phantom.reverse_segment_id.enabled &&
               source_phantom.reverse_segment_id.id == target_phantom.reverse_segment_id.id &&
               source_phantom.GetReverseWeightPlusOffset() >
                   target_phantom.GetReverseWeightPlusOffset();
    }

    double GetPathDistance(const std::vector<NodeID> &packed_path,
                           const PhantomNode &source_phantom,
                           const PhantomNode &target_phantom) const
    {
        std::vector<PathData> unpacked_path;
        PhantomNodes nodes;
        nodes.source_phantom = source_phantom;
        nodes.target_phantom = target_phantom;
        UnpackPath(packed_path.begin(), packed_path.end(), nodes, unpacked_path);

        using util::coordinate_calculation::detail::DEGREE_TO_RAD;
        using util::coordinate_calculation::detail::EARTH_RADIUS;

        double distance = 0;
        double prev_lat =
            static_cast<double>(toFloating(source_phantom.location.lat)) * DEGREE_TO_RAD;
        double prev_lon =
            static_cast<double>(toFloating(source_phantom.location.lon)) * DEGREE_TO_RAD;
        double prev_cos = std::cos(prev_lat);
        for (const auto &p : unpacked_path)
        {
            const auto current_coordinate = facade->GetCoordinateOfNode(p.turn_via_node);

            const double current_lat =
                static_cast<double>(toFloating(current_coordinate.lat)) * DEGREE_TO_RAD;
            const double current_lon =
                static_cast<double>(toFloating(current_coordinate.lon)) * DEGREE_TO_RAD;
            const double current_cos = std::cos(current_lat);

            const double sin_dlon = std::sin((prev_lon - current_lon) / 2.0);
            const double sin_dlat = std::sin((prev_lat - current_lat) / 2.0);

            const double aharv = sin_dlat * sin_dlat + prev_cos * current_cos * sin_dlon * sin_dlon;
            const double charv = 2. * std::atan2(std::sqrt(aharv), std::sqrt(1.0 - aharv));
            distance += EARTH_RADIUS * charv;

            prev_lat = current_lat;
            prev_lon = current_lon;
            prev_cos = current_cos;
        }

        const double current_lat =
            static_cast<double>(toFloating(target_phantom.location.lat)) * DEGREE_TO_RAD;
        const double current_lon =
            static_cast<double>(toFloating(target_phantom.location.lon)) * DEGREE_TO_RAD;
        const double current_cos = std::cos(current_lat);

        const double sin_dlon = std::sin((prev_lon - current_lon) / 2.0);
        const double sin_dlat = std::sin((prev_lat - current_lat) / 2.0);

        const double aharv = sin_dlat * sin_dlat + prev_cos * current_cos * sin_dlon * sin_dlon;
        const double charv = 2. * std::atan2(std::sqrt(aharv), std::sqrt(1.0 - aharv));
        distance += EARTH_RADIUS * charv;

        return distance;
    }

    // Requires the heaps for be empty
    // If heaps should be adjusted to be initialized outside of this function,
    // the addition of force_loop parameters might be required
    double GetNetworkDistanceWithCore(SearchEngineData::QueryHeap &forward_heap,
                                      SearchEngineData::QueryHeap &reverse_heap,
                                      SearchEngineData::QueryHeap &forward_core_heap,
                                      SearchEngineData::QueryHeap &reverse_core_heap,
                                      const PhantomNode &source_phantom,
                                      const PhantomNode &target_phantom,
                                      int duration_upper_bound = INVALID_EDGE_WEIGHT) const
    {
        BOOST_ASSERT(forward_heap.Empty());
        BOOST_ASSERT(reverse_heap.Empty());

        if (source_phantom.forward_segment_id.enabled)
        {
            forward_heap.Insert(source_phantom.forward_segment_id.id,
                                -source_phantom.GetForwardWeightPlusOffset(),
                                source_phantom.forward_segment_id.id);
        }
        if (source_phantom.reverse_segment_id.enabled)
        {
            forward_heap.Insert(source_phantom.reverse_segment_id.id,
                                -source_phantom.GetReverseWeightPlusOffset(),
                                source_phantom.reverse_segment_id.id);
        }

        if (target_phantom.forward_segment_id.enabled)
        {
            reverse_heap.Insert(target_phantom.forward_segment_id.id,
                                target_phantom.GetForwardWeightPlusOffset(),
                                target_phantom.forward_segment_id.id);
        }
        if (target_phantom.reverse_segment_id.enabled)
        {
            reverse_heap.Insert(target_phantom.reverse_segment_id.id,
                                target_phantom.GetReverseWeightPlusOffset(),
                                target_phantom.reverse_segment_id.id);
        }

        const bool constexpr DO_NOT_FORCE_LOOPS =
            false; // prevents forcing of loops, since offsets are set correctly

        int duration = INVALID_EDGE_WEIGHT;
        std::vector<NodeID> packed_path;
        SearchWithCore(forward_heap,
                       reverse_heap,
                       forward_core_heap,
                       reverse_core_heap,
                       duration,
                       packed_path,
                       DO_NOT_FORCE_LOOPS,
                       DO_NOT_FORCE_LOOPS,
                       duration_upper_bound);

        double distance = std::numeric_limits<double>::max();
        if (duration != INVALID_EDGE_WEIGHT)
        {
            return GetPathDistance(packed_path, source_phantom, target_phantom);
        }
        return distance;
    }

    // Requires the heaps for be empty
    // If heaps should be adjusted to be initialized outside of this function,
    // the addition of force_loop parameters might be required
    double GetNetworkDistance(SearchEngineData::QueryHeap &forward_heap,
                              SearchEngineData::QueryHeap &reverse_heap,
                              const PhantomNode &source_phantom,
                              const PhantomNode &target_phantom,
                              int duration_upper_bound = INVALID_EDGE_WEIGHT) const
    {
        BOOST_ASSERT(forward_heap.Empty());
        BOOST_ASSERT(reverse_heap.Empty());

        if (source_phantom.forward_segment_id.enabled)
        {
            forward_heap.Insert(source_phantom.forward_segment_id.id,
                                -source_phantom.GetForwardWeightPlusOffset(),
                                source_phantom.forward_segment_id.id);
        }
        if (source_phantom.reverse_segment_id.enabled)
        {
            forward_heap.Insert(source_phantom.reverse_segment_id.id,
                                -source_phantom.GetReverseWeightPlusOffset(),
                                source_phantom.reverse_segment_id.id);
        }

        if (target_phantom.forward_segment_id.enabled)
        {
            reverse_heap.Insert(target_phantom.forward_segment_id.id,
                                target_phantom.GetForwardWeightPlusOffset(),
                                target_phantom.forward_segment_id.id);
        }
        if (target_phantom.reverse_segment_id.enabled)
        {
            reverse_heap.Insert(target_phantom.reverse_segment_id.id,
                                target_phantom.GetReverseWeightPlusOffset(),
                                target_phantom.reverse_segment_id.id);
        }

        const bool constexpr DO_NOT_FORCE_LOOPS =
            false; // prevents forcing of loops, since offsets are set correctly

        int duration = INVALID_EDGE_WEIGHT;
        std::vector<NodeID> packed_path;
        Search(forward_heap,
               reverse_heap,
               duration,
               packed_path,
               DO_NOT_FORCE_LOOPS,
               DO_NOT_FORCE_LOOPS,
               duration_upper_bound);

        if (duration == INVALID_EDGE_WEIGHT)
        {
            return std::numeric_limits<double>::max();
        }

        return GetPathDistance(packed_path, source_phantom, target_phantom);
    }
};
}
}
}

#endif // ROUTING_BASE_HPP


#include "game/CWorm.h"

#include "CVec.h"
#include "util/angle.h"
#include "util/log.h"
#include "gusgame.h"
#include "game/CGameObject.h"
#include "game/WormInputHandler.h"
#include "weapon_type.h"
#include "particle.h"
#include "CWormHuman.h"
#ifndef DEDICATED_ONLY
#include "base_animator.h"
#include "animators.h"
#include "sprite_set.h"
#include "sprite.h"
#include "gfx.h"
#include "CViewport.h"
#include "font.h"
#include "blitters/blitters.h"
#endif
#include "weapon.h"
#include "game/CMap.h"
#include "CGameScript.h"
#include "CGameMode.h"

#include "LuaCallbacks.h"
#include "luaapi/context.h"
#include "lua/bindings-objects.h"
#include "game/Game.h"
#include "gusanos/network.h"
#include "gusanos/net_worm.h"
#include "CServer.h"
#include "CClientNetEngine.h"

#include <math.h>
#include <string>
#include <vector>
#include <boost/lexical_cast.hpp>
using boost::lexical_cast;

LuaReference CWorm::metaTable;

Weapon* CWorm::getCurrentWeaponRef()
{
	if(iCurrentWeapon < 0) return NULL;
	if((size_t)(int)iCurrentWeapon >= m_weapons.size()) return NULL;
	return m_weapons[(size_t)(int)iCurrentWeapon];
}

std::vector<WeaponType*> CWorm::getWeaponTypes() const {
	std::vector<WeaponType*> wpns;
	wpns.reserve(m_weapons.size());
	const_foreach(w, m_weapons) {
		if(!*w) continue;
		wpns.push_back((*w)->getType());
	}
	return wpns;
}

void CWorm::base_setWeapon( size_t index, WeaponType* type )
{
	if(index >= m_weapons.size())
		return;

	luaDelete(m_weapons[index]);
	m_weapons[index] = 0;

	if ( type )
		m_weapons[index] = new Weapon( type, this );
}

void CWorm::setWeapon( size_t index, WeaponType* type )
{
	if( !m_node || !network.isClient() ) {
		base_setWeapon(index, type);

		// NetWorm code
		if( m_node && !network.isClient() ) {
			BitStream *data = new BitStream;
			addEvent(data, SetWeapon);
			Encoding::encode(*data, index, gusGame.options.maxWeapons);
			if ( type )
			{
				data->addBool(true);
				Encoding::encode(*data, type->getIndex(), gusGame.weaponList.size());
			}else
				data->addBool(false);
			m_node->sendEvent(eNet_ReliableOrdered, Net_REPRULE_AUTH_2_ALL, data);
		}
	}
}

void CWorm::setWeapons( std::vector<WeaponType*> const& weaps )
{
	if(weaps.size() > m_weapons.size())
		return;

	bWeaponsReady = true;

	// Here is where the interception of the server can be done on the weapon selection
	clearWeapons();
	for ( size_t i = 0; i < weaps.size(); ++i ) {
		setWeapon( i, weaps[i] );
	}
}

void CWorm::base_clearWeapons()
{
	for ( size_t i = 0; i < m_weapons.size(); ++i) {
		luaDelete(m_weapons[i]);
		m_weapons[i] = 0;
	}
}

void CWorm::clearWeapons()
{
	if( !m_node || !network.isClient() ) {
		base_clearWeapons();

		// NetWorm code
		if( m_node && !network.isClient() ) {
			BitStream *data = new BitStream;
			addEvent(data, ClearWeapons);
			m_node->sendEvent(eNet_ReliableOrdered, Net_REPRULE_AUTH_2_ALL, data);
		}
	}
}

void CWorm::calculateReactionForce(VectorD2<long> origin, Direction d)
{
	VectorD2<long> step;
	long len = 0;

	int bottom = gusGame.options.worm_weaponHeight;
	int top = bottom - gusGame.options.worm_height + 1;
	int left = (-gusGame.options.worm_width) / 2;
	int right = (gusGame.options.worm_width) / 2;

	switch(d) {
			case Down: {
				origin += VectorD2<long>(left, top);
				step = VectorD2<long>(1, 0);
				len = gusGame.options.worm_width;
			}
			break;

			case Left: {
				origin += VectorD2<long>(right, top + 1);
				step = VectorD2<long>(0, 1);
				len = gusGame.options.worm_height - 2;
			}
			break;

			case Up: {
				origin += VectorD2<long>(left, bottom);
				step = VectorD2<long>(1, 0);
				len = gusGame.options.worm_width;
			}
			break;

			case Right: {
				origin += VectorD2<long>(left, top + 1);
				step = VectorD2<long>(0, 1);
				len = gusGame.options.worm_height - 2;
			}
			break;

			default:
			return;
	}

	for(reacts[d] = 0; len > 0; --len) {
		Material const& g = game.gameMap()->getMaterial(origin.x, origin.y);

		if(!g.worm_pass) {
			++reacts[d];
		}

		origin += step;
	}

}

void CWorm::calculateAllReactionForces(VectorD2<float>& nextPos, VectorD2<long>& inextPos)
{
	//static const float correctionSpeed = 70.0f / 100.0f;
	static const float correctionSpeed = 1.0f;

	// Calculate all reaction forces
	calculateReactionForce(inextPos, Down);
	calculateReactionForce(inextPos, Left);
	calculateReactionForce(inextPos, Up);
	calculateReactionForce(inextPos, Right);

	// Add more if the worm is outside the screen
	if(inextPos.x < 5)
		reacts[Right] += 5;
	else if(inextPos.x > (long)game.gameMap()->GetWidth() - 5)
		reacts[Left] += 5;

	if(inextPos.y < 5)
		reacts[Down] += 5;
	else if(inextPos.y > (long)game.gameMap()->GetHeight() - 5)
		reacts[Up] += 5;

	if(reacts[Down] < 2 && reacts[Up] > 0
	        && (reacts[Left] > 0 || reacts[Right] > 0)) {
		pos().write().y -= correctionSpeed;
		// Update next position as well
		nextPos.y -= correctionSpeed;
		inextPos.y = static_cast<long>(nextPos.y);

		// Recalculate horizontal reaction forces.
		// Liero does not recalculate vertical ones,
		// so that is not a bug.

		calculateReactionForce(inextPos, Left);
		calculateReactionForce(inextPos, Right);
	}

	if(reacts[Up] < 2 && reacts[Down] > 0
	        && (reacts[Left] > 0 || reacts[Right] > 0)) {
		// Move one pixel per second
		pos().write().y += correctionSpeed;
		// Update next position as well
		nextPos.y += correctionSpeed;
		inextPos.y = static_cast<long>(nextPos.y);

		// Recalculate horizontal reaction forces.
		// Liero does not recalculate vertical ones,
		// so that is not a bug.

		calculateReactionForce(inextPos, Left);
		calculateReactionForce(inextPos, Right);
	}

	if(gusGame.options.worm_disableWallHugging) {
		if(reacts[Up] == 1 && reacts[Down] == 1
		        && (reacts[Left] > 0 || reacts[Right] > 0)) {
			reacts[Up] = 0;
			reacts[Down] = 0;
		}
	}
}

void CWorm::processPhysics()
{
	if(reacts[Up] > 0) {
		// Friction
		velocity().write().x *= gusGame.options.worm_friction;
	}

	velocity() *= gusGame.options.worm_airFriction;

	if(velocity().get().x > 0.f) {
		if(reacts[Left] > 0) {
			if(velocity().get().x > gusGame.options.worm_bounceLimit) {
				// TODO: Play bump sound
				velocity().write().x *= -gusGame.options.worm_bounceQuotient;
			} else
				velocity().write().x = 0.f;
		}
	} else if(velocity().get().x < 0.f) {
		if(reacts[Right] > 0) {
			if(velocity().get().x < -gusGame.options.worm_bounceLimit) {
				// TODO: Play bump sound
				velocity().write().x *= -gusGame.options.worm_bounceQuotient;
			} else
				velocity().write().x = 0.f;
		}
	}

	if(velocity().get().y > 0.f) {
		if(reacts[Up] > 0) {
			if(velocity().get().y > gusGame.options.worm_bounceLimit) {
				// TODO: Play bump sound
				velocity().write().y *= -gusGame.options.worm_bounceQuotient;
			} else
				velocity().write().y = 0.f;
		}
	} else if(velocity().get().y < 0.f) {
		if(reacts[Down] > 0) {
			if(velocity().get().y < -gusGame.options.worm_bounceLimit) {
				// TODO: Play bump sound
				velocity().write().y *= -gusGame.options.worm_bounceQuotient;
			} else
				velocity().write().y = 0.f;
		}
	}

	if(reacts[Up] == 0) {
		velocity().write().y += gusGame.options.worm_gravity;
	}

	if(velocity().get().x >= 0.f) {
		if(reacts[Left] < 2)
			pos().write().x += velocity().get().x;
	} else {
		if(reacts[Right] < 2)
			pos().write().x += velocity().get().x;
	}

	if(velocity().get().y >= 0.f) {
		if(reacts[Up] < 2)
			pos().write().y += velocity().get().y;
	} else {
		if(reacts[Down] < 2)
			pos().write().y += velocity().get().y;
	}
}

void CWorm::processJumpingAndNinjaropeControls()
{
	if(jumping && reacts[Up]) {
		//Jump

		velocity().write().y -= gusGame.options.worm_jumpForce;
		jumping = false;
	}
}

void CWorm::processMoveAndDig(void)
{
	float acc = gusGame.options.worm_acceleration;

	if(reacts[Up] <= 0)
		acc *= gusGame.options.worm_airAccelerationFactor;

	if(movingLeft && !movingRight) {
		if(velocity().get().x > -gusGame.options.worm_maxSpeed) {
			velocity().write().x -= acc;
		}

		if(iMoveDirectionSide == DIR_RIGHT) {
			iMoveDirectionSide = DIR_LEFT;
		}

		animate = true;
	} else if(movingRight && !movingLeft) {
		if(velocity().get().x < gusGame.options.worm_maxSpeed) {
			velocity().write().x += acc;
		}

		if(iMoveDirectionSide == DIR_LEFT) {
			iMoveDirectionSide = DIR_RIGHT;
		}

		animate = true;
	} else if(movingRight && movingLeft) {
		// TODO: Digging
		animate = false;
	} else {
		animate = false;
	}
}

void CWorm::think()
{
	if(!bPrepared) return;

	if(!game.gameScript()->gusEngineUsed()) {
		// we do that in any case, it may be that some map object was trying to kill us
		if(getAlive()) {
			if ( health <= 0 )
				die();
		}

#ifndef DEDICATED_ONLY
		// NOTE: This was from Worm::think() which isn't used right now
		renderPos = pos();
#endif
		return;
	}

	if(getAlive()) {
		if ( health <= 0 )
			die();

		VectorD2<float> next = pos() + velocity();

		VectorD2<long> inext(static_cast<long>(next.x), static_cast<long>(next.y));

		calculateAllReactionForces(next, inext);

		processJumpingAndNinjaropeControls();
		processPhysics();
		processMoveAndDig();

		for ( size_t i = 0; i < m_weapons.size(); ++i ) {
			if ( m_weapons[i] )
				m_weapons[i]->think( (int)i == iCurrentWeapon, i );
		}

#ifndef DEDICATED_ONLY
		if(animate)
			m_animator->tick();
		else
			m_animator->reset();

		if ( m_currentFirecone ) {
			if ( m_fireconeTime == 0 )
				m_currentFirecone = NULL;
			--m_fireconeTime;

			m_fireconeAnimator->tick();
		}
#endif

	} else {
		if ( m_timeSinceDeath > gusGame.options.maxRespawnTime && gusGame.options.maxRespawnTime >= 0 ) {
			respawn();
		}
		++m_timeSinceDeath;
	}

#ifndef DEDICATED_ONLY
	// NOTE: This was from Worm::think() which isn't used right now
	renderPos = pos();
#endif
	NetWorm_think();
}

#ifndef DEDICATED_ONLY
Vec CWorm::getRenderPos()
{
	return renderPos;// - Vec(0,0.5);
}
#endif

Angle CWorm::getPointingAngle()
{
	return (iMoveDirectionSide == DIR_RIGHT) ? getAimAngle() : (Angle(360.0) - getAimAngle());
}

int CWorm::getWeaponIndexOffset( int offset )
{
	if ( m_weapons.size() > 0 ) {
		if(offset < 0)
			offset = -1;
		else if(offset > 0)
			offset = 1;
		else
			return iCurrentWeapon;

		int i = iCurrentWeapon;
		do
			i = (i + offset + m_weapons.size()) % m_weapons.size();
		while(!m_weapons[i] && i != iCurrentWeapon);

		return i;
	} else {
		return iCurrentWeapon;
	}
}

void CWorm::setDir(int d)
{
	if(d > 0) iMoveDirectionSide = DIR_RIGHT;
	else if(d < 0) iMoveDirectionSide = DIR_LEFT;
}

bool CWorm::isCollidingWith( Vec const& point, float radius )
{
	if ( !getAlive() )
		return false;

	float top = pos().get().y - gusGame.options.worm_boxTop;
	if(point.y < top) {
		float left = pos().get().x - gusGame.options.worm_boxRadius;
		if(point.x < left)
			return (point - Vec(left, top)).lengthSqr() < radius*radius;

		float right = pos().get().x + gusGame.options.worm_boxRadius;
		if(point.x > right)
			return (point - Vec(right, top)).lengthSqr() < radius*radius;

		return top - point.y < radius;
	}

	float bottom = pos().get().y + gusGame.options.worm_boxBottom;
	if(point.y > bottom) {
		float left = pos().get().x - gusGame.options.worm_boxRadius;
		if(point.x < left)
			return (point - Vec(left, bottom)).lengthSqr() < radius*radius;

		float right = pos().get().x + gusGame.options.worm_boxRadius;
		if(point.x > right)
			return (point - Vec(right, bottom)).lengthSqr() < radius*radius;

		return point.y - bottom < radius;
	}

	float left = pos().get().x - gusGame.options.worm_boxRadius;
	if(point.x < left)
		return left - point.x < radius;

	float right = pos().get().x + gusGame.options.worm_boxRadius;
	if(point.x > right)
		return point.x - right < radius;

	return true;
}

bool CWorm::isActive()
{
	return getAlive();
}

void CWorm::removeRefsToPlayer(CWormInputHandler* player)
{
	if ( m_lastHurt == player )
		m_lastHurt = NULL;
	CGameObject::removeRefsToPlayer(player);
}

//#define DEBUG_WORM_REACTS

#ifndef DEDICATED_ONLY
#include "AuxLib.h"

void CWorm::draw(CViewport* viewport)
{
	// This is the Gusanos worm draw function.
	// Earlier, we drew this in either OLX or Gus.
	// Now, we draw the worm itself in OLX and
	// only Gus specific stuff here.

	if(!bPrepared) return;

	if (getAlive() && isVisible(viewport) && gusSkinVisble) {
		ALLEGRO_BITMAP* where = viewport->dest;
		IVec rPos = viewport->convertCoords( IVec(renderPos) );

		{
			// CNinjaRope::Draw(), called from CWorm::Draw(), draws the ninjarope.

			if ( getCurrentWeaponRef() )
				getCurrentWeaponRef()->drawBottom(where, rPos.x, rPos.y);

			// CWorm::Draw() draws the worm skin itself

			if ( getCurrentWeaponRef() )
				getCurrentWeaponRef()->drawTop(where, rPos.x, rPos.y);

			if ( m_currentFirecone ) {
				Vec distance = Vec(getPointingAngle(), (double)m_fireconeDistance);
				m_currentFirecone->getSprite(m_fireconeAnimator->getFrame(), getPointingAngle())->
						draw(where, rPos.x + static_cast<int>(distance.x), rPos.y + static_cast<int>(distance.y));
			}
		}

#ifdef DEBUG_WORM_REACTS
		{
			gusGame.infoFont->draw(where, lexical_cast<std::string>(reacts[Up]), rPos.x, rPos.y + 15, 0);
			gusGame.infoFont->draw(where, lexical_cast<std::string>(reacts[Down]), rPos.x, rPos.y - 15, 0);
			gusGame.infoFont->draw(where, lexical_cast<std::string>(reacts[Left]), rPos.x + 15, rPos.y, 0);
			gusGame.infoFont->draw(where, lexical_cast<std::string>(reacts[Right]), rPos.x - 15, rPos.y, 0);
		}
#endif

	}

}
#endif //DEDICATED_ONLY

void CWorm::respawn()
{
	// Check if its already allowed to respawn
	if(bCanRespawnNow)
		bRespawnRequested = true;
}

void CWorm::dig()
{
	if( weOwnThis() || !m_node ) {
		if ( getAlive() ) {
			dig( pos(), getPointingAngle() );

			// NetWorm code
			if( weOwnThis() && m_node ) {
				BitStream *data = new BitStream;
				addEvent(data, Dig);
				game.gameMap()->vectorEncoding.encode<Vec>(*data, pos());
				data->addInt(int(getPointingAngle()), Angle::prec);
				m_node->sendEvent(eNet_ReliableOrdered, Net_REPRULE_AUTH_2_ALL, data);
			}
		}
	}
}

void CWorm::dig( const Vec& digPos, Angle angle )
{
	if( gusGame.digObject )
		gusGame.digObject->newParticle( gusGame.digObject, digPos, Vec(angle), getDir(), m_owner, angle );
}

void CWorm::base_die() {
	if(game.gameScript()->gusEngineUsed()) {
		LUACALLBACK(wormDeath).call()(getLuaReference())();
	}

	bAlive = false;
	fTimeofDeath = tLX->currentTime;

	if (m_owner) {
		gusGame.displayKillMsg(m_owner, m_lastHurt); //TODO: Record what weapon it was?
	}

	// OLX code. based on CClient::InjureWorm
	if(weOwnThis()) {
		if(game.isServer())
			cServer->killWorm(getID(), m_lastHurt ? m_lastHurt->getWorm()->getID() : -1);
		else {
			this->Kill(false);
			this->clearInput();

			// Let the server know that i am dead
			cClient->getNetEngine()->SendDeath(getID(), m_lastHurt ? m_lastHurt->getWorm()->getID() : -1);
		}
	}

	cNinjaRope.write().remove();
	m_timeSinceDeath = 0;
	if ( gusGame.deathObject ) {
		gusGame.deathObject->newParticle( gusGame.deathObject, pos(), velocity(), getDir(), m_owner, Vec(velocity()).getAngle() );
	}
}

void CWorm::die()
{
	if( weOwnThis() || !m_node )
		base_die();
}

void CWorm::changeWeaponTo( unsigned int weapIndex )
{
	if( m_node ) {
		BitStream *data = new BitStream;
		addEvent(data, ChangeWeapon);
		Encoding::encode(*data, weapIndex, m_weapons.size());
		m_node->sendEvent(eNet_ReliableOrdered, Net_REPRULE_OWNER_2_AUTH | Net_REPRULE_AUTH_2_PROXY, data);
	}

	if ( getCurrentWeaponRef() ) {
		getCurrentWeaponRef()->actionStop( Weapon::PRIMARY_TRIGGER );
		getCurrentWeaponRef()->actionStop( Weapon::SECONDARY_TRIGGER );
	}
	if ( weapIndex < m_weapons.size() && m_weapons[weapIndex] )
		iCurrentWeapon = weapIndex;
}

bool CWorm::isChangingWpn() {
	if(changing) return true;
	if(bForceWeapon_Name) return true;
	if(CWormHumanInputHandler* h = dynamic_cast<CWormHumanInputHandler*>(m_inputHandler))
		if(h->getInputWeapon().isDown()) return true;
	return false;
}

void CWorm::damage( float amount, CWormInputHandler* damager )
{
	if( weOwnThis() || !m_node ) {
		// TODO: maybe we could implement an armor system? ;O
		m_lastHurt = damager;
		health -= amount;
		if ( health < 0.f )
			health = 0.f;
	}
}

void CWorm::addRopeLength( float distance )
{
	cNinjaRope.write().addLength(distance);
}

#ifndef DEDICATED_ONLY
void CWorm::showFirecone( SpriteSet* sprite, int frames, float distance )
{
	if(sprite) {
		m_fireconeTime = frames;
		m_currentFirecone = sprite;
		delete m_fireconeAnimator;
		m_fireconeAnimator = new AnimLoopRight( sprite, frames );
		m_fireconeDistance = distance;
	}
}
#endif

void CWorm::actionStart( Actions action )
{
	switch ( action ) {
			case MOVELEFT:
			movingLeft = true;
			break;

			case MOVERIGHT:
			movingRight = true;
			break;

			case FIRE:
			if ( getAlive() && getCurrentWeaponRef() )
				getCurrentWeaponRef()->actionStart( Weapon::PRIMARY_TRIGGER );
			break;

			case JUMP:
			jumping = true;
			break;

			case NINJAROPE:
			if ( getAlive() ) {
				CGameObject::ScopedLXCompatibleSpeed wormVelScope(*this);
				CGameObject::ScopedLXCompatibleSpeed ninjaVelScope(cNinjaRope.write());
				cNinjaRope.write().Shoot(getFaceDirection());
			}
			break;

			case CHANGEWEAPON:
			changing = true;
			break;

			case RESPAWN:
			respawn();
			break;

			default:
			break;
	}
}

void CWorm::actionStop( Actions action )
{
	switch ( action ) {
			case MOVELEFT:
			movingLeft = false;
			break;

			case MOVERIGHT:
			movingRight = false;
			break;

			case FIRE:
			if ( getAlive() && getCurrentWeaponRef() )
				getCurrentWeaponRef()->actionStop( Weapon::PRIMARY_TRIGGER );
			break;

			case JUMP:
			jumping = false;
			break;

			case NINJAROPE:
			cNinjaRope.write().remove();
			break;

			case CHANGEWEAPON:
			changing = false;
			break;

			default:
			break;
	}
}

void CWorm::finalize()
{
	LUACALLBACK(wormRemoved).call()(getLuaReference())();

	for ( size_t i = 0; i < m_weapons.size(); ++i) {
		luaDelete(m_weapons[i]);
		m_weapons[i] = 0;
	}

	game.onRemoveWorm(this);
	thisRef.obj.overwriteShared(NULL);

	if(m_node) delete m_node; m_node = 0;
	if(m_interceptor) delete m_interceptor; m_interceptor = 0;
}


#ifndef __penglei_bitmap_h__
#define __penglei_bitmap_h__

#include "bitmanip.hpp"
#include "stdtypes.hpp"
#include "valvec.hpp"
#include <boost/mpl/bool.hpp>
#include <xmmintrin.h>

namespace terark {

template<int TotalBitsMaybeUnaligned, class BlockT = bm_uint_t>
class static_bitmap {
public:
    enum { BlockBits = sizeof(BlockT) * 8 };
	enum { TotalBits = TotalBitsMaybeUnaligned };
    enum { TotalBitsAligned = (TotalBits + BlockBits - 1) & ~(BlockBits - 1)};
    enum { BlockN = TotalBitsAligned / sizeof(BlockT) / 8};
    enum { BlockCount = BlockN };
	enum { ExtraBits = TotalBitsAligned - TotalBitsMaybeUnaligned };
	enum {  LastBits = ExtraBits ? BlockBits - ExtraBits : 0 };
	static const BlockT ExtraMax = (BlockT(1) << ExtraBits) - 1;
	static const BlockT LastMask = (BlockT(1) <<  LastBits) - 1;
    typedef BlockT block_t;
	typedef boost::mpl::bool_<0!=ExtraBits> has_extra;
protected:
    BlockT  bm[BlockN];
public:
    explicit static_bitmap(bool val = false) { fill_all(val); }
	void fill(bool value) {
		if (ExtraBits) {
			const BlockT e = value ? ~BlockT(0) : 0;
			for (int i = 0; i < BlockN-1; ++i) bm[i] = e;
			BlockT& x = bm[BlockN-1];
			x = (x & ~LastMask) | (value ? LastMask : 0);
		} else {
			fill_all(value);
		}
	}
	void fill_all(bool value) {
        const BlockT e = value ? ~BlockT(0) : 0;
        for (int i = 0; i < BlockN; ++i) bm[i] = e;
	}

	// when TotalBits is not aligned with BlockBits,
	// there are some some extra free bits in the high bits
	// of last block
	BlockT get_extra() const { return get_extra_aux(has_extra()); }
	void set_extra(BlockT x) { set_extra_aux(x, has_extra()); }
private:
	BlockT get_extra_aux(boost::mpl::bool_<1>) const {
		assert(0 != ExtraBits);
		return (bm[BlockN-1] & ~LastMask) >> LastBits;
	}
	void set_extra_aux(BlockT x, boost::mpl::bool_<1>) {
		assert(x <= ExtraMax);
		assert(0 != ExtraBits);
		BlockT& last = bm[BlockN-1];
		last = (last & LastMask) | (x << LastBits);
	}
	BlockT get_extra_aux(boost::mpl::bool_<0>) const {
		assert(0);
		return 0;
	}
	void set_extra_aux(BlockT, boost::mpl::bool_<0>) {
		assert(0);
	}

public:
    BlockT  block(int i) const { return bm[i]; }
    BlockT& block(int i)       { return bm[i]; }

#define TERARK_DefineBitMapBinaryOperator(OP, Expression) \
	static_bitmap& operator OP(const static_bitmap& that) { \
		for (int i = 0; i < BlockN-(ExtraBits?1:0); ++i) { \
			BlockT& x = this->bm[i]; \
			BlockT  y = that. bm[i]; \
			x = Expression; \
		} \
		if (ExtraBits) { \
			BlockT& x = this->bm[BlockN-1]; \
			BlockT  y = that. bm[BlockN-1]; \
			x = ((Expression) & LastMask) | (x & ~LastMask); \
		} \
		return *this; \
	}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
	TERARK_DefineBitMapBinaryOperator(-=, x &~y)
	TERARK_DefineBitMapBinaryOperator(+=, x | y) // same as |=
	TERARK_DefineBitMapBinaryOperator(|=, x | y)
	TERARK_DefineBitMapBinaryOperator(^=, x ^ y)
	TERARK_DefineBitMapBinaryOperator(&=, x & y)

    static_bitmap& operator<<=(int n) { shl(n); return *this; }
    void shl(int n, int realBlocks = BlockN) { // shift bit left
        assert(realBlocks <= BlockN);
        assert(n < BlockBits * realBlocks);
        assert(n > 0);
		if (ExtraBits) {
			BlockT extra = get_extra();
			set_extra(0);
			do_shl(n, realBlocks);
			set_extra(extra);
		} else {
			do_shl(n, realBlocks);
		}
	}
private:
    void do_shl(int n, int realBlocks) { // shift bit left
        int c = n / BlockBits;
        if (c > 0) {
            // copy backward, expected to be loop unrolled
            // when this function is calling by shl(n)
            for (int i = realBlocks-1; i >= c; --i)
                bm[i] = bm[i-c];
            memset(bm, 0, sizeof(BlockT)*c);
        }
        if ((n %= BlockBits) != 0) {
            BlockT r = bm[c] >> (BlockBits - n);
            bm[c] <<= n;
            for (int i = c+1; i < realBlocks; ++i) {
                BlockT r2 = bm[i] >> (BlockBits - n);
                bm[i] <<= n;
                bm[i]  |= r;
                r = r2;
            }
        }
    }
public:
    void set(int n, bool val) {
        assert(n >= 0);
        assert(n < TotalBits);
        if (val)
        	terark_bit_set1(bm, n);
        else
        	terark_bit_set0(bm, n);
    }
    void set1(int n) {
        assert(n >= 0);
        assert(n < TotalBits);
		terark_bit_set1(bm, n);
	}
    void set0(int n) {
        assert(n >= 0);
        assert(n < TotalBits);
		terark_bit_set0(bm, n);
    }
	bool operator[](int n) { // alias of 'is1'
        assert(n >= 0);
        assert(n < TotalBits);
		return terark_bit_test(bm, n);
	}
	bool operator[](int n) const { // same as is1(n)
        assert(n >= 0);
        assert(n < TotalBits);
		return terark_bit_test(bm, n);
	}
    bool is1(int n) const {
        assert(n >= 0);
        assert(n < TotalBits);
		return terark_bit_test(bm, n);
    }
    bool is0(int n) const {
        assert(n >= 0);
        assert(n < TotalBits);
		return !terark_bit_test(bm, n);
    }
	bool is_all0() const {
		if (ExtraBits) {
			for (int i = 0; i < BlockN-1; ++i)
				if (bm[i])
					return false;
			return (bm[BlockN-1] & LastMask) == 0;
		}
		for (int i = 0; i < BlockN; ++i)
			if (bm[i])
				return false;
		return true;
	}
	bool is_all1() const {
		if (ExtraBits) {
			for (int i = 0; i < BlockN-1; ++i)
				if (BlockT(-1) != bm[i])
					return false;
			return (bm[BlockN-1] & LastMask) == LastMask;
		}
		for (int i = 0; i < BlockN; ++i)
			if (BlockT(-1) != bm[i])
				return false;
		return true;
	}
	bool has_any0() const { return !is_all1(); }
	bool has_any1() const { return !is_all0(); }

    int popcnt() const {
        int c = 0; // small BlcokN will triger loop unroll optimization
        for (int i = 0; i < BlockN; ++i) c += fast_popcount(bm[i]);
		if (ExtraBits)
			c -= fast_popcount(bm[BlockN-1] & ~LastMask);
        return c;
    }

    /// popcnt in blocks [0, align_bits(bits)/BlockBits)
    int popcnt_aligned(int bits) const {
        assert(bits >= 1);
        assert(bits <= TotalBits);
        // align to block boundary
        int n = (bits + BlockBits-1) / BlockBits;
        int c = 0; // small BlcokN will trigger loop unroll optimization
        for (int i = 0; i < BlockN; ++i) {
            if (i < n)
                c += fast_popcount(bm[i]);
            else
                break;
        }
		if (ExtraBits && BlockN == n)
			c -= fast_popcount(bm[BlockN-1] & ~LastMask);
        return c;
    }

	/// popcnt in bits: [0, bitpos), not counting bm[bitpos]
    int popcnt_index(int bitpos) const {
        assert(bitpos >= 0);
        assert(bitpos < TotalBits);
	//	assert(is1(bitpos)); // often holds, but not required to always holds
        int n = bitpos/BlockBits;
        int c = 0; // small BlcokN will trigger loop unroll optimization
        for (int i = 0; i < BlockN; ++i) {
            if (i < n)
                c += fast_popcount(bm[i]);
            else
                break;
        }
		if (int shift = bitpos % BlockBits)
        	c += fast_popcount_trail(bm[n], shift);
        return c;
    }

    static int align_bits(int n) {
        return (n + BlockBits-1) & ~(BlockBits-1);
    }
};

class TERARK_DLL_EXPORT febitvec {
protected:
    bm_uint_t* m_words;
	size_t m_size;
	size_t m_capacity;
    static const size_t AllocUnitBits = 64;

	void push_back_slow_path(bool val);
	void ensure_set1_slow_path(size_t i);

public:
	static size_t align_bits(size_t nbits) { return (nbits+WordBits-1) & ~(WordBits-1); }

	febitvec() { m_words = NULL; m_size = 0; m_capacity = 0; }
    explicit febitvec(size_t bits, bool val = false);
    febitvec(size_t bits, bool val, bool padding);
	febitvec(size_t bits, valvec_no_init);
	febitvec(size_t bits, valvec_reserve);

	febitvec(const febitvec&); // copy-cons
	febitvec& operator=(const febitvec&); // assign
#if defined(HSM_HAS_MOVE)
	febitvec(febitvec&&) noexcept; // move-cons
	febitvec& operator=(febitvec&&) noexcept; // move-assign
#endif

	febitvec(const febitvec& y, size_t beg, size_t len);

	~febitvec();

	void append(const febitvec& y);
	void append(const febitvec& y, size_t beg, size_t len);
	void assign(const febitvec&);
	void risk_memcpy(const febitvec&);
	void copy(size_t destBeg, const febitvec& y);
	void copy(size_t destBeg, const febitvec& y, size_t srcBeg, size_t len);

	void clear();
	void erase_all();
	void fill(bool val);
	void grow(size_t cnt, bool val=false);
	void reserve(size_t newcap);
    void resize(size_t newsize, bool val=false);
	void resize_no_init(size_t newsize);
	void resize_fill(size_t newsize, bool val=false);
	void risk_release_ownership();
	void shrink_to_fit();

	febitvec& operator-=(const febitvec& y);
	febitvec& operator^=(const febitvec& y);
	febitvec& operator&=(const febitvec& y);
	febitvec& operator|=(const febitvec& y);
	void block_or(const febitvec& y, size_t yblstart, size_t blcnt);
	void block_and(const febitvec& y, size_t yblstart, size_t blcnt);

	void push_back(bool val) {
		if (m_size < m_capacity)
			this->set(m_size++, val);
		else
			push_back_slow_path(val);
	}

	void unchecked_push_back(bool val) {
		assert(m_size < m_capacity);
		this->set(m_size++, val);
	}

	void pop_back() { assert(m_size > 0); --m_size; }

    bool operator[](size_t i) const { // alias of 'is1'
        assert(i < m_size);
		return terark_bit_test(m_words, i);
	}
    bool is1(size_t i) const {
        assert(i < m_size);
		return terark_bit_test(m_words, i);
    }
    bool is0(size_t i) const {
        assert(i < m_size);
		return !terark_bit_test(m_words, i);
    }
	bool back() const {
		assert(m_size > 0);
		return terark_bit_test(m_words, m_size-1);
	}
	void set(size_t i, bool val) {
        assert(i < m_size);
		val ? set1(i) : set0(i);
	}
    void set0(size_t i) {
        assert(i < m_size);
		terark_bit_set0(m_words, i);
    }
    void set1(size_t i) {
        assert(i < m_size);
		terark_bit_set1(m_words, i);
    }
	void ensure_set(size_t i, bool val) {
		val ? ensure_set1(i) : ensure_set0(i);
	}
	void ensure_set0(size_t i) {
		if (terark_likely(i < m_size))
			terark_bit_set0(m_words, i);
		else
			resize(i+1);
	}
	void ensure_set1(size_t i) {
		if (terark_likely(i < m_size))
			terark_bit_set1(m_words, i);
		else
			ensure_set1_slow_path(i);
	}
	void set0(size_t first, size_t num);
	void set1(size_t first, size_t num);
	void set (size_t first, size_t num, bool val);

	void beg_end_set0(size_t beg, size_t end);
	void beg_end_set1(size_t beg, size_t end);
	void beg_end_set (size_t beg, size_t end, bool val);

	void set_word(size_t word_idx, bm_uint_t bits) {
		m_words[word_idx] = bits;
	}
	bm_uint_t get_word(size_t word_idx) const {
		return m_words[word_idx];
	}
	size_t num_words() const { return (m_size + WordBits - 1) / WordBits; }

	bool isall0() const;
   	bool isall1() const;

	size_t popcnt() const;
	size_t popcnt(size_t blstart, size_t blcnt) const;

	///@returns number of continuous one/zero bits starts at bitpos
	size_t one_seq_len(size_t bitpos) const;
	size_t zero_seq_len(size_t bitpos) const;

	///@returns number of continuous one/zero bits ends at endpos
	///@note return_value = endpos - start;
	///        where bits[start-1] is 0 and bits[start, ... endpos) are all 1/0
	size_t one_seq_revlen(size_t endpos) const;
	size_t zero_seq_revlen(size_t endpos) const;

	size_t mem_size() const {
		return ((m_size + AllocUnitBits-1) & ~(AllocUnitBits-1)) / 8;
	}
	static size_t s_mem_size(size_t bits) {
		return ((bits + AllocUnitBits-1) & ~(AllocUnitBits-1)) / 8;
	}
	size_t blsize() const { return (m_size + WordBits -1) / WordBits; }
	size_t size() const { return m_size; }
	size_t capacity() const { return m_capacity; }
	size_t unused() const { return m_capacity - m_size; }
	bool  empty() const { return m_size == 0; }
	void  swap(febitvec& y) {
		std::swap(m_words, y.m_words);
		std::swap(m_size , y.m_size);
		std::swap(m_capacity, y.m_capacity);
	}

protected:
	template<class Uint>
	void push_uint_tpl(size_t width, Uint val);
public:
	void push_uint(size_t width, byte_t val)
	   { push_uint(width, (uint)(val)); }
	void push_uint(size_t width, ushort val)
	   { push_uint(width, (uint)(val)); }
	void push_uint(size_t width, ulong val) {
#if (ULONG_MAX == UINT_MAX)
		push_uint(width, (uint)(val));
#elif (ULONG_MAX == ULLONG_MAX)
		push_uint(width, (ullong)(val));
#else
		#error "Bad long int type"
#endif
	}
	void push_uint(size_t width, uint  val);
	void push_uint(size_t width, ullong val);

protected:
	template<class Uint>
	void set_uint_tpl(size_t bitpos, size_t width, Uint val);
public:
	void set_uint(size_t bitpos, size_t width, byte_t val)
	   { set_uint(bitpos, width, (uint)(val)); }
	void set_uint(size_t bitpos, size_t width, ushort val)
	   { set_uint(bitpos, width, (uint)(val)); }
	void set_uint(size_t bitpos, size_t width, ulong val) {
#if (ULONG_MAX == UINT_MAX)
		set_uint(bitpos, width, (uint)(val));
#elif (ULONG_MAX == ULLONG_MAX)
		set_uint(bitpos, width, (ullong)(val));
#else
		#error "Bad long int type"
#endif
	}
	void set_uint(size_t bitpos, size_t width, uint val);
	void set_uint(size_t bitpos, size_t width, ullong val);

protected:
	template<class Uint>
	static
	void s_set_uint_tpl(Uint* base, size_t bitpos, size_t width, Uint val);
public:
	static
	void s_set_uint(byte_t* base, size_t bitpos, size_t width, byte_t val)
	   { s_set_uint((uint*)base, bitpos, width, (uint)(val)); }
	static
	void s_set_uint(ushort* base, size_t bitpos, size_t width, ushort val)
	   { s_set_uint((uint*)base, bitpos, width, (uint)(val)); }
	static
	void s_set_uint(ulong* base, size_t bitpos, size_t width, ulong val) {
#if (ULONG_MAX == UINT_MAX)
		s_set_uint((uint*)base, bitpos, width, (uint)(val));
#elif (ULONG_MAX == ULLONG_MAX)
		s_set_uint((ullong*)base, bitpos, width, (ullong)(val));
#else
		#error "Bad long int type"
#endif
	}
	static
	void s_set_uint(uint* base, size_t bitpos, size_t width, uint val);
	static
	void s_set_uint(ullong* base, size_t bitpos, size_t width, ullong val);

//---------------------------------------------------------------------------

	template<class Uint>
	static
	Uint s_get_uint(const Uint* base, size_t bitpos, size_t width) {
		const Uint UintBits = Uint(sizeof(Uint) * 8);
		assert(width <= UintBits);
		const Uint* p = base + bitpos / UintBits;
		size_t offset = bitpos % UintBits;
		Uint mask = width < UintBits ? ~(Uint(-1) << width) : Uint(-1);
		Uint low  = p[0] >> offset;
		if (offset + width <= UintBits)
			return mask & low;
		else
			return mask & (low | (p[1] << (UintBits - offset)));
	}
	template<class Uint>
	Uint get_uint(size_t bitpos, size_t width) const {
		assert(bitpos < m_size);
		assert(bitpos + width <= m_size);
		return s_get_uint((const Uint*)(m_words), bitpos, width);
	}
	template<class Uint>
	void get2_uints(size_t bitpos, size_t width, Uint vals[2]) const {
		assert(bitpos < m_size);
		assert(bitpos + width <= m_size);
		const Uint* base = (const Uint*)(m_words);
		vals[0] = s_get_uint(base, bitpos, width);
		vals[1] = s_get_uint(base, bitpos+width, width);
	}

	      bm_uint_t* bldata()       { return m_words; }
	const bm_uint_t* bldata() const { return m_words; }

	      void* data()       { return m_words; }
	const void* data() const { return m_words; }

	void risk_mmap_from(unsigned char* base, size_t length) {
		assert((length * 8) % AllocUnitBits == 0);
		m_words = (bm_uint_t*)base;
		m_size = length * 8;
		m_capacity = m_size;
	}

	void risk_set_data(bm_uint_t*  bits) { m_words = bits; }
	void risk_set_size(    size_t nbits) { m_size = nbits; }
	void risk_set_capacity(size_t nbits) { assert(nbits % WordBits == 0); m_capacity = nbits; }

	static bool fast_is1(const bm_uint_t* bits, size_t i) { return  terark_bit_test(bits, i); }
	static bool fast_is0(const bm_uint_t* bits, size_t i) { return !terark_bit_test(bits, i); }

    static size_t fast_one_seq_len(const bm_uint_t* bits, size_t bitpos);

    void prefetch_bit(size_t i) const
        { _mm_prefetch((const char*)&m_words[i/WordBits], _MM_HINT_T0); }
    static void fast_prefetch_bit(const bm_uint_t* m_words, size_t i)
        { _mm_prefetch((const char*)&m_words[i/WordBits], _MM_HINT_T0); }
};

} // namespace terark

#endif


#include <boost/scope_exit.hpp>
#include "CharacterInterface.h"
#include "Qor/Sound.h"
#include <glm/gtx/vector_angle.hpp>
#include "World.h"
using namespace std;
using namespace glm;

CharacterInterface :: CharacterInterface(
    const shared_ptr<Controller>& input,
    const shared_ptr<Node>& character,
    World* world,
    Cache<Resource, std::string>* resources
):
    NodeInterface(input, character),
    m_pWorld(world),
    m_pResources(resources)
{
    lock_sprite();
    BOOST_SCOPE_EXIT(this_) {
        this_->unlock_sprite();
    } BOOST_SCOPE_EXIT_END

    m_ButtonNames = {
        "up",
        "down",
        "left",
        "right",

        "shoot",
        "sprint",
        "action",
        "strafe",
        "reload",
        "previous_weapon",
        "next_weapon"
    };

    m_StateNames = {
        "up",
        "down",
        "left",
        "right",

        "stand",
        "walk",

        "death"
    };

    for(unsigned int i=0; i<(unsigned int)Button::MAX; ++i)
        try{
            m_Buttons[i] = input->button_id(m_ButtonNames[i]);
        }catch(const out_of_range&){
            m_Buttons[i] = numeric_limits<unsigned int>::max();
        }
    for(unsigned int i=0; i<(unsigned int)State::MAX; ++i)
        try{
            m_States[i] = m_pCharacter->state_id(m_StateNames[i]);
        }catch(const out_of_range&){
            m_States[i] = numeric_limits<unsigned int>::max();
        }

    set_state(State::STAND);
    set_state(State::DOWN);

    auto crosshair = make_shared<Sprite>(
        resources->transform("arrow.png"),
        resources,
        "", // no skin
        vec3(0.0f, 0.0f, 10.0f)
    );
    crosshair->mesh()->offset(vec3(0.0f, -m_fCrosshairDist, 0.0f));
    m_pCharacter->add(crosshair);
    crosshair->each([](Node* n){
        n->layer(World::INDICATOR);
    }, Node::Each::RECURSIVE | Node::Each::INCLUDE_SELF);

    m_pCrosshair = crosshair;

    m_vDir = vec2(0.0f, 1.0f);
    m_CrosshairEase.stop(Angle::degrees(180));
}

void CharacterInterface :: event()
{
    lock_input();
    BOOST_SCOPE_EXIT_ALL(this) {
        unlock_input();
    };
    lock_sprite();
    BOOST_SCOPE_EXIT_ALL(this) {
        unlock_sprite();
    };

    if(m_pCharacter->dead()) // block control on death
    {
        m_pCharacter->velocity(glm::vec3(0.0f));
        return;
    }

    auto crosshair = m_pCrosshair.lock();
    assert(crosshair);

    const bool strafe = button(Button::STRAFE);

    m_vMove = vec2(0.0f);
    if(button(Button::LEFT))
        m_vMove += vec2(-button(Button::LEFT).pressure(), 0.0f);
    if(button(Button::RIGHT))
        m_vMove += vec2(button(Button::RIGHT).pressure(), 0.0f);
    if(button(Button::UP))
        m_vMove += vec2(0.0f, -button(Button::UP).pressure());
    if(button(Button::DOWN))
        m_vMove += vec2(0.0f, button(Button::DOWN).pressure());

    if(m_vMove != vec2())
        m_vMove = glm::normalize(m_vMove);

    if(!strafe)
    {
        m_CrosshairEase.resume();

        if(m_vMove != vec2())
        {
            vec2 old_dir = m_vDir;
            m_vDir = normalize(m_vMove);

            Angle a(angle(old_dir, m_vDir), Angle::RADIANS);
            if(fabs(a.degrees()) > K_EPSILON)
            {
                //auto atest = orientedAngle(
                //    vec2(m_vDir.x, -m_vDir.y),
                //    vec2(0.0f, 1.0f)
                //);
                //LOGf("atest: %s", atest);
                m_CrosshairEase.stop(
                    Angle::radians(
                        orientedAngle(
                            vec2(m_vDir.x, -m_vDir.y),
                            vec2(0.0f, 1.0f)
                        )
                    ),
                    Freq::Time(100),
                    //Freq::Time((fabs(a.degrees())  > 90.0f + K_EPSILON) ? 0 : 100),
                    INTERPOLATE(linear<Angle>)
                    //[](const Angle& a, const Angle& b, float t){
                    //    return a + (b-a)*t;
                    //}
                );
            }
        }

        // TODO: get direction from crosshair angle
        if(fabs(m_vDir.y) > K_EPSILON)
            set_state(m_vDir.y > 0.0f ? State::DOWN : State::UP);
        else if(fabs(m_vDir.x) > K_EPSILON)
            set_state(m_vDir.x > 0.0f ? State::RIGHT: State::LEFT);
    }
    else
    {
        m_CrosshairEase.pause();
    }

    //crosshair->reset_orientation();
    //*crosshair->matrix() = rotate(mat4(),
    //    orientedAngle(
    //        vec2(m_vDir.x, -m_vDir.y), vec2(0.0f, 1.0f)
    //    ),
    //    Axis::Z
    //);

    m_fSpeed = m_fWalkSpeed * std::max<float>(
        std::max<float>(button(Button::LEFT).pressure(), button(Button::RIGHT).pressure()),
        std::max<float>(button(Button::UP).pressure(), button(Button::DOWN).pressure())
    );
    if(button(Button::SPRINT) && !strafe)
    {
        m_fSpeed = m_fWalkSpeed * m_fSprintMult;
        m_pCharacter->speed(m_fSprintMult);
    }
    else
    {
        m_pCharacter->resume();
    }

    set_state(length(m_vMove) > K_EPSILON ? State::WALK : State::STAND);

    if(button(Button::PREVIOUS_WEAPON).pressed_now())
        m_pCharacter->switch_weapon(-1);
    if(button(Button::NEXT_WEAPON).pressed_now())
        m_pCharacter->switch_weapon(1);
}

void CharacterInterface :: logic(Freq::Time t)
{
    lock_sprite();
    BOOST_SCOPE_EXIT_ALL(this) {
        unlock_sprite();
    };
    lock_input();
    BOOST_SCOPE_EXIT_ALL(this) {
        unlock_input();
    };

    if(m_pCharacter->dead()) // block control on death
        return;

    m_CrosshairEase.logic(t);
    auto crosshair = m_pCrosshair.lock();
    assert(crosshair);
    //*crosshair->matrix() = m_CrosshairEase.get();

    crosshair->reset_orientation();
    glm::vec3 pos = Matrix::translation(*crosshair->matrix());

    *crosshair->matrix() = glm::rotate(m_CrosshairEase.get().radians(), Axis::Z);
    //crosshair->pend(); // will pend below
    crosshair->position(pos);

    //if(length(m_vMove) > K_EPSILON)
    m_pCharacter->velocity(vec3(m_vMove * m_fSpeed, 0.0f));

    //if(!m_pCharacter->clip())
    //    reload();

    if(button(Button::RELOAD).pressed())
        reload();

    if(m_pCharacter->weapon() && !m_pCharacter->weapon()->delayed() && button(Button::SHOOT).pressed())
    {
        if(m_pCharacter->clip())
        {
            unsigned num_bullets = m_pCharacter->shoot();

            if(num_bullets)
            {
                auto bulletsound = make_shared<Sound>(
                    m_pResources->transform(m_pCharacter->weapon()->name() + ".wav"),
                    m_pResources
                );
                m_pCharacter->add(bulletsound);
                bulletsound->play();
                auto bulletsoundptr = bulletsound.get();
                bulletsound->on_tick.connect([bulletsoundptr](Freq::Time){
                    if(not bulletsoundptr->source()->playing())
                        bulletsoundptr->detach();
                });
            }

            auto path = m_pResources->transform(m_pCharacter->weapon()->bullet_gfx() + ".png");
            for(unsigned i=0; i<num_bullets; i++)
            {
                auto bullet = make_shared<Sprite>(path, m_pResources);
                float vary = (rand() % (1+kit::round_int((1.0f - m_pCharacter->weapon()->accuracy()) * 45.0f))) * 1.0f;
                vary *= (rand() % 2) ? 1.0f : -1.0f;
                auto dir = (m_CrosshairEase.get() - Angle(90.0f + vary, Angle::DEGREES)).vector();
                auto bulletptr = bullet.get();
                auto range = make_shared<float>(m_pCharacter->weapon()->range());
                auto bullet_speed = m_pCharacter->weapon()->bullet_speed();
                bullet->on_tick.connect([bulletptr, dir, range, bullet_speed, vary] (Freq::Time t){
                    bulletptr->move(vec3(
                        dir.x * bullet_speed * t.s(),
                        dir.y * bullet_speed * t.s(),
                        0.0f
                    ));
                    *range -= t.s() * bullet_speed;
                    if(*range <= 0.0f)
                        bulletptr->detach();
                });
                crosshair->add(bullet);
                bullet->position(glm::vec3(0.0f, 0.0f, 0.0f));
                bullet->collapse(); // collapse to player
                bullet->collapse(); // collapse to root / map
                auto bpos = bullet->position();
                bullet->position(glm::vec3(
                    bpos.x, bpos.y, m_pCharacter->position().z + 0.25f
                ));
                m_pWorld->setup_bullet(bullet);
            }
        }else{
            reload();
        }
    }
}

void CharacterInterface :: reload()
{
    if(m_pCharacter->reload())
    {
        auto sound = make_shared<Sound>(
            "reload.wav",
            m_pResources
        );
        m_pCharacter->add(sound);
        sound->play();
        auto soundptr = sound.get();
        sound->on_tick.connect([soundptr](Freq::Time){
            if(not soundptr->source()->playing())
                soundptr->detach();
        });
    }
}


#include "ompl/control/planners/ltl/LTLPlanner.h"
#include "ompl/control/planners/PlannerIncludes.h"
#include "ompl/control/planners/ltl/ProductGraph.h"
#include "ompl/control/planners/ltl/LTLProblemDefinition.h"
#include "ompl/datastructures/PDF.h"
#include "ompl/util/Console.h"
#include <algorithm>
#include <boost/unordered_map.hpp>
#include <limits>
#include <map>
#include <vector>

#include <stdio.h>

ompl::control::LTLPlanner::LTLPlanner(const LTLSpaceInformationPtr& ltlsi, const ProductGraphPtr& a, double exploreTime) :
    ompl::base::Planner(ltlsi, "LTLPlanner"),
    ltlsi_(ltlsi.get()),
    abstraction_(a),
    prodStart_(NULL),
    exploreTime_(exploreTime)
{
    specs_.approximateSolutions = true;
}

ompl::control::LTLPlanner::~LTLPlanner(void)
{
    clearMotions();
}

void ompl::control::LTLPlanner::setup()
{
    base::Planner::setup();
}

void ompl::control::LTLPlanner::clear()
{
    base::Planner::clear();
    availDist_.clear();
    abstractInfo_.clear();
    clearMotions();
}

ompl::base::PlannerStatus ompl::control::LTLPlanner::solve(const ompl::base::PlannerTerminationCondition& ptc)
{
    // TODO make solve work when called more than once!
    checkValidity();
    const base::State* start = pis_.nextStart();
    prodStart_ = ltlsi_->getProdGraphState(start);

    if (pis_.haveMoreStartStates())
        OMPL_WARN("Multiple start states given. Using only the first start state.");

    Motion* startMotion = new Motion(ltlsi_);
    si_->copyState(startMotion->state, start);
    ltlsi_->nullControl(startMotion->control);
    startMotion->abstractState = prodStart_;

    motions_.push_back(startMotion);
    abstractInfo_[prodStart_].addMotion(startMotion);
    updateWeight(prodStart_);
    availDist_.add(prodStart_, abstractInfo_[prodStart_].weight);

    abstraction_->buildGraph(prodStart_, boost::bind(&LTLPlanner::initAbstractInfo, this, _1));

    if (!sampler_)
        sampler_ = si_->allocStateSampler();
    if (!controlSampler_)
        controlSampler_ = ltlsi_->allocControlSampler();

    bool solved = false;
    Motion* soln;

    while (ptc()==false && !solved)
    {
        const std::vector<ProductGraph::State*> lead = abstraction_->computeLead(prodStart_, boost::bind(&LTLPlanner::abstractEdgeWeight, this, _1, _2));
        buildAvail(lead);
        solved = explore(lead, soln, exploreTime_);
    }

    if (solved)
    {
        //build solution path
        std::vector<Motion*> path;
        while (soln != NULL)
        {
            path.push_back(soln);
            soln = soln->parent;
        }
        PathControl* pc = new PathControl(si_);
        for (int i = path.size()-1; i >= 0; --i)
        {
            if (path[i]->parent != NULL) {
                pc->append(path[i]->state, path[i]->control, path[i]->steps * ltlsi_->getPropagationStepSize());
            }
            else {
                pc->append(path[i]->state);
            }
        }
        pdef_->addSolutionPath(base::PathPtr(pc));
    }

    OMPL_INFORM("Created %u states", motions_.size());
    return base::PlannerStatus(solved, false);
}

void ompl::control::LTLPlanner::getTree(std::vector<base::State*>& tree) const
{
    tree.resize(motions_.size());
    for (unsigned int i = 0; i < motions_.size(); ++i)
        tree[i] = motions_[i]->state;
}

std::vector<ompl::control::ProductGraph::State*> ompl::control::LTLPlanner::getHighLevelPath(const std::vector<base::State*>& path, ProductGraph::State* start) const
{
    std::vector<ProductGraph::State*> hlPath(path.size());
    hlPath[0] = (start != NULL ? start : ltlsi_->getProdGraphState(path[0]));
    for (unsigned int i = 1; i < path.size(); ++i)
    {
        hlPath[i] = ltlsi_->getProdGraphState(path[i]);
        if (!hlPath[i]->isValid())
            OMPL_WARN("High-level path fails automata");
    }
    return hlPath;
}

ompl::control::LTLPlanner::Motion::Motion(void) : state(NULL), control(NULL), parent(NULL), steps(0)
{
}

ompl::control::LTLPlanner::Motion::Motion(const SpaceInformation* si) :
    state(si->allocState()),
    control(si->allocControl()),
    parent(NULL),
    steps(0)
{
}

ompl::control::LTLPlanner::Motion::~Motion(void)
{
}

ompl::control::LTLPlanner::ProductGraphStateInfo::ProductGraphStateInfo(void) :
    numSel(0),
    pdfElem(NULL)
{
}

void ompl::control::LTLPlanner::ProductGraphStateInfo::addMotion(Motion* m)
{
    motionElems[m] = motions.add(m, 1.);
}

double ompl::control::LTLPlanner::updateWeight(ProductGraph::State* as)
{
    ProductGraphStateInfo& info = abstractInfo_[as];
    /* TODO weight should include freeVolume, for cases in which decomposition
       does not respect obstacles. */
    info.weight = ((info.motions.size()+1)*info.volume) / (info.autWeight*(info.numSel+1)*(info.numSel+1));
    return info.weight;
}

void ompl::control::LTLPlanner::initAbstractInfo(ProductGraph::State* as)
{
    ProductGraphStateInfo& info = abstractInfo_[as];
    info.numSel = 0;
    info.pdfElem = NULL;
	info.volume = abstraction_->getRegionVolume(as);
	unsigned int autDist = std::max(abstraction_->getCosafeAutDistance(as),
		abstraction_->getSafeAutDistance(as));
    //TODO try something larger than epsilon
    if (autDist == 0)
        info.autWeight = std::numeric_limits<double>::epsilon();
    else
        info.autWeight = autDist;
    info.weight = info.volume/info.autWeight;
}

void ompl::control::LTLPlanner::buildAvail(const std::vector<ProductGraph::State*>& lead)
{
    for (unsigned int i = 0; i < availDist_.size(); ++i)
        abstractInfo_[availDist_[i]].pdfElem = NULL;
    availDist_.clear();
    unsigned int numTreePts = 1;
    for (int i = lead.size()-1; i >= 0; --i)
    {
        ProductGraph::State* as = lead[i];
        ProductGraphStateInfo& info = abstractInfo_[as];
        if (!info.motions.empty())
        {
            info.pdfElem = availDist_.add(as, info.weight);
            numTreePts += info.motions.size();
            if (rng_.uniform01() < 0.5)
                break;
        }
    }
}

bool ompl::control::LTLPlanner::explore(const std::vector<ProductGraph::State*>& lead, Motion*& soln, double duration)
{
    bool solved = false;
    base::PlannerTerminationCondition ptc = base::timedPlannerTerminationCondition(duration);
    base::GoalPtr goal = pdef_->getGoal();
    while (!ptc() && !solved)
    {
        ProductGraph::State* as = availDist_.sample(rng_.uniform01());
        ++abstractInfo_[as].numSel;
        updateWeight(as);

        PDF<Motion*>& motions = abstractInfo_[as].motions;
        Motion* v = motions.sample(rng_.uniform01());
        PDF<Motion*>::Element* velem = abstractInfo_[as].motionElems[v];
        double vweight = motions.getWeight(velem);
        if (vweight > 1e-20)
            motions.update(velem, vweight/(vweight+1.));

        Control* rctrl = ltlsi_->allocControl();
        controlSampler_->sampleNext(rctrl, v->control, v->state);
        unsigned int cd = controlSampler_->sampleStepCount(ltlsi_->getMinControlDuration(), ltlsi_->getMaxControlDuration());

        base::State* newState = si_->allocState();
        cd = ltlsi_->propagateWhileValid(v->state, rctrl, cd, newState);
        if (cd < ltlsi_->getMinControlDuration())
        {
            si_->freeState(newState);
            ltlsi_->freeControl(rctrl);
			continue;
        }
        Motion* m = new Motion();
        m->state = newState;
        m->control = rctrl;
        m->steps = cd;
        m->parent = v;
        // Since the state was determined to be valid by SpaceInformation, we don't need to check automaton states
        m->abstractState = ltlsi_->getProdGraphState(m->state);
        motions_.push_back(m);

        abstractInfo_[m->abstractState].addMotion(m);
        updateWeight(m->abstractState);
        // update weight if hl state already exists in avail
        if (abstractInfo_[m->abstractState].pdfElem != NULL)
            availDist_.update(abstractInfo_[m->abstractState].pdfElem, abstractInfo_[m->abstractState].weight);
        else
        {
            // otherwise, only add hl state to avail if it already exists in lead
			if (std::find(lead.begin(), lead.end(), m->abstractState) != lead.end())
            {
                PDF<ProductGraph::State*>::Element* elem = availDist_.add(m->abstractState, abstractInfo_[m->abstractState].weight);
                abstractInfo_[m->abstractState].pdfElem = elem;
            }
        }

        solved = goal->isSatisfied(m->state);
        if (solved)
        {
            soln = m;
            break;
        }
    }
    return solved;
}

double ompl::control::LTLPlanner::abstractEdgeWeight(ProductGraph::State* a, ProductGraph::State* b) const
{
    const ProductGraphStateInfo& infoA = abstractInfo_.find(a)->second;
    const ProductGraphStateInfo& infoB = abstractInfo_.find(b)->second;
    return 1./(infoA.weight * infoB.weight);
}

void ompl::control::LTLPlanner::clearMotions(void)
{
    availDist_.clear();
    for (std::vector<Motion*>::iterator i = motions_.begin(); i != motions_.end(); ++i)
    {
        Motion* m = *i;
        if (m->state != NULL)
            si_->freeState(m->state);
        if (m->control != NULL)
            ltlsi_->freeControl(m->control);
        delete m;
    }
    motions_.clear();
    pis_.clear();
    pis_.update();
}