module Physics.Hpysics.VClip where

import Physics.Hpysics.Types
import Physics.Hpysics.Utils
import Physics.Hpysics.Body
import Physics.Hpysics.Poly
import Data.List
import Data.Ord (comparing)
import Control.Monad.Reader
import Control.Monad.Writer
import Data.Maybe

-- * Implementation of V-Clip
--
-- Based on /V-Clip: fast and robust polyhedral collision detection/ by Brian
-- Mirtich <http://portal.acm.org/citation.cfm?id=285857.285860>

-- | Status of VClip algorithm
data VClipStatus = Done | Continue | Penetration
    deriving (Show, Read, Eq)

-- | Returns Voronoi plane of two features, such that the first feature is in
-- positive half-space. Only two kinds of Voronoi planes exist: vertex-edge and face-edge.
voronoiPlane :: PolyFeature -> PolyFeature -> Plane
-- vertex-edge contain the vertex and are perpendicular to the edge
voronoiPlane vertex@((Vertex vi1),p) edge@((Edge a b),_) = let
    (v1,v2) = computeEdge $ pack p $
        if vi1 == a then Edge a b else if vi1 == b then Edge b a
            else error "vertex and edge are not neighbour"
    normal = normalize $ v1`sub`v2
    shift = normal <.> v1
    in if distToPlaneSigned v2 (Plane normal shift) < 0-- ok: v2 lies in negative half-space
        then Plane normal shift
        else Plane (neg normal) (-shift)
voronoiPlane face@((Face vsi),p1) edge@((Edge ai bi),_) = let
-- NOTE: this doesn't verify that edge and face are indeed neighbour
    (Plane n s, _) = computeFace face
    (a,b) = computeEdge edge
    controlPoint = computeVertex $ pack p1 $ Vertex $ fromJust $ find (\i->i/=ai && i /= bi) vsi
    normal = normalize $ (b`sub`a) `cross` n -- perpendicular to square and to edge
    shift = normal <.> a
    in if distToPlaneSigned controlPoint (Plane normal shift) > 0 -- ok: square lies in positive half-space
        then Plane normal shift
        else Plane (neg normal) (-shift)
-- flipping
voronoiPlane f1@((Edge _ _),_) f2@((Vertex _),_) = let
    Plane n s = voronoiPlane f2 f1
    in Plane (neg n) (-s)
voronoiPlane f1@((Edge _ _),_) f2@((Face _),_) = let
    Plane n s = voronoiPlane f2 f1
    in Plane (neg n) (-s)
voronoiPlane _ _ = error "Invalid features"

-- | Clip the first feature (which must be an edge) against Voronoi planes of
-- the second feature and subset of its neighbours.
--
-- Returns @(retVal, lmax, lmin, mbNmax, mbNmin)@ where:
--
-- * @retVal@ is @False@ iff edge is completely clipped
--
-- * @lmax@ and @lmin@ are bounds of lambda for which lambda*a+(1-lambda)*b
-- lies in the region
--
-- * @mbNmax@ and @mbNmin@ are neighbour features, whose Voronoi plane clipped
-- edge from both sides (if any of them is @Nothing@, then edge was not clipped
-- from respective side)
clipEdge :: PolyFeature -> PolyFeature -> [PolyFeature] -> (Bool, FloatType, FloatType, Maybe PolyFeature, Maybe PolyFeature)
clipEdge edge@((Edge _ _),_) f2 = foldl (\accum@(retVal, lmax, lmin, mbNmax, mbNmin) feature -> let
    Plane n s = voronoiPlane f2 feature
    (a,b) = computeEdge edge
    da = a<.>n-s
    db = b<.>n-s
    in if da < 0 && db < 0 then (False, 1, 0, Just feature, Just feature) -- simple exclusion
        else if retVal == False then accum
        else let lambda = db/(db-da)
        in if da < 0 -- lambda is new lmax candidate
            then if lambda > lmax
                    then accum
                    else (lambda >= lmin, lambda, lmin, Just feature, mbNmin)
            else if db < 0 -- lambda is new lmin candidate
                then if lambda < lmin
                        then accum
                        else (lambda <= lmax, lmax, lambda, mbNmax, Just feature)
                else accum) $
    (True, 1, 0, Nothing, Nothing)

-- | Detect penetration or dislodge from local minimum. @Penetration@ gives
-- closest plane to vertex, @Continue@ gives face whose plane has maximum
-- distance to vertex.
handleLocalMin :: PolyFeature -> Poly -> (VClipStatus, PolyFeature)
handleLocalMin vertex@((Vertex _),_) p2 = let
    v = computeVertex vertex
    faceDistancePairs = [(s, distToPlaneSigned v (computeFacePlane s)) | s <- faces p2]
    maxPair = maximumBy (comparing snd) faceDistancePairs
    in (if snd maxPair > 0 then Continue else Penetration, fst maxPair)

-- | Determines whether point (vertex) violates Voronoi plane, i.e. lies in the
-- negative half-space
vertexViolatesPlane :: Vec -> Plane -> Bool
vertexViolatesPlane v plane = distToPlaneSigned v plane < 0

-- | Indicates the sign of derivative D'(edge,feature)(lambda).
-- Returns @Nothing@ if penetration is detected.
derivative :: PolyFeature -> PolyFeature -> FloatType -> PolyFeature -> Maybe FloatType
derivative edge@((Edge _ _),_) feature lambda neighbour = let
    (a,b) = computeEdge edge
    ba = a`sub`b -- direction of increasing lambda
    x = lc a b lambda 
    in case fst feature of
        Vertex _ -> let v = computeVertex feature in
            if distance x v < floatEpsilon then Nothing else Just $ ba <.> (x`sub`v)
        Face _ -> let {plane@(Plane n _) = computeFacePlane feature; dist = distToPlaneSigned x plane} in
            if abs dist < floatEpsilon then Nothing else Just $ n <.> ba * dist
        -- neighbour is used only in Edge case, and neighbour of edge is never an edge again
        Edge _ _ -> derivative edge neighbour lambda undefined

-- | Post-clipping checks and updates. The first feature should be edge.
--
-- If edge is simply excluded by some VP(E,N), then update feature to N.
-- Otherwise perform derivative check.
--
-- Returns possibly updated second feature (if status is @Done@, it's not updated and penetration is not detected)
postClip :: PolyFeature -> PolyFeature -> (Bool, FloatType, FloatType, Maybe PolyFeature, Maybe PolyFeature) -> (VClipStatus, PolyFeature)
postClip edge@((Edge _ _),_) feature (retVal, lmax, lmin, mbNmax, mbNmin) =
    if mbNmin == mbNmax && isJust mbNmin
        then (Continue, fromJust mbNmin)
        else
        (\cont -> case mbNmin of
            Nothing -> cont
            Just n -> case derivative edge feature lmin n of
                Just d -> if d > 0
                    then (Continue, n)
                    else cont
                Nothing -> (Penetration, feature))$
        (\cont -> case mbNmax of
            Nothing -> cont
            Just n -> case derivative edge feature lmax n of
                Just d -> if d < 0
                    then (Continue, n)
                    else cont
                Nothing -> (Penetration, feature))$
        (Done, feature)

vClipStep :: PolyFeature -> PolyFeature -> (VClipStatus, PolyFeature, PolyFeature)
-- VVstate
vClipStep f1@((Vertex _),_) f2@((Vertex _),_) = let
    v1 = computeVertex f1
    v2 = computeVertex f2
    mbe1 = find (vertexViolatesPlane v2 . voronoiPlane f1) (neighbours f1)
    in case mbe1 of
        Just e1 -> (Continue, e1, f2)
        Nothing -> do
            let mbe2 = find (vertexViolatesPlane v1 . voronoiPlane f2) (neighbours f2)
            case mbe2 of
                Just e2 -> (Continue, f1, e2)
                Nothing -> (Done, f1, f2)
-- VEstate
vClipStep f1@((Vertex _),_) edge@((Edge _ _),_) = let
    v1 = computeVertex f1
    mbN = find (vertexViolatesPlane v1 . voronoiPlane edge) (neighbours edge)
    in case mbN of
        Just n -> (Continue, f1, n)
        Nothing -> let clipped = clipEdge edge f1 (neighbours f1)
                       (status, feature) = postClip edge f1 clipped
                   in if status == Penetration -- avoid contact of edge and vertex
                        then (Penetration, feature, head (neighbourFaces edge))
                        else (status, feature, edge)
-- VFstate
vClipStep f1@((Vertex _),_) face@((Face _),s2) = let
    plane = computeFacePlane face
    v1 = computeVertex f1
    minPair = minimumBy (comparing snd) $ map (\e -> (e, distToPlaneSigned v1 $ voronoiPlane face e)) (neighbours face)
    in if snd minPair < 0 -- violates Voronoi plane
        then (Continue, f1, fst minPair)
        else
            let mbNewEdge = find (\e->let (_,x) = computeEdge e in
                    distToPlane x plane < distToPlane v1 plane - floatEpsilon ||
                    distToPlaneSigned v1 plane > 0 && distToPlaneSigned x plane < 0)
                        (neighbours f1)
            in case mbNewEdge of
                Just newEdge -> (Continue, newEdge, face)
                Nothing -> if distToPlaneSigned v1 plane > 0
                    then (Done, f1, face)
                    else case handleLocalMin f1 s2 of
                        (Continue, newFace) -> (Continue, f1, newFace)
                        (Penetration, newFace) -> (Penetration, f1, newFace)
-- EEstate
vClipStep e1@((Edge a1 b1),_) e2@((Edge a2 b2),_) = let
    eeState e1 e2 =
        -- clip e2 against vertex-edge planes of VR(e1)
        let clipped1 = clipEdge e2 e1 (neighbourVertices e1)
            (status1, feature1) = postClip e2 e1 clipped1
            clipped2 = clipEdge e2 e1 (neighbourFaces e1)
            (status2, feature2) = postClip e2 e1 clipped2
        in  if status1 /= Done then (status1, feature1, e2) else
            if status2 /= Done then (status2, feature2, e2) else
                (Done, e1, e2)
    result1@(status1, _, _) = eeState e1 e2
    result2@(status2, f2, f1) = eeState e2 e1
    result@(status, ff1, ff2) = if status1 /= Done then result1 else (status2, f1, f2)
    isEdge (Edge _ _, _) = True
    isEdge _ = False
    in if status == Penetration && isEdge ff1 && isEdge ff2 -- avoid contact of two edges
        then (Penetration, ff1, head (neighbourFaces ff2))
        else result
-- EFstate
vClipStep edge@(Edge ai bi,p1) face@(Face _,p2) = let
    clipped@(retVal, lmax, lmin, mbNmax, mbNmin) = clipEdge edge face (neighbours face)
    (a,b) = computeEdge edge
    plane = computeFacePlane face
    in if not retVal -- edge is excluded
        then let new2 = minimumBy (comparing $ distToEdge edge) $
                    featuresOfFace face
             in (Continue, edge, new2)
        else let
            dmin = distToPlaneSigned (lc a b lmin) plane
            dmax = distToPlaneSigned (lc a b lmax) plane
            in if dmin > 0 && dmax < 0 || dmin < 0 && dmax > 0
                then (Penetration, edge, face)
                else case derivative edge face lmin undefined of
                    Just d -> if d >= 0
                        then maybe (Continue, pack p1 $ Vertex bi, face)
                          (\n -> (Continue, edge, n)) mbNmin
                        else maybe (Continue, pack p1 $ Vertex ai, face)
                          (\n -> (Continue, edge, n)) mbNmax
                    Nothing -> (Penetration, edge, face)
vClipStep (Face _,_) (Face _,_) = error "vClipStep is not defined for two faces"
vClipStep x y = let (status, f2, f1) = vClipStep y x
                 in (status, f1, f2)

vClipIteration :: PolyFeature -> PolyFeature -> (VClipStatus, PolyFeature, PolyFeature)
vClipIteration f1 f2 = let
    ret@(status, f1', f2') = vClipStep f1 f2
    in if status == Continue then vClipIteration f1' f2' else ret
vClip :: Poly -> Poly -> (VClipStatus, PolyFeature, PolyFeature)
vClip s1 s2 = vClipIteration (head$vertices s1) (head$vertices s2)