-- | Determines whether given point in the square's plane lies inside the square
isPointInsideSquare :: Plane -> Vec -> Vec -> Vec -> Bool
-- v1 lies inside a square iff its projections on edges are internal
-- alternatively: iff sum of the distances to edges equals to 2*side
isPointInsideSquare plane center vertex v2 = let
    edges = edgesOfSquare plane center vertex
    in all isRight (map (snd.closestPoints (Vertex v2)) edges)

-- finding closest features
-- TODO probably should be changed to Lin-Canny algorithm

-- | Finds closest points between two features
closestPoints :: Feature -> Feature -> (Either Feature Vec, Either Feature Vec)
closestPoints f1@(Vertex v1) f2 = (Right v1, closestPointOrFeature) where
    closestPointOrFeature = case f2 of
        Vertex v2 -> Right v2
        Edge a b -> let
            lambda = projectToLineLambda a b v1
            in
            if lambda < 0
            then Left $ Vertex b
            else if lambda > 1
            then Left $ Vertex a
            else Right $ lc a b lambda
        Square (normal, shift) center vertex -> let
            -- projection v1 to plane
            v2 = projectToPlane (normal, shift) v1
            edges = edgesOfSquare (normal, shift) center vertex
            in if isPointInsideSquare (normal, shift) center vertex v2
                then Right v2
                else Left $ minimumBy (comparing $ distanceToEdge v1) edges
closestPoints f1@(Edge a1 b1) f2@(Edge a2 b2) = 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)(b1+b2)
    -- l (a1-b1)(a2-b2)+m (a2-b2)^2 = -(a2-b2)(b1+b2)
    a1_b1 = a1`sub`b1
    a2_b2 = a2`sub`b2
    dot12 = a1_b1 <.> a2_b2
    bb    = b1`add`b2
    sol   = solveLinear2
        (dotSquare a1_b1) dot12 (- a1_b1 <.> bb)
        dot12 (dotSquare a2_b2) (- a2_b2 <.> bb)
    in case sol of
        Just (l, m) -> let
            res1 = if l < 0 then Left $ Vertex b1
              else if l > 1 then Left $ Vertex a1
                            else Right$ lc a1 b1 l
            res2 = if m < 0 then Left $ Vertex b2
              else if m > 1 then Left $ Vertex a2
                            else Right$ lc a2 b2 m
            in (res1, res2)
        -- 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 (Left (Vertex v), Right (projectToLine a2 b2 v))
                        else (Right (projectToLine a1 b1 v), Left (Vertex v))
                Nothing -> let
                    (c1,c2) = minimumBy (comparing $ uncurry distance)
                        [(v1,v2) | v1 <- [a1,b1], v2 <- [a2,b2]]
                    in (Left$Vertex c1, Left$Vertex c2)
{-
closestPoints f1@(Edge a b) f2@(Square normal shift center vertex) = let
    -- the following cases are possible:
    -- 1. segment intersects square in some interior point
    -- 2. segment doesn't intersect plane, and its end which is closer to the
    --    plane projects into interior of the square
    -- 3. in all other cases closest to segment point of square lies on its
    --    boundary
    distA = distToPlaneSigned a normal shift
    distB = distToPlaneSigned b normal shift
    segmentIntersectsPlane = (distA >= 0 && distB <= 0) || (distA <=0 && distB >= 0)
    intersection = fromJust$planeLineIntersection normal shift a b
    closerEnd = let { da = distToPlane a normal shift; db = distToPlane b normal shift }
        in if da < db then a else b
    closerEndProjection = projectToPlane normal shift closerEnd
    insideSquare = isPointInsideSquare normal shift center vertex 
    edgeEdgeCP = map (closestPoints f1) (edgesOfSquare normal shift center vertex)
    internalEdgeEdgeCP = filter (isRight.snd) edgeEdgeCP
    in 
        if segmentIntersectsPlane && insideSquare intersection
            then (Right intersection, Right intersection)
            else
        if not segmentIntersectsPlane && insideSquare closerEndProjection
            then (Left $ Vertex closerEnd, Right closerEndProjection)
            else
-}
-- aux
extractPoint :: Either Feature Vec -> Vec
extractPoint (Right p) = p
extractPoint (Left (Vertex p)) = p
extractPoint _ = error "extractPoint on non-vertex feature"
distanceToEdge point (Edge a b) = distToLine a b point
isRight (Right _) = True
isRight (Left _) = False