module Algorithms.Geometry.SoS.Orientation( SoS

                                          , sideTest
                                          , sideTest'

                                          , toSymbolic
                                          ) where

import Algorithms.Geometry.SoS.Determinant
import Algorithms.Geometry.SoS.Sign
import Algorithms.Geometry.SoS.Symbolic
import Control.Lens hiding (snoc,cons)
import Data.Ext
import Data.Geometry.Matrix
import Data.Geometry.Point
import Data.Geometry.Vector
import GHC.TypeNats

--------------------------------------------------------------------------------



-- | A dimension d has support for SoS when we can: compute a
-- dterminant of a d+1 by d+1 dimensional matrix.
type SoS d = (Arity d, HasDeterminant (d+1))

-- | Given a query point q, and a vector of d points defining a
-- hyperplane test if q lies above or below the hyperplane. Each point
-- is assumed to have an unique index of type i that can be used to
-- disambiguate it in case of degeneracies.
--
-- some 1D examples:
--
-- >>> sideTest (Point1 0 :+ 0) (Vector1 $ Point1 2 :+ 1)
-- Negative
-- >>> sideTest (Point1 10 :+ 0) (Vector1 $ Point1 2 :+ 1)
-- Positive
-- >>> sideTest (Point1 2 :+ 0) (Vector1 $ Point1 2 :+ 1)
-- Positive
-- >>> sideTest (Point1 2 :+ 3) (Vector1 $ Point1 2 :+ 1)
-- Negative
--
-- some 2D examples:
--
-- >>> sideTest (Point2 1 2 :+ 0) $ Vector2 (Point2 0 0 :+ 1) (Point2 2 2 :+ 3)
-- Positive
-- >>> sideTest (Point2 1 (-2) :+ 0) $ Vector2 (Point2 0 0 :+ 1) (Point2 2 2 :+ 3)
-- Negative
-- >>> sideTest (Point2 1 1 :+ 0) $ Vector2 (Point2 0 0 :+ 1) (Point2 2 2 :+ 3)
-- Positive
-- >>> sideTest (Point2 1 1 :+ 10) $ Vector2 (Point2 0 0 :+ 1) (Point2 2 2 :+ 3)
-- Negative
-- >>> sideTest (Point2 1 1 :+ 10) $ Vector2 (Point2 0 0 :+ 3) (Point2 2 2 :+ 1)
-- Negative
sideTest      :: (SoS d, Num r, Ord r, Ord i)
              => Point d r :+ i -> Vector d (Point d r :+ i) -> Sign
sideTest :: (Point d r :+ i) -> Vector d (Point d r :+ i) -> Sign
sideTest Point d r :+ i
q Vector d (Point d r :+ i)
ps = Vector (d + 1) (Point d (Symbolic (i, Int) r)) -> Sign
forall r i (d :: Nat).
(Num r, Ord r, Ord i, HasDeterminant (d + 1), Arity d,
 Arity (d + 1)) =>
Vector (d + 1) (Point d (Symbolic i r)) -> Sign
sideTest'' (Vector (d + 1) (Point d (Symbolic (i, Int) r)) -> Sign)
-> (Vector (d + 1) (Point d r :+ i)
    -> Vector (d + 1) (Point d (Symbolic (i, Int) r)))
-> Vector (d + 1) (Point d r :+ i)
-> Sign
forall b c a. (b -> c) -> (a -> b) -> a -> c
. ((Point d r :+ i) -> Point d (Symbolic (i, Int) r))
-> Vector (d + 1) (Point d r :+ i)
-> Vector (d + 1) (Point d (Symbolic (i, Int) r))
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap (Point d r :+ i) -> Point d (Symbolic (i, Int) r)
forall i (d :: Nat) r.
(Ord i, Arity d) =>
(Point d r :+ i) -> Point d (Symbolic (i, Int) r)
toSymbolic (Vector (d + 1) (Point d r :+ i) -> Sign)
-> Vector (d + 1) (Point d r :+ i) -> Sign
forall a b. (a -> b) -> a -> b
$ (Point d r :+ i)
-> Vector d (Point d r :+ i) -> Vector (d + 1) (Point d r :+ i)
forall (d :: Nat) r.
(Arity d, Arity (d + 1)) =>
r -> Vector d r -> Vector (d + 1) r
cons Point d r :+ i
q Vector d (Point d r :+ i)
ps

-- | Given an input point, transform its number type to include
-- symbolic $\varepsilon$ expressions so that we can use SoS.
toSymbolic          :: (Ord i, Arity d) => Point d r :+ i -> Point d (Symbolic (i,Int) r)
toSymbolic :: (Point d r :+ i) -> Point d (Symbolic (i, Int) r)
toSymbolic (Point d r
p :+ i
i) = Point d r
pPoint d r
-> (Point d r -> Point d (Symbolic (i, Int) r))
-> Point d (Symbolic (i, Int) r)
forall a b. a -> (a -> b) -> b
&(Vector d r -> Identity (Vector d (Symbolic (i, Int) r)))
-> Point d r -> Identity (Point d (Symbolic (i, Int) r))
forall (d :: Nat) r r'.
Lens (Point d r) (Point d r') (Vector d r) (Vector d r')
vector ((Vector d r -> Identity (Vector d (Symbolic (i, Int) r)))
 -> Point d r -> Identity (Point d (Symbolic (i, Int) r)))
-> (Vector d r -> Vector d (Symbolic (i, Int) r))
-> Point d r
-> Point d (Symbolic (i, Int) r)
forall s t a b. ASetter s t a b -> (a -> b) -> s -> t
%~ (Int -> r -> Symbolic (i, Int) r)
-> Vector d r -> Vector d (Symbolic (i, Int) r)
forall i (f :: * -> *) a b.
FunctorWithIndex i f =>
(i -> a -> b) -> f a -> f b
imap (\Int
j r
x -> r -> (i, Int) -> Symbolic (i, Int) r
forall i r. Ord i => r -> i -> Symbolic i r
symbolic r
x (i
i,Int
j))

-- | Given a point q and a vector of d points defining a hyperplane,
-- test on which side of the hyperplane q lies.
--
-- TODO: Specify what the sign means
sideTest'      :: (Num r, Ord r, Ord i, HasDeterminant (d+1), Arity d, Arity (d+1))
               => Point d (Symbolic i r) -> Vector d (Point d (Symbolic i r)) -> Sign
sideTest' :: Point d (Symbolic i r) -> Vector d (Point d (Symbolic i r)) -> Sign
sideTest' Point d (Symbolic i r)
q Vector d (Point d (Symbolic i r))
ps = Vector (d + 1) (Point d (Symbolic i r)) -> Sign
forall r i (d :: Nat).
(Num r, Ord r, Ord i, HasDeterminant (d + 1), Arity d,
 Arity (d + 1)) =>
Vector (d + 1) (Point d (Symbolic i r)) -> Sign
sideTest'' (Vector (d + 1) (Point d (Symbolic i r)) -> Sign)
-> Vector (d + 1) (Point d (Symbolic i r)) -> Sign
forall a b. (a -> b) -> a -> b
$ Point d (Symbolic i r)
-> Vector d (Point d (Symbolic i r))
-> Vector (d + 1) (Point d (Symbolic i r))
forall (d :: Nat) r.
(Arity d, Arity (d + 1)) =>
r -> Vector d r -> Vector (d + 1) r
cons Point d (Symbolic i r)
q Vector d (Point d (Symbolic i r))
ps

-- | Given a vector of points, tests if the point encoded in the first
-- row is above/below the hyperplane defined by the remaining points
-- (rows).
sideTest'' :: (Num r, Ord r, Ord i, HasDeterminant (d+1), Arity d, Arity (d+1))
           => Vector (d+1) (Point d (Symbolic i r)) -> Sign
sideTest'' :: Vector (d + 1) (Point d (Symbolic i r)) -> Sign
sideTest'' = Matrix (d + 1) (d + 1) (Symbolic i r) -> Sign
forall (d :: Nat) i r.
(HasDeterminant d, Ord i, Num r, Ord r) =>
Matrix d d (Symbolic i r) -> Sign
signDet (Matrix (d + 1) (d + 1) (Symbolic i r) -> Sign)
-> (Vector (d + 1) (Point d (Symbolic i r))
    -> Matrix (d + 1) (d + 1) (Symbolic i r))
-> Vector (d + 1) (Point d (Symbolic i r))
-> Sign
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Vector (d + 1) (Vector (d + 1) (Symbolic i r))
-> Matrix (d + 1) (d + 1) (Symbolic i r)
forall (n :: Nat) (m :: Nat) r.
Vector n (Vector m r) -> Matrix n m r
Matrix (Vector (d + 1) (Vector (d + 1) (Symbolic i r))
 -> Matrix (d + 1) (d + 1) (Symbolic i r))
-> (Vector (d + 1) (Point d (Symbolic i r))
    -> Vector (d + 1) (Vector (d + 1) (Symbolic i r)))
-> Vector (d + 1) (Point d (Symbolic i r))
-> Matrix (d + 1) (d + 1) (Symbolic i r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (Point d (Symbolic i r) -> Vector (d + 1) (Symbolic i r))
-> Vector (d + 1) (Point d (Symbolic i r))
-> Vector (d + 1) (Vector (d + 1) (Symbolic i r))
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap Point d (Symbolic i r) -> Vector (d + 1) (Symbolic i r)
forall r (d :: Nat).
(Num r, Arity d, Arity (d + 1)) =>
Point d r -> Vector (d + 1) r
mkLambdaRow

-- | Given a point produces the vector/row corresponding to this point
-- in a homogeneous matrix represetnation. I.e. we add a 1 as an
-- additonal column at the end.
mkLambdaRow :: (Num r, Arity d, Arity (d+1)) => Point d r -> Vector (d+1) r
mkLambdaRow :: Point d r -> Vector (d + 1) r
mkLambdaRow = (Vector d r -> r -> Vector (d + 1) r)
-> r -> Vector d r -> Vector (d + 1) r
forall a b c. (a -> b -> c) -> b -> a -> c
flip Vector d r -> r -> Vector (d + 1) r
forall (d :: Nat) r.
(Arity (d + 1), Arity d) =>
Vector d r -> r -> Vector (d + 1) r
snoc r
1 (Vector d r -> Vector (d + 1) r)
-> (Point d r -> Vector d r) -> Point d r -> Vector (d + 1) r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Getting (Vector d r) (Point d r) (Vector d r)
-> Point d r -> Vector d r
forall s (m :: * -> *) a. MonadReader s m => Getting a s a -> m a
view Getting (Vector d r) (Point d r) (Vector d r)
forall (d :: Nat) r r'.
Lens (Point d r) (Point d r') (Vector d r) (Vector d r')
vector