{-# 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
    , module Geometry.Vector3D
    , module Geometry.LHS
    , module Geometry.Polygon
    , loopPairs
    ) where
import Geometry.Data
import Geometry.Polygon
import Geometry.Intersect
import Geometry.Bezier
import Geometry.Vector
import Geometry.Vector3D
import Geometry.LHS
import ListHelp
--import Geometry.ConvexPoly

import qualified Data.Set as S
--import Data.Maybe
--import Data.List
-- | 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
-- | 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 (V2 0 0) = error $ "problem with function: errorNormalizeV "++show i
errorNormalizeV _ !p = normalizeV p
-- | Debug version of 'angleVV'.
errorAngleVV :: Int -> Point2 -> Point2 -> Float
errorAngleVV !i (V2 0 0) _ = error $ "problem with function: errorAngleVV "++show i
errorAngleVV !i _ (V2 0 0) = error $ "problem with function: errorAngleVV "++show i
errorAngleVV _  !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  _ !x !y !z 
    | x == y    = dist x z
    | otherwise = closestPointOnLineParam x y z

-- | Return midpoint between two points.
midPoint :: Point2 -> Point2 -> Point2
midPoint !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 endpoints of the
-- segment.
circOnSegNoEndpoints :: Point2 -> Point2 -> Point2 -> Float -> Bool
{-# INLINE circOnSegNoEndpoints #-}
circOnSegNoEndpoints !p1 !p2 !c !rad = intersectSegSegTest p1 p2 (c -.- thenormal) (c +.+ thenormal)
  where 
    thenormal = rad *.* vNormal (normalizeV $ p1 -.- p2)
-- | Test whether a circle is on a segment by intersecting a normal and testing
-- the distance to the endpoints of the segment.
-- Perhaps a better order of arguments.
circOnSeg :: Point2 -> Float -> Point2 -> Point2 -> Bool
{-# INLINE circOnSeg #-}
circOnSeg !c !rad !p1 !p2 = magV (p1 -.- c) <= rad 
    || magV (p2 -.- c) <= rad
    || intersectSegSegTest p1 p2 (c -.- thenormal) (c +.+ thenormal)
  where 
    thenormal = rad *.* vNormal (normalizeV $ p1 -.- p2)

-- | Test whether a segment intersects a circle by intersecting a normal and testing
-- the distance to the endpoints of the segment.
segOnCirc :: Point2 -> Point2 -> Point2 -> Float -> Bool
{-# INLINE segOnCirc #-}
segOnCirc !p1 !p2 !c !rad = magV (p1 -.- c) <= rad 
    || magV (p2 -.- c) <= rad
    || intersectSegSegTest p1 p2 (c -.- thenormal) (c +.+ thenormal)
  where 
    thenormal = rad *.* vNormal (normalizeV $ p1 -.- p2)
cylinderOnSeg :: Point3 -> Point3 -> Point3 -> Float -> Bool
{-# INLINE cylinderOnSeg #-}
cylinderOnSeg = undefined
-- | 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 according to the line.
reflectIn :: Point2 -> Point2 -> Point2
reflectIn line vec = rotateV (2 * angleBetween line vec) 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)]

-- this shouldn't be here
doublePairSet :: Ord a => (a,a) -> S.Set (a,a)
doublePairSet (x,y) = S.fromList [(x,y),(y,x)]

doubleV2 :: V2 a -> [V2 a]
doubleV2 (V2 x y) = [V2 x y,V2 y x]
-- split a list into triples, forms triangles from a polygon
polyToTris'' :: [s] -> [s]
polyToTris'' (a:as) = go a as
  where
    go !x (y:z:ys) = x : y : z : go x (z:ys)
    go _ _ = []
polyToTris'' _ = []

polyToTris :: [s] -> [s]
{-# INLINABLE polyToTris #-}
--polyToTris (x:xs) = foldr (f x) [] $ foldPairs xs 
polyToTris (x:xs) = foldr (f x) [] $ zip xs $ tail xs
  where
    f a (b,c) ls = a:b:c:ls
polyToTris _ = []

polyToTris' :: [s] -> [s]
{-# INLINE polyToTris' #-}
polyToTris' [] = []
polyToTris' (a:as) = prependTwo a as

prependTwo :: a -> [a] -> [a]
prependTwo sep (x:y:xs) = sep : x : y : prependTwo sep (y:xs)
prependTwo _ _ = []

-- | Return n equidistant points on a circle with a radius of 600.
nRays :: Int -> [Point2]
nRays n = nRaysRad n 600
-- | 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)) (V2 x 0)
-- | Test whether an angle is to the left of another angle, according to the
-- smallest change in rotation between them.
-- This appears to sometimes fail if the angles are not normalized.
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...
-- TODO write tests
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

mixAngles :: Float -> Float -> Float -> Float
mixAngles frac a1 a2
    | abs (a1 - a2) <= pi = normalizeAngle $ frac * a1 + (1 - frac) * a2
    | a1 > a2   = mixAngles frac (a1 - 2*pi) a2
    | otherwise = mixAngles frac (a1 + 2*pi) a2

-- | 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

-- | 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 (V2 0 0) (vNormal line) rAng
    in  rAng -.- p

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 :: Int .. 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] :: [Int]

divideLineExact :: Float -> Point2 -> Point2 -> [Point2]
divideLineExact x a b = map ( (a +.+ ) . ( *.* v) ) [0 , x .. d]
  where 
    d = dist a b
    v = normalizeV $ b -.- a


-- | 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)]
--{-# INLINE digitalLine #-}
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 :: Float)
-- | Given two pairs of 'Int's, create a list of pairs of 'Int's that form a
-- rectangle between them.
digitalRect :: (Int,Int) -> (Int,Int) -> [(Int,Int)]
{-# INLINE digitalRect #-}
digitalRect (a,b) (c,d) = [(s,t) | s <- [minx .. maxx] , t <- [miny .. maxy]]
  where
    maxx = max a c
    minx = min a c
    maxy = max b d
    miny = min b d
-- | Given two Ints, creates the list of Ints between these.
intervalList :: Int -> Int -> [Int]
{-# INLINE intervalList #-}
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

arcStepwise
    :: Float    -- ^ Maximum distance between points
    -> Float    -- ^ Angle to travel
    -> Point2   -- ^ Center
    -> Point2   -- ^ Start vector from center
    -> [Point2]
arcStepwise ssize a c v
    | a < 0 = reverse $ arcStepwisePositive ssize (negate a) c (rotateV a v)
    | otherwise = arcStepwisePositive ssize a c v

arcStepwisePositive
    :: Float    -- ^ Maximum distance between points
    -> Float    -- ^ Angle to travel, assumed to be positive
    -> Point2   -- ^ Center
    -> Point2   -- ^ Start vector from center
    -> [Point2]
arcStepwisePositive ssize a cen v = (cen +.+) . (`rotateV` v) <$> rots
  where
    rots :: [Float]
    rots = map ((a*) . (/ fromIntegral n ) . fromIntegral) [0 .. n]
    n :: Int
    n = ceiling (a * magV v / ssize)

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

-- | Given a list of points, returns pairs of points linking the points into a
-- loop.
--loopPairs :: [a] -> [(a,a)]
--loopPairs [] = error "tried to make loop with empty list of elements"
--loopPairs [_] = error "tried to make loop with singleton list of elements"
--loopPairs (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
