#############################################################################
#
# Copyright (c) 2010 by Casey Duncan and contributors
# All Rights Reserved.
#
# This software is subject to the provisions of the MIT License
# A copy of the license should accompany this distribution.
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
#
#############################################################################
"""
**Grease collision detection systems**

Grease uses two-phase broad and narrow collision detection. *Broad-phase*
collision systems are used to efficiently identify pairs that may be colliding
without resorting to a brute-force check of all possible pairs. *Narrow-phase*
collision systems use the pairs generated by the broad-phase and perform more
precise collision tests to determine if a collision has actually occurred. The
narrow-phase system also calculates more details about each collision,
including collision point and normal vector for use in collision response.

A typical collision detection system consists of a narrow-phase system that
contains a broad-phased system. The narrow-phase system is usually the only

one that the application directly interacts with, though the application is
free to use the broad-phased system directly if desired. This could be
useful in cases where speed, rather than precision is paramount.

The narrow-phase system can be assigned handler objects to run after
collision detection. These can perform tasks like handling collision response
or dispatching collision events to application handlers.

Note that broad-phase systems can return false positives, though they should
never return false negatives. Do not assume that all pairs returned by a
broad-phase system are actually in collision.
"""

__version__ = '$Id$'

from parpg.bGrease.geometry import Vec2d
from bisect import bisect_right


class Pair(tuple):
        """Pair of entities in collision. This is an ordered sequence of two
        entities, that compares and hashes unordered.

        Also stores additional collision point and normal vectors
        for each entity.

        Sets of ``Pair`` objects are exposed in the ``collision_pairs``
        attribute of collision systems to indicate the entity pairs in
        collision.
        """
        info = None
        """A sequence of (entity, collision point, collision normal)
	for each entity in the pair
	"""

        def __new__(cls, entity1, entity2, point=None, normal=None):
                pair = tuple.__new__(cls, (entity1, entity2))
                return pair

        def __hash__(self):
                return hash(self[0]) ^ hash(self[1])

        def __eq__(self, other):
                other = tuple(other)
                return tuple(self) == other or (self[1], self[0]) == other

        def __repr__(self):
                return '%s%r' % (self.__class__.__name__, tuple(self))

        def set_point_normal(self, point0, normal0, point1, normal1):
                """Set the collision point and normal for both entities"""
                self.info = (
                        (self[0], point0, normal0),
                        (self[1], point1, normal1),
                )


class BroadSweepAndPrune(object):
        """2D Broad-phase sweep and prune bounding box collision detector

        This algorithm is efficient for collision detection between many
        moving bodies. It has linear algorithmic complexity and takes
        advantage of temporal coherence between frames. It also does
        not suffer from bad worst-case performance (like RDC can). 
        Unlike spacial hashing, it does not need to be optimized for 
        specific space and body sizes.

        Other algorithms may be more efficient for collision detection with
        stationary bodies, bodies that are always evenly distributed, or ad-hoc
        queries.

        :param collision_component: Name of the collision component used by this
        	system, defaults to 'collision'. This component supplies each
        	entities' aabb and collision masks.
        :type collision_component: str
        """
        world = None
        """|World| object this system belongs to"""

        collision_component = None
        """Name of world's collision component used by this system"""

        LEFT_ATTR = "left"
        RIGHT_ATTR = "right"
        TOP_ATTR = "top"
        BOTTOM_ATTR = "bottom"

        def __init__(self, collision_component='collision'):
                self.collision_component = collision_component
                self._by_x = None
                self._by_y = None
                self._collision_pairs = None

        def set_world(self, world):
                """Bind the system to a world"""
                self.world = world

        def step(self, dt):
                """Update the system for this time step, updates and sorts the 
                axis arrays.
                """
                component = getattr(self.world.components, self.collision_component)
                LEFT = self.LEFT_ATTR
                RIGHT = self.RIGHT_ATTR
                TOP = self.TOP_ATTR
                BOTTOM = self.BOTTOM_ATTR
                if self._by_x is None:
                        # Build axis lists from scratch
                        # Note we cache the box positions here
                        # so that we can perform hit tests efficiently
                        # it also isolates us from changes made to the 
                        # box positions after we run
                        by_x = self._by_x = []
                        append_x = by_x.append
                        by_y = self._by_y = []
                        append_y = by_y.append
                        for data in component.itervalues():
                                append_x([data.aabb.left, LEFT, data])
                                append_x([data.aabb.right, RIGHT, data])
                                append_y([data.aabb.bottom, BOTTOM, data])
                                append_y([data.aabb.top, TOP, data])
                else:
                        by_x = self._by_x
                        by_y = self._by_y
                        removed = []
                        for entry in by_x:
                                entry[0] = getattr(entry[2].aabb, entry[1])
                        for entry in by_y:
                                entry[0] = getattr(entry[2].aabb, entry[1])
                        # Removing entities is inefficient, but expected to be rare
                        if component.deleted_entities:
                                deleted_entities = component.deleted_entities
                                deleted_x = []
                                deleted_y = []
                                for i, (_, _, data) in enumerate(by_x):
                                        if data.entity in deleted_entities:
                                                deleted_x.append(i)
                                deleted_x.reverse()
                                for i in deleted_x:
                                        del by_x[i]
                                for i, (_, _, data) in enumerate(by_y):
                                        if data.entity in deleted_entities:
                                                deleted_y.append(i)
                                deleted_y.reverse()
                                for i in deleted_y:
                                        del by_y[i]
                        # Tack on new entities
                        for entity in component.new_entities:
                                data = component[entity]
                                by_x.append([data.aabb.left, LEFT, data])
                                by_x.append([data.aabb.right, RIGHT, data])
                                by_y.append([data.aabb.bottom, BOTTOM, data])
                                by_y.append([data.aabb.top, TOP, data])

                # Tim-sort is highly efficient with mostly sorted lists.
                # Because positions tend to change little each frame
                # we take advantage of this here. Obviously things are
                # less efficient with very fast moving, or teleporting entities
                by_x.sort()
                by_y.sort()
                self._collision_pairs = None

        @property
        def collision_pairs(self):
                """Set of candidate collision pairs for this timestep"""
                if self._collision_pairs is None:
                        if self._by_x is None:
                                # Axis arrays not ready
                                return set()

                        LEFT = self.LEFT_ATTR
                        RIGHT = self.RIGHT_ATTR
                        TOP = self.TOP_ATTR
                        BOTTOM = self.BOTTOM_ATTR
                        # Build candidates overlapping along the x-axis
                        component = getattr(self.world.components, self.collision_component)
                        xoverlaps = set()
                        add_xoverlap = xoverlaps.add
                        discard_xoverlap = xoverlaps.discard
                        open = {}
                        for _, side, data in self._by_x:
                                if side is LEFT:
                                        for open_entity, (from_mask, into_mask) in open.iteritems():
                                                if data.from_mask & into_mask or from_mask & data.into_mask:
                                                        add_xoverlap(Pair(data.entity, open_entity))
                                        open[data.entity] = (data.from_mask, data.into_mask)
                                elif side is RIGHT:
                                        del open[data.entity]

                        if len(xoverlaps) <= 10 and len(xoverlaps)*4 < len(self._by_y):
                                # few candidates were found, so just scan the x overlap candidates
                                # along y. This requires an additional sort, but it should
                                # be cheaper than scanning everyone and its simpler
                                # than a separate brute-force check
                                entities = set([entity for entity, _ in xoverlaps] 
                                               + [entity for _, entity in xoverlaps])
                                by_y = []
                                for entity in entities:
                                        data = component[entity]
                                        # We can use tuples here, which are cheaper to create
                                        by_y.append((data.aabb.bottom, BOTTOM, data))
                                        by_y.append((data.aabb.top, TOP, data))
                                by_y.sort()
                        else:
                                by_y = self._by_y

                        # Now check the candidates along the y-axis
                        open = set()
                        add_open = open.add
                        discard_open = open.discard
                        self._collision_pairs = set()
                        add_pair = self._collision_pairs.add
                        for _, side, data in by_y:
                                if side is BOTTOM:
                                        for open_entity in open:
                                                pair = Pair(data.entity, open_entity)
                                                if pair in xoverlaps:
                                                        discard_xoverlap(pair)
                                                        add_pair(pair)
                                                        if not xoverlaps:
                                                                # No more candidates, bail
                                                                return self._collision_pairs
                                        add_open(data.entity)
                                elif side is TOP:
                                        discard_open(data.entity)
                return self._collision_pairs

        def query_point(self, x_or_point, y=None, from_mask=0xffffffff):
                """Hit test at the point specified. 

                :param x_or_point: x coordinate (float) or sequence of (x, y) floats.

                :param y: y coordinate (float) if x is not a sequence

                :param from_mask: Bit mask used to filter query results. This value
                	is bit ANDed with candidate entities' ``collision.into_mask``.
                	If the result is non-zero, then it is considered a hit. By
                	default all entities colliding with the input point are
                	returned.

                :return: A set of entities where the point is inside their bounding
                	boxes as of the last time step.
                """
                if self._by_x is None:
                        # Axis arrays not ready
                        return set()
                if y is None:
                        x, y = x_or_point
                else:
                        x = x_or_point
                LEFT = self.LEFT_ATTR
                RIGHT = self.RIGHT_ATTR
                TOP = self.TOP_ATTR
                BOTTOM = self.BOTTOM_ATTR
                x_index = bisect_right(self._by_x, [x])
                x_hits = set()
                add_x_hit = x_hits.add
                discard_x_hit = x_hits.discard
                if x_index <= len(self._by_x) // 2:
                        # closer to the left, scan from left to right
                        while (x == self._by_x[x_index][0] 
                               and self._by_x[x_index][1] is LEFT 
                               and x_index < len(self._by_x)):
                                # Ensure we hit on exact left edge matches
                                x_index += 1
                        for _, side, data in self._by_x[:x_index]:
                                if side is LEFT and from_mask & data.into_mask:
                                        add_x_hit(data.entity)
                                else:
                                        discard_x_hit(data.entity)
                else:
                        # closer to the right
                        for _, side, data in reversed(self._by_x[x_index:]):
                                if side is RIGHT and from_mask & data.into_mask:
                                        add_x_hit(data.entity)
                                else:
                                        discard_x_hit(data.entity)
                if not x_hits:
                        return x_hits

                y_index = bisect_right(self._by_y, [y])
                y_hits = set()
                add_y_hit = y_hits.add
                discard_y_hit = y_hits.discard
                if y_index <= len(self._by_y) // 2:
                        # closer to the bottom
                        while (y == self._by_y[y_index][0] 
                               and self._by_y[y_index][1] is BOTTOM 
                               and y_index < len(self._by_y)):
                                # Ensure we hit on exact bottom edge matches
                                y_index += 1
                        for _, side, data in self._by_y[:y_index]:
                                if side is BOTTOM:
                                        add_y_hit(data.entity)
                                else:
                                        discard_y_hit(data.entity)
                else:
                        # closer to the top
                        for _, side, data in reversed(self._by_y[y_index:]):
                                if side is TOP:
                                        add_y_hit(data.entity)
                                else:
                                        discard_y_hit(data.entity)
                if y_hits:
                        return x_hits & y_hits
                else:
                        return y_hits


class Circular(object):
        """Basic narrow-phase collision detector which treats all entities as
        circles with their radius defined in the collision component.

        :param handlers: A sequence of collision handler functions that are invoked
        	after collision detection.
        :type handlers: sequence of functions

        :param collision_component: Name of collision component for this system,
        	defaults to 'collision'. This supplies each entity's collision
        	radius and masks.
        :type collision_component: str

        :param position_component: Name of position component for this system,
        	defaults to 'position'. This supplies each entity's position.
        :type position_component: str

        :param update_aabbs: If True (the default), then the entities'
        	`collision.aabb` fields will be updated using their position
        	and collision radius before invoking the broad phase system. 
        	Set this False if another system updates the aabbs.
        :type update_aabbs: bool

        :param broad_phase: A broad-phase collision system to use as a source
        	for collision pairs. If not specified, a :class:`BroadSweepAndPrune`
        	system will be created automatically.
        """
        world = None
        """|World| object this system belongs to"""

        position_component = None
        """Name of world's position component used by this system"""

        collision_component = None
        """Name of world's collision component used by this system"""

        update_aabbs = True
        """Flag to indicate whether the system updates the entities' `collision.aabb`
	field before invoking the broad phase collision system
	"""

        handlers = None
        """A sequence of collision handler functions invoke after collision
	detection
	"""

        broad_phase = None
        """Broad phase collision system used as a source for collision pairs"""

        def __init__(self, handlers=(), position_component='position', 
                     collision_component='collision', update_aabbs=True, broad_phase=None):
                self.handlers = tuple(handlers)
                if broad_phase is None:
                        broad_phase = BroadSweepAndPrune(collision_component)
                self.collision_component = collision_component
                self.position_component = position_component
                self.update_aabbs = bool(update_aabbs)
                self.broad_phase = broad_phase
                self._collision_pairs = None

        def set_world(self, world):
                """Bind the system to a world"""
                self.world = world
                self.broad_phase.set_world(world)
                for handler in self.handlers:
                        if hasattr(handler, 'set_world'):
                                handler.set_world(world)

        def step(self, dt):
                """Update the collision system for this time step and invoke
                the handlers
                """
                if self.update_aabbs:
                        for position, collision in self.world.components.join(
                                self.position_component, self.collision_component):
                                aabb = collision.aabb
                                x, y = position.position
                                radius = collision.radius
                                aabb.left = x - radius
                                aabb.right = x + radius
                                aabb.bottom = y - radius
                                aabb.top = y + radius
                self.broad_phase.step(dt)
                self._collision_pairs = None
                for handler in self.handlers:
                        handler(self)

        @property
        def collision_pairs(self):
                """The set of entity pairs in collision in this timestep"""
                if self._collision_pairs is None:
                        position = getattr(self.world.components, self.position_component)
                        collision = getattr(self.world.components, self.collision_component)
                        pairs = self._collision_pairs = set()
                        for pair in self.broad_phase.collision_pairs:
                                entity1, entity2 = pair
                                position1 = position[entity1].position
                                position2 = position[entity2].position
                                radius1 = collision[entity1].radius
                                radius2 = collision[entity2].radius
                                separation = position2 - position1
                                if separation.get_length_sqrd() <= (radius1 + radius2)**2:
                                        normal = separation.normalized()
                                        pair.set_point_normal(
                                                normal * radius1 + position1, normal,
                                                normal * -radius2 + position2, -normal)
                                        pairs.add(pair)
                return self._collision_pairs

        def query_point(self, x_or_point, y=None, from_mask=0xffffffff):
                """Hit test at the point specified. 

                :param x_or_point: x coordinate (float) or sequence of (x, y) floats.

                :param y: y coordinate (float) if x is not a sequence

                :param from_mask: Bit mask used to filter query results. This value
                	is bit ANDed with candidate entities' ``collision.into_mask``.
                	If the result is non-zero, then it is considered a hit. By
                	default all entities colliding with the input point are
                	returned.

                :return: A set of entities where the point is inside their collision
                	radii as of the last time step.

                """
                if y is None:
                        point = Vec2d(x_or_point)
                else:
                        point = Vec2d(x_or_point, y)
                hits = set()
                position = getattr(self.world.components, self.position_component)
                collision = getattr(self.world.components, self.collision_component)
                for entity in self.broad_phase.query_point(x_or_point, y, from_mask):
                        separation = point - position[entity].position
                        if separation.get_length_sqrd() <= collision[entity].radius**2:
                                hits.add(entity)
                return hits


def dispatch_events(collision_system):
        """Collision handler that dispatches `on_collide()` events to entities
        marked for collision by the specified collision system. The `on_collide()`
        event handler methods are defined by the application on the desired entity
        classes. These methods should have the following signature::

        	def on_collide(self, other_entity, collision_point, collision_normal):
        		'''Handle A collision between this entity and `other_entity`

        		- other_entity (Entity): The other entity in collision with 
        		  `self`

        		- collision_point (Vec2d): The point on this entity (`self`)
        		  where the collision occurred. Note this may be `None` for 
        		  some collision systems that do not report it.

        		- collision_normal (Vec2d): The normal vector at the point of
        		  collision. As with `collision_point`, this may be None for
        		  some collision systems.
        		'''

        Note the arguments to `on_collide()` are always passed positionally, so you
        can use different argument names than above if desired.

        If a pair of entities are in collision, then the event will be dispatched
        to both objects in arbitrary order if all of their collision masks align.
        """
        collision = getattr(collision_system.world.components, 
                            collision_system.collision_component)
        for pair in collision_system.collision_pairs:
                entity1, entity2 = pair
                if pair.info is not None:
                        args1, args2 = pair.info
                else:
                        args1 = entity1, None, None
                        args2 = entity2, None, None
                try:
                        on_collide = entity1.on_collide
                        masks_align = collision[entity2].from_mask & collision[entity1].into_mask
                except (AttributeError, KeyError):
                        pass
                else:
                        if masks_align:
                                on_collide(*args2)
                try:
                        on_collide = entity2.on_collide
                        masks_align = collision[entity1].from_mask & collision[entity2].into_mask
                except (AttributeError, KeyError):
                        pass
                else:
                        if masks_align:
                                on_collide(*args1)