--------------------------------------------------------------------------------
-- |
-- Module      :  Algorithms.Geometry.WSPD
-- Copyright   :  (C) Frank Staals
-- License     :  see the LICENSE file
-- Maintainer  :  Frank Staals
--
-- Algorithm to construct a well separated pair decomposition (wspd).
--
--------------------------------------------------------------------------------
module Algorithms.Geometry.WSPD
  ( fairSplitTree
  , wellSeparatedPairs
  , NodeData(NodeData)
  , WSP
  , SplitTree
  , nodeData
  , Level(..)
  , reIndexPoints
  , distributePoints
  , distributePoints'
  ) where

import           Algorithms.Geometry.WSPD.Types
import           Control.Lens hiding (Level, levels)
import           Control.Monad.Reader
import           Control.Monad.ST (ST,runST)
import           Data.BinaryTree
import           Data.Ext
import qualified Data.Foldable as F
import           Data.Geometry.Box
import           Data.Geometry.Point
-- import           Data.Geometry.Properties
-- import           Data.Geometry.Transformation
import           Data.Geometry.Vector
import qualified Data.Geometry.Vector as GV
import qualified Data.IntMap.Strict as IntMap
import qualified Data.LSeq as LSeq
import           Data.LSeq (LSeq, toSeq,pattern (:<|))
import qualified Data.List as L
import qualified Data.List.NonEmpty as NonEmpty
import           Data.Maybe
import           Data.Ord (comparing)
import           Data.Range
import qualified Data.Range as Range
import qualified Data.Sequence as S
import qualified Data.Vector as V
import qualified Data.Vector.Mutable as MV
import           GHC.TypeLits

-- import           Debug.Trace

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

-- | Construct a split tree
--
-- running time: \(O(n \log n)\)
fairSplitTree     :: (Fractional r, Ord r, Arity d, 1 <= d
                     , Show r, Show p
                     )
                  => NonEmpty.NonEmpty (Point d r :+ p) -> SplitTree d p r ()
fairSplitTree :: NonEmpty (Point d r :+ p) -> SplitTree d p r ()
fairSplitTree NonEmpty (Point d r :+ p)
pts = (SplitTree d p r ()
 -> Int -> SplitTree d p r () -> SplitTree d p r ())
-> ((Point d r :+ p) -> SplitTree d p r ())
-> BinLeafTree Int (Point d r :+ p)
-> SplitTree d p r ()
forall b v a.
(b -> v -> b -> b) -> (a -> b) -> BinLeafTree v a -> b
foldUp SplitTree d p r ()
-> Int -> SplitTree d p r () -> SplitTree d p r ()
forall (d :: Nat) r p.
(ImplicitPeano (Peano d), ArityPeano (Peano (FromPeano (Peano d))),
 KnownNat (FromPeano (Peano d)), KnownNat d, Ord r,
 Peano (FromPeano (Peano d) + 1)
 ~ 'S (Peano (FromPeano (Peano d)))) =>
BinLeafTree (NodeData d r ()) (Point d r :+ p)
-> Int
-> BinLeafTree (NodeData d r ()) (Point d r :+ p)
-> BinLeafTree (NodeData d r ()) (Point d r :+ p)
node' (Point d r :+ p) -> SplitTree d p r ()
forall v a. a -> BinLeafTree v a
Leaf (BinLeafTree Int (Point d r :+ p) -> SplitTree d p r ())
-> BinLeafTree Int (Point d r :+ p) -> SplitTree d p r ()
forall a b. (a -> b) -> a -> b
$ Int
-> Vector d (PointSeq d (Int :+ p) r)
-> BinLeafTree Int (Point d r :+ p)
forall r (d :: Nat) p.
(Fractional r, Ord r, Arity d, 1 <= d, Show r, Show p) =>
Int
-> Vector d (PointSeq d (Int :+ p) r)
-> BinLeafTree Int (Point d r :+ p)
fairSplitTree' Int
n Vector d (PointSeq d (Int :+ p) r)
pts'
  where
    pts' :: Vector d (PointSeq d (Int :+ p) r)
pts' = (Int
 -> NonEmpty (Point d r :+ (Int :+ p)) -> PointSeq d (Int :+ p) r)
-> Vector d (NonEmpty (Point d r :+ (Int :+ p)))
-> Vector d (PointSeq d (Int :+ p) r)
forall i (f :: * -> *) a b.
FunctorWithIndex i f =>
(i -> a -> b) -> f a -> f b
imap Int
-> NonEmpty (Point d r :+ (Int :+ p)) -> PointSeq d (Int :+ p) r
forall (d :: Nat) r (p :: Nat -> * -> *) extra.
(ImplicitPeano (Peano d), Ord r,
 ArityPeano (Peano (FromPeano (Peano d))),
 KnownNat (FromPeano (Peano d)), KnownNat d, AsAPoint p,
 Peano (FromPeano (Peano d) + 1)
 ~ 'S (Peano (FromPeano (Peano d)))) =>
Int -> NonEmpty (p d r :+ extra) -> LSeq 1 (p d r :+ extra)
sortOn (Vector d (NonEmpty (Point d r :+ (Int :+ p)))
 -> Vector d (PointSeq d (Int :+ p) r))
-> (NonEmpty (Point d r :+ p)
    -> Vector d (NonEmpty (Point d r :+ (Int :+ p))))
-> NonEmpty (Point d r :+ p)
-> Vector d (PointSeq d (Int :+ p) r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. NonEmpty (Point d r :+ (Int :+ p))
-> Vector d (NonEmpty (Point d r :+ (Int :+ p)))
forall (f :: * -> *) a. Applicative f => a -> f a
pure (NonEmpty (Point d r :+ (Int :+ p))
 -> Vector d (NonEmpty (Point d r :+ (Int :+ p))))
-> (NonEmpty (Point d r :+ p)
    -> NonEmpty (Point d r :+ (Int :+ p)))
-> NonEmpty (Point d r :+ p)
-> Vector d (NonEmpty (Point d r :+ (Int :+ p)))
forall b c a. (b -> c) -> (a -> b) -> a -> c
. NonEmpty (Point d r :+ p) -> NonEmpty (Point d r :+ (Int :+ p))
forall extra.
NonEmpty (Point d r :+ extra)
-> NonEmpty (Point d r :+ (Int :+ extra))
g (NonEmpty (Point d r :+ p) -> Vector d (PointSeq d (Int :+ p) r))
-> NonEmpty (Point d r :+ p) -> Vector d (PointSeq d (Int :+ p) r)
forall a b. (a -> b) -> a -> b
$ NonEmpty (Point d r :+ p)
pts
    n :: Int
n    = PointSeq d (Int :+ p) r -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length (PointSeq d (Int :+ p) r -> Int) -> PointSeq d (Int :+ p) r -> Int
forall a b. (a -> b) -> a -> b
$ Vector d (PointSeq d (Int :+ p) r)
pts'Vector d (PointSeq d (Int :+ p) r)
-> Getting
     (PointSeq d (Int :+ p) r)
     (Vector d (PointSeq d (Int :+ p) r))
     (PointSeq d (Int :+ p) r)
-> PointSeq d (Int :+ p) r
forall s a. s -> Getting a s a -> a
^.C 0
-> Lens'
     (Vector d (PointSeq d (Int :+ p) r)) (PointSeq d (Int :+ p) r)
forall (proxy :: Nat -> *) (i :: Nat) (d :: Nat) r.
(Arity d, KnownNat i, (i + 1) <= d) =>
proxy i -> Lens' (Vector d r) r
GV.element (C 0
forall (n :: Nat). C n
C :: C 0)

    sortOn' :: Int -> NonEmpty (p d r :+ extra) -> NonEmpty (p d r :+ extra)
sortOn' Int
i = ((p d r :+ extra) -> r)
-> NonEmpty (p d r :+ extra) -> NonEmpty (p d r :+ extra)
forall o a. Ord o => (a -> o) -> NonEmpty a -> NonEmpty a
NonEmpty.sortWith ((p d r :+ extra) -> Getting r (p d r :+ extra) r -> r
forall s a. s -> Getting a s a -> a
^.(p d r -> Const r (p d r))
-> (p d r :+ extra) -> Const r (p d r :+ extra)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core((p d r -> Const r (p d r))
 -> (p d r :+ extra) -> Const r (p d r :+ extra))
-> ((r -> Const r r) -> p d r -> Const r (p d r))
-> Getting r (p d r :+ extra) r
forall b c a. (b -> c) -> (a -> b) -> a -> c
.Int -> Lens' (p d r) r
forall (d :: Nat) (p :: Nat -> * -> *) r.
(Arity d, AsAPoint p) =>
Int -> Lens' (p d r) r
unsafeCoord Int
i)
    sortOn :: Int -> NonEmpty (p d r :+ extra) -> LSeq 1 (p d r :+ extra)
sortOn  Int
i = NonEmpty (p d r :+ extra) -> LSeq 1 (p d r :+ extra)
forall a. NonEmpty a -> LSeq 1 a
LSeq.fromNonEmpty (NonEmpty (p d r :+ extra) -> LSeq 1 (p d r :+ extra))
-> (NonEmpty (p d r :+ extra) -> NonEmpty (p d r :+ extra))
-> NonEmpty (p d r :+ extra)
-> LSeq 1 (p d r :+ extra)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Int -> NonEmpty (p d r :+ extra) -> NonEmpty (p d r :+ extra)
forall (d :: Nat) r (p :: Nat -> * -> *) extra.
(ImplicitPeano (Peano d), Ord r,
 ArityPeano (Peano (FromPeano (Peano d))),
 KnownNat (FromPeano (Peano d)), KnownNat d, AsAPoint p,
 Peano (FromPeano (Peano d) + 1)
 ~ 'S (Peano (FromPeano (Peano d)))) =>
Int -> NonEmpty (p d r :+ extra) -> NonEmpty (p d r :+ extra)
sortOn' (Int
i Int -> Int -> Int
forall a. Num a => a -> a -> a
+ Int
1)
    -- sorts the points on the first coordinate, and then associates each point
    -- with an index,; its rank in terms of this first coordinate.
    g :: NonEmpty (Point d r :+ extra)
-> NonEmpty (Point d r :+ (Int :+ extra))
g = (Int -> (Point d r :+ extra) -> Point d r :+ (Int :+ extra))
-> NonEmpty Int
-> NonEmpty (Point d r :+ extra)
-> NonEmpty (Point d r :+ (Int :+ extra))
forall a b c.
(a -> b -> c) -> NonEmpty a -> NonEmpty b -> NonEmpty c
NonEmpty.zipWith (\Int
i (Point d r
p :+ extra
e) -> Point d r
p Point d r -> (Int :+ extra) -> Point d r :+ (Int :+ extra)
forall core extra. core -> extra -> core :+ extra
:+ (Int
i Int -> extra -> Int :+ extra
forall core extra. core -> extra -> core :+ extra
:+ extra
e)) ([Int] -> NonEmpty Int
forall a. [a] -> NonEmpty a
NonEmpty.fromList [Int
0..])
      (NonEmpty (Point d r :+ extra)
 -> NonEmpty (Point d r :+ (Int :+ extra)))
-> (NonEmpty (Point d r :+ extra) -> NonEmpty (Point d r :+ extra))
-> NonEmpty (Point d r :+ extra)
-> NonEmpty (Point d r :+ (Int :+ extra))
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Int
-> NonEmpty (Point d r :+ extra) -> NonEmpty (Point d r :+ extra)
forall (d :: Nat) r (p :: Nat -> * -> *) extra.
(ImplicitPeano (Peano d), Ord r,
 ArityPeano (Peano (FromPeano (Peano d))),
 KnownNat (FromPeano (Peano d)), KnownNat d, AsAPoint p,
 Peano (FromPeano (Peano d) + 1)
 ~ 'S (Peano (FromPeano (Peano d)))) =>
Int -> NonEmpty (p d r :+ extra) -> NonEmpty (p d r :+ extra)
sortOn' Int
1

    -- node' :: b -> a -> b -> b
    -- node'       :: SplitTree d p r () -> Int -> SplitTree d p r () -> SplitTree d p r ()
    node' :: BinLeafTree (NodeData d r ()) (Point d r :+ p)
-> Int
-> BinLeafTree (NodeData d r ()) (Point d r :+ p)
-> BinLeafTree (NodeData d r ()) (Point d r :+ p)
node' BinLeafTree (NodeData d r ()) (Point d r :+ p)
l Int
j BinLeafTree (NodeData d r ()) (Point d r :+ p)
r = BinLeafTree (NodeData d r ()) (Point d r :+ p)
-> NodeData d r ()
-> BinLeafTree (NodeData d r ()) (Point d r :+ p)
-> BinLeafTree (NodeData d r ()) (Point d r :+ p)
forall v a.
BinLeafTree v a -> v -> BinLeafTree v a -> BinLeafTree v a
Node BinLeafTree (NodeData d r ()) (Point d r :+ p)
l (Int -> Box d () r -> () -> NodeData d r ()
forall (d :: Nat) r a. Int -> Box d () r -> a -> NodeData d r a
NodeData Int
j (BinLeafTree (NodeData d r ()) (Point d r :+ p) -> Box d () r
forall r (d :: Nat) p a. Ord r => SplitTree d p r a -> Box d () r
bbOf BinLeafTree (NodeData d r ()) (Point d r :+ p)
l Box d () r -> Box d () r -> Box d () r
forall a. Semigroup a => a -> a -> a
<> BinLeafTree (NodeData d r ()) (Point d r :+ p) -> Box d () r
forall r (d :: Nat) p a. Ord r => SplitTree d p r a -> Box d () r
bbOf BinLeafTree (NodeData d r ()) (Point d r :+ p)
r) ()) BinLeafTree (NodeData d r ()) (Point d r :+ p)
r


-- | Given a split tree, generate the Well separated pairs
--
-- running time: \(O(s^d n)\)
wellSeparatedPairs   :: (Floating r, Ord r, Arity d, Arity (d + 1))
                     => r -> SplitTree d p r a -> [WSP d p r a]
wellSeparatedPairs :: r -> SplitTree d p r a -> [WSP d p r a]
wellSeparatedPairs r
s = SplitTree d p r a -> [WSP d p r a]
f
  where
    f :: SplitTree d p r a -> [WSP d p r a]
f (Leaf Point d r :+ p
_)     = []
    f (Node SplitTree d p r a
l NodeData d r a
_ SplitTree d p r a
r) = r -> SplitTree d p r a -> SplitTree d p r a -> [WSP d p r a]
forall r (d :: Nat) p a.
(Floating r, Ord r, Arity d, Arity (d + 1)) =>
r -> SplitTree d p r a -> SplitTree d p r a -> [WSP d p r a]
findPairs r
s SplitTree d p r a
l SplitTree d p r a
r [WSP d p r a] -> [WSP d p r a] -> [WSP d p r a]
forall a. [a] -> [a] -> [a]
++ SplitTree d p r a -> [WSP d p r a]
f SplitTree d p r a
l [WSP d p r a] -> [WSP d p r a] -> [WSP d p r a]
forall a. [a] -> [a] -> [a]
++ SplitTree d p r a -> [WSP d p r a]
f SplitTree d p r a
r



-- -- | Given a split tree, generate the well separated pairs such that one set is
-- -- a singleton.
-- -- running time: \(O(s^d n\log n)\)
-- wellSeparatedPairSingletons   :: (Fractional r, Ord r, AlwaysTrueWSPD d)
--                               => r -> SplitTree d p r a -> [(Point d r :+ p, PointSet d p r (Sized a))]
-- wellSeparatedPairSingletons s t = concatMap split $ wellSeparatedPairs s t'
--   where
--     split (l,r) = undefined
--       -- | measure l <= measure r = map (,r) $ F.toList l
--       -- | otherwise              = map (,l) $ F.toList r
--     t' = foldUpData (\l nd r -> )

--     t


--------------------------------------------------------------------------------
-- * Building the split tree

-- | Given the points, sorted in every dimension, recursively build a split tree
--
-- The algorithm works in rounds. Each round takes \( O(n) \) time, and halves the
-- number of points. Thus, the total running time is \( O(n log n) \).
--
-- The algorithm essentially builds a path in the split tree; at every node on
-- the path that we construct, we split the point set into two sets (L,R)
-- according to the longest side of the bounding box.
--
-- The smaller set is "assigned" to the current node and set asside. We
-- continue to build the path with the larger set until the total number of
-- items remaining is less than n/2.
--
-- To start the next round, each node on the path needs to have the points
-- assigned to that node, sorted in each dimension (i.e. the Vector
-- (PointSeq))'s. Since we have the level assignment, we can compute these
-- lists by traversing each original input list (i.e. one for every dimension)
-- once, and partition the points based on their level assignment.
fairSplitTree'       :: (Fractional r, Ord r, Arity d, 1 <= d
                        , Show r, Show p
                        )
                     => Int -> GV.Vector d (PointSeq d (Idx :+ p) r)
                     -> BinLeafTree Int (Point d r :+ p)
fairSplitTree' :: Int
-> Vector d (PointSeq d (Int :+ p) r)
-> BinLeafTree Int (Point d r :+ p)
fairSplitTree' Int
n Vector d (PointSeq d (Int :+ p) r)
pts
    | Int
n Int -> Int -> Bool
forall a. Ord a => a -> a -> Bool
<= Int
1    = let p :: Point d r :+ (Int :+ p)
p = LSeq (1 + 0) (Point d r :+ (Int :+ p)) -> Point d r :+ (Int :+ p)
forall (n :: Nat) a. LSeq (1 + n) a -> a
LSeq.head (LSeq (1 + 0) (Point d r :+ (Int :+ p)) -> Point d r :+ (Int :+ p))
-> LSeq (1 + 0) (Point d r :+ (Int :+ p))
-> Point d r :+ (Int :+ p)
forall a b. (a -> b) -> a -> b
$ Vector d (PointSeq d (Int :+ p) r)
ptsVector d (PointSeq d (Int :+ p) r)
-> Getting
     (PointSeq d (Int :+ p) r)
     (Vector d (PointSeq d (Int :+ p) r))
     (PointSeq d (Int :+ p) r)
-> PointSeq d (Int :+ p) r
forall s a. s -> Getting a s a -> a
^.C 0
-> Lens'
     (Vector d (PointSeq d (Int :+ p) r)) (PointSeq d (Int :+ p) r)
forall (proxy :: Nat -> *) (i :: Nat) (d :: Nat) r.
(Arity d, KnownNat i, (i + 1) <= d) =>
proxy i -> Lens' (Vector d r) r
GV.element (C 0
forall (n :: Nat). C n
C :: C 0) in (Point d r :+ p) -> BinLeafTree Int (Point d r :+ p)
forall v a. a -> BinLeafTree v a
Leaf ((Point d r :+ (Int :+ p)) -> Point d r :+ p
forall core t extra. (core :+ (t :+ extra)) -> core :+ extra
dropIdx Point d r :+ (Int :+ p)
p)
    | Bool
otherwise = ((Level, BinLeafTree Int (Point d r :+ p))
 -> BinLeafTree Int (Point d r :+ p)
 -> BinLeafTree Int (Point d r :+ p))
-> BinLeafTree Int (Point d r :+ p)
-> Vector (Level, BinLeafTree Int (Point d r :+ p))
-> BinLeafTree Int (Point d r :+ p)
forall (t :: * -> *) a b.
Foldable t =>
(a -> b -> b) -> b -> t a -> b
foldr (Level, BinLeafTree Int (Point d r :+ p))
-> BinLeafTree Int (Point d r :+ p)
-> BinLeafTree Int (Point d r :+ p)
forall a.
(Level, BinLeafTree Int a)
-> BinLeafTree Int a -> BinLeafTree Int a
node' (Vector (BinLeafTree Int (Point d r :+ p))
-> BinLeafTree Int (Point d r :+ p)
forall a. Vector a -> a
V.last Vector (BinLeafTree Int (Point d r :+ p))
path) (Vector (Level, BinLeafTree Int (Point d r :+ p))
 -> BinLeafTree Int (Point d r :+ p))
-> Vector (Level, BinLeafTree Int (Point d r :+ p))
-> BinLeafTree Int (Point d r :+ p)
forall a b. (a -> b) -> a -> b
$ Vector Level
-> Vector (BinLeafTree Int (Point d r :+ p))
-> Vector (Level, BinLeafTree Int (Point d r :+ p))
forall a b. Vector a -> Vector b -> Vector (a, b)
V.zip Vector Level
nodeLevels (Vector (BinLeafTree Int (Point d r :+ p))
-> Vector (BinLeafTree Int (Point d r :+ p))
forall a. Vector a -> Vector a
V.init Vector (BinLeafTree Int (Point d r :+ p))
path)
  where
    -- note that points may also be assigned level 'Nothing'.
    (Vector (Maybe Level)
levels, nodeLevels' :: NonEmpty Level
nodeLevels'@(Level
maxLvl NonEmpty.:| [Level]
_)) = (forall s. ST s (Vector (Maybe Level), NonEmpty Level))
-> (Vector (Maybe Level), NonEmpty Level)
forall a. (forall s. ST s a) -> a
runST ((forall s. ST s (Vector (Maybe Level), NonEmpty Level))
 -> (Vector (Maybe Level), NonEmpty Level))
-> (forall s. ST s (Vector (Maybe Level), NonEmpty Level))
-> (Vector (Maybe Level), NonEmpty Level)
forall a b. (a -> b) -> a -> b
$ do
        MVector s (Maybe Level)
lvls  <- Int
-> Maybe Level -> ST s (MVector (PrimState (ST s)) (Maybe Level))
forall (m :: * -> *) a.
PrimMonad m =>
Int -> a -> m (MVector (PrimState m) a)
MV.replicate Int
n Maybe Level
forall a. Maybe a
Nothing
        NonEmpty Level
ls    <- ReaderT (MVector s (Maybe Level)) (ST s) (NonEmpty Level)
-> MVector s (Maybe Level) -> ST s (NonEmpty Level)
forall r (m :: * -> *) a. ReaderT r m a -> r -> m a
runReaderT (Int
-> Int
-> Vector d (PointSeq d (Int :+ p) r)
-> Level
-> [Level]
-> ReaderT (MVector s (Maybe Level)) (ST s) (NonEmpty Level)
forall r (d :: Nat) p s.
(Fractional r, Ord r, Arity d, Show r, Show p) =>
Int
-> Int
-> Vector d (PointSeq d (Int :+ p) r)
-> Level
-> [Level]
-> RST s (NonEmpty Level)
assignLevels (Int
n Int -> Int -> Int
forall a. Integral a => a -> a -> a
`div` Int
2) Int
0 Vector d (PointSeq d (Int :+ p) r)
pts (Int -> Maybe Int -> Level
Level Int
0 Maybe Int
forall a. Maybe a
Nothing) []) MVector s (Maybe Level)
lvls
        Vector (Maybe Level)
lvls' <- MVector (PrimState (ST s)) (Maybe Level)
-> ST s (Vector (Maybe Level))
forall (m :: * -> *) a.
PrimMonad m =>
MVector (PrimState m) a -> m (Vector a)
V.unsafeFreeze MVector s (Maybe Level)
MVector (PrimState (ST s)) (Maybe Level)
lvls
        (Vector (Maybe Level), NonEmpty Level)
-> ST s (Vector (Maybe Level), NonEmpty Level)
forall (f :: * -> *) a. Applicative f => a -> f a
pure (Vector (Maybe Level)
lvls',NonEmpty Level
ls)

    -- TODO: We also need to report the levels in the order in which they are
    -- assigned to nodes

    nodeLevels :: Vector Level
nodeLevels = [Level] -> Vector Level
forall a. [a] -> Vector a
V.fromList ([Level] -> Vector Level)
-> (NonEmpty Level -> [Level]) -> NonEmpty Level -> Vector Level
forall b c a. (b -> c) -> (a -> b) -> a -> c
. [Level] -> [Level]
forall a. [a] -> [a]
L.reverse ([Level] -> [Level])
-> (NonEmpty Level -> [Level]) -> NonEmpty Level -> [Level]
forall b c a. (b -> c) -> (a -> b) -> a -> c
. NonEmpty Level -> [Level]
forall a. NonEmpty a -> [a]
NonEmpty.toList (NonEmpty Level -> Vector Level) -> NonEmpty Level -> Vector Level
forall a b. (a -> b) -> a -> b
$ NonEmpty Level
nodeLevels'

    -- levels = traceShow ("Levels",levels',maxLvl) levels'

    -- path = traceShow ("path", path',nodeLevels) path'
    distrPts :: Vector (Vector d (PointSeq d (Int :+ p) r))
distrPts = Int
-> Vector (Maybe Level)
-> Vector d (PointSeq d (Int :+ p) r)
-> Vector (Vector d (PointSeq d (Int :+ p) r))
forall (d :: Nat) r p.
(Arity d, Show r, Show p) =>
Int
-> Vector (Maybe Level)
-> Vector d (PointSeq d (Int :+ p) r)
-> Vector (Vector d (PointSeq d (Int :+ p) r))
distributePoints (Int
1 Int -> Int -> Int
forall a. Num a => a -> a -> a
+ Level
maxLvlLevel -> Getting Int Level Int -> Int
forall s a. s -> Getting a s a -> a
^.Getting Int Level Int
Lens' Level Int
unLevel) Vector (Maybe Level)
levels Vector d (PointSeq d (Int :+ p) r)
pts

    path :: Vector (BinLeafTree Int (Point d r :+ p))
path = Vector d (PointSeq d (Int :+ p) r)
-> BinLeafTree Int (Point d r :+ p)
forall (d :: Nat) r p.
(ImplicitPeano (Peano d), Ord r, Fractional r,
 ArityPeano (Peano (FromPeano (Peano d))),
 KnownNat (FromPeano (Peano d)), KnownNat d, Show r, Show p,
 (1 <=? d) ~ 'True,
 Peano (FromPeano (Peano d) + 1)
 ~ 'S (Peano (FromPeano (Peano d)))) =>
Vector d (LSeq 1 (Point d r :+ (Int :+ p)))
-> BinLeafTree Int (Point d r :+ p)
recurse (Vector d (PointSeq d (Int :+ p) r)
 -> BinLeafTree Int (Point d r :+ p))
-> Vector (Vector d (PointSeq d (Int :+ p) r))
-> Vector (BinLeafTree Int (Point d r :+ p))
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> Vector (Vector d (PointSeq d (Int :+ p) r))
distrPts -- (traceShow ("distributed pts",distrPts) distrPts)

    -- node' (lvl,lc) rc | traceShow ("node' ",lvl,lc,rc) False = undefined
    node' :: (Level, BinLeafTree Int a)
-> BinLeafTree Int a -> BinLeafTree Int a
node' (Level
lvl,BinLeafTree Int a
lc) BinLeafTree Int a
rc = case Level
lvlLevel -> Getting (First Int) Level Int -> Maybe Int
forall s a. s -> Getting (First a) s a -> Maybe a
^?(Maybe Int -> Const (First Int) (Maybe Int))
-> Level -> Const (First Int) Level
Lens' Level (Maybe Int)
widestDim((Maybe Int -> Const (First Int) (Maybe Int))
 -> Level -> Const (First Int) Level)
-> ((Int -> Const (First Int) Int)
    -> Maybe Int -> Const (First Int) (Maybe Int))
-> Getting (First Int) Level Int
forall b c a. (b -> c) -> (a -> b) -> a -> c
.(Int -> Const (First Int) Int)
-> Maybe Int -> Const (First Int) (Maybe Int)
forall a b. Prism (Maybe a) (Maybe b) a b
_Just of
                          Maybe Int
Nothing -> [Char] -> BinLeafTree Int a
forall a. HasCallStack => [Char] -> a
error [Char]
"Unknown widest dimension"
                          Just Int
j  -> BinLeafTree Int a -> Int -> BinLeafTree Int a -> BinLeafTree Int a
forall v a.
BinLeafTree v a -> v -> BinLeafTree v a -> BinLeafTree v a
Node BinLeafTree Int a
lc Int
j BinLeafTree Int a
rc
    recurse :: Vector d (LSeq 1 (Point d r :+ (Int :+ p)))
-> BinLeafTree Int (Point d r :+ p)
recurse Vector d (LSeq 1 (Point d r :+ (Int :+ p)))
pts' = Int
-> Vector d (LSeq 1 (Point d r :+ (Int :+ p)))
-> BinLeafTree Int (Point d r :+ p)
forall r (d :: Nat) p.
(Fractional r, Ord r, Arity d, 1 <= d, Show r, Show p) =>
Int
-> Vector d (PointSeq d (Int :+ p) r)
-> BinLeafTree Int (Point d r :+ p)
fairSplitTree' (LSeq 1 (Point d r :+ (Int :+ p)) -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length (LSeq 1 (Point d r :+ (Int :+ p)) -> Int)
-> LSeq 1 (Point d r :+ (Int :+ p)) -> Int
forall a b. (a -> b) -> a -> b
$ Vector d (LSeq 1 (Point d r :+ (Int :+ p)))
pts'Vector d (LSeq 1 (Point d r :+ (Int :+ p)))
-> Getting
     (LSeq 1 (Point d r :+ (Int :+ p)))
     (Vector d (LSeq 1 (Point d r :+ (Int :+ p))))
     (LSeq 1 (Point d r :+ (Int :+ p)))
-> LSeq 1 (Point d r :+ (Int :+ p))
forall s a. s -> Getting a s a -> a
^.C 0
-> Lens'
     (Vector d (LSeq 1 (Point d r :+ (Int :+ p))))
     (LSeq 1 (Point d r :+ (Int :+ p)))
forall (proxy :: Nat -> *) (i :: Nat) (d :: Nat) r.
(Arity d, KnownNat i, (i + 1) <= d) =>
proxy i -> Lens' (Vector d r) r
GV.element (C 0
forall (n :: Nat). C n
C :: C 0))
                                  (Vector d (LSeq 1 (Point d r :+ (Int :+ p)))
-> Vector d (LSeq 1 (Point d r :+ (Int :+ p)))
forall (d :: Nat) p r.
(Arity d, 1 <= d) =>
Vector d (PointSeq d (Int :+ p) r)
-> Vector d (PointSeq d (Int :+ p) r)
reIndexPoints Vector d (LSeq 1 (Point d r :+ (Int :+ p)))
pts')

-- | Assign the points to their the correct class. The 'Nothing' class is
-- considered the last class
distributePoints          :: (Arity d , Show r, Show p)
                          => Int -> V.Vector (Maybe Level)
                          -> GV.Vector d (PointSeq d (Idx :+ p) r)
                          -> V.Vector (GV.Vector d (PointSeq d (Idx :+ p) r))
distributePoints :: Int
-> Vector (Maybe Level)
-> Vector d (PointSeq d (Int :+ p) r)
-> Vector (Vector d (PointSeq d (Int :+ p) r))
distributePoints Int
k Vector (Maybe Level)
levels = Vector d (Vector (PointSeq d (Int :+ p) r))
-> Vector (Vector d (PointSeq d (Int :+ p) r))
forall (d :: Nat) a.
Arity d =>
Vector d (Vector a) -> Vector (Vector d a)
transpose (Vector d (Vector (PointSeq d (Int :+ p) r))
 -> Vector (Vector d (PointSeq d (Int :+ p) r)))
-> (Vector d (PointSeq d (Int :+ p) r)
    -> Vector d (Vector (PointSeq d (Int :+ p) r)))
-> Vector d (PointSeq d (Int :+ p) r)
-> Vector (Vector d (PointSeq d (Int :+ p) r))
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (PointSeq d (Int :+ p) r -> Vector (PointSeq d (Int :+ p) r))
-> Vector d (PointSeq d (Int :+ p) r)
-> Vector d (Vector (PointSeq d (Int :+ p) r))
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap (Int
-> Vector (Maybe Level)
-> PointSeq d (Int :+ p) r
-> Vector (PointSeq d (Int :+ p) r)
forall (d :: Nat) p r.
Int
-> Vector (Maybe Level)
-> PointSeq d (Int :+ p) r
-> Vector (PointSeq d (Int :+ p) r)
distributePoints' Int
k Vector (Maybe Level)
levels)

transpose :: Arity d => GV.Vector d (V.Vector a) -> V.Vector (GV.Vector d a)
transpose :: Vector d (Vector a) -> Vector (Vector d a)
transpose = [Vector d a] -> Vector (Vector d a)
forall a. [a] -> Vector a
V.fromList ([Vector d a] -> Vector (Vector d a))
-> (Vector d (Vector a) -> [Vector d a])
-> Vector d (Vector a)
-> Vector (Vector d a)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. ([a] -> Vector d a) -> [[a]] -> [Vector d a]
forall a b. (a -> b) -> [a] -> [b]
map [a] -> Vector d a
forall (d :: Nat) r. Arity d => [r] -> Vector d r
GV.vectorFromListUnsafe ([[a]] -> [Vector d a])
-> (Vector d (Vector a) -> [[a]])
-> Vector d (Vector a)
-> [Vector d a]
forall b c a. (b -> c) -> (a -> b) -> a -> c
. [[a]] -> [[a]]
forall a. [[a]] -> [[a]]
L.transpose
          ([[a]] -> [[a]])
-> (Vector d (Vector a) -> [[a]]) -> Vector d (Vector a) -> [[a]]
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (Vector a -> [a]) -> [Vector a] -> [[a]]
forall a b. (a -> b) -> [a] -> [b]
map Vector a -> [a]
forall a. Vector a -> [a]
V.toList ([Vector a] -> [[a]])
-> (Vector d (Vector a) -> [Vector a])
-> Vector d (Vector a)
-> [[a]]
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Vector d (Vector a) -> [Vector a]
forall (t :: * -> *) a. Foldable t => t a -> [a]
F.toList

-- | Assign the points to their the correct class. The 'Nothing' class is
-- considered the last class
distributePoints'              :: Int                      -- ^ number of classes
                               -> V.Vector (Maybe Level)   -- ^ level assignment
                               -> PointSeq d (Idx :+ p) r  -- ^ input points
                               -> V.Vector (PointSeq d (Idx :+ p) r)
distributePoints' :: Int
-> Vector (Maybe Level)
-> PointSeq d (Int :+ p) r
-> Vector (PointSeq d (Int :+ p) r)
distributePoints' Int
k Vector (Maybe Level)
levels PointSeq d (Int :+ p) r
pts
  = (Seq (Point d r :+ (Int :+ p)) -> PointSeq d (Int :+ p) r)
-> Vector (Seq (Point d r :+ (Int :+ p)))
-> Vector (PointSeq d (Int :+ p) r)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap Seq (Point d r :+ (Int :+ p)) -> PointSeq d (Int :+ p) r
forall a (n :: Nat). Seq a -> LSeq n a
fromSeqUnsafe (Vector (Seq (Point d r :+ (Int :+ p)))
 -> Vector (PointSeq d (Int :+ p) r))
-> Vector (Seq (Point d r :+ (Int :+ p)))
-> Vector (PointSeq d (Int :+ p) r)
forall a b. (a -> b) -> a -> b
$ (forall s. ST s (MVector s (Seq (Point d r :+ (Int :+ p)))))
-> Vector (Seq (Point d r :+ (Int :+ p)))
forall a. (forall s. ST s (MVector s a)) -> Vector a
V.create ((forall s. ST s (MVector s (Seq (Point d r :+ (Int :+ p)))))
 -> Vector (Seq (Point d r :+ (Int :+ p))))
-> (forall s. ST s (MVector s (Seq (Point d r :+ (Int :+ p)))))
-> Vector (Seq (Point d r :+ (Int :+ p)))
forall a b. (a -> b) -> a -> b
$ do
    MVector s (Seq (Point d r :+ (Int :+ p)))
v <- Int
-> Seq (Point d r :+ (Int :+ p))
-> ST
     s (MVector (PrimState (ST s)) (Seq (Point d r :+ (Int :+ p))))
forall (m :: * -> *) a.
PrimMonad m =>
Int -> a -> m (MVector (PrimState m) a)
MV.replicate Int
k Seq (Point d r :+ (Int :+ p))
forall a. Monoid a => a
mempty
    PointSeq d (Int :+ p) r
-> ((Point d r :+ (Int :+ p)) -> ST s ()) -> ST s ()
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
t a -> (a -> m b) -> m ()
forM_ PointSeq d (Int :+ p) r
pts (((Point d r :+ (Int :+ p)) -> ST s ()) -> ST s ())
-> ((Point d r :+ (Int :+ p)) -> ST s ()) -> ST s ()
forall a b. (a -> b) -> a -> b
$ \Point d r :+ (Int :+ p)
p ->
      MVector (PrimState (ST s)) (Seq (Point d r :+ (Int :+ p)))
-> Int -> (Point d r :+ (Int :+ p)) -> ST s ()
forall (m :: * -> *) a.
PrimMonad m =>
MVector (PrimState m) (Seq a) -> Int -> a -> m ()
append MVector s (Seq (Point d r :+ (Int :+ p)))
MVector (PrimState (ST s)) (Seq (Point d r :+ (Int :+ p)))
v ((Point d r :+ (Int :+ p)) -> Int
level Point d r :+ (Int :+ p)
p) Point d r :+ (Int :+ p)
p
    MVector s (Seq (Point d r :+ (Int :+ p)))
-> ST s (MVector s (Seq (Point d r :+ (Int :+ p))))
forall (f :: * -> *) a. Applicative f => a -> f a
pure MVector s (Seq (Point d r :+ (Int :+ p)))
v
  where
    level :: (Point d r :+ (Int :+ p)) -> Int
level Point d r :+ (Int :+ p)
p = Int -> (Level -> Int) -> Maybe Level -> Int
forall b a. b -> (a -> b) -> Maybe a -> b
maybe (Int
kInt -> Int -> Int
forall a. Num a => a -> a -> a
-Int
1) Level -> Int
_unLevel (Maybe Level -> Int) -> Maybe Level -> Int
forall a b. (a -> b) -> a -> b
$ Vector (Maybe Level)
levels Vector (Maybe Level) -> Int -> Maybe Level
forall a. Vector a -> Int -> a
V.! (Point d r :+ (Int :+ p)
p(Point d r :+ (Int :+ p))
-> Getting Int (Point d r :+ (Int :+ p)) Int -> Int
forall s a. s -> Getting a s a -> a
^.((Int :+ p) -> Const Int (Int :+ p))
-> (Point d r :+ (Int :+ p)) -> Const Int (Point d r :+ (Int :+ p))
forall core extra extra'.
Lens (core :+ extra) (core :+ extra') extra extra'
extra(((Int :+ p) -> Const Int (Int :+ p))
 -> (Point d r :+ (Int :+ p))
 -> Const Int (Point d r :+ (Int :+ p)))
-> ((Int -> Const Int Int) -> (Int :+ p) -> Const Int (Int :+ p))
-> Getting Int (Point d r :+ (Int :+ p)) Int
forall b c a. (b -> c) -> (a -> b) -> a -> c
.(Int -> Const Int Int) -> (Int :+ p) -> Const Int (Int :+ p)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core)
    append :: MVector (PrimState m) (Seq a) -> Int -> a -> m ()
append MVector (PrimState m) (Seq a)
v Int
i a
p = MVector (PrimState m) (Seq a) -> Int -> m (Seq a)
forall (m :: * -> *) a.
PrimMonad m =>
MVector (PrimState m) a -> Int -> m a
MV.read MVector (PrimState m) (Seq a)
v Int
i m (Seq a) -> (Seq a -> m ()) -> m ()
forall (m :: * -> *) a b. Monad m => m a -> (a -> m b) -> m b
>>= MVector (PrimState m) (Seq a) -> Int -> Seq a -> m ()
forall (m :: * -> *) a.
PrimMonad m =>
MVector (PrimState m) a -> Int -> a -> m ()
MV.write MVector (PrimState m) (Seq a)
v Int
i (Seq a -> m ()) -> (Seq a -> Seq a) -> Seq a -> m ()
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (Seq a -> a -> Seq a
forall a. Seq a -> a -> Seq a
S.|> a
p)

fromSeqUnsafe :: S.Seq a -> LSeq n a
fromSeqUnsafe :: Seq a -> LSeq n a
fromSeqUnsafe = LSeq 0 a -> LSeq n a
forall (m :: Nat) (n :: Nat) a. LSeq m a -> LSeq n a
LSeq.promise (LSeq 0 a -> LSeq n a) -> (Seq a -> LSeq 0 a) -> Seq a -> LSeq n a
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Seq a -> LSeq 0 a
forall a. Seq a -> LSeq 0 a
LSeq.fromSeq

-- | Given a sequence of points, whose index is increasing in the first
-- dimension, i.e. if idx p < idx q, then p[0] < q[0].
-- Reindex the points so that they again have an index
-- in the range [0,..,n'], where n' is the new number of points.
--
-- running time: O(n' * d) (more or less; we are actually using an intmap for
-- the lookups)
--
-- alternatively: I can unsafe freeze and thaw an existing vector to pass it
-- along to use as mapping. Except then I would have to force the evaluation
-- order, i.e. we cannot be in 'reIndexPoints' for two of the nodes at the same
-- time.
--
-- so, basically, run reIndex points in ST as well.
reIndexPoints      :: (Arity d, 1 <= d)
                   => GV.Vector d (PointSeq d (Idx :+ p) r)
                   -> GV.Vector d (PointSeq d (Idx :+ p) r)
reIndexPoints :: Vector d (PointSeq d (Int :+ p) r)
-> Vector d (PointSeq d (Int :+ p) r)
reIndexPoints Vector d (PointSeq d (Int :+ p) r)
ptsV = (PointSeq d (Int :+ p) r -> PointSeq d (Int :+ p) r)
-> Vector d (PointSeq d (Int :+ p) r)
-> Vector d (PointSeq d (Int :+ p) r)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap PointSeq d (Int :+ p) r -> PointSeq d (Int :+ p) r
reIndex Vector d (PointSeq d (Int :+ p) r)
ptsV
  where
    pts :: PointSeq d (Int :+ p) r
pts = Vector d (PointSeq d (Int :+ p) r)
ptsVVector d (PointSeq d (Int :+ p) r)
-> Getting
     (PointSeq d (Int :+ p) r)
     (Vector d (PointSeq d (Int :+ p) r))
     (PointSeq d (Int :+ p) r)
-> PointSeq d (Int :+ p) r
forall s a. s -> Getting a s a -> a
^.C 0
-> Lens'
     (Vector d (PointSeq d (Int :+ p) r)) (PointSeq d (Int :+ p) r)
forall (proxy :: Nat -> *) (i :: Nat) (d :: Nat) r.
(Arity d, KnownNat i, (i + 1) <= d) =>
proxy i -> Lens' (Vector d r) r
GV.element (C 0
forall (n :: Nat). C n
C :: C 0)

    reIndex :: PointSeq d (Int :+ p) r -> PointSeq d (Int :+ p) r
reIndex = ((Point d r :+ (Int :+ p)) -> Point d r :+ (Int :+ p))
-> PointSeq d (Int :+ p) r -> PointSeq d (Int :+ p) r
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap (\Point d r :+ (Int :+ p)
p -> Point d r :+ (Int :+ p)
p(Point d r :+ (Int :+ p))
-> ((Point d r :+ (Int :+ p)) -> Point d r :+ (Int :+ p))
-> Point d r :+ (Int :+ p)
forall a b. a -> (a -> b) -> b
&((Int :+ p) -> Identity (Int :+ p))
-> (Point d r :+ (Int :+ p)) -> Identity (Point d r :+ (Int :+ p))
forall core extra extra'.
Lens (core :+ extra) (core :+ extra') extra extra'
extra(((Int :+ p) -> Identity (Int :+ p))
 -> (Point d r :+ (Int :+ p)) -> Identity (Point d r :+ (Int :+ p)))
-> ((Int -> Identity Int) -> (Int :+ p) -> Identity (Int :+ p))
-> (Int -> Identity Int)
-> (Point d r :+ (Int :+ p))
-> Identity (Point d r :+ (Int :+ p))
forall b c a. (b -> c) -> (a -> b) -> a -> c
.(Int -> Identity Int) -> (Int :+ p) -> Identity (Int :+ p)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core ((Int -> Identity Int)
 -> (Point d r :+ (Int :+ p)) -> Identity (Point d r :+ (Int :+ p)))
-> (Int -> Int)
-> (Point d r :+ (Int :+ p))
-> Point d r :+ (Int :+ p)
forall s t a b. ASetter s t a b -> (a -> b) -> s -> t
%~ Maybe Int -> Int
forall a. HasCallStack => Maybe a -> a
fromJust (Maybe Int -> Int) -> (Int -> Maybe Int) -> Int -> Int
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (Int -> IntMap Int -> Maybe Int) -> IntMap Int -> Int -> Maybe Int
forall a b c. (a -> b -> c) -> b -> a -> c
flip Int -> IntMap Int -> Maybe Int
forall a. Int -> IntMap a -> Maybe a
IntMap.lookup IntMap Int
mapping')
    mapping' :: IntMap Int
mapping' = [(Int, Int)] -> IntMap Int
forall a. [(Int, a)] -> IntMap a
IntMap.fromAscList ([(Int, Int)] -> IntMap Int) -> [(Int, Int)] -> IntMap Int
forall a b. (a -> b) -> a -> b
$ [Int] -> [Int] -> [(Int, Int)]
forall a b. [a] -> [b] -> [(a, b)]
zip (((Point d r :+ (Int :+ p)) -> Int)
-> [Point d r :+ (Int :+ p)] -> [Int]
forall a b. (a -> b) -> [a] -> [b]
map ((Point d r :+ (Int :+ p))
-> Getting Int (Point d r :+ (Int :+ p)) Int -> Int
forall s a. s -> Getting a s a -> a
^.((Int :+ p) -> Const Int (Int :+ p))
-> (Point d r :+ (Int :+ p)) -> Const Int (Point d r :+ (Int :+ p))
forall core extra extra'.
Lens (core :+ extra) (core :+ extra') extra extra'
extra(((Int :+ p) -> Const Int (Int :+ p))
 -> (Point d r :+ (Int :+ p))
 -> Const Int (Point d r :+ (Int :+ p)))
-> ((Int -> Const Int Int) -> (Int :+ p) -> Const Int (Int :+ p))
-> Getting Int (Point d r :+ (Int :+ p)) Int
forall b c a. (b -> c) -> (a -> b) -> a -> c
.(Int -> Const Int Int) -> (Int :+ p) -> Const Int (Int :+ p)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core) ([Point d r :+ (Int :+ p)] -> [Int])
-> (PointSeq d (Int :+ p) r -> [Point d r :+ (Int :+ p)])
-> PointSeq d (Int :+ p) r
-> [Int]
forall b c a. (b -> c) -> (a -> b) -> a -> c
. PointSeq d (Int :+ p) r -> [Point d r :+ (Int :+ p)]
forall (t :: * -> *) a. Foldable t => t a -> [a]
F.toList (PointSeq d (Int :+ p) r -> [Int])
-> PointSeq d (Int :+ p) r -> [Int]
forall a b. (a -> b) -> a -> b
$ PointSeq d (Int :+ p) r
pts) [Int
0..]

-- | ST monad with access to the vector storign the level of the points.
type RST s = ReaderT (MV.MVector s (Maybe Level)) (ST s)

{- HLINT ignore assignLevels -}
-- | Assigns the points to a level. Returns the list of levels used. The first
-- level in the list is the level assigned to the rest of the nodes. Their
-- level is actually still set to Nothing in the underlying array.
assignLevels                  :: (Fractional r, Ord r, Arity d
                                 , Show r, Show p
                                 )
                              => Int -- ^ Number of items we need to collect
                              -> Int -- ^ Number of items we collected so far
                              -> GV.Vector d (PointSeq d (Idx :+ p) r)
                              -> Level -- ^ next level to use
                              -> [Level] -- ^ Levels used so far
                              -> RST s (NonEmpty.NonEmpty Level)
assignLevels :: Int
-> Int
-> Vector d (PointSeq d (Int :+ p) r)
-> Level
-> [Level]
-> RST s (NonEmpty Level)
assignLevels Int
h Int
m Vector d (PointSeq d (Int :+ p) r)
pts Level
l [Level]
prevLvls
  | Int
m Int -> Int -> Bool
forall a. Ord a => a -> a -> Bool
>= Int
h    = NonEmpty Level -> RST s (NonEmpty Level)
forall (f :: * -> *) a. Applicative f => a -> f a
pure (Level
l Level -> [Level] -> NonEmpty Level
forall a. a -> [a] -> NonEmpty a
NonEmpty.:| [Level]
prevLvls)
  | Bool
otherwise = do
    Vector d (PointSeq d (Int :+ p) r)
pts' <- Vector d (PointSeq d (Int :+ p) r)
-> RST s (Vector d (PointSeq d (Int :+ p) r))
forall (d :: Nat) p r s.
Arity d =>
Vector d (PointSeq d (Int :+ p) r)
-> RST s (Vector d (PointSeq d (Int :+ p) r))
compactEnds Vector d (PointSeq d (Int :+ p) r)
pts
    -- find the widest dimension j = i+1
    let j :: Int
j    = Vector d (PointSeq d (Int :+ p) r) -> Int
forall r (d :: Nat) p.
(Num r, Ord r, Arity d) =>
Vector d (PointSeq d p r) -> Int
widestDimension Vector d (PointSeq d (Int :+ p) r)
pts'
        i :: Int
i    = Int
j Int -> Int -> Int
forall a. Num a => a -> a -> a
- Int
1 -- traceShow  ("i",j,pts') j - 1
        extJ :: Range r
extJ = (Vector d (PointSeq d (Int :+ p) r) -> Vector d (Range r)
forall (d :: Nat) p r.
Arity d =>
Vector d (PointSeq d p r) -> Vector d (Range r)
extends Vector d (PointSeq d (Int :+ p) r)
pts')Vector d (Range r)
-> Getting (Range r) (Vector d (Range r)) (Range r) -> Range r
forall s a. s -> Getting a s a -> a
^.Int -> Lens' (Vector d (Range r)) (Range r)
forall (d :: Nat) a.
(Arity d, KnownNat d) =>
Int -> Lens' (Vector d a) a
ix' Int
i
        mid :: r
mid  = Range r -> r
forall r. Fractional r => Range r -> r
midPoint Range r
extJ

    -- find the set of points that we have to delete, by looking at the sorted
    -- list L_j. As a side effect, this will remove previously assigned points
    -- from L_j.
    (PointSeq d (Int :+ p) r
lvlJPts,PointSeq d (Int :+ p) r
deletePts) <- Int
-> PointSeq d (Int :+ p) r
-> r
-> RST s (PointSeq d (Int :+ p) r, PointSeq d (Int :+ p) r)
forall r (d :: Nat) p s.
(Ord r, Arity d, Show r, Show p) =>
Int
-> PointSeq d (Int :+ p) r
-> r
-> RST s (PointSeq d (Int :+ p) r, PointSeq d (Int :+ p) r)
findAndCompact Int
j (Vector d (PointSeq d (Int :+ p) r)
pts'Vector d (PointSeq d (Int :+ p) r)
-> Getting
     (PointSeq d (Int :+ p) r)
     (Vector d (PointSeq d (Int :+ p) r))
     (PointSeq d (Int :+ p) r)
-> PointSeq d (Int :+ p) r
forall s a. s -> Getting a s a -> a
^.Int
-> Lens'
     (Vector d (PointSeq d (Int :+ p) r)) (PointSeq d (Int :+ p) r)
forall (d :: Nat) a.
(Arity d, KnownNat d) =>
Int -> Lens' (Vector d a) a
ix' Int
i) r
mid
    let pts'' :: Vector d (PointSeq d (Int :+ p) r)
pts''     = Vector d (PointSeq d (Int :+ p) r)
pts'Vector d (PointSeq d (Int :+ p) r)
-> (Vector d (PointSeq d (Int :+ p) r)
    -> Vector d (PointSeq d (Int :+ p) r))
-> Vector d (PointSeq d (Int :+ p) r)
forall a b. a -> (a -> b) -> b
&Int
-> Lens'
     (Vector d (PointSeq d (Int :+ p) r)) (PointSeq d (Int :+ p) r)
forall (d :: Nat) a.
(Arity d, KnownNat d) =>
Int -> Lens' (Vector d a) a
ix' Int
i ((PointSeq d (Int :+ p) r -> Identity (PointSeq d (Int :+ p) r))
 -> Vector d (PointSeq d (Int :+ p) r)
 -> Identity (Vector d (PointSeq d (Int :+ p) r)))
-> PointSeq d (Int :+ p) r
-> Vector d (PointSeq d (Int :+ p) r)
-> Vector d (PointSeq d (Int :+ p) r)
forall s t a b. ASetter s t a b -> b -> s -> t
.~ PointSeq d (Int :+ p) r
lvlJPts
        l' :: Level
l'        = Level
lLevel -> (Level -> Level) -> Level
forall a b. a -> (a -> b) -> b
&(Maybe Int -> Identity (Maybe Int)) -> Level -> Identity Level
Lens' Level (Maybe Int)
widestDim ((Maybe Int -> Identity (Maybe Int)) -> Level -> Identity Level)
-> Int -> Level -> Level
forall s t a b. ASetter s t a (Maybe b) -> b -> s -> t
?~ Int
j
    PointSeq d (Int :+ p) r
-> ((Point d r :+ (Int :+ p))
    -> ReaderT (MVector s (Maybe Level)) (ST s) ())
-> ReaderT (MVector s (Maybe Level)) (ST s) ()
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
t a -> (a -> m b) -> m ()
forM_ PointSeq d (Int :+ p) r
deletePts (((Point d r :+ (Int :+ p))
  -> ReaderT (MVector s (Maybe Level)) (ST s) ())
 -> ReaderT (MVector s (Maybe Level)) (ST s) ())
-> ((Point d r :+ (Int :+ p))
    -> ReaderT (MVector s (Maybe Level)) (ST s) ())
-> ReaderT (MVector s (Maybe Level)) (ST s) ()
forall a b. (a -> b) -> a -> b
$ \Point d r :+ (Int :+ p)
p ->
      (Point d r :+ (Int :+ p))
-> Level -> ReaderT (MVector s (Maybe Level)) (ST s) ()
forall c p s. (c :+ (Int :+ p)) -> Level -> RST s ()
assignLevel Point d r :+ (Int :+ p)
p Level
l'
    Int
-> Int
-> Vector d (PointSeq d (Int :+ p) r)
-> Level
-> [Level]
-> RST s (NonEmpty Level)
forall r (d :: Nat) p s.
(Fractional r, Ord r, Arity d, Show r, Show p) =>
Int
-> Int
-> Vector d (PointSeq d (Int :+ p) r)
-> Level
-> [Level]
-> RST s (NonEmpty Level)
assignLevels Int
h (Int
m Int -> Int -> Int
forall a. Num a => a -> a -> a
+ PointSeq d (Int :+ p) r -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length PointSeq d (Int :+ p) r
deletePts) Vector d (PointSeq d (Int :+ p) r)
pts'' (Level -> Level
nextLevel Level
l) (Level
l' Level -> [Level] -> [Level]
forall a. a -> [a] -> [a]
: [Level]
prevLvls)

-- | Remove already assigned pts from the ends of all vectors.
compactEnds        :: Arity d
                   => GV.Vector d (PointSeq d (Idx :+ p) r)
                   -> RST s (GV.Vector d (PointSeq d (Idx :+ p) r))
compactEnds :: Vector d (PointSeq d (Int :+ p) r)
-> RST s (Vector d (PointSeq d (Int :+ p) r))
compactEnds = (PointSeq d (Int :+ p) r
 -> ReaderT
      (MVector s (Maybe Level)) (ST s) (PointSeq d (Int :+ p) r))
-> Vector d (PointSeq d (Int :+ p) r)
-> RST s (Vector d (PointSeq d (Int :+ p) r))
forall (t :: * -> *) (f :: * -> *) a b.
(Traversable t, Applicative f) =>
(a -> f b) -> t a -> f (t b)
traverse PointSeq d (Int :+ p) r
-> ReaderT
     (MVector s (Maybe Level)) (ST s) (PointSeq d (Int :+ p) r)
forall (d :: Nat) p r s.
PointSeq d (Int :+ p) r -> RST s (PointSeq d (Int :+ p) r)
compactEnds'

-- | Assign level l to point p
assignLevel     :: (c :+ (Idx :+ p)) -> Level -> RST s ()
assignLevel :: (c :+ (Int :+ p)) -> Level -> RST s ()
assignLevel c :+ (Int :+ p)
p Level
l = ReaderT (MVector s (Maybe Level)) (ST s) (MVector s (Maybe Level))
forall r (m :: * -> *). MonadReader r m => m r
ask ReaderT (MVector s (Maybe Level)) (ST s) (MVector s (Maybe Level))
-> (MVector s (Maybe Level) -> RST s ()) -> RST s ()
forall (m :: * -> *) a b. Monad m => m a -> (a -> m b) -> m b
>>= \MVector s (Maybe Level)
levels -> ST s () -> RST s ()
forall (t :: (* -> *) -> * -> *) (m :: * -> *) a.
(MonadTrans t, Monad m) =>
m a -> t m a
lift (ST s () -> RST s ()) -> ST s () -> RST s ()
forall a b. (a -> b) -> a -> b
$ MVector (PrimState (ST s)) (Maybe Level)
-> Int -> Maybe Level -> ST s ()
forall (m :: * -> *) a.
PrimMonad m =>
MVector (PrimState m) a -> Int -> a -> m ()
MV.write MVector s (Maybe Level)
MVector (PrimState (ST s)) (Maybe Level)
levels (c :+ (Int :+ p)
p(c :+ (Int :+ p)) -> Getting Int (c :+ (Int :+ p)) Int -> Int
forall s a. s -> Getting a s a -> a
^.((Int :+ p) -> Const Int (Int :+ p))
-> (c :+ (Int :+ p)) -> Const Int (c :+ (Int :+ p))
forall core extra extra'.
Lens (core :+ extra) (core :+ extra') extra extra'
extra(((Int :+ p) -> Const Int (Int :+ p))
 -> (c :+ (Int :+ p)) -> Const Int (c :+ (Int :+ p)))
-> ((Int -> Const Int Int) -> (Int :+ p) -> Const Int (Int :+ p))
-> Getting Int (c :+ (Int :+ p)) Int
forall b c a. (b -> c) -> (a -> b) -> a -> c
.(Int -> Const Int Int) -> (Int :+ p) -> Const Int (Int :+ p)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core) (Level -> Maybe Level
forall a. a -> Maybe a
Just Level
l)

-- | Get the level of a point
levelOf   :: (c :+ (Idx :+ p)) -> RST s (Maybe Level)
levelOf :: (c :+ (Int :+ p)) -> RST s (Maybe Level)
levelOf c :+ (Int :+ p)
p = ReaderT (MVector s (Maybe Level)) (ST s) (MVector s (Maybe Level))
forall r (m :: * -> *). MonadReader r m => m r
ask ReaderT (MVector s (Maybe Level)) (ST s) (MVector s (Maybe Level))
-> (MVector s (Maybe Level) -> RST s (Maybe Level))
-> RST s (Maybe Level)
forall (m :: * -> *) a b. Monad m => m a -> (a -> m b) -> m b
>>= \MVector s (Maybe Level)
levels -> ST s (Maybe Level) -> RST s (Maybe Level)
forall (t :: (* -> *) -> * -> *) (m :: * -> *) a.
(MonadTrans t, Monad m) =>
m a -> t m a
lift (ST s (Maybe Level) -> RST s (Maybe Level))
-> ST s (Maybe Level) -> RST s (Maybe Level)
forall a b. (a -> b) -> a -> b
$ MVector (PrimState (ST s)) (Maybe Level)
-> Int -> ST s (Maybe Level)
forall (m :: * -> *) a.
PrimMonad m =>
MVector (PrimState m) a -> Int -> m a
MV.read MVector s (Maybe Level)
MVector (PrimState (ST s)) (Maybe Level)
levels (c :+ (Int :+ p)
p(c :+ (Int :+ p)) -> Getting Int (c :+ (Int :+ p)) Int -> Int
forall s a. s -> Getting a s a -> a
^.((Int :+ p) -> Const Int (Int :+ p))
-> (c :+ (Int :+ p)) -> Const Int (c :+ (Int :+ p))
forall core extra extra'.
Lens (core :+ extra) (core :+ extra') extra extra'
extra(((Int :+ p) -> Const Int (Int :+ p))
 -> (c :+ (Int :+ p)) -> Const Int (c :+ (Int :+ p)))
-> ((Int -> Const Int Int) -> (Int :+ p) -> Const Int (Int :+ p))
-> Getting Int (c :+ (Int :+ p)) Int
forall b c a. (b -> c) -> (a -> b) -> a -> c
.(Int -> Const Int Int) -> (Int :+ p) -> Const Int (Int :+ p)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core)

-- | Test if the point already has a level assigned to it.
hasLevel :: c :+ (Idx :+ p) -> RST s Bool
hasLevel :: (c :+ (Int :+ p)) -> RST s Bool
hasLevel = (Maybe Level -> Bool)
-> ReaderT (MVector s (Maybe Level)) (ST s) (Maybe Level)
-> RST s Bool
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap Maybe Level -> Bool
forall a. Maybe a -> Bool
isJust (ReaderT (MVector s (Maybe Level)) (ST s) (Maybe Level)
 -> RST s Bool)
-> ((c :+ (Int :+ p))
    -> ReaderT (MVector s (Maybe Level)) (ST s) (Maybe Level))
-> (c :+ (Int :+ p))
-> RST s Bool
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (c :+ (Int :+ p))
-> ReaderT (MVector s (Maybe Level)) (ST s) (Maybe Level)
forall c p s. (c :+ (Int :+ p)) -> RST s (Maybe Level)
levelOf

-- | Remove allready assigned points from the sequence
--
-- pre: there are points remaining
compactEnds'              :: PointSeq d (Idx :+ p) r
                          -> RST s (PointSeq d (Idx :+ p) r)
compactEnds' :: PointSeq d (Int :+ p) r -> RST s (PointSeq d (Int :+ p) r)
compactEnds' (l0 :<| s0) = (Seq (Point d r :+ (Int :+ p)) -> PointSeq d (Int :+ p) r)
-> ReaderT
     (MVector s (Maybe Level)) (ST s) (Seq (Point d r :+ (Int :+ p)))
-> RST s (PointSeq d (Int :+ p) r)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap Seq (Point d r :+ (Int :+ p)) -> PointSeq d (Int :+ p) r
forall a (n :: Nat). Seq a -> LSeq n a
fromSeqUnsafe (ReaderT
   (MVector s (Maybe Level)) (ST s) (Seq (Point d r :+ (Int :+ p)))
 -> RST s (PointSeq d (Int :+ p) r))
-> (Seq (Point d r :+ (Int :+ p))
    -> ReaderT
         (MVector s (Maybe Level)) (ST s) (Seq (Point d r :+ (Int :+ p))))
-> Seq (Point d r :+ (Int :+ p))
-> RST s (PointSeq d (Int :+ p) r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Seq (Point d r :+ (Int :+ p))
-> ReaderT
     (MVector s (Maybe Level)) (ST s) (Seq (Point d r :+ (Int :+ p)))
forall c p s.
Seq (c :+ (Int :+ p))
-> ReaderT (MVector s (Maybe Level)) (ST s) (Seq (c :+ (Int :+ p)))
goL (Seq (Point d r :+ (Int :+ p)) -> RST s (PointSeq d (Int :+ p) r))
-> Seq (Point d r :+ (Int :+ p)) -> RST s (PointSeq d (Int :+ p) r)
forall a b. (a -> b) -> a -> b
$ Point d r :+ (Int :+ p)
l0 (Point d r :+ (Int :+ p))
-> Seq (Point d r :+ (Int :+ p)) -> Seq (Point d r :+ (Int :+ p))
forall a. a -> Seq a -> Seq a
S.<| LSeq 0 (Point d r :+ (Int :+ p)) -> Seq (Point d r :+ (Int :+ p))
forall (n :: Nat) a. LSeq n a -> Seq a
toSeq LSeq 0 (Point d r :+ (Int :+ p))
s0
  where
    goL :: Seq (c :+ (Int :+ p))
-> ReaderT (MVector s (Maybe Level)) (ST s) (Seq (c :+ (Int :+ p)))
goL s :: Seq (c :+ (Int :+ p))
s@(Seq (c :+ (Int :+ p)) -> ViewL (c :+ (Int :+ p))
forall a. Seq a -> ViewL a
S.viewl -> c :+ (Int :+ p)
l S.:< Seq (c :+ (Int :+ p))
s') = (c :+ (Int :+ p)) -> RST s Bool
forall c p s. (c :+ (Int :+ p)) -> RST s Bool
hasLevel c :+ (Int :+ p)
l RST s Bool
-> (Bool
    -> ReaderT
         (MVector s (Maybe Level)) (ST s) (Seq (c :+ (Int :+ p))))
-> ReaderT (MVector s (Maybe Level)) (ST s) (Seq (c :+ (Int :+ p)))
forall (m :: * -> *) a b. Monad m => m a -> (a -> m b) -> m b
>>= \case
                                     Bool
False -> Seq (c :+ (Int :+ p))
-> ReaderT (MVector s (Maybe Level)) (ST s) (Seq (c :+ (Int :+ p)))
forall c p s.
Seq (c :+ (Int :+ p))
-> ReaderT (MVector s (Maybe Level)) (ST s) (Seq (c :+ (Int :+ p)))
goR Seq (c :+ (Int :+ p))
s
                                     Bool
True  -> Seq (c :+ (Int :+ p))
-> ReaderT (MVector s (Maybe Level)) (ST s) (Seq (c :+ (Int :+ p)))
goL Seq (c :+ (Int :+ p))
s'
    goL Seq (c :+ (Int :+ p))
_ = [Char]
-> ReaderT (MVector s (Maybe Level)) (ST s) (Seq (c :+ (Int :+ p)))
forall a. HasCallStack => [Char] -> a
error [Char]
"Unreachable, but cannot prove it in Haskell"
    goR :: Seq (c :+ (Int :+ p))
-> ReaderT (MVector s (Maybe Level)) (ST s) (Seq (c :+ (Int :+ p)))
goR s :: Seq (c :+ (Int :+ p))
s@(Seq (c :+ (Int :+ p)) -> ViewR (c :+ (Int :+ p))
forall a. Seq a -> ViewR a
S.viewr -> Seq (c :+ (Int :+ p))
s' S.:> c :+ (Int :+ p)
r) = (c :+ (Int :+ p)) -> RST s Bool
forall c p s. (c :+ (Int :+ p)) -> RST s Bool
hasLevel c :+ (Int :+ p)
r RST s Bool
-> (Bool
    -> ReaderT
         (MVector s (Maybe Level)) (ST s) (Seq (c :+ (Int :+ p))))
-> ReaderT (MVector s (Maybe Level)) (ST s) (Seq (c :+ (Int :+ p)))
forall (m :: * -> *) a b. Monad m => m a -> (a -> m b) -> m b
>>= \case
                                     Bool
False -> Seq (c :+ (Int :+ p))
-> ReaderT (MVector s (Maybe Level)) (ST s) (Seq (c :+ (Int :+ p)))
forall (f :: * -> *) a. Applicative f => a -> f a
pure Seq (c :+ (Int :+ p))
s
                                     Bool
True  -> Seq (c :+ (Int :+ p))
-> ReaderT (MVector s (Maybe Level)) (ST s) (Seq (c :+ (Int :+ p)))
goR Seq (c :+ (Int :+ p))
s'
    goR Seq (c :+ (Int :+ p))
_ = [Char]
-> ReaderT (MVector s (Maybe Level)) (ST s) (Seq (c :+ (Int :+ p)))
forall a. HasCallStack => [Char] -> a
error [Char]
"Unreachable, but cannot prove it in Haskell"


-- | Given the points, ordered by their j^th coordinate, split the point set
-- into a "left" and a "right" half, i.e. the points whose j^th coordinate is
-- at most the given mid point m, and the points whose j^th coordinate is
-- larger than m.
--
-- We return a pair (Largest set, Smallest set)
--
--
--fi ndAndCompact works by simultaneously traversing the points from left to
-- right, and from right to left. As soon as we find a point crossing the mid
-- point we stop and return. Thus, in principle this takes only O(|Smallest
-- set|) time.
--
-- running time: O(|Smallest set|) + R, where R is the number of *old* points
-- (i.e. points that should have been removed) in the list.
findAndCompact                   :: (Ord r, Arity d
                                    , Show r, Show p
                                    )
                                 => Int
                                    -- ^ the dimension we are in, i.e. so that we know
                                    -- which coordinate of the point to compare
                                 -> PointSeq d (Idx :+ p) r
                                 -> r -- ^ the mid point
                                 -> RST s ( PointSeq d (Idx :+ p) r
                                          , PointSeq d (Idx :+ p) r
                                          )
findAndCompact :: Int
-> PointSeq d (Int :+ p) r
-> r
-> RST s (PointSeq d (Int :+ p) r, PointSeq d (Int :+ p) r)
findAndCompact Int
j (l0 :<| s0) r
m = (FindAndCompact d r (Int :+ p)
 -> (PointSeq d (Int :+ p) r, PointSeq d (Int :+ p) r))
-> ReaderT
     (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p))
-> RST s (PointSeq d (Int :+ p) r, PointSeq d (Int :+ p) r)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap FindAndCompact d r (Int :+ p)
-> (PointSeq d (Int :+ p) r, PointSeq d (Int :+ p) r)
forall (d :: Nat) r p (n :: Nat).
FindAndCompact d r p
-> (LSeq n (Point d r :+ p), LSeq n (Point d r :+ p))
select (ReaderT
   (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p))
 -> RST s (PointSeq d (Int :+ p) r, PointSeq d (Int :+ p) r))
-> (Seq (Point d r :+ (Int :+ p))
    -> ReaderT
         (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p)))
-> Seq (Point d r :+ (Int :+ p))
-> RST s (PointSeq d (Int :+ p) r, PointSeq d (Int :+ p) r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Seq (Point d r :+ (Int :+ p))
-> ReaderT
     (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p))
stepL (Seq (Point d r :+ (Int :+ p))
 -> RST s (PointSeq d (Int :+ p) r, PointSeq d (Int :+ p) r))
-> Seq (Point d r :+ (Int :+ p))
-> RST s (PointSeq d (Int :+ p) r, PointSeq d (Int :+ p) r)
forall a b. (a -> b) -> a -> b
$ Point d r :+ (Int :+ p)
l0 (Point d r :+ (Int :+ p))
-> Seq (Point d r :+ (Int :+ p)) -> Seq (Point d r :+ (Int :+ p))
forall a. a -> Seq a -> Seq a
S.<| LSeq 0 (Point d r :+ (Int :+ p)) -> Seq (Point d r :+ (Int :+ p))
forall (n :: Nat) a. LSeq n a -> Seq a
toSeq LSeq 0 (Point d r :+ (Int :+ p))
s0
  where
    -- stepL and stepR together build a data structure (FAC l r S) that
    -- contains the left part of the list, i.e. the points before midpoint, and
    -- the right part of the list., and a value S that indicates which part is
    -- the short side.

    -- stepL takes a step on the left side of the list; if the left point l
    -- already has been assigned, we continue waling along (and "ignore" the
    -- point). If it has not been assigned, and is before the mid point, we
    -- take a step from the right, and add l onto the left part. If it is
    -- larger than the mid point, we have found our split.
    -- stepL :: S.Seq (Point d r :+ (Idx :+ p)) -> ST s (FindAndCompact d r (Idx :+ p))
    stepL :: Seq (Point d r :+ (Int :+ p))
-> ReaderT
     (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p))
stepL Seq (Point d r :+ (Int :+ p))
s = case Seq (Point d r :+ (Int :+ p)) -> ViewL (Point d r :+ (Int :+ p))
forall a. Seq a -> ViewL a
S.viewl Seq (Point d r :+ (Int :+ p))
s of
      ViewL (Point d r :+ (Int :+ p))
S.EmptyL  -> FindAndCompact d r (Int :+ p)
-> ReaderT
     (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p))
forall (f :: * -> *) a. Applicative f => a -> f a
pure (FindAndCompact d r (Int :+ p)
 -> ReaderT
      (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p)))
-> FindAndCompact d r (Int :+ p)
-> ReaderT
     (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p))
forall a b. (a -> b) -> a -> b
$ Seq (Point d r :+ (Int :+ p))
-> Seq (Point d r :+ (Int :+ p))
-> ShortSide
-> FindAndCompact d r (Int :+ p)
forall (d :: Nat) r p.
Seq (Point d r :+ p)
-> Seq (Point d r :+ p) -> ShortSide -> FindAndCompact d r p
FAC Seq (Point d r :+ (Int :+ p))
forall a. Monoid a => a
mempty Seq (Point d r :+ (Int :+ p))
forall a. Monoid a => a
mempty ShortSide
L
      Point d r :+ (Int :+ p)
l S.:< Seq (Point d r :+ (Int :+ p))
s' -> (Point d r :+ (Int :+ p)) -> RST s Bool
forall c p s. (c :+ (Int :+ p)) -> RST s Bool
hasLevel Point d r :+ (Int :+ p)
l RST s Bool
-> (Bool
    -> ReaderT
         (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p)))
-> ReaderT
     (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p))
forall (m :: * -> *) a b. Monad m => m a -> (a -> m b) -> m b
>>= \case
                     Bool
False -> if Point d r :+ (Int :+ p)
l(Point d r :+ (Int :+ p))
-> Getting r (Point d r :+ (Int :+ p)) r -> r
forall s a. s -> Getting a s a -> a
^.(Point d r -> Const r (Point d r))
-> (Point d r :+ (Int :+ p)) -> Const r (Point d r :+ (Int :+ p))
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core((Point d r -> Const r (Point d r))
 -> (Point d r :+ (Int :+ p)) -> Const r (Point d r :+ (Int :+ p)))
-> ((r -> Const r r) -> Point d r -> Const r (Point d r))
-> Getting r (Point d r :+ (Int :+ p)) r
forall b c a. (b -> c) -> (a -> b) -> a -> c
.Int -> Lens' (Point d r) r
forall (d :: Nat) (p :: Nat -> * -> *) r.
(Arity d, AsAPoint p) =>
Int -> Lens' (p d r) r
unsafeCoord Int
j r -> r -> Bool
forall a. Ord a => a -> a -> Bool
<= r
m
                                 then (Point d r :+ (Int :+ p))
-> FindAndCompact d r (Int :+ p) -> FindAndCompact d r (Int :+ p)
forall (d :: Nat) r p.
(Point d r :+ p) -> FindAndCompact d r p -> FindAndCompact d r p
addL Point d r :+ (Int :+ p)
l (FindAndCompact d r (Int :+ p) -> FindAndCompact d r (Int :+ p))
-> ReaderT
     (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p))
-> ReaderT
     (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p))
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> Seq (Point d r :+ (Int :+ p))
-> ReaderT
     (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p))
stepR Seq (Point d r :+ (Int :+ p))
s'
                                 else FindAndCompact d r (Int :+ p)
-> ReaderT
     (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p))
forall (f :: * -> *) a. Applicative f => a -> f a
pure (FindAndCompact d r (Int :+ p)
 -> ReaderT
      (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p)))
-> FindAndCompact d r (Int :+ p)
-> ReaderT
     (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p))
forall a b. (a -> b) -> a -> b
$ Seq (Point d r :+ (Int :+ p))
-> Seq (Point d r :+ (Int :+ p))
-> ShortSide
-> FindAndCompact d r (Int :+ p)
forall (d :: Nat) r p.
Seq (Point d r :+ p)
-> Seq (Point d r :+ p) -> ShortSide -> FindAndCompact d r p
FAC Seq (Point d r :+ (Int :+ p))
forall a. Monoid a => a
mempty Seq (Point d r :+ (Int :+ p))
s ShortSide
L
                     Bool
True  -> Seq (Point d r :+ (Int :+ p))
-> ReaderT
     (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p))
stepL Seq (Point d r :+ (Int :+ p))
s' -- delete, continue left

    -- stepR :: S.Seq (Point d r :+ (Idx :+ p)) -> ST s (FindAndCompact d r (Idx :+ p))
    stepR :: Seq (Point d r :+ (Int :+ p))
-> ReaderT
     (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p))
stepR Seq (Point d r :+ (Int :+ p))
s = case Seq (Point d r :+ (Int :+ p)) -> ViewR (Point d r :+ (Int :+ p))
forall a. Seq a -> ViewR a
S.viewr Seq (Point d r :+ (Int :+ p))
s of
      ViewR (Point d r :+ (Int :+ p))
S.EmptyR  -> FindAndCompact d r (Int :+ p)
-> ReaderT
     (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p))
forall (f :: * -> *) a. Applicative f => a -> f a
pure (FindAndCompact d r (Int :+ p)
 -> ReaderT
      (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p)))
-> FindAndCompact d r (Int :+ p)
-> ReaderT
     (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p))
forall a b. (a -> b) -> a -> b
$ Seq (Point d r :+ (Int :+ p))
-> Seq (Point d r :+ (Int :+ p))
-> ShortSide
-> FindAndCompact d r (Int :+ p)
forall (d :: Nat) r p.
Seq (Point d r :+ p)
-> Seq (Point d r :+ p) -> ShortSide -> FindAndCompact d r p
FAC Seq (Point d r :+ (Int :+ p))
forall a. Monoid a => a
mempty Seq (Point d r :+ (Int :+ p))
forall a. Monoid a => a
mempty ShortSide
R
      Seq (Point d r :+ (Int :+ p))
s' S.:> Point d r :+ (Int :+ p)
r -> (Point d r :+ (Int :+ p)) -> RST s Bool
forall c p s. (c :+ (Int :+ p)) -> RST s Bool
hasLevel Point d r :+ (Int :+ p)
r RST s Bool
-> (Bool
    -> ReaderT
         (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p)))
-> ReaderT
     (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p))
forall (m :: * -> *) a b. Monad m => m a -> (a -> m b) -> m b
>>= \case
                     Bool
False -> if Point d r :+ (Int :+ p)
r(Point d r :+ (Int :+ p))
-> Getting r (Point d r :+ (Int :+ p)) r -> r
forall s a. s -> Getting a s a -> a
^.(Point d r -> Const r (Point d r))
-> (Point d r :+ (Int :+ p)) -> Const r (Point d r :+ (Int :+ p))
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core((Point d r -> Const r (Point d r))
 -> (Point d r :+ (Int :+ p)) -> Const r (Point d r :+ (Int :+ p)))
-> ((r -> Const r r) -> Point d r -> Const r (Point d r))
-> Getting r (Point d r :+ (Int :+ p)) r
forall b c a. (b -> c) -> (a -> b) -> a -> c
.Int -> Lens' (Point d r) r
forall (d :: Nat) (p :: Nat -> * -> *) r.
(Arity d, AsAPoint p) =>
Int -> Lens' (p d r) r
unsafeCoord Int
j r -> r -> Bool
forall a. Ord a => a -> a -> Bool
>= r
m
                                 then (Point d r :+ (Int :+ p))
-> FindAndCompact d r (Int :+ p) -> FindAndCompact d r (Int :+ p)
forall (d :: Nat) r p.
(Point d r :+ p) -> FindAndCompact d r p -> FindAndCompact d r p
addR Point d r :+ (Int :+ p)
r (FindAndCompact d r (Int :+ p) -> FindAndCompact d r (Int :+ p))
-> ReaderT
     (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p))
-> ReaderT
     (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p))
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> Seq (Point d r :+ (Int :+ p))
-> ReaderT
     (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p))
stepL Seq (Point d r :+ (Int :+ p))
s'
                                 else FindAndCompact d r (Int :+ p)
-> ReaderT
     (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p))
forall (f :: * -> *) a. Applicative f => a -> f a
pure (FindAndCompact d r (Int :+ p)
 -> ReaderT
      (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p)))
-> FindAndCompact d r (Int :+ p)
-> ReaderT
     (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p))
forall a b. (a -> b) -> a -> b
$ Seq (Point d r :+ (Int :+ p))
-> Seq (Point d r :+ (Int :+ p))
-> ShortSide
-> FindAndCompact d r (Int :+ p)
forall (d :: Nat) r p.
Seq (Point d r :+ p)
-> Seq (Point d r :+ p) -> ShortSide -> FindAndCompact d r p
FAC Seq (Point d r :+ (Int :+ p))
s Seq (Point d r :+ (Int :+ p))
forall a. Monoid a => a
mempty ShortSide
R
                     Bool
True  -> Seq (Point d r :+ (Int :+ p))
-> ReaderT
     (MVector s (Maybe Level)) (ST s) (FindAndCompact d r (Int :+ p))
stepR Seq (Point d r :+ (Int :+ p))
s'


    addL :: (Point d r :+ p) -> FindAndCompact d r p -> FindAndCompact d r p
addL Point d r :+ p
l FindAndCompact d r p
x = FindAndCompact d r p
xFindAndCompact d r p
-> (FindAndCompact d r p -> FindAndCompact d r p)
-> FindAndCompact d r p
forall a b. a -> (a -> b) -> b
&(Seq (Point d r :+ p) -> Identity (Seq (Point d r :+ p)))
-> FindAndCompact d r p -> Identity (FindAndCompact d r p)
forall (d :: Nat) r p.
Lens' (FindAndCompact d r p) (Seq (Point d r :+ p))
leftPart  ((Seq (Point d r :+ p) -> Identity (Seq (Point d r :+ p)))
 -> FindAndCompact d r p -> Identity (FindAndCompact d r p))
-> (Seq (Point d r :+ p) -> Seq (Point d r :+ p))
-> FindAndCompact d r p
-> FindAndCompact d r p
forall s t a b. ASetter s t a b -> (a -> b) -> s -> t
%~ (Point d r :+ p
l (Point d r :+ p) -> Seq (Point d r :+ p) -> Seq (Point d r :+ p)
forall a. a -> Seq a -> Seq a
S.<|)
    addR :: (Point d r :+ p) -> FindAndCompact d r p -> FindAndCompact d r p
addR Point d r :+ p
r FindAndCompact d r p
x = FindAndCompact d r p
xFindAndCompact d r p
-> (FindAndCompact d r p -> FindAndCompact d r p)
-> FindAndCompact d r p
forall a b. a -> (a -> b) -> b
&(Seq (Point d r :+ p) -> Identity (Seq (Point d r :+ p)))
-> FindAndCompact d r p -> Identity (FindAndCompact d r p)
forall (d :: Nat) r p.
Lens' (FindAndCompact d r p) (Seq (Point d r :+ p))
rightPart ((Seq (Point d r :+ p) -> Identity (Seq (Point d r :+ p)))
 -> FindAndCompact d r p -> Identity (FindAndCompact d r p))
-> (Seq (Point d r :+ p) -> Seq (Point d r :+ p))
-> FindAndCompact d r p
-> FindAndCompact d r p
forall s t a b. ASetter s t a b -> (a -> b) -> s -> t
%~ (Seq (Point d r :+ p) -> (Point d r :+ p) -> Seq (Point d r :+ p)
forall a. Seq a -> a -> Seq a
S.|> Point d r :+ p
r)

    select :: FindAndCompact d r p
-> (LSeq n (Point d r :+ p), LSeq n (Point d r :+ p))
select = ASetter
  (Seq (Point d r :+ p), Seq (Point d r :+ p))
  (LSeq n (Point d r :+ p), LSeq n (Point d r :+ p))
  (Seq (Point d r :+ p))
  (LSeq n (Point d r :+ p))
-> (Seq (Point d r :+ p) -> LSeq n (Point d r :+ p))
-> (Seq (Point d r :+ p), Seq (Point d r :+ p))
-> (LSeq n (Point d r :+ p), LSeq n (Point d r :+ p))
forall s t a b. ASetter s t a b -> (a -> b) -> s -> t
over ASetter
  (Seq (Point d r :+ p), Seq (Point d r :+ p))
  (LSeq n (Point d r :+ p), LSeq n (Point d r :+ p))
  (Seq (Point d r :+ p))
  (LSeq n (Point d r :+ p))
forall (r :: * -> * -> *) a b.
Bitraversable r =>
Traversal (r a a) (r b b) a b
both Seq (Point d r :+ p) -> LSeq n (Point d r :+ p)
forall a (n :: Nat). Seq a -> LSeq n a
fromSeqUnsafe ((Seq (Point d r :+ p), Seq (Point d r :+ p))
 -> (LSeq n (Point d r :+ p), LSeq n (Point d r :+ p)))
-> (FindAndCompact d r p
    -> (Seq (Point d r :+ p), Seq (Point d r :+ p)))
-> FindAndCompact d r p
-> (LSeq n (Point d r :+ p), LSeq n (Point d r :+ p))
forall b c a. (b -> c) -> (a -> b) -> a -> c
. FindAndCompact d r p
-> (Seq (Point d r :+ p), Seq (Point d r :+ p))
forall (d :: Nat) r p.
FindAndCompact d r p
-> (Seq (Point d r :+ p), Seq (Point d r :+ p))
select'

    -- select' f | traceShow ("select'", f) False = undefined
    select' :: FindAndCompact d r p
-> (Seq (Point d r :+ p), Seq (Point d r :+ p))
select' (FAC Seq (Point d r :+ p)
l Seq (Point d r :+ p)
r ShortSide
L) = (Seq (Point d r :+ p)
r, Seq (Point d r :+ p)
l)
    select' (FAC Seq (Point d r :+ p)
l Seq (Point d r :+ p)
r ShortSide
R) = (Seq (Point d r :+ p)
l, Seq (Point d r :+ p)
r)


-- | Find the widest dimension of the point set
--
-- pre: points are sorted according to their dimension
widestDimension :: (Num r, Ord r, Arity d) => GV.Vector d (PointSeq d p r) -> Int
widestDimension :: Vector d (PointSeq d p r) -> Int
widestDimension = (Int, r) -> Int
forall a b. (a, b) -> a
fst ((Int, r) -> Int)
-> (Vector d (PointSeq d p r) -> (Int, r))
-> Vector d (PointSeq d p r)
-> Int
forall b c a. (b -> c) -> (a -> b) -> a -> c
. ((Int, r) -> (Int, r) -> Ordering) -> [(Int, r)] -> (Int, r)
forall (t :: * -> *) a.
Foldable t =>
(a -> a -> Ordering) -> t a -> a
L.maximumBy (((Int, r) -> r) -> (Int, r) -> (Int, r) -> Ordering
forall a b. Ord a => (b -> a) -> b -> b -> Ordering
comparing (Int, r) -> r
forall a b. (a, b) -> b
snd) ([(Int, r)] -> (Int, r))
-> (Vector d (PointSeq d p r) -> [(Int, r)])
-> Vector d (PointSeq d p r)
-> (Int, r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. [Int] -> [r] -> [(Int, r)]
forall a b. [a] -> [b] -> [(a, b)]
zip [Int
1..] ([r] -> [(Int, r)])
-> (Vector d (PointSeq d p r) -> [r])
-> Vector d (PointSeq d p r)
-> [(Int, r)]
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Vector d r -> [r]
forall (t :: * -> *) a. Foldable t => t a -> [a]
F.toList (Vector d r -> [r])
-> (Vector d (PointSeq d p r) -> Vector d r)
-> Vector d (PointSeq d p r)
-> [r]
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Vector d (PointSeq d p r) -> Vector d r
forall r (d :: Nat) p.
(Num r, Arity d) =>
Vector d (PointSeq d p r) -> Vector d r
widths

widths :: (Num r, Arity d) => GV.Vector d (PointSeq d p r) -> GV.Vector d r
widths :: Vector d (PointSeq d p r) -> Vector d r
widths = (Range r -> r) -> Vector d (Range r) -> Vector d r
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap Range r -> r
forall r. Num r => Range r -> r
Range.width (Vector d (Range r) -> Vector d r)
-> (Vector d (PointSeq d p r) -> Vector d (Range r))
-> Vector d (PointSeq d p r)
-> Vector d r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Vector d (PointSeq d p r) -> Vector d (Range r)
forall (d :: Nat) p r.
Arity d =>
Vector d (PointSeq d p r) -> Vector d (Range r)
extends


{- HLINT ignore extends -}
-- | get the extends of the set of points in every dimension, i.e. the left and
-- right boundaries.
--
-- pre: points are sorted according to their dimension
extends :: Arity d => GV.Vector d (PointSeq d p r) -> GV.Vector d (Range r)
extends :: Vector d (PointSeq d p r) -> Vector d (Range r)
extends = (Int -> PointSeq d p r -> Range r)
-> Vector d (PointSeq d p r) -> Vector d (Range r)
forall i (f :: * -> *) a b.
FunctorWithIndex i f =>
(i -> a -> b) -> f a -> f b
imap (\Int
i PointSeq d p r
pts ->
                     r -> r -> Range r
forall a. a -> a -> Range a
ClosedRange ((LSeq (1 + 0) (Point d r :+ p) -> Point d r :+ p
forall (n :: Nat) a. LSeq (1 + n) a -> a
LSeq.head PointSeq d p r
LSeq (1 + 0) (Point d r :+ p)
pts)(Point d r :+ p) -> Getting r (Point d r :+ p) r -> r
forall s a. s -> Getting a s a -> a
^.(Point d r -> Const r (Point d r))
-> (Point d r :+ p) -> Const r (Point d r :+ p)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core((Point d r -> Const r (Point d r))
 -> (Point d r :+ p) -> Const r (Point d r :+ p))
-> ((r -> Const r r) -> Point d r -> Const r (Point d r))
-> Getting r (Point d r :+ p) r
forall b c a. (b -> c) -> (a -> b) -> a -> c
.Int -> Lens' (Point d r) r
forall (d :: Nat) (p :: Nat -> * -> *) r.
(Arity d, AsAPoint p) =>
Int -> Lens' (p d r) r
unsafeCoord (Int
i Int -> Int -> Int
forall a. Num a => a -> a -> a
+ Int
1))
                                 ((LSeq (1 + 0) (Point d r :+ p) -> Point d r :+ p
forall (n :: Nat) a. LSeq (1 + n) a -> a
LSeq.last PointSeq d p r
LSeq (1 + 0) (Point d r :+ p)
pts)(Point d r :+ p) -> Getting r (Point d r :+ p) r -> r
forall s a. s -> Getting a s a -> a
^.(Point d r -> Const r (Point d r))
-> (Point d r :+ p) -> Const r (Point d r :+ p)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core((Point d r -> Const r (Point d r))
 -> (Point d r :+ p) -> Const r (Point d r :+ p))
-> ((r -> Const r r) -> Point d r -> Const r (Point d r))
-> Getting r (Point d r :+ p) r
forall b c a. (b -> c) -> (a -> b) -> a -> c
.Int -> Lens' (Point d r) r
forall (d :: Nat) (p :: Nat -> * -> *) r.
(Arity d, AsAPoint p) =>
Int -> Lens' (p d r) r
unsafeCoord (Int
i Int -> Int -> Int
forall a. Num a => a -> a -> a
+ Int
1)))


--------------------------------------------------------------------------------
-- * Finding Well Separated Pairs

findPairs                     :: (Floating r, Ord r, Arity d, Arity (d + 1))
                              => r -> SplitTree d p r a -> SplitTree d p r a
                              -> [WSP d p r a]
findPairs :: r -> SplitTree d p r a -> SplitTree d p r a -> [WSP d p r a]
findPairs r
s SplitTree d p r a
l SplitTree d p r a
r
  | r -> SplitTree d p r a -> SplitTree d p r a -> Bool
forall r (d :: Nat) p a.
(Floating r, Ord r, Arity d) =>
r -> SplitTree d p r a -> SplitTree d p r a -> Bool
areWellSeparated' r
s SplitTree d p r a
l SplitTree d p r a
r   = [(SplitTree d p r a
l,SplitTree d p r a
r)]
  | SplitTree d p r a -> r
forall (d :: Nat) r p a. (Arity d, Num r) => SplitTree d p r a -> r
maxWidth SplitTree d p r a
l r -> r -> Bool
forall a. Ord a => a -> a -> Bool
<=  SplitTree d p r a -> r
forall (d :: Nat) r p a. (Arity d, Num r) => SplitTree d p r a -> r
maxWidth SplitTree d p r a
r = (SplitTree d p r a -> [WSP d p r a])
-> [SplitTree d p r a] -> [WSP d p r a]
forall (t :: * -> *) a b. Foldable t => (a -> [b]) -> t a -> [b]
concatMap (r -> SplitTree d p r a -> SplitTree d p r a -> [WSP d p r a]
forall r (d :: Nat) p a.
(Floating r, Ord r, Arity d, Arity (d + 1)) =>
r -> SplitTree d p r a -> SplitTree d p r a -> [WSP d p r a]
findPairs r
s SplitTree d p r a
l) ([SplitTree d p r a] -> [WSP d p r a])
-> [SplitTree d p r a] -> [WSP d p r a]
forall a b. (a -> b) -> a -> b
$ SplitTree d p r a -> [SplitTree d p r a]
forall v a. BinLeafTree v a -> [BinLeafTree v a]
children' SplitTree d p r a
r
  | Bool
otherwise                 = (SplitTree d p r a -> [WSP d p r a])
-> [SplitTree d p r a] -> [WSP d p r a]
forall (t :: * -> *) a b. Foldable t => (a -> [b]) -> t a -> [b]
concatMap (r -> SplitTree d p r a -> SplitTree d p r a -> [WSP d p r a]
forall r (d :: Nat) p a.
(Floating r, Ord r, Arity d, Arity (d + 1)) =>
r -> SplitTree d p r a -> SplitTree d p r a -> [WSP d p r a]
findPairs r
s SplitTree d p r a
r) ([SplitTree d p r a] -> [WSP d p r a])
-> [SplitTree d p r a] -> [WSP d p r a]
forall a b. (a -> b) -> a -> b
$ SplitTree d p r a -> [SplitTree d p r a]
forall v a. BinLeafTree v a -> [BinLeafTree v a]
children' SplitTree d p r a
l


-- -- | Test if the two sets are well separated with param s
-- areWellSeparated                     :: (Arity d, Arity (d + 1), Fractional r, Ord r)
--                                      => r -- ^ separation factor
--                                      -> SplitTree d p r a
--                                      -> SplitTree d p r a -> Bool
-- areWellSeparated _ (Leaf _) (Leaf _) = True
-- areWellSeparated s l        r        = boxBox s (bbOf l)   (bbOf r)


-- areWellSeparated s (Leaf p)      (Node _ nd _) = pointBox s (p^.core) (nd^.bBox)
-- areWellSeparated s (Node _ nd _) (Leaf p)      = pointBox s (p^.core) (nd^.bBox)
-- areWellSeparated s (Node _ ld _) (Node _ rd _) = boxBox   s (ld^.bBox) (rd^.bBox)

{- HLINT ignore boxBox -}
-- -- | Test if the point and the box are far enough appart
-- pointBox       :: (Fractional r, Ord r, AlwaysTruePFT d, AlwaysTrueTransformation d)
--                => r -> Point d r -> Box d p r -> Bool
-- pointBox s p b = not $ p `inBox` b'
--   where
--     v  = (centerPoint b)^.vector
--     b' = translateBy v . scaleUniformlyBy s . translateBy ((-1) *^ v) $ b

-- -- | Test if the two boxes are sufficiently far appart
-- boxBox         :: (Fractional r, Ord r, Arity d, Arity (d + 1))
--                => r -> Box d p r -> Box d p r -> Bool
-- boxBox s lb rb = boxBox' lb rb && boxBox' rb lb
--   where
--     boxBox' b' b = not $ b' `intersects` bOut
--       where
--         v    = (centerPoint b)^.vector
--         bOut = translateBy v . scaleUniformlyBy s . translateBy ((-1) *^ v) $ b

--------------------------------------------------------------------------------
-- * Alternative def if wellSeparated that uses fractional


areWellSeparated'                     :: (Floating r, Ord r, Arity d)
                                      => r
                                      -> SplitTree d p r a
                                      -> SplitTree d p r a
                                      -> Bool
areWellSeparated' :: r -> SplitTree d p r a -> SplitTree d p r a -> Bool
areWellSeparated' r
_ (Leaf Point d r :+ p
_) (Leaf Point d r :+ p
_) = Bool
True
areWellSeparated' r
s SplitTree d p r a
l        SplitTree d p r a
r        = r -> Box d () r -> Box d () r -> Bool
forall r (d :: Nat) p.
(Floating r, Ord r, Arity d) =>
r -> Box d p r -> Box d p r -> Bool
boxBox1 r
s (SplitTree d p r a -> Box d () r
forall r (d :: Nat) p a. Ord r => SplitTree d p r a -> Box d () r
bbOf SplitTree d p r a
l) (SplitTree d p r a -> Box d () r
forall r (d :: Nat) p a. Ord r => SplitTree d p r a -> Box d () r
bbOf SplitTree d p r a
r)

-- (Leaf p)      (Node _ nd _) = pointBox' s (p^.core) (nd^.bBox)
-- areWellSeparated' s (Node _ nd _) (Leaf p)      = pointBox' s (p^.core) (nd^.bBox)
-- areWellSeparated' s (Node _ ld _) (Node _ rd _) = boxBox'   s (ld^.bBox) (rd^.bBox)

boxBox1         :: (Floating r, Ord r, Arity d) => r -> Box d p r -> Box d p r -> Bool
boxBox1 :: r -> Box d p r -> Box d p r -> Bool
boxBox1 r
s Box d p r
lb Box d p r
rb = Point d r -> Point d r -> r
forall r (d :: Nat).
(Floating r, Arity d) =>
Point d r -> Point d r -> r
euclideanDist (Box d p r -> Point d r
forall (d :: Nat) r p.
(Arity d, Fractional r) =>
Box d p r -> Point d r
centerPoint Box d p r
lb) (Box d p r -> Point d r
forall (d :: Nat) r p.
(Arity d, Fractional r) =>
Box d p r -> Point d r
centerPoint Box d p r
rb) r -> r -> Bool
forall a. Ord a => a -> a -> Bool
>= (r
sr -> r -> r
forall a. Num a => a -> a -> a
+r
1)r -> r -> r
forall a. Num a => a -> a -> a
*r
d
  where
    diam :: Box d p r -> r
diam Box d p r
b = Point d r -> Point d r -> r
forall r (d :: Nat).
(Floating r, Arity d) =>
Point d r -> Point d r -> r
euclideanDist (Box d p r
bBox d p r
-> Getting (Point d r) (Box d p r) (Point d r) -> Point d r
forall s a. s -> Getting a s a -> a
^.((CWMin (Point d r) :+ p)
 -> Const (Point d r) (CWMin (Point d r) :+ p))
-> Box d p r -> Const (Point d r) (Box d p r)
forall (d :: Nat) p r. Lens' (Box d p r) (CWMin (Point d r) :+ p)
minP(((CWMin (Point d r) :+ p)
  -> Const (Point d r) (CWMin (Point d r) :+ p))
 -> Box d p r -> Const (Point d r) (Box d p r))
-> ((Point d r -> Const (Point d r) (Point d r))
    -> (CWMin (Point d r) :+ p)
    -> Const (Point d r) (CWMin (Point d r) :+ p))
-> Getting (Point d r) (Box d p r) (Point d r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
.(CWMin (Point d r) -> Const (Point d r) (CWMin (Point d r)))
-> (CWMin (Point d r) :+ p)
-> Const (Point d r) (CWMin (Point d r) :+ p)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core((CWMin (Point d r) -> Const (Point d r) (CWMin (Point d r)))
 -> (CWMin (Point d r) :+ p)
 -> Const (Point d r) (CWMin (Point d r) :+ p))
-> ((Point d r -> Const (Point d r) (Point d r))
    -> CWMin (Point d r) -> Const (Point d r) (CWMin (Point d r)))
-> (Point d r -> Const (Point d r) (Point d r))
-> (CWMin (Point d r) :+ p)
-> Const (Point d r) (CWMin (Point d r) :+ p)
forall b c a. (b -> c) -> (a -> b) -> a -> c
.(Point d r -> Const (Point d r) (Point d r))
-> CWMin (Point d r) -> Const (Point d r) (CWMin (Point d r))
forall a1 a2. Iso (CWMin a1) (CWMin a2) a1 a2
cwMin) (Box d p r
bBox d p r
-> Getting (Point d r) (Box d p r) (Point d r) -> Point d r
forall s a. s -> Getting a s a -> a
^.((CWMax (Point d r) :+ p)
 -> Const (Point d r) (CWMax (Point d r) :+ p))
-> Box d p r -> Const (Point d r) (Box d p r)
forall (d :: Nat) p r. Lens' (Box d p r) (CWMax (Point d r) :+ p)
maxP(((CWMax (Point d r) :+ p)
  -> Const (Point d r) (CWMax (Point d r) :+ p))
 -> Box d p r -> Const (Point d r) (Box d p r))
-> ((Point d r -> Const (Point d r) (Point d r))
    -> (CWMax (Point d r) :+ p)
    -> Const (Point d r) (CWMax (Point d r) :+ p))
-> Getting (Point d r) (Box d p r) (Point d r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
.(CWMax (Point d r) -> Const (Point d r) (CWMax (Point d r)))
-> (CWMax (Point d r) :+ p)
-> Const (Point d r) (CWMax (Point d r) :+ p)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core((CWMax (Point d r) -> Const (Point d r) (CWMax (Point d r)))
 -> (CWMax (Point d r) :+ p)
 -> Const (Point d r) (CWMax (Point d r) :+ p))
-> ((Point d r -> Const (Point d r) (Point d r))
    -> CWMax (Point d r) -> Const (Point d r) (CWMax (Point d r)))
-> (Point d r -> Const (Point d r) (Point d r))
-> (CWMax (Point d r) :+ p)
-> Const (Point d r) (CWMax (Point d r) :+ p)
forall b c a. (b -> c) -> (a -> b) -> a -> c
.(Point d r -> Const (Point d r) (Point d r))
-> CWMax (Point d r) -> Const (Point d r) (CWMax (Point d r))
forall a1 a2. Iso (CWMax a1) (CWMax a2) a1 a2
cwMax)
    d :: r
d      = r -> r -> r
forall a. Ord a => a -> a -> a
max (Box d p r -> r
forall (d :: Nat) r p.
(ImplicitPeano (Peano d), Floating r,
 ArityPeano (Peano (FromPeano (Peano d))),
 KnownNat (FromPeano (Peano d)), KnownNat d,
 Peano (FromPeano (Peano d) + 1)
 ~ 'S (Peano (FromPeano (Peano d)))) =>
Box d p r -> r
diam Box d p r
lb) (Box d p r -> r
forall (d :: Nat) r p.
(ImplicitPeano (Peano d), Floating r,
 ArityPeano (Peano (FromPeano (Peano d))),
 KnownNat (FromPeano (Peano d)), KnownNat d,
 Peano (FromPeano (Peano d) + 1)
 ~ 'S (Peano (FromPeano (Peano d)))) =>
Box d p r -> r
diam Box d p r
rb)




--------------------------------------------------------------------------------
-- * Helper stuff


-- | Computes the maximum width of a splitTree
maxWidth                             :: (Arity d, Num r)
                                     => SplitTree d p r a -> r
maxWidth :: SplitTree d p r a -> r
maxWidth (Leaf Point d r :+ p
_)                    = r
0
maxWidth (Node SplitTree d p r a
_ (NodeData Int
i Box d () r
b a
_) SplitTree d p r a
_) = Maybe r -> r
forall a. HasCallStack => Maybe a -> a
fromJust (Maybe r -> r) -> Maybe r -> r
forall a b. (a -> b) -> a -> b
$ Int -> Box d () r -> Maybe r
forall (d :: Nat) r p.
(Arity d, Num r) =>
Int -> Box d p r -> Maybe r
widthIn' Int
i Box d () r
b

-- | 'Computes' the bounding box of a split tree
bbOf                             :: Ord r => SplitTree d p r a -> Box d () r
bbOf :: SplitTree d p r a -> Box d () r
bbOf (Leaf Point d r :+ p
p)                    = Point d r -> Box (Dimension (Point d r)) () (NumType (Point d r))
forall g.
(IsBoxable g, Ord (NumType g)) =>
g -> Box (Dimension g) () (NumType g)
boundingBox (Point d r -> Box (Dimension (Point d r)) () (NumType (Point d r)))
-> Point d r
-> Box (Dimension (Point d r)) () (NumType (Point d r))
forall a b. (a -> b) -> a -> b
$ Point d r :+ p
p(Point d r :+ p)
-> Getting (Point d r) (Point d r :+ p) (Point d r) -> Point d r
forall s a. s -> Getting a s a -> a
^.Getting (Point d r) (Point d r :+ p) (Point d r)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core
bbOf (Node SplitTree d p r a
_ (NodeData Int
_ Box d () r
b a
_) SplitTree d p r a
_) = Box d () r
b


children'              :: BinLeafTree v a -> [BinLeafTree v a]
children' :: BinLeafTree v a -> [BinLeafTree v a]
children' (Leaf a
_)     = []
children' (Node BinLeafTree v a
l v
_ BinLeafTree v a
r) = [BinLeafTree v a
l,BinLeafTree v a
r]


-- | Turn a traversal into lens
ix'   :: (Arity d, KnownNat d) => Int -> Lens' (GV.Vector d a) a
ix' :: Int -> Lens' (Vector d a) a
ix' Int
i = Traversing (->) f (Vector d a) (Vector d a) a a
-> Over (->) f (Vector d a) (Vector d a) a a
forall (p :: * -> * -> *) (f :: * -> *) s t a.
(HasCallStack, Conjoined p, Functor f) =>
Traversing p f s t a a -> Over p f s t a a
singular (Int -> Traversal' (Vector d a) a
forall (d :: Nat) r. Arity d => Int -> Traversal' (Vector d r) r
GV.element' Int
i)


dropIdx                 :: core :+ (t :+ extra) -> core :+ extra
dropIdx :: (core :+ (t :+ extra)) -> core :+ extra
dropIdx (core
p :+ (t
_ :+ extra
e)) = core
p core -> extra -> core :+ extra
forall core extra. core -> extra -> core :+ extra
:+ extra
e

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