'

{-# LANGUAGE TemplateHaskell #-}
{-# LANGUAGE FlexibleInstances #-}
{-# LANGUAGE MultiWayIf #-}
{-# LANGUAGE LambdaCase #-}
{-# LANGUAGE RankNTypes #-}
{-# LANGUAGE DeriveGeneric #-}
module Purity.Physics where

import Linear
import Control.Lens
import Control.Monad
import Data.Foldable
import Debug.Trace

newtype Surface q a = Surface
  {_surfaceNormal :: q a -- Surface is at and normal to tip of vector
  } deriving (Eq,Ord)
makeLenses ''Surface

groundSurface :: Num a => Surface V3 a
groundSurface = Surface (V3 0 1 0)

data PhysDomain q a
  = AABBDomain (q a)
  -- | SphereDomain a
  -- Origin is lower edge
  -- Height then radius
  -- CapsuleDomain a a

domainVolume :: (Floating a, Metric q, Foldable q) => PhysDomain q a -> a
domainVolume (AABBDomain size) = product size
--domainVolume (SphereDomain r) = 4/3 * pi * r * r * r
--domainVolume (CapsuleDomain h r) = (4/3 * r + h) * pi * r * r

domainCenter :: (Floating a, Additive q) => PhysDomain q a -> q a
domainCenter (AABBDomain size) = 0.5 *^ size

data PhysModel q a = PhysModel
  {_physDomain :: PhysDomain q a
  ,_currentVelocity :: q a
  ,_currentOrigin :: q a
  ,_currentOrientation :: Quaternion a
  }
makeLenses ''PhysModel

data PhysFuture q a = PhysFuture
  {_presentModel :: PhysModel q a
  ,_appliedForce :: q a
  }
makeLenses ''PhysFuture

--translateDomain :: (Num a, Additive q) => q a -> PhysDomain q a -> PhysDomain q a
--translateDomain off (AABB low high) = AABB (low ^+^ off) (high ^+^ off)

translationConstantAccel :: (Fractional a, Additive q) => a -> q a -> q a -> q a
translationConstantAccel dt v a = dt *^ v ^+^ 0.5 * dt * dt *^ a

velocityConstantAccel :: (Fractional a, Additive q) => a -> q a -> q a -> q a
velocityConstantAccel dt v a = v ^+^ dt *^ a

readFuture :: (Fractional a, Additive q) => a -> PhysFuture q a -> PhysModel q a
readFuture dt f = updateVel . updatePos $ f^.presentModel
  where
    updateVel = currentVelocity %~ (^+^ dt *^ f^.appliedForce)
    updatePos = currentOrigin %~ (^+^ translationConstantAccel dt (f^.presentModel.currentVelocity) (f^.appliedForce))

--domainsIntersect :: (Applicative q, Foldable q, Metric q, Ord a, Floating a) => q a -> PhysDomain q a -> q a -> PhysDomain q a -> Bool
--domainsIntersect offA (AABBDomain lowA highA) offB (AABBDomain lowB highB) = vecBelow (lowA ^+^ offA) (offB ^+^ highB) || vecBelow (lowB ^+^ offB) (offA ^+^ highA)
--domainsIntersect offA (SphereDomain rA) offB (SphereDomain rB) = distance offA offB < rA + rB
--domainsIntersect offA (SphereDomain rA) offB (AABBDomain lowB highB) = or $ interOn <$> offA <*> offB <*> lowB <*> highB
  --where
    --interOn a b l h = a + rA > b + l || a - rA < b + h
--domainsIntersect a aabb@(AABBDomain _ _) b sph@(SphereDomain _) = domainsIntersect b sph a aabb

vecBelow :: (Applicative q, Foldable q, Ord a) => q a -> q a -> Bool
vecBelow a b = and $ (<) <$> a <*> b

timeOfIntersection :: (Foldable q, Applicative q, Show a, Ord a, Floating a, Metric q) => PhysFuture q a -> PhysFuture q a -> Maybe a
timeOfIntersection fa fb = fmap (rectifyQuads . toList) . transpose $ intervalOfIntersection fa fb

maybeMin :: Ord a => V2 (Maybe a) -> Maybe a
maybeMin (V2 (Just a) (Just b)) = Just $ min a b
maybeMin (V2 (Just a) _) = Just a
maybeMin (V2 _ x) = x

intervalOfIntersection :: (Foldable q, Applicative q, Show a, Ord a, Floating a, Metric q) => PhysFuture q a -> PhysFuture q a -> q (V2 (V3 a))
intervalOfIntersection fa fb = soln 
  <$> fa^.appliedForce
  <*> fb^.appliedForce
  <*> fa^.presentModel.currentVelocity
  <*> fb^.presentModel.currentVelocity
  <*> fa^.presentModel.currentOrigin
  <*> fb^.presentModel.currentOrigin
  <*> qA
  <*> qB
  where
    (AABBDomain qA) = fa^.presentModel.physDomain
    (AABBDomain qB) = fb^.presentModel.physDomain
    soln aA' aB' vA' vB' xA' xB' qA' qB' = V2 (V3 ((aB' - aA')/2) (vB' - vA') (xB' - xA' - qA')) (V3 ((aA' - aB')/2) (vA' - vB') (xA' - xB' - qB'))

quadLowerBound :: (Ord a,Floating a,Show a) => a -> a -> a -> Maybe a
quadLowerBound a b c = traceShow (a,b,c) $ if a == 0
  then if b == 0
    then if c < 0
      then Just 0 -- Constantly colliding
      else Nothing -- Never colliding
    else if b > 0
      then Just $ max 0 (-c/b) -- From -c/b to forever
      else if (-c/b) > 0 -- From start of time to -c/b
        then Just 0
        else Nothing
  else if det >= 0 
    then let (m,p) = ((-b - sqrt det)/(2 * a),(-b + sqrt det)/(2 * a)) in if p < 0
      then if a > 0
        then Nothing
        else Just 0
      else Just $ max 0 m
    else if a > 0
      then Nothing -- Swoop never collide
      else Just 0 -- Swoop always collide
  where
    det = b * b - 4 * a * c

data Sign = Plus | Minus | Zero deriving (Eq,Show)

signOf :: (Ord a, Num a) => a -> Sign
signOf x = if x > 0 then Plus else (if x < 0 then Minus else Zero)

oppSign :: Sign -> Sign
oppSign Plus = Minus
oppSign Minus = Plus
oppSign Zero = Zero

multSign :: Sign -> Sign -> Sign
multSign Plus = id
multSign Minus = oppSign
multSign Zero = const Zero

paritySign :: Integral a => a -> Sign
paritySign a = if odd a then Minus else Plus

polyLeftSign :: (Ord a, Num a, Eq a) => [a] -> Sign
polyLeftSign xs = case cleanPoly of
  [] -> Zero -- Was actually zero
  (x:_) -> multSign (paritySign order) (signOf x)
  where
    order = length cleanPoly - 1
    cleanPoly = dropWhile (== 0) xs -- Poly with no leading zeros

quadOddRoots :: (Show a, Ord a, Floating a) => V3 a -> [a]
quadOddRoots (V3 a b c) = traceShow (a,b,c) $ case signOf det of
  Plus -> case signOf a of
    Plus -> [(-b - sqrt det)/(2 * a),(-b + sqrt det)/(2 * a)]
    Minus -> [(-b + sqrt det)/(2 * a),(-b - sqrt det)/(2 * a)]
    Zero -> linOddRoots b c -- Was actually linear
  _ -> [] -- Either no roots, or only an even one
  where
    det = b * b - 4 * a * c

evalPoly :: Num a => a -> V3 a -> a
evalPoly x (V3 a b c) = a * x * x + b * x + c

rectifyQuads :: (Show a, Ord a, Floating a) => [V3 a] -> Maybe a
rectifyQuads qs = safeHead . dropWhile (\x -> not . all (<= 0) $ fmap (evalPoly x) qs) $ roots
  where
    roots = foldr merge [] $ fmap quadOddRoots qs
    safeHead [] = Nothing
    safeHead (x:_) = Just x

flipsBefore :: Ord a => a -> [a] -> Sign
flipsBefore t (x:xs) = if t >= x
  then oppSign (flipsBefore t xs)
  else Plus
flipsBefore _ [] = Plus

merge :: Ord a => [a] -> [a] -> [a]
merge xs [] = xs
merge [] ys = ys
merge (x:xs) (y:ys) 
  | (x <= y)  = x:(merge xs (y:ys)) 
  | otherwise = y:(merge (x:xs) ys)

-- bx + c = 0 => x = -c/b
linOddRoots :: (Eq a, Fractional a) => a -> a -> [a]
linOddRoots b c = if b == 0 then [] else [-c/b]

turningPoint :: (Metric q, Floating a) => PhysFuture q a -> a
turningPoint f = norm (f^.presentModel.currentVelocity) / norm (f^.appliedForce)
    
objAtZero :: PhysFuture V3 Float
objAtZero = PhysFuture
  {_appliedForce = zero
  ,_presentModel = PhysModel
    {_physDomain = AABBDomain (V3 2 2 2)
    ,_currentOrigin = V3 (-1) (-1) (-1)
    ,_currentVelocity = zero
    ,_currentOrientation = axisAngle (V3 0 0 (-1)) 0
    }
  }

objFalling :: PhysFuture V3 Float
objFalling = PhysFuture
  {_appliedForce = V3 0 (-1) 0
  ,_presentModel = PhysModel
    {_physDomain = AABBDomain (V3 2 2 2)
    ,_currentOrigin = V3 (-1) 5 (-1)
    ,_currentVelocity = zero
    ,_currentOrientation = axisAngle (V3 0 0 (-1)) 0
    }
  }

objLeft :: PhysFuture V3 Float
objLeft = PhysFuture
  {_appliedForce = V3 (-1) 0 0
  ,_presentModel = PhysModel
    {_physDomain = AABBDomain (V3 2 2 2)
    ,_currentOrigin = V3 (-5) (-5) 0
    ,_currentVelocity = V3 20 5 0
    ,_currentOrientation = axisAngle (V3 0 0 (-1)) 0
    }
  }

objRight :: PhysFuture V3 Float
objRight = PhysFuture
  {_appliedForce = V3 1 0 0
  ,_presentModel = PhysModel
    {_physDomain = AABBDomain (V3 2 2 2)
    ,_currentOrigin = V3 5 (-5) 0
    ,_currentVelocity = V3 (-20) 5 0
    ,_currentOrientation = axisAngle (V3 0 0 (-1)) 0
    }
  }

worryAbout :: (Foldable q, Metric q, Applicative q, Show a, Ord a, Floating a, Epsilon a) => a -> PhysFuture q a -> PhysFuture q a -> PhysModel q a
worryAbout dt fa fb = case mfilter (< dt) (timeOfIntersection fa fb) of
  Nothing -> readFuture dt fa
  Just cTime -> readFuture (dt - cTime) PhysFuture {_presentModel = readFuture cTime fa, _appliedForce = fa^.appliedForce ^+^ normalResponse (readFuture cTime fa) (readFuture cTime fb)}

normalResponse :: (Floating a, Epsilon a, Metric q) => PhysModel q a -> PhysModel q a -> q a
normalResponse mover kicker = normalize kickNormal ^* norm dv
  where
    dv = mover^.currentVelocity ^-^ kicker^.currentVelocity
    kickNormal = (mover^.currentOrigin ^+^ mover^.physDomain.to domainCenter) ^-^ (kicker^.currentOrigin ^+^ kicker^.physDomain.to domainCenter)