module Collision where

import Control.Monad

bodyToAxisWidth par perp cosAngle sinAngle = par*cosAngle + perp*sinAngle

testBodyToAxisWidth par perp angle = bodyToAxisWidth par perp cosa sina
    where cosa = abs $ cos angle
          sina = abs $ sin angle

--AABB position and half-width
penetration sdiff aw bw | result > 0 = Just result
                        | otherwise = Nothing
    where result = aw + bw - (abs sdiff)

projection sdiff pen = signum sdiff * pen

--voronoi regions

voronoiCorner ax ay bx by bw bh = do
  x <- voronoiSide ax bx bw
  y <- voronoiSide ay by bh
  return (x, y)

voronoiSide a b bw | (diff < (-bw)) = Just $ b - bw
                   | (diff > bw) = Just $ b + bw
                   | otherwise = Nothing
                   where diff = a - b

--verlet integration

verletFirst s v a dt dtHalf = (s + v*dt + aterm*dt, v + aterm)
    where aterm = a*dtHalf

verletSecond v' a' dtHalf = v' + a'*dtHalf

{- simulation 
--verletFirst -> new position and half-step velocity
--handle Collisions -> fix position and half-step velocity
--calc accelerations based on new positions -> a'
--verletSecond -> new velocity
--apply terminal velocity and calc drag accelerations
-}

data Body = Body { mass :: Float,
                   halfSize,
                   position, velocity, acceleration :: (Float, Float) }
            deriving (Show, Eq)

test1 = Body 1 (3, 3) (0, 0) (10, 0) (0, 0)
test2 = Body 1 (3, 3) (20, 0.25) (-5, 0) (0, 0)
test3 = Body 1 (3, 3) (10, 0.5) (-10, 0) (0, 0)
test4 = Body 1 (3, 3) (10, 0.0) (5, 20) (0, -10)


tests1 = [test1, test2]
tests2 = [test1, test2, test3]
tests3 = [test4]

--begin in contact
test8 = Body 1 (2, 2) (0, 0) (10, 0) (0, 0)
test9 = Body 1 (2, 2) (3, 0) (-5, 0) (0, 0)

showBodies bodies = unlines $ map show bodies

printSim steps dt bodies =
    mapM_ putStrLn (map showBodies $ take steps $ iterate (simulate dt) bodies)

verletFirstBody dt dtHalf b@(Body _ _ (x, y) (vx, vy) (ax, ay)) =
    b{position=(x', y'), velocity=(vxHalf, vyHalf)}
        where (x', vxHalf) = verletFirst x vx ax dt dtHalf
              (y', vyHalf) = verletFirst y vy ay dt dtHalf

verletSecondBody dtHalf b@(Body _ _ _ (vxHalf, vyHalf) (ax, ay)) =
    b{velocity=(vx, vy)}
        where vx = verletSecond vxHalf ax dtHalf
              vy = verletSecond vyHalf ay dtHalf

-- TODO: calc new accelerations, calc drag/terminal velocity
simulate dt bodies =
    (map (verletSecondBody dtHalf)
             $ collideBodies $ map (verletFirstBody dt dtHalf) bodies)
        where dtHalf = 0.5*dt

collideBodies [] = []
collideBodies (body:bodies) = (bresult:(collideBodies rest))
    where (bresult, rest) =
              (foldl (\(b1, finished) b2 ->
                          let (br1, br2) = collideBody b1 b2 in
                          (br1, br2:finished))
               (body, []) bodies)

collideBody b1 b2 =
    case (collide (mass b1) (halfSize b1)
          (position b1) (velocity b1)
          (mass b2) (halfSize b2)
          (position b2) (velocity b2)) of
      Nothing -> (b1, b2)
      Just ((s1', v1'), (s2', v2')) -> (b1{position=s1', velocity=v1'},
                                        b2{position=s2', velocity=v2'})

collide ma (wa, ha) (xa, ya) (vxa, vya) mb (wb, hb) (xb, yb) (vxb, vyb) = do
  let xdiff = xa - xb
  xpen <- penetration xdiff wa wb
  let ydiff = ya - yb
  ypen <- penetration ydiff ha hb
  if xpen < ypen
    then do
        proj <- return $ projection xdiff xpen
        let partialProj = proj*mb/(ma+mb) -- a hack, but what else is fair?
        let (vxa', vxb') = elasticVelocity ma vxa mb vxb 1
        return (((xa+partialProj, ya), (vxa', vya)),
                ((xb-proj+partialProj, yb), (vxb', vyb)))
    else do
        proj <- return $ projection ydiff ypen
        let partialProj = 0.5*proj
        let (vya', vyb') = elasticVelocity ma vya mb vyb 1
        return (((xa, ya+partialProj), (vxa, vya')),
                ((xb, yb-proj+partialProj), (vxb, vyb')))

elasticVelocity m1 u1 m2 u2 e = (v1, v2)
    where v1 = (u1*(m1 - e*m2) + bounce*m2*u2) / msum
          v2 = (u2*(m2 - e*m1) + bounce*m1*u1) / msum
          msum = m1 + m2
          bounce = e+1