module Physics.Hpysics.Poly where

import Physics.Hpysics.Types
import Physics.Hpysics.Utils
import Data.List
import Data.Maybe

-- | Features of a polyhedron
data Feature = Vertex Int
             | Edge Int Int
             | Face [Int] -- ^ plane, vertices
    deriving (Show, Read, Eq)
type PolyFeature  = (Feature, Poly)

featureType :: Feature -> String
featureType (Vertex _) = "Vertex"
featureType (Edge _ _) = "Edge"
featureType (Face _) = "Face"

pack :: Poly -> Feature -> PolyFeature
pack p x = (x,p)

translatePoly :: Vec -> Poly -> Poly
translatePoly shift p = p { vertices_v = map (add shift) $ vertices_v p }
rotatePoly :: Vec4 -> Poly -> Poly
rotatePoly q p = p { vertices_v = map (rotatePoint q) $ vertices_v p }
transformPoly :: Vec -> Vec4 -> Poly -> Poly
transformPoly shift q = translatePoly shift . rotatePoly q

-- | For polyhedral shape, returns all its vertices
vertices :: Poly -> [PolyFeature]
vertices p = map (pack p . Vertex) $ zipWith const [0..] $ vertices_v p
computeVertex :: PolyFeature -> Vec
computeVertex (Vertex n,p) = vertices_v p !! n

-- | For polyhedral shape, returns all its edges
edges :: Poly -> [PolyFeature]
edges p = map (pack p . uncurry Edge) $ edges_i p
computeEdge :: PolyFeature -> (Vec,Vec)
computeEdge (Edge a b,p) = (vertices_v p !! a, vertices_v p !! b)

-- | For polyhedral shape, returns all its faces. Polyhedron lies in the
-- negative half-space w.r.t. each face, i.e. normal looks to outside.
faces :: Poly -> [PolyFeature]
faces p = map (pack p . Face) $ faces_i p
computeFace :: PolyFeature -> (Plane, [Vec])
computeFace (Face vertexIndeces, p) = let
    faceVertices = [vertices_v p!!i | i <- vertexIndeces]
    anotherVertexIndex = maybe (length vertexIndeces) id
        $ findIndex (/=0)
        $ zipWith (-) [0..]
        $ sort vertexIndeces
    -- XXX if polyhedron is degenerate this will fail with "index too large"
    anotherVertex = vertices_v p !! anotherVertexIndex
    (normal,shift) = (normalize $ (a`sub`b)`cross`(c`sub`b), normal<.>a)
        where [a,b,c] = take 3 faceVertices
    plane = if distToPlaneSigned anotherVertex (Plane normal shift) < 0
                then Plane normal shift
                else Plane (neg normal) (-shift)
    in (plane, faceVertices)
computeFacePlane = fst.computeFace

-- | For polyhedral shape, returns all its features (vertices, edges, faces)
features :: Poly -> [PolyFeature]
features p = vertices p ++ edges p ++ faces p

-- Neighbours functions

-- | Find all neighbours of given feature.
--
-- The neighbors of a vertex are the edges incident to that vertex (and they
-- start from that vertex). The neighbors of a face are the edges bounding
-- the face. An edge has exactly four neighbors: the two vertices at its
-- endpoints and the the two faces it bounds.

neighbours :: PolyFeature -> [PolyFeature]
neighbours pf@(feature,p) = case feature of
    Vertex _ -> map (pack p) $ mapMaybe (incidentTo feature) (map fst $ edges p)
    Face vs  -> map (pack p) $ zipWith Edge vs (rotate vs)
    Edge a b -> neighbourVertices pf ++ neighbourFaces pf
    where
    -- | Determines whether edge and vertex are incident. If they are, return
    -- edge which starts at vertex.
    incidentTo (Vertex v) (Edge a b) =
        if a == v then Just (Edge a b) else
        if b == v then Just (Edge b a) else
        Nothing
    incidentTo e@(Edge _ _) v@(Vertex _) = incidentTo v e

-- | For edge, return its neighbour vertices
neighbourVertices :: PolyFeature -> [PolyFeature]
neighbourVertices ((Edge a b),p) = map (pack p) $ [Vertex a, Vertex b]

-- | For edge, return its neighbour faces
neighbourFaces :: PolyFeature -> [PolyFeature]
neighbourFaces (edge@(Edge _ _),p) = [ (face,p) | (face,_) <- faces p, face `boundedBy` edge ]

-- | Determines whether two edges are the same
edgeSameAs :: Feature -> Feature -> Bool
edgeSameAs (Edge a1 b1) (Edge a2 b2) = (a1==a2&&b1==b2)||(a1==b2&&a2==b1)

-- | Determines whether the face is bounded by edge
boundedBy :: Feature -> Feature -> Bool
boundedBy (Face vs) edge = any (edgeSameAs edge) $ zipWith Edge vs (rotate vs)

-- | Returns minimal distance between edge and another given feature (edge must
-- be the first argument). Another feature may be vertex or edge.
distToEdge :: PolyFeature -> PolyFeature -> FloatType
distToEdge edge@((Edge _ _),_) vertex@((Vertex _),_) = let
    (a,b) = computeEdge edge
    v = computeVertex vertex
    lambda = projectToLineLambda a b v
    in distance v $
        if lambda < 0
            then b
            else if lambda > 1
                then a
                else lc a b lambda
distToEdge edge1@((Edge _ _),_) edge2@((Edge _ _),_) = let
    -- v1 = l(a1-b1)+b1
    -- v2 = m(a2-b2)+b2
    -- (v1-v2)^2 -> min
    -- 0 = d(v1-v2)^2/dl = 2(a1-b1)(v1-v2) and the same for m
    -- (this can also be obtained from perpendicularity)
    --  l (a1-b1)^2-m (a1-b1)(a2-b2) = (a1-b1)(b2-b1)
    -- -l (a1-b1)(a2-b2)+m (a2-b2)^2 = (a2-b2)(b1-b2)
    (a1,b1) = computeEdge edge1
    (a2,b2) = computeEdge edge2
    a1_b1 = a1`sub`b1
    a2_b2 = a2`sub`b2
    mdot12 = - a1_b1 <.> a2_b2
    b1_b2    = b1`sub`b2
    sol   = solveLinear2
        (dotSquare a1_b1) mdot12 (-a1_b1<.>b1_b2)
        mdot12 (dotSquare a2_b2) ( a2_b2<.>b1_b2)
    in case sol of
        Just (l, m) -> let
            p1 = if l < 0 then b1
              else if l > 1 then a1
                            else lc a1 b1 l
            p2 = if m < 0 then b2
              else if m > 1 then a2
                            else lc a2 b2 m
            in distance p1 p2
        -- if the system of linear equations is singular, then segments are
        -- parallel. One of two alternatives is possible: either at least one of
        -- 4 vertices projects into interior of other segment ("good" vertex), or not.
        -- In the latter case we just need to find a pair of closest vertices
        Nothing -> let
            linePointPairs = [((a1,b1),a2), ((a1,b1),b2), ((a2,b2),a1), ((a2,b2),b1)]
            maybeGoodPoint = find ((\l->l>0 && l<1).(uncurry$uncurry projectToLineLambda)) linePointPairs
            in case maybeGoodPoint of
                Just (_, v) ->
                    if v == a1 || v == b1 -- we need to know what segment contains this good point
                        then distance v $ projectToLine a2 b2 v
                        else distance v $ projectToLine a1 b1 v
                Nothing -> minimum [distance v1 v2 | v1 <- [a1,b1], v2 <- [a2,b2]]

featuresOfFace :: PolyFeature -> [PolyFeature]
featuresOfFace face = verticesOfFace face ++ edgesOfFace face
edgesOfFace (Face vs,p) = map (pack p) $ zipWith Edge vs (rotate vs)
verticesOfFace (Face vs,p) = map (pack p.Vertex) vs

rect :: FloatType -> FloatType -> FloatType -> Poly
rect a' b' c' = let {a=a'/2;b=b'/2;c=c'/2} in Poly
    { vertices_v =
        [ Vec   a    b    c  -- 0
        , Vec   a    b  (-c) -- 1
        , Vec   a  (-b)   c  -- 2
        , Vec   a  (-b) (-c) -- 3
        , Vec (-a)   b    c  -- 4
        , Vec (-a)   b  (-c) -- 5
        , Vec (-a) (-b)   c  -- 6
        , Vec (-a) (-b) (-c) -- 7
        ]
    , edges_i = 
        -- plane +a
        [ (0,1)
        , (0,2)
        , (1,3)
        , (2,3)
        -- plane -a
        , (4,5)
        , (4,6)
        , (5,7)
        , (6,7)
        -- edges parallel to x axes
        , (0,4)
        , (1,5)
        , (2,6)
        , (3,7)
        ]
    , faces_i =
        [ [0,1,3,2] -- +a
        , [4,5,7,6] -- -a
        , [0,1,5,4] -- +b
        , [2,3,7,6] -- -b
        , [0,2,6,4] -- +c
        , [1,3,7,5] -- -c
        ]
    }
cube height = rect height height height
tetrahedron :: [Vec] -> Poly
tetrahedron vs = Poly
    { vertices_v = vs
    , edges_i = [(i,j) | i<-[0..3],j<-[0..3],i<j]
    , faces_i = [filter (/=i) [0..3] | i <- [0..3]]
    }