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


-- | This module has types and functions for dealing with collision detection on
-- simple 2D shapes.
module Data.SG.Shape (Shape'(..), moveShape, rotateShape, scaleShape, shapePoints,
  boundingBox, overlap, intersectLineShape) where

import Control.Arrow
import Data.List
import Data.Maybe

import Data.SG.Geometry
import Data.SG.Geometry.TwoDim
import Data.SG.Matrix
import Data.SG.Vector

-- | A type for simple 2D convex shapes.  It is expected that you will define a
-- synonym in your own application such as @type Shape = Shape' Double@, hence
-- the funny name.
data Shape' a
       = Rectangle {shapeCentre :: Point2' a, rectSize :: (a, a)}
         -- ^ A rectangle with a centre, and a width (distance from the centre
         -- to the left or right side of the rectangle) and a height (distance
         -- from the centre to the top or bottom side of the rectangle.  So the
         -- rectangle with corners (1,1) and (3,2) is @Rectangle (Point2 (2,1.5))
         -- (1, 0.5)@.  Technically a rectangle is a polygon, of course, but a
         -- rectangle (which is axis-aligned) can be processed faster by most algorithms.
       | Circle {shapeCentre :: Point2' a, circSize :: a}
         -- ^ A circle with a centre and a radius.
       | Polygon {shapeCentre :: Point2' a,
           -- Points are offsets from centre (and join in loop):
           polyPoints :: [Rel2' a]}
         -- ^ A polygon with a centre, and a list of points.  The points are relative
         -- vectors from the centre of the polygon, and are expected to be in clockwise
         -- order.  For example, the triangle with corners (1,1) (3,3) and (3,1)
         -- could be @Polygon (Point2 (2.5, 1.5)) [Rel2 (-1.5,-0.5), Rel2 (0.5,1.5),
         -- Rel2 (-1.5, 1.5)]@.
         --
         -- Note that whereabouts the centre is inside the polygon is up to you
         -- (it does not /have to be/ the geometric average of the points), but
         -- it should at least be inside the polygon, or else some algorithms will
         -- behave strangely with it.
         --
         -- The list of points should have at least 3 points in it, or else some
         -- algorithms will behave strangely.
         --
         -- If your points are not in clockwise order (with the X-Y axes being
         -- how they are in graphs, not on screens), funny things will happen with
         -- the collision detection.
       deriving (Show, Read, Eq, Ord)

-- | Moves a shape by a given vector (by moving the centre).
moveShape :: Num a => Rel2' a -> Shape' a -> Shape' a
moveShape x s = s {shapeCentre = shapeCentre s `plusDir` x}

-- | Given an angle in /radians/, rotates the shape by that angle in an anti-clockwise
-- direction.  A circle will remain untouched, a polygon will have its points rotated,
-- and a rectangle will become a polygon and get rotated (even if you pass 0 as the angle).
rotateShape :: forall a. Floating a => a -> Shape' a -> Shape' a
rotateShape _ s@(Circle {}) = s
rotateShape a s@(Rectangle c _) = rotateShape a (Polygon c $ polygonPoints s)
rotateShape a (Polygon c ps) = Polygon c $ map (multMatrix mat) ps
  where
    mat :: Matrix22' a
    mat = rotateZaxis a

-- | Scales the size of the shape (for all edges, from the centre) by the given
-- factor.
scaleShape :: Num a => a -> Shape' a -> Shape' a
scaleShape a (Circle c r) = Circle c (r*a)
scaleShape a (Rectangle c (w, h)) = Rectangle c (w*a, h*a)
scaleShape a (Polygon c ps) = Polygon c $ map (scaleRel a) ps

pts :: Num a => Point2' a -> (a, a) -> (Point2' a, Point2' a)
pts (Point2 (x, y)) (adjX, adjY) = (Point2 (x - adjX, y - adjY), Point2 (x + adjX, y + adjY))

-- | Gives back the bounding box of a shape in terms of the minimum X-Y and
-- the maximum X-Y corners of the bounding box.
boundingBox :: (Num a, Ord a) => Shape' a -> (Point2' a, Point2' a)
boundingBox (Circle c r) = pts c (r, r)
boundingBox (Rectangle c (w, h)) = pts c (w, h)
boundingBox (Polygon p ps)
  = (p `plusDir` foldl (fmapNum2 min) (simpleVec 0) ps
    ,p `plusDir` foldl (fmapNum2 max) (simpleVec 0) ps)

twoFromList :: [a] -> Maybe (a, a)
twoFromList [] = Nothing
twoFromList [x] = Just (x, x)
twoFromList (x:y:_) = Just (x, y)

between :: Ord a => (a, a) -> a -> Bool
between (l, h) x = l <= x && x <= h

-- | Given a line and a shape, finds all possible intersections of the line
-- with the shape.  Since the shapes are convex, continuous 2D shapes, there
-- will either be no intersections or two (which could be the same point).
-- The returned value is distance along the line in multiples of the direction
-- vector (the return value is the same idea as 'intersectLineCircle').
intersectLineShape :: forall a. (Floating a, Ord a) => Line2' a -> Shape' a -> Maybe (a, a)
-- For circle, use existing function:
intersectLineShape l (Circle c r) = intersectLineCircle l (c, r)
-- For rectangle, use axis alignment:
intersectLineShape l (Rectangle (Point2 (x,y)) (w, h))
  = let leftE = fmap (flip alongLine l &&& id) $ valueAtX l (x-w)
        rightE = fmap (flip alongLine l &&& id) $ valueAtX l (x+w)
        topE = fmap (flip alongLine l &&& id) $ valueAtY l (y-h)
        bottomE = fmap (flip alongLine l &&& id) $ valueAtY l (y+h)
    in twoFromList $ map snd $
         (filter (between (y-h, y+h) . getY . fst) $ catMaybes [leftE, rightE])
         ++ (filter (between (x-w, x+w) . getX . fst) $ catMaybes [topE, bottomE])
-- For polygons, treat the line as a 0-length item in the perpendicular direction;
-- project all the polygon points onto that direction, and any that cross the 0-point
-- intersect.
intersectLineShape l (Polygon c ps)
  = twoFromList $ mapMaybe check $ pairsInLoop ps'
  where
    -- To translate points to the line, we must add the centre of the polygon,
    -- and subtract the start of the line:
    translate = (fmapNum2 (-) c (getLineStart l) `plusDir`)
    
    ps' = map (flip projectPointOnto2 $ id &&& perpendicular2 $ getLineDir l)
      $ map translate ps

    sc = mag $ getLineDir l

    check :: (Point2' a, Point2' a) -> Maybe a
    check (p@(Point2 (_, y)), p'@(Point2 (_, y')))
      = if signum y /= signum y'
          then fmap ((/ sc) . getX) $ pointAtY (p `lineTo` p') 0
          else Nothing

-- | Checks for overlap between the two shapes.  If they do not collide,
-- returns Nothing.  If they do collide, gives back suggested angles away from
-- each other.  These are not necessarily the shortest direction to separate
-- the two shapes, but should be decent for doing collision resolution (by using
-- them as surface normals, or push-away vectors)
--
-- The first vector returned is the direction in which the first shape should
-- head (or the surface normal to bounce the first shape off), whereas the
-- second vector returned is the direction in which the second shape should
-- head (or the surface normal to bounce the second shape off).
--
-- This function includes an initial quick test, followed by a more detailed test
-- if necessary.
overlap :: (Floating a, Ord a) => Shape' a -> Shape' a -> Maybe (Rel2' a, Rel2' a)
overlap a b
  | not (possibleOverlap a b) = Nothing
  | otherwise = detailedOverlap a b

-- | A quick test for possible intersection.
--
-- If it returns False, there is definitely no overlap.  If it returns True, there
-- might be some overlap.  For two circles, radiuses are checked (and the answer is
-- always accurate), for any other combination of shapes it checks bounding boxes.
possibleOverlap :: (Floating a, Ord a) => Shape' a -> Shape' a -> Bool
possibleOverlap (Circle ca ra) (Circle cb rb)
  = magSq (ca `fromPt` cb) <= ((ra+rb)*(ra+rb))
possibleOverlap a b
  = not $ don'tOverlap getX || don'tOverlap getY
  where
    (a1, a2) = boundingBox a
    (b1, b2) = boundingBox b
    don'tOverlap f = f a2 < f b1 || f a1 > f b2

-- Projects an already-moved shape onto that axis.  Returns a list of pairs where
-- each item of the pair also has an index for that point (for circles, this is
-- always -1).
projectShape :: (Ord a, Floating a) => Shape' a -> Rel2' a -> [(Int, a)]
projectShape (Circle c r) axis
  = let a = c `projectPointOnto` axis in [(-1,a - r), (-1, a + r)]
-- I am assuming (perhaps incorrectly) that projecting each point onto the axis
-- will be sufficient (rather than projecting each side)
projectShape (Polygon c ps) axis
  = zip [0..] $ map (((c `projectPointOnto` axis') +) . (`projectOnto` axis')) ps
  where axis' = unitVector axis
-- A rectangle has four points, all permutations of (+-w, +-h)
-- Projection is done using the dot product.  We can speed things up by calculating
-- the two components of the dot product once, then adding them in different ways
-- to achieve the projection.
projectShape (Rectangle c (w,h)) axis
  = zip [0..] $ map ((c `projectPointOnto` axis) +) [-dotx+doty,dotx+doty,dotx-doty,-dotx-doty]
  where
    dotx = w * getX (unitVector axis)
    doty = h * getY (unitVector axis)

-- All adjacent pairings, including last-first
pairsInLoop :: [a] -> [(a,a)]
pairsInLoop [] = []
pairsInLoop [_] = []
pairsInLoop xs = pairs' xs
  where
    -- all patterns are taken care of, despite what GHC thinks
    pairs' [x] = [(x, head xs)]
    pairs' (x:y:ys) = (x, y) : pairs' (y:ys)
    pairs' _ = error "Unreachable code in pairsInLoop in Shape module"

-- | Collects a list of (unit-vector) axes perpendicular to all the edges of the
-- polygon, pointed outwards.  The list will be empty for circles.
collectAxes :: (Floating a, Ord a) => Shape' a -> [Rel2' a]
collectAxes (Circle {}) = []
collectAxes (Polygon _ ps) = map unitVector [perpendicular2 (a + b) | (a,b) <- pairsInLoop ps]
collectAxes (Rectangle {}) = map (flip Rel2 1) [(-1,0), (1,0), (0, -1), (0, 1)]

-- | Given a shape, gets a list of relative vectors from the centre of the shape
-- to the points of the shape.  For polygons, this is the points list (unmodified).
--  For rectangles, it will be vectors to the four corners, and for circles, the
-- list will be empty.
polygonPoints :: Num a => Shape' a -> [Rel2' a]
polygonPoints (Circle {}) = []
polygonPoints (Rectangle _ (w, h))
  = map (flip Rel2 $ w*w + h*h) [(-w,h), (w, h), (w, -h), (-w, -h)]
polygonPoints (Polygon _ ps) = ps

-- | Given a shape, gets a list of points that make up the vertices of the
-- shape.  For circles, this list will be empty.
shapePoints :: Num a => Shape' a -> [Point2' a]
shapePoints s = map (shapeCentre s `plusDir`) (polygonPoints s)

-- | Gets a list of lines representing each side of the shape (headed clockwise).
--  For circles, the list will be empty.
polygonLines :: (Floating a) => Shape' a -> [Line2' a]
polygonLines s
  = map (uncurry lineTo)
      . pairsInLoop . map (shapeCentre s `plusDir`)
        . polygonPoints $ s
        
-- Gives back the reflected unit vector for each shape's angle away from the other.
-- returns Nothing if there was no collision after all.  You should only call this
-- if quickOverlap returned True.
detailedOverlap :: forall a. (Num a, Ord a, Floating a) => Shape' a -> Shape' a -> Maybe (Rel2' a, Rel2' a)
detailedOverlap (Circle pa _) (Circle pb _)
-- Rely on quickOverlap having been called:
  = let a_min_b = pa `fromPt` pb in Just (unitVector a_min_b, unitVector $ negate a_min_b)
-- We actually need to handle circle vs something, different than two polygons,
-- because a circle and polygon can intersect without points being contained inside
-- the other, which screws up our angle of incidence tests and so on.
--
-- We test which lines intersect the circle, and use those to form the angle of
-- incidence for the circle.  For the reverse, we just use the vector from the
-- centre of the circle to the average of the line intersections
detailedOverlap (Circle pa ra) pb
  | null intersections = Nothing
  | otherwise = Just ({- Angle from polygon -}
                 averageUnitVec $ map (perpendicular2 . getLineDir . fst) intersections
                ,{- Angle from circle -}
                 averageUnitVec $ map (`fromPt` pa)
                   $ map (uncurry $ flip alongLine) intersections
                )
  where
    intersections = filter (\(_,x) -> 0 <= x && x <= 1)
      $ concat [if a == b then [(l, a)] else [(l, a),(l, b)]
               | (l, Just (a,b)) <- map (id &&& flip intersectLineCircle (pa, ra))
          $ polygonLines pb]

detailedOverlap pa pb@(Circle {}) = fmap (\(x,y) -> (y,x)) $ detailedOverlap pb pa
-- Must be no circles now:
detailedOverlap pa pb
  = case foldl1 intersect' (map (uncurry getOverlaps) projected) of
      (aps, bps) | null aps && null bps -> Nothing
                 | otherwise ->
                    let aLines = getLineIndexes (length $ collectAxes pa) aps
                        bLines = getLineIndexes (length $ collectAxes pb) bps
                    in Just $ averageUnitVec *** averageUnitVec
                        $ unzip $ map (getPerpUnit *** getPerpUnit) $
                        map (fst *** fst) $ filter inBound $ findAllIntersections2
                          (map (polygonLines pb !!) bLines, map (polygonLines pa !!) aLines)
      
  where
    axes = collectAxes pa ++ collectAxes pb

    getPerpUnit = unitVector . perpendicular2 . (\(Line2 _ dir) -> dir)

    inBound ((_, ad), (_, bd)) = 0 <= ad && ad <= 1 && 0 <= bd && bd <= 1

    -- Given number of points, and some point indexes, gets the indexes of all
    -- the lines adjacent to those points.  If an empty list is given for the points,
    -- all line indexes are returned.
    getLineIndexes :: Int -> [Int] -> [Int]
    getLineIndexes total [] = [0 .. total - 1]
    getLineIndexes total ns = nub $ map (`mod` total) $ concatMap (\n -> [n-1,n]) ns

    projected :: [([(Int, a)], [(Int, a)])]
    projected = map (projectShape pa &&& projectShape pb) axes

    -- We can shortcut if any pair of lists involved is empty:
    intersect' :: ([Int], [Int]) -> ([Int], [Int]) -> ([Int], [Int])
    intersect' (as, bs) (cs, ds)
      | (null as && null bs) || (null cs && null ds) = ([], [])
      | otherwise = (as `intersect` cs, bs `intersect` ds)
    

    getOverlaps :: [(Int, a)] -> [(Int, a)] -> ([Int], [Int])
    getOverlaps as bs
      | maxa < minb || mina > maxb = ([], [])
      | otherwise = (map fst $ filter (overlapb . snd) as
                    ,map fst $ filter (overlapa . snd) bs)
      where
        getMinMax = minimum &&& maximum

        (mina, maxa) = getMinMax $ map snd as
        (minb, maxb) = getMinMax $ map snd bs
        overlapa x = mina <= x && x <= maxa
        overlapb x = minb <= x && x <= maxb