{-# LANGUAGE BangPatterns #-}
{-|
Module      : Geometry
Description : Geometry helpers

This module provides geometry functions that manipulate pairs of floats.
Conventions: 
Seg refers to a segment, typically defined by two points, and will typically not extend beyond either of these points.
Line refers to a line defined by two points, and extends beyond the two points.
-}
module Geometry
    ( module Geometry
    , module Geometry.Data
    , module Geometry.Intersect
    , module Geometry.Bezier
    , module Geometry.Vector
    )
    where
import Geometry.Data
import Geometry.Intersect
import Geometry.Bezier
import Geometry.Vector

import Data.Function
import Data.List
import Data.Maybe

import Control.Applicative

-- | Return a point a distance away from a first point towards a second point.
-- Does not go past the second point.
alongSegBy :: Float -> Point2 -> Point2 -> Point2
alongSegBy !x !a !b = a +.+ y *.* normalizeV (b -.- a)
  where
    y = min x $ dist a b

-- | Given a line and a point return the point on the line closest to the
-- point.
closestPointOnLine 
    :: Point2 -- ^ First line point. 
    -> Point2 -- ^ Second line point.
    -> Point2 -- ^ Point not on line.
    -> Point2
{-# INLINE closestPointOnLine #-}
closestPointOnLine !a  !b  !p  = a +.+ u *.* (b -.- a)
  where u = closestPointOnLineParam a b p

-- | Given a line and a point return a value corresponding to how far along the
-- line the point is.
closestPointOnLineParam 
    :: Point2   -- ^ First line point. 
    -> Point2   -- ^ Second line point.
    -> Point2   -- ^ Point not on line.
    -> Float
{-# INLINE closestPointOnLineParam #-}
closestPointOnLineParam !a !b !p
    = (p -.- a) `dotV` (b -.- a) / (b -.- a) `dotV` (b -.- a)

-- | Draw a rectangle based on maximal N E S W values.
rectNESW :: Float -> Float -> Float -> Float -> [Point2]
rectNESW !a !b !c !d = [(b,a),(b,c),(d,c),(d,a) ]

-- | Draw a rectangle based on maximal N S E W values.
rectNSEW :: Float -> Float -> Float -> Float -> [Point2]
rectNSEW !n !s !e !w = rectNESW n e s w

-- | Draw a rectangle based on maximal N S W E values.
rectNSWE :: Float -> Float -> Float -> Float -> [Point2]
rectNSWE !n !s !w !e = [ (w,n), (w,s), (e,s), (e,n)]

-- | Test whether a point is in a polygon or on the polygon border.
-- Supposes the points in the
-- polygon are listed in anticlockwise order.
pointInOrOnPolygon :: Point2 -> [Point2] -> Bool
pointInOrOnPolygon !p (x:xs) = all (\l -> not (uncurry isRHS l p)) $ zip (x:xs) (xs ++ [x])

-- | Test whether a point is strictly inside a polygon.
-- Supposes the points in the polygon are listed in anticlockwise order.
pointInPolygon :: Point2 -> [Point2] -> Bool
pointInPolygon !p [] = False
pointInPolygon !p (x:xs) = all (\l -> uncurry (errorIsLHS 1) l p) $ zip (x:xs) (xs ++ [x])

-- | Debug version of 'pointInPolygon'.
errorPointInPolygon :: Int -> Point2 -> [Point2] -> Bool
errorPointInPolygon !i !p xs 
    | length xs == 1 = error "one point polygon"
    | length xs == 2 = error "two point polygon"
    | nub xs == xs = pointInPolygon p xs
    | otherwise = error $ "errorPointInPolygon "++ show i

-- | Debug version of 'normalizeV'.
errorNormalizeV :: Int -> Point2 -> Point2
errorNormalizeV !i !(0,0) = error $ "problem with function: errorNormalizeV "++show i
errorNormalizeV !i !p = normalizeV p

-- | Debug version of 'angleVV'.
errorAngleVV :: Int -> Point2 -> Point2 -> Float
errorAngleVV !i !(0,0) _ = error $ "problem with function: errorAngleVV "++show i
errorAngleVV !i _ !(0,0) = error $ "problem with function: errorAngleVV "++show i
errorAngleVV !i !p !p' = angleVV p p'

-- | Debug version of 'isLHS'.
errorIsLHS :: Int -> Point2 -> Point2 -> Point2 -> Bool
errorIsLHS !i !x !y 
    | x == y = error $ "problem with function: errorIsLHS " ++show i
    | otherwise = isLHS x y

-- | Debug version of 'closestPointOnLine'
errorClosestPointOnLine :: Int -> Point2 -> Point2 -> Point2 -> Point2
errorClosestPointOnLine !i !x !y 
    | x == y = error $ "problem with function: errorClosestPointOnLine " ++show i
    | otherwise = closestPointOnLine x y

-- | Debug version of 'closestPointOnLineParam'
errorClosestPointOnLineParam :: Int -> Point2 -> Point2 -> Point2 -> Float
errorClosestPointOnLineParam  !i !x! y! z 
    | x == y    = dist x z
    | otherwise = closestPointOnLineParam  x y z

-- | Normalize a vector to be unit length.
-- For (0,0) return (0,0).
safeNormalizeV :: Point2 -> Point2
safeNormalizeV !(0,0) = (0,0)
safeNormalizeV !p     = normalizeV p

-- | Test whether a point is on the LHS of a line.
-- Returns False if the line is of zero length.
isLHS 
    :: Point2  -- ^ First line point. 
    -> Point2  -- ^ Second line point.
    -> Point2  -- ^ Point not on line.
    -> Bool
{-# INLINE isLHS #-}
isLHS 
    !(x,y)      
    !(x',y')   
    !(x'',y'') 
    | (x,y) == (x',y')  = False
    | otherwise = a1 * b2 - a2 * b1 > 0
  where 
    a1 = x' - x
    a2 = y' - y
    b1 = x'' - x
    b2 = y'' - y

-- | Test whether a point is on the LHS of a line.
-- Returns False if the line is of zero length.
isRHS 
    :: Point2 -- ^ First line point. 
    -> Point2 -- ^ Second line point.
    -> Point2 -- ^ Point not on line.
    -> Bool
{-# INLINE isRHS #-}
isRHS 
    !(x,y)       
    !(x',y')    
    !(x'',y'')  
    | (x,y) == (x',y')  = False
    | otherwise = a1 * b2 - a2 * b1 < 0
  where 
    a1 = x' - x
    a2 = y' - y
    b1 = x'' - x
    b2 = y'' - y

-- | Reorder points to be anticlockwise around their center.
orderPolygon :: [Point2] -> [Point2]
orderPolygon [] = []
orderPolygon ps = sortBy (compare `on` \p -> argV (p -.- cen)) ps
  where 
    cen = 1/ fromIntegral (length ps) *.* foldr1 (+.+) ps

-- | Return distance between two points.
dist :: Point2 -> Point2 -> Float
{-# INLINE dist #-}
dist !p1 !p2 = magV (p2 -.- p1)

-- | Return midpoint between two points.
pHalf :: Point2 -> Point2 -> Point2
pHalf !a !b = 0.5 *.* (a +.+ b)

-- | Test whether a circle is on a segment by intersecting a new normal segment through the 
-- center of the circle with the segment itself.
-- Returns False if the circle center is beyond the enpoints of the
-- segment.
circOnSegNoEndpoints :: Point2 -> Point2 -> Point2 -> Float -> Bool
{-# INLINE circOnSegNoEndpoints #-}
circOnSegNoEndpoints !p1 !p2 !c !rad = isJustTrue (fmap (\p -> magV (p -.- c) < rad) y)
  where 
    y = intersectSegLine' p1 p2 c (c +.+ vNormal (p1 -.- p2))
    isJustTrue (Just True) = True
    isJustTrue _           = False

-- | Test whether a circle is on a segment by intersecting a normal and testing
-- the distance to the endpoints of the segment.
circOnSeg :: Point2 -> Point2 -> Point2 -> Float -> Bool
{-# INLINE circOnSeg #-}
circOnSeg !p1 !p2 !c !rad = magV (p1 -.- c) <= rad || magV (p2 -.- c) <= rad
                             || isJustTrue (fmap (\p -> magV (p -.- c) < rad) y)
  where 
    y = intersectSegLine' p1 p2 c (c +.+ vNormal (p1 -.- p2))
    isJustTrue (Just True) = True
    isJustTrue _           = False

-- | Find the difference between two Nums.
difference :: (Ord a, Num a) => a -> a -> a
difference x y 
    | x > y     = x - y
    | otherwise = y - x

-- | Given vector line direction and a vector movement,
-- reflects the movement accoring to the line.
reflectIn :: Point2 -> Point2 -> Point2
reflectIn line vec = 
    let angle = 2 * angleBetween line vec
    in  rotateV angle vec

-- | Find angle between two points.
-- Not normalised, ranges from -2*pi to 2*pi.
angleBetween :: Point2 -> Point2 -> Float
angleBetween v1 v2 = argV v1 - argV v2

-- | Return a list containing two copies of a pair.
doublePair :: (a,a) -> [(a,a)]
doublePair (x,y) = [(x,y),(y,x)]

-- | Test whether two polygons intersect by testing the intersection of each
-- consecutive pair of points.
polysIntersect :: [Point2] -> [Point2] -> Bool
polysIntersect (p:ps) (q:qs) 
    = any isJust $ (\(a,b) (c,d) -> myIntersectSegSeg a b c d) <$> pairs1 <*> pairs2
  where 
    pairs1 = zip (p:ps) (ps++[p])
    pairs2 = zip (q:qs) (qs++[q])

polysIntersect [] _ = False
polysIntersect _ [] = False

-- | Test whether any polygons from a first list intersect with any polygons from
-- a second list.
anyPolyssIntersect :: [[Point2]] -> [[Point2]] -> Bool
anyPolyssIntersect x y = or $ polysIntersect <$> x <*> y

-- | Return n equidistant points on a circle with a radius of 600.
nRays :: Int -> [Point2]
nRays n = take n $ iterate (rotateV (2*pi/fromIntegral n)) (600,0)

-- | Return n equidistant points on a circle with a radius of x.
nRaysRad :: Int -> Float -> [Point2]
nRaysRad n x = take n $ iterate (rotateV (2*pi/fromIntegral n)) (x,0)

-- | Test whether an angle is to the left of another angle, according to the
-- smallest change in rotation between them.
isLeftOfA :: Float -> Float -> Bool
isLeftOfA angle1 angle2 = 
    (angle1 - angle2 < pi && angle1 > angle2)
    || (angle2 - angle1 > pi && angle2 > angle1)

-- | Test whether a vector is to the left of another, according to the smallest
-- change of rotation between them.
isLeftOf :: Point2 -> Point2 -> Bool
isLeftOf x y = isLeftOfA (argV x) (argV y)

-- | Find the difference between two angles.
-- Possibly not correct...
diffAngles :: Float -> Float -> Float
diffAngles x y 
    | diff > pi  = diffAngles (x - 2*pi) y
    | diff >= 0  = diff
    | diff > -pi = -diff
    | otherwise  = diffAngles (x + 2*pi) y
  where 
    diff = x-y

differenceAngles = diffAngles
angleDifference = diffAngles

-- | Given a triangle where we know the length of a first side,
-- the length of a second side, and the angle between the first side and the
-- third side, finds the length of the third side.
-- Note this doesn't necessarily find ALL solutions, asin is a map not a function.
ssaTri :: Float -> Float -> Float -> Float
ssaTri ab bc a 
    | sin a == 0 = 0
    | bc == 0    = ab
    | otherwise = 
        let c = asin ( (ab * sin a)/bc)
            b = pi - (a + c)
        in  sin b * bc / sin a

-- | Given two points of a triangle and a third point, return
-- the point which lies between pa and pc' on a line from pb of length bc.
-- Note that there are likely two such points, this should return the point
-- closer to pc'.
ssaTriPoint :: Point2 -> Point2 -> Point2 -> Float -> Point2
ssaTriPoint pa pb pc' bc
    = let ab = magV (pa -.- pb)
          a  = errorAngleVV 6 (pb -.- pa) (pc' -.- pa)
          ac = ssaTri ab bc a
      in  pa +.+ (ac *.* errorNormalizeV 47 (pc' -.- pa))

-- | Safe version of 'ssaTriPoint'.
ssaTriPoint' :: Point2 -> Point2 -> Point2 -> Float -> Maybe Point2
ssaTriPoint' pa pb pc' bc
    | dist pb (closestPointOnSeg pa pc' pb) >= bc
        = Nothing
    | otherwise
        = Just $ ssaTriPoint pa pb pc' bc
-- | A potential correction of 'ssaTriPoint'.
-- This should be tested and benchmarked.
ssaTriPointCorrect :: Point2 -> Point2 -> Point2 -> Float -> Maybe Point2
ssaTriPointCorrect pa pb pc' bc
    | param <= 1 && param >= 0 = Just p
    | otherwise                = Nothing
  where 
    p = ssaTriPoint pa pb pc' bc
    param = closestPointOnLineParam pa pc' p

-- | Given a segment and external point, find the closest point on the segment.
closestPointOnSeg :: Point2 -> Point2 -> Point2 -> Point2
closestPointOnSeg segP1 segP2 p
    | errorClosestPointOnLineParam 3 segP1 segP2 p <= 0 = segP1
    | errorClosestPointOnLineParam 4 segP1 segP2 p >= 1 = segP2
    | otherwise                                  = errorClosestPointOnLine 2 segP1 segP2 p

-- | Return Just a point if it is inside a circle, Nothing otherwise.
pointInCircle :: Point2 -> Float -> Point2 -> Maybe Point2
pointInCircle p r c 
    | p == c = Just p
    | magV (p -.- c) < r = Just p
    | otherwise = Nothing

-- | Determines if a moving point intersects with a circle,
-- if so, returns a point on circle that intersects with the line passing
-- throught the circle : HOPEFULLY THE CORRECT OF THE TWO!
collidePointCirc :: Point2 -> Point2 -> Float -> Point2 -> Maybe Point2
collidePointCirc p1 p2 rad c = ssaTriPoint' p2 c p1 rad

-- | As 'collidePointCirc', but changes the point to a measure of the distance.
collidePointCirc' :: Point2 -> Point2 -> Float -> Point2 -> Maybe Float
collidePointCirc' p1 p2 rad c = fmap (\x -> magV (x -.- p1))
                                    (collidePointCirc p1 p2 rad c)

-- | As 'collidePointCirc', but returns both the point and the measure of the distance.
collidePointCirc'' :: Point2 -> Point2 -> Float -> Point2 -> Maybe (Point2,Float)
collidePointCirc'' p1 p2 rad c = (,) <$> collidePointCirc p1 p2 rad c
                                     <*>  collidePointCirc' p1 p2 rad c
                                     
-- | As 'collidePointCirc', but uses the supposedly correct version of ssaTriPoint.
collidePointCircCorrect :: Point2 -> Point2 -> Float -> Point2 -> Maybe Point2
collidePointCircCorrect p1 p2 rad c = ssaTriPointCorrect p2 c p1 rad

-- | Finds the height of a triangle using herons formula.
-- The base is the line between the first two points.
heron :: Point2 -> Point2 -> Point2 -> Float
heron x y z 
    | x == y = 0
    | otherwise = 
        let a = magV $ x -.- y
            b = magV $ y -.- z
            c = magV $ z -.- x
            s = (a+b+c)/2
            area = sqrt(s*(s-a)*(s-b)*(s-c))
        in  2*area/a

-- | Multiplies reflection in normal by factor.
reflectInParam :: Float -> Point2 -> Point2 -> Point2
reflectInParam x line vec = 
    let angle = 2 * angleBetween line vec
        rAng = rotateV angle vec
        p = x *.* errorClosestPointOnLine 3 (0,0) (vNormal line) rAng
    in  rAng -.- p

--reflectIn' :: Point2 -> Point2 -> Point2 -> Point2 -> Point2
--reflectIn' l1 l2 v1 v2 = v1 +.+ reflectIn (l1 -.- l2) (v2 -.- v1)

--isOnSeg :: Point2 -> Point2 -> Point2 -> Bool
--isOnSeg l1 l2 p = 
--    errorClosestPointOnLineParam 10 l1 (l1 +.+ vNormal (l2 -.- l1)) p == 0
--    && errorClosestPointOnLineParam 11 l1 l2 p <= 1
--    && errorClosestPointOnLineParam 12 l1 l2 p >= 0

-- | Divide a segment into a list of points with a maximal distance between
-- them.
-- the take 5000 here is a hack, otherwise divideLine seems to sometimes
-- generate an infinite list, and I don't know why
divideLine :: Float -> Point2 -> Point2 -> [Point2]
--divideLine x a b = map (\i -> a +.+ (i / (fromIntegral numPoints)) *.* (b -.- a)) 
divideLine x a b = 
    take 5000 
    $ map (\i -> a +.+ (fromIntegral i / fromIntegral numPoints *.* (b -.- a)) )
          ns
  where 
    d = dist a b
    numPoints = max 1 $ ceiling $ d / x
    ns = [0 .. numPoints]

-- | As 'divideLine', but must return an odd number of points.
divideLineOddNumPoints :: Float -> Point2 -> Point2 -> [Point2]
--divideLine x a b = map (\i -> a +.+ (i / (fromIntegral numPoints)) *.* (b -.- a)) 
divideLineOddNumPoints x a b = take 5000 
    $ map (\i -> a +.+ (fromIntegral i / fromIntegral numPoints *.* (b -.- a)) )
          ns
  where 
    d = dist a b
    numPoints' = max 1 $ ceiling $ d / x
    numPoints | even numPoints' = numPoints'
              | otherwise       = numPoints' + 1
    ns = [0 .. numPoints]

-- | Given two pairs of Ints, returns a list of pairs of Ints that form 
-- a digital line between them.
digitalLine :: (Int,Int) -> (Int,Int) -> [(Int,Int)]
digitalLine (x1,y1) (x2,y2) 
    | abs (x1-x2) > abs (y1-y2) = [ (x,( (y1-y2) * x + x1*y2 - x2*y1) `rdiv` (x1-x2) ) 
                                    | x <- intervalList x1 x2 ]
    | otherwise                 = [ ( ((x1-x2) * y + y1*x2 - y2*x1) `rdiv` (y1-y2)  , y)
                                    | y <- intervalList y1 y2 ]
  where 
    rdiv a b = round $ fromIntegral a / fromIntegral b

-- | Given two Ints, creates the list of Ints between these.
intervalList :: Int -> Int -> [Int]
intervalList x y 
    | y >= x = [x .. y]
    | otherwise = reverse [y..x]

-- | Create points on the circumference of a circle with maximal distance
-- between them.
divideCircle :: Float -> Point2 -> Float -> [Point2]
divideCircle x cen rad = map (cen +.+) $ nRaysRad n rad
  where 
    n = ceiling $ rad * 2 * pi / x

--nPointsOnCirc :: Int -> Float -> [Point2]
--nPointsOnCirc n rad = take n $ iterate (rotateV (2*pi/fromIntegral n)) (rad,0)

--lineInPolygon :: Point2 -> Point2 -> [Point2] -> Bool
--lineInPolygon a b ps = 
--    pointInPolygon a ps 
--    || pointInPolygon b ps 
--    || any (isJust . uncurry (intersectSegSeg' a b)) pss
--  where 
--    pss = zip ps (tail ps ++ [head ps])

-- | Given a list of points, returns pairs of points linking the points into a
-- loop.
makeLoopPairs :: [Point2] -> [(Point2,Point2)]
makeLoopPairs [] = error "tried to make loop with empty list of points"
makeLoopPairs [x] = error "tried to make loop with singleton list of points"
makeLoopPairs (x:xs) = zip (x:xs) (xs ++ [x])

-- | Test whether a point is in a cone.
-- Note the pair is ordered.
-- Doesn't work for obtuse angles.
pointIsInCone 
    :: Point2           -- ^ Cone point. 
    -> (Point2,Point2)  -- ^ Points delimiting the left and right boundaries of the cone.
    -> Point2           -- ^ Point to test.
    -> Bool
pointIsInCone c (rightp,leftp) p = isLHS c rightp p && isLHS leftp c p