Kinematics.hs

-- |
-- Module:      Data.Geo.Jord.Kinematics
-- Copyright:   (c) 2018 Cedric Liegeois
-- License:     BSD3
-- Maintainer:  Cedric Liegeois <ofmooseandmen@yahoo.fr>
-- Stability:   experimental
-- Portability: portable
--
-- Types and functions for working with kinematics calculations assuming a __spherical__ earth model.
--
-- All functions are implemented using the vector-based approached described in
-- <http://www.navlab.net/Publications/A_Nonsingular_Horizontal_Position_Representation.pdf Gade, K. (2010). A Non-singular Horizontal Position Representation>
-- and in <https://calhoun.nps.edu/bitstream/handle/10945/29516/sometacticalalgo00shud.pdf Shudde, Rex H. (1986). Some tactical algorithms for spherical geometry>
--
module Data.Geo.Jord.Kinematics
    (
    -- * The 'Track' type.
    Track(..)
    -- * The 'Course' type.
    , Course
    -- * The 'Cpa' type.
    , Cpa
    , cpaTime
    , cpaDistance
    , cpaPosition1
    , cpaPosition2
    -- * The 'Intercept' type.
    , Intercept
    , interceptTime
    , interceptDistance
    , interceptPosition
    , interceptorBearing
    , interceptorSpeed
    -- * Calculations
    , course
    , position
    , position84
    , cpa
    , cpa84
    , intercept
    , intercept84
    , interceptBySpeed
    , interceptBySpeed84
    , interceptByTime
    , interceptByTime84
    ) where

import Control.Applicative
import Data.Geo.Jord.Angle
import Data.Geo.Jord.AngularPosition
import Data.Geo.Jord.Duration
import Data.Geo.Jord.Earth
import Data.Geo.Jord.Geodetics
import Data.Geo.Jord.LatLong
import Data.Geo.Jord.Length
import Data.Geo.Jord.NVector
import Data.Geo.Jord.Quantity
import Data.Geo.Jord.Speed
import Data.Geo.Jord.Transformation
import Data.Geo.Jord.Vector3d
import Data.Maybe (isNothing)

-- | 'Track' represents the state of a vehicle by its current position, bearing and speed.
data Track a = Track
    { trackPos :: a -- ^ position of the track.
    , trackBearing :: Angle -- ^ bearing of the track.
    , trackSpeed :: Speed -- ^ speed of the track.
    } deriving (Eq, Show)

-- | 'GreatCircle' from track.
instance (NTransform a, Show a) => IsGreatCircle (Track a) where
    greatCircleE t = greatCircleE (trackPos t, trackBearing t)

-- | 'Course' represents the cardinal direction in which the vehicle is to be steered.
newtype Course =
    Course Vector3d
    deriving (Eq, Show)

instance IsVector3d Course where
    vec (Course v) = v

-- | Time to, and distance at, closest point of approach (CPA) as well as position of both tracks at CPA.
data Cpa a = Cpa
    { cpaTime :: Duration -- ^ time to CPA.
    , cpaDistance :: Length -- ^ distance at CPA.
    , cpaPosition1 :: a -- ^ position of track 1 at CPA.
    , cpaPosition2 :: a -- ^ position of track 2 at CPA.
    } deriving (Eq, Show)

-- | Time, distance and position of intercept as well as speed and initial bearing of interceptor.
data Intercept a = Intercept
    { interceptTime :: Duration -- ^ time to intercept.
    , interceptDistance :: Length -- ^ distance at intercept.
    , interceptPosition :: a -- ^ position of intercept.
    , interceptorBearing :: Angle -- ^ initial bearing of interceptor.
    , interceptorSpeed :: Speed -- ^ speed of interceptor.
    } deriving (Eq, Show)

-- | @course p b@ computes the course of a vehicle currently at position @p@ and following bearing @b@.
course :: (NTransform a) => a -> Angle -> Course
course p b = Course (Vector3d (vz (head r)) (vz (r !! 1)) (vz (r !! 2)))
  where
    ll = nvectorToLatLong . pos . toNVector $ p
    lat = latitude ll
    lon = longitude ll
    r = mdot (mdot (rz (negate' lon)) (ry lat)) (rx b)

-- | @position t d r@ computes the position of a track @t@ after duration @d@ has elapsed and using the earth radius @r@.
--
-- @
--     let p0 = latLongHeight (readLatLong "531914N0014347W") (metres 15000)
--     let b = decimalDegrees 96.0217
--     let s = kilometresPerHour 124.8
--     let p1 = decimalLatLongHeight 53.1882691 0.1332741 (metres 15000)
--     position (Track p0 b s) (hours 1) r84 = p1
-- @
position :: (NTransform a) => Track a -> Duration -> Length -> a
position (Track p0 b s) d = position' p0 s (course p0 b) (toSeconds d)

-- | 'position' using the mean radius of the WGS84 reference ellipsoid.
position84 :: (NTransform a) => Track a -> Duration -> a
position84 t d = position t d r84

-- | @cpa t1 t2 r@ computes the closest point of approach between tracks @t1@ and @t2@ and using the earth radius @r@.
--
-- @
--     let p1 = decimalLatLong 20 (-60)
--     let b1 = decimalDegrees 10
--     let s1 = knots 15
--     let p2 = decimalLatLong 34 (-50)
--     let b2 = decimalDegrees 220
--     let s2 = knots 300
--     let t1 = Track p1 b1 s1
--     let t2 = Track p2 b2 s2
--     let c = cpa t1 t2 r84
--     fmap cpaTime c = Just (milliseconds 11396155)
--     fmap cpaDistance c = Just (kilometres 124.2317453)
-- @
cpa :: (Eq a, NTransform a) => Track a -> Track a -> Length -> Maybe (Cpa a)
cpa (Track p1 b1 s1) (Track p2 b2 s2) r
    | p1 == p2 = Just (Cpa zero zero p1 p2)
    | t < 0 = Nothing
    | otherwise = Just (Cpa (seconds t) d cp1 cp2)
  where
    c1 = course p1 b1
    c2 = course p2 b2
    t = timeToCpa p1 c1 s1 p2 c2 s2 r
    cp1 = position' p1 s1 c1 t r
    cp2 = position' p2 s2 c2 t r
    d = surfaceDistance cp1 cp2 r

-- | 'cpa' using the mean radius of the WGS84 reference ellipsoid.
cpa84 :: (Eq a, NTransform a) => Track a -> Track a -> Maybe (Cpa a)
cpa84 t1 t2 = cpa t1 t2 r84

-- | @intercept t p r@ computes the __minimum__ speed of interceptor at
-- position @p@ needed for an intercept with target track @t@ to take place
-- using the earth radius @r@. Intercept time, position, distance and interceptor
-- bearing are derived from this minimum speed. Returns 'Nothing' if intercept
-- cannot be achieved e.g.:
--
--     * interceptor and target are at the same position
--
--     * interceptor is "behind" the target
--
-- @
--     let t = Track (decimalLatLong 34 (-50)) (decimalDegrees 220) (knots 600)
--     let ip = (decimalLatLong 20 (-60))
--     let i = intercept t ip r84
--     fmap interceptorSpeed i = Just (knots 52.633367756059)
--     fmap interceptTime i = Just (seconds 5993.831)
-- @
intercept :: (Eq a, NTransform a) => Track a -> a -> Length -> Maybe (Intercept a)
intercept t p r = interceptByTime t p (seconds (timeToIntercept t p r)) r

-- | 'intercept' using the mean radius of the WGS84 reference ellipsoid.
intercept84 :: (Eq a, NTransform a) => Track a -> a -> Maybe (Intercept a)
intercept84 t p = intercept t p r84

-- | @interceptBySpeed t p s r@ computes the time needed by interceptor at
-- position @p@ and travelling at speed @s@ to intercept target track @t@
-- using the earth radius @r@. Returns 'Nothing' if intercept
-- cannot be achieved e.g.:
--
--     * interceptor and target are at the same position
--
--     * interceptor speed is below minimum speed returned by 'intercept'
interceptBySpeed :: (Eq a, NTransform a) => Track a -> a -> Speed -> Length -> Maybe (Intercept a)
interceptBySpeed t p s r
    | isNothing minInt = Nothing
    | fmap interceptorSpeed minInt == Just s = minInt
    | otherwise = interceptByTime t p (seconds (timeToInterceptSpeed t p s r)) r
  where
    minInt = intercept t p r

-- | 'interceptBySpeed' using the mean radius of the WGS84 reference ellipsoid.
interceptBySpeed84 :: (Eq a, NTransform a) => Track a -> a -> Speed -> Maybe (Intercept a)
interceptBySpeed84 t p s = interceptBySpeed t p s r84

-- | @interceptByTime t p d r@ computes the speed of interceptor at
-- position @p@ needed for an intercept with target track @t@ to take place
-- after duration @d@ and using the earth radius @r@. Returns 'Nothing' if
-- given duration is <= 0 or interceptor and target are at the same position.
--
-- @
--     let t = Track (decimalLatLong 34 (-50)) (decimalDegrees 220) (knots 600)
--     let ip = (decimalLatLong 20 (-60))
--     let d = seconds 2700
--     let i = interceptByTime t ip d r84
--     fmap interceptorSpeed i = Just (knots 730.959238)
--     fmap interceptorBearing i = Just (decimalDegrees 26.1199030)
--     fmap interceptPosition i = Just (decimalLatLong 28.1366797 (-55.4559475))
--     fmap interceptDistance i = Just (metres 1015302.3815)
--     fmap interceptTime i = Just (seconds 2700)
-- @
--
-- Note: contrary to 'intercept' and 'interceptBySpeed' this function handles
-- cases where the interceptor has to catch up the target.
interceptByTime :: (Eq a, NTransform a) => Track a -> a -> Duration -> Length -> Maybe (Intercept a)
interceptByTime t p d r
    | toMilliseconds d <= 0 = Nothing
    | trackPos t == p = Nothing
    | otherwise = fmap (\b -> Intercept d idist ipos b is) ib
  where
    ipos = position t d r
    idist = surfaceDistance p ipos r
    ib = initialBearing p ipos <|> initialBearing p (trackPos t)
    is = metresPerSecond (toMetres idist / toSeconds d)

-- | 'interceptByTime' using the mean radius of the WGS84 reference ellipsoid.
interceptByTime84 :: (Eq a, NTransform a) => Track a -> a -> Duration -> Maybe (Intercept a)
interceptByTime84 t p d = interceptByTime t p d r84

-- | position from speed course and seconds.
position' :: (NTransform a) => a -> Speed -> Course -> Double -> Length -> a
position' p0 s c sec r = fromNVector (nvectorHeight (nvector (vx v1) (vy v1) (vz v1)) h0)
  where
    nv0 = toNVector p0
    v0 = vec . pos $nv0
    h0 = height nv0
    v1 = position'' v0 s (vec c) sec r

-- | position from speed course and seconds.
position'' :: Vector3d -> Speed -> Vector3d -> Double -> Length -> Vector3d
position'' v0 s c sec r = v1
  where
    a = toMetresPerSecond s / toMetres r * sec
    v1 = vadd (vscale v0 (cos a)) (vscale c (sin a))

-- | time to CPA.
timeToCpa :: (NTransform a) => a -> Course -> Speed -> a -> Course -> Speed -> Length -> Double
timeToCpa p1 c1 s1 p2 c2 s2 r = cpaNrRec v10 c10 w1 v20 c20 w2 0 0
  where
    v10 = vec3d p1
    c10 = vec c1
    rm = toMetres r
    w1 = toMetresPerSecond s1 / rm
    v20 = vec . pos . toNVector $ p2
    c20 = vec c2
    w2 = toMetresPerSecond s2 / rm

-- | time to intercept with minimum speed
timeToIntercept :: (NTransform a) => Track a -> a -> Length -> Double
timeToIntercept (Track p2 b2 s2) p1 r = intMinNrRec v10v20 v10c2 w2 (sep v10 v20 c2 s2 r) t0 0
  where
    v10 = vec3d p1
    v20 = vec3d p2
    c2 = vec (course p2 b2)
    v10v20 = vdot v10 v20
    v10c2 = vdot v10 c2
    s2mps = toMetresPerSecond s2
    rm = toMetres r
    w2 = s2mps / rm
    s0 = ad v10 v20 -- initial angular distance between target and interceptor
    t0 = rm * s0 / s2mps -- assume target is travelling towards interceptor

-- | time to intercept with speed.
timeToInterceptSpeed :: (NTransform a) => Track a -> a -> Speed -> Length -> Double
timeToInterceptSpeed (Track p2 b2 s2) p1 s1 r =
    intSpdNrRec v10v20 v10c2 w1 w2 (sep v10 v20 c2 s2 r) t0 0
  where
    v10 = vec3d p1
    v20 = vec3d p2
    c2 = vec (course p2 b2)
    v10v20 = vdot v10 v20
    v10c2 = vdot v10 c2
    rm = toMetres r
    w1 = toMetresPerSecond s1 / rm
    w2 = toMetresPerSecond s2 / rm
    t0 = 0.1

rx :: Angle -> [Vector3d]
rx a = [Vector3d 1 0 0, Vector3d 0 c s, Vector3d 0 (-s) c]
  where
    c = cos' a
    s = sin' a

ry :: Angle -> [Vector3d]
ry a = [Vector3d c 0 (-s), Vector3d 0 1 0, Vector3d s 0 c]
  where
    c = cos' a
    s = sin' a

rz :: Angle -> [Vector3d]
rz a = [Vector3d c s 0, Vector3d (-s) c 0, Vector3d 0 0 1]
  where
    c = cos' a
    s = sin' a

cpaA :: Vector3d -> Vector3d -> Double -> Vector3d -> Vector3d -> Double -> Double
cpaA v10 c10 w1 v20 c20 w2 = negate (vdot (vscale v10 w1) c20 + vdot (vscale v20 w2) c10)

cpaB :: Vector3d -> Vector3d -> Double -> Vector3d -> Vector3d -> Double -> Double
cpaB v10 c10 w1 v20 c20 w2 = vdot (vscale c10 w1) v20 + vdot (vscale c20 w2) v10

cpaC :: Vector3d -> Vector3d -> Double -> Vector3d -> Vector3d -> Double -> Double
cpaC v10 c10 w1 v20 c20 w2 = negate (vdot (vscale v10 w1) v20 - vdot (vscale c20 w2) c10)

cpaD :: Vector3d -> Vector3d -> Double -> Vector3d -> Vector3d -> Double -> Double
cpaD v10 c10 w1 v20 c20 w2 = vdot (vscale c10 w1) c20 - vdot (vscale v20 w2) v10

cpaFt :: Double -> Double -> Double -> Double -> Double -> Double -> Double -> Double -> Double
cpaFt cw1t cw2t sw1t sw2t a b c d =
    a * sw1t * sw2t + b * cw1t * cw2t + c * sw1t * cw2t + d * cw1t * sw2t

cpaDft ::
       Double
    -> Double
    -> Double
    -> Double
    -> Double
    -> Double
    -> Double
    -> Double
    -> Double
    -> Double
    -> Double
cpaDft w1 w2 cw1t cw2t sw1t sw2t a b c d =
    negate ((c * w2 + d * w1) * sw1t * sw2t) + (d * w2 + c * w1) * cw1t * cw2t +
    (a * w2 - b * w1) * sw1t * cw2t -
    (b * w2 - a * w1) * cw1t * sw2t

cpaStep :: Vector3d -> Vector3d -> Double -> Vector3d -> Vector3d -> Double -> Double -> Double
cpaStep v10 c10 w1 v20 c20 w2 t =
    cpaFt cw1t cw2t sw1t sw2t a b c d / cpaDft w1 w2 cw1t cw2t sw1t sw2t a b c d
  where
    cw1t = cos (w1 * t)
    cw2t = cos (w2 * t)
    sw1t = sin (w1 * t)
    sw2t = sin (w2 * t)
    a = cpaA v10 c10 w1 v20 c20 w2
    b = cpaB v10 c10 w1 v20 c20 w2
    c = cpaC v10 c10 w1 v20 c20 w2
    d = cpaD v10 c10 w1 v20 c20 w2

-- | Newton-Raphson for CPA time.
cpaNrRec ::
       Vector3d -> Vector3d -> Double -> Vector3d -> Vector3d -> Double -> Double -> Int -> Double
cpaNrRec v10 c10 w1 v20 c20 w2 ti i
    | i == 50 = -1.0 -- no convergence
    | abs fi < 1e-11 = ti1
    | otherwise = cpaNrRec v10 c10 w1 v20 c20 w2 ti1 (i + 1)
  where
    fi = cpaStep v10 c10 w1 v20 c20 w2 ti
    ti1 = ti - fi

-- | Newton-Raphson for min speed intercept.
intMinNrRec :: Double -> Double -> Double -> (Double -> Double) -> Double -> Int -> Double
intMinNrRec v10v20 v10c2 w2 st ti i
    | i == 50 = -1.0 -- no convergence
    | abs fi < 1e-11 = ti1
    | otherwise = intMinNrRec v10v20 v10c2 w2 st ti1 (i + 1)
  where
    cosw2t = cos (w2 * ti)
    sinw2t = sin (w2 * ti)
    v10dv2dt = (-w2) * (v10v20 * sinw2t - v10c2 * cosw2t)
    v10d2v2dt2 = (-1.0 * w2 * w2) * (v10v20 * cosw2t + v10c2 * sinw2t)
    si = st ti
    sinS = sin si
    a = (-1.0) / sinS
    b = cos si / (sinS * sinS)
    f = ti * a * v10dv2dt - si
    d2sdt2 = a * (b * v10dv2dt * v10dv2dt + v10d2v2dt2)
    df = ti * d2sdt2
    fi = f / df
    ti1 = ti - fi

-- | Newton-Raphson for speed intercept.
intSpdNrRec :: Double -> Double -> Double -> Double -> (Double -> Double) -> Double -> Int -> Double
intSpdNrRec v10v20 v10c2 w1 w2 st ti i
    | i == 50 = -1.0 -- no convergence
    | abs fi < 1e-11 = ti1
    | otherwise = intSpdNrRec v10v20 v10c2 w1 w2 st ti1 (i + 1)
  where
    cosw2t = cos (w2 * ti)
    sinw2t = sin (w2 * ti)
    si = st ti
    f = si / ti - w1
    dsdt = (w2 * (v10v20 * sinw2t - v10c2 * cosw2t)) / sin si
    df = (dsdt - (si / ti)) / ti
    fi = f / df
    ti1 = ti - fi

-- | angular separation in radians at ti between v10 and track with initial position v20,
-- course c2 and speed s2.
sep :: Vector3d -> Vector3d -> Vector3d -> Speed -> Length -> Double -> Double
sep v10 v20 c2 s2 r ti = ad v10 (position'' v20 s2 c2 ti r)

-- | angle in radians between 2 n-vectors (as vector3d), copied from Geodetics
-- without the sign and returing radians.
ad :: Vector3d -> Vector3d -> Double
ad v1 v2 = atan2 (vnorm (vcross v1 v2)) (vdot v1 v2)

vec3d :: (NTransform a) => a -> Vector3d
vec3d = vec . pos . toNVector