{-# LANGUAGE TypeOperators, TypeFamilies, FlexibleContexts #-}
-- | The implementation assumes IEEE 754 arithmetic.

module Geodetics.Path where

import Control.Monad
import Geodetics.Ellipsoids
import Geodetics.Geodetic
import Numeric.Units.Dimensional.Prelude
import Prelude ()


-- | Lower and upper exclusive bounds within which a path is valid. 
type PathValidity = (Length Double, Length Double)

-- | A path is a parametric function of distance along the path. The result is the
-- position, and the direction of the path at that point as heading and elevation angles.
--
-- A well-behaved path must be continuous and monotonic (that is, if the distance increases
-- then the result is further along the path) for all distances within its validity range.
-- Ideally the physical distance along the path from the origin to the result should be equal
-- to the argument. If this is not practical then a reasonably close approximation may be used,
-- such as a spheroid instead of an ellipsoid, provided that this is documented. 
-- Outside its validity the path function may
-- return anything or bottom.
data Path e = Path {
      pathFunc :: Length Double -> (Geodetic e, Angle Double, Angle Double),
      pathValidity :: PathValidity
   }
   
-- | Convenience value for paths that are valid for all distances.
alwaysValid :: PathValidity
alwaysValid = (negate inf, inf) where
   inf = (1.0 *~ meter) / (0 *~ one)  -- Assumes IEE arithmetic.


-- | True if the path is valid at that distance.
pathValidAt :: Path e -> Length Double -> Bool
pathValidAt path d = d > x1 && d < x2
   where (x1,x2) = pathValidity path


-- | Find where a path meets a given condition using bisection. This is not the
-- most efficient algorithm, but it will always produce a result if there is one
-- within the initial bounds. If there is more than one result then an arbitrary
-- one will be returned.
-- 
-- The initial bounds must return one GT or EQ value and one LT or EQ value. If they
-- do not then @Nothing@ is returned.
bisect :: 
   Path e 
   -> (Geodetic e -> Ordering)        -- ^ Evaluation function.
   -> Length Double                   -- ^ Required accuracy in terms of distance along the path.
   -> Length Double -> Length Double  -- ^ Initial bounds.
   -> Maybe (Length Double)
bisect path f t b1 b2 = do
      guard $ pathValidAt path b1
      guard $ pathValidAt path b2
      let r = pairResults b1 b2
      guard $ hasRoot r
      bisect1 $ sortPair r
   where
      f' d = let (p, _, _) = pathFunc path d in f p 
      pairResults d1 d2 = ((d1, f' d1), (d2, f' d2))
      hasRoot (v1, v2) = snd v1 <= EQ && EQ <= snd v2
      sortPair (v1, v2) = if snd v1 <= snd v2 then (v1, v2) else (v2, v1)
      bisect1 ((d1, r1), (d2, r2)) =
         let d3 = (d1 + d2) / _2
             r3 = f' d3
             c1 = ((d1, r1), (d3, r3))
             c2 = ((d3, r3), (d2, r2))
         in if abs (d1 - d2) <= t 
            then return d3
            else bisect1 $ if hasRoot c1 then c1 else c2


-- | Try to find the intersection point of two ground paths (i.e. ignoring 
-- altitude). Returns the distance of the intersection point along each path
-- using a modified Newton-Raphson method. If the two paths are 
-- fairly straight and not close to parallel then this will converge rapidly.
--
-- The algorithm projects great-circle paths forwards using the bearing at the 
-- estimate to find the estimated intersection, and then uses the distances to this 
-- intersection as the next estimates.
--
-- If either estimate departs from its path validity then @Nothing@ is returned.
intersect :: (Ellipsoid e) =>
   Length Double -> Length Double     -- ^ Starting estimates.
   -> Length Double                   -- ^ Required accuracy.
   -> Int                             -- ^ Iteration limit. Returns @Nothing@ if this is reached.  
   -> Path e -> Path e                -- ^ Paths to intersect.
   -> Maybe (Length Double, Length Double)
intersect d1 d2 accuracy n path1 path2
   | not $ pathValidAt path1 d1     = Nothing
   | not $ pathValidAt path2 d2     = Nothing
   | n <= 0                         = Nothing
   | mag < (1e-15 *~ one)           = Nothing
   | mag3 (nv1 `cross3` nv2) * r <= accuracy = Just (d1, d2)
       -- Assumes that sin (accuracy/r) == accuracy/r
   | otherwise = 
      if abs d1a + abs d2a < abs d1b + abs d2b
         then intersect (d1 + d1a) (d2 + d2a) accuracy (pred n) path1 path2
         else intersect (d1 + d1b) (d2 + d2b) accuracy (pred n) path1 path2
   where
      (pt1, h1, _) = pathFunc path1 d1
      (pt2, h2, _) = pathFunc path2 d2
      vectors :: Angle Double -> Angle Double -> Angle Double 
                 -> (Vec3 (Dimensionless Double), Vec3 (Dimensionless Double))
      vectors lat lon b = (
          -- Unit vector of normal to surface at (lat,lon)
         (cosLat*cosLon, cosLat*sinLon, sinLat),
         -- Normal of great circle defined by bearing b at (lat,lon)
         (sinLon * cosB - sinLat * cosLon * sinB,
          negate cosLon * cosB - sinLat * sinLon * sinB,
           cosLat * sinB))
         where
            sinLon = sin lon
            sinLat = sin lat
            cosLon = cos lon
            cosLat = cos lat
            sinB = sin b
            cosB = cos b
      mag3 (x,y,z) = sqrt $ x*x + y*y + z*z
      (nv1, gc1) = vectors (latitude pt1) (longitude pt1) h1
      (nv2, gc2) = vectors (latitude pt2) (longitude pt2) h2
      nv3 = gc1 `cross3` gc2         -- Intersection of the great circles
      mag = mag3 nv3
      nv3a = scale3 nv3 (_1 / mag)   -- Scale to unit. See outer function for case when mag3 == 0
      nv3b = negate3 nv3a            -- Antipodal result. Take the closest.
      -- Find "nearest" intersection, defined as smaller of sum of distances to current points.
      d1a = gcDist gc1 nv1 nv3a * r
      d2a = gcDist gc2 nv2 nv3a * r
      d1b = gcDist gc1 nv1 nv3b * r
      d2b = gcDist gc2 nv2 nv3b * r
      -- Signed angle between v1 and v2, 
      gcDist norm v1 v2 = 
         let c = v1 `cross3` v2 
         in (if c `dot3` norm < _0 then negate else id) $ atan2 (mag3 c) (v1 `dot3` v2) 
      r = majorRadius $ ellipsoid pt1
          
{- Note on derivation of "intersect"

The algorithm is a variant of the Newton-Raphson method, and shares its advantage
of rapid convergence in many useful cases. Each path has a current approximation to
the intersection, and the next approximation is computed by projecting both paths 
along great circles from the current approximation and finding the point where those
great circles intersect. A spherical Earth is assumed for simplicity. 

The Great Circle calculations use a vector method rather than spherical trigonometry.
This avoids a lot of transcendental functions and also the singularities inherent in 
polar coordinate systems. This implementation is based on formulae from 
<http://www.movable-type.co.uk/scripts/latlong-vectors.html>, which in turn is based on 
"A Non-singular Horizontal Position Representation" by Kenneth Gade, THE JOURNAL OF 
NAVIGATION (2010), 63, 395–417.
<http://www.navlab.net/Publications/A_Nonsingular_Horizontal_Position_Representation.pdf>


"pt1" is the current approximation for the result on "path1".  The vector "nv1" is the 
unit vector pointing "pt1". "gc1" is the normal to the great circle tangent to 
"path1" at "pt1". "nv2" and "gc2" are similarly derived from "path2".

The intersection of the planes of "gc1" and "gc2" is given by their cross product, "nv3".
This is scaled to a unit vector "nv3a", and the other intersection of the great circles is
at "nv3b", opposite. The distances from nv1 and nv2 to nv3a and nv3b are computed using
the corresponding great-circle normals to determine whether the distances are positive or 
negative along the paths. The nearest solution (defined in terms of great-circle distance) 
is taken as the basis for the next approximation.
-}

-- | A ray from a point heading in a straight line in 3 dimensions. 
rayPath :: (Ellipsoid e) => 
   Geodetic e          -- ^ Start point.
   -> Angle Double     -- ^ Bearing.
   -> Angle Double     -- ^ Elevation.
   -> Path e
rayPath pt1 bearing elevation = Path ray alwaysValid
   where
      ray distance = (Geodetic lat long alt (ellipsoid pt1), bearing2, elevation2)
         where
            pt2' = pt1' `add3` (delta `scale3` distance)      -- ECEF of result point.
            (lat,long,alt) = earthToGeo (ellipsoid pt1) pt2'  -- Geodetic of result point.
            (dE,dN,dU) = transform3 (trans3 $ ecefMatrix lat long) delta  -- Direction of ray at result point.
            elevation2 = asin dU
            bearing2 = if dE == _0 && dN == _0 then bearing else atan2 dE dN  -- Allow for vertical elevation.
            
      ecefMatrix lat long =   -- Transform matrix for vectors from (East, North, Up) to (X,Y,Z).
         ((negate sinLong, negate cosLong*sinLat, cosLong*cosLat),
              --    East X      North X               Up X
          (       cosLong, negate sinLong*sinLat, sinLong*cosLat),
              --    East Y      North Y               Up Y
          (  _0           ,      cosLat         , sinLat))
              --    East Z      North Z               Up Z
         where
            sinLong = sin long
            cosLong = cos long
            sinLat = sin lat
            cosLat = cos lat
      
      direction = (sinB*cosE, cosB*cosE, sinE)  -- Direction of ray in ENU
      delta = transform3 (ecefMatrix (latitude pt1) (longitude pt1)) direction  -- Convert to ECEF
      pt1' = geoToEarth pt1    -- ECEF of origin point.
      sinB = sin bearing
      cosB = cos bearing
      sinE = sin elevation
      cosE = cos elevation
      
-- | Rhumb line: path following a constant course. Also known as a loxodrome.
--
-- The valid range stops a few arc-minutes short of the poles to ensure that the 
-- polar singularities are not included. Anyone using a rhumb line that close to a pole
-- must be going round the twist anyway.
--
-- Based on *Practical Sailing Formulas for Rhumb-Line Tracks on an Oblate Earth* 
-- by G.H. Kaplan, U.S. Naval Observatory. Except for points close to the poles 
-- the approximation is accurate to within a few meters over 1000km.
rhumbPath :: (Ellipsoid e) =>
   Geodetic e            -- ^ Start point.
   -> Angle Double       -- ^ Course.
   -> Path e
rhumbPath pt course = Path rhumb validity
   where
      rhumb distance = (Geodetic lat (properAngle lon) _0 (ellipsoid pt), course, _0)
         where
            lat' = lat0 + distance * cosC / m0   -- Kaplan Eq 13.
            lat = lat0 + (m0 / (a*(_1-e2))) * ((_1-_3*e2/_4)*(lat'-lat0)
                                              + (_3*e2/_8)*(sin (_2*lat') - sin (_2*lat0)))
            lon | abs cosC > 1e-7 *~ one 
                     = lon0 + tanC * (q lat - q0)     -- Kaplan Eq 16.
                | otherwise
                     = lon0 + distance * sinC / latitudeRadius (ellipsoid pt) ((lat0 + lat')/_2)
      validity
         | cosC > _0  = ((negate pi/_2 - latitude pt) * b / cosC, (pi/_2 - latitude pt) * b / cosC)
         | otherwise  = ((pi/_2 - latitude pt) * b / cosC, (negate pi/_2 - latitude pt) * b / cosC)
      q0 = q lat0
      q phi = log (tan (pi/_4+phi/_2)) + e * log ((_1-eSinPhi)/(_1+eSinPhi)) / _2
         where                                -- Factor out expression from Eq 16 of Kaplan
            eSinPhi = e * sin phi
      sinC = sin course
      cosC = cos course
      tanC = tan course
      lat0 = latitude pt
      lon0 = longitude pt
      e2 = eccentricity2 $ ellipsoid pt
      e = sqrt e2
      m0 = meridianRadius (ellipsoid pt) lat0
      a = majorRadius $ ellipsoid pt
      b = minorRadius $ ellipsoid pt
   

-- | A path following the line of latitude around the Earth eastwards.
--
-- This is equivalent to @rhumbPath pt (pi/2)@
latitudePath :: (Ellipsoid e) =>
   Geodetic e           -- ^ Start point.
   -> Path e
latitudePath pt = Path line alwaysValid
   where
      line distance = (pt2, pi/_2, _0) 
         where
            pt2 = Geodetic 
               (latitude pt) (longitude pt + distance / r)
               _0 (ellipsoid pt)
      r = latitudeRadius (ellipsoid pt) (latitude pt)


-- | A path from the specified point to the North Pole. Use negative distances
-- for the southward path.
--
-- This is equivalent to @rhumbPath pt _0@
longitudePath :: (Ellipsoid e) =>
   Geodetic e    -- ^ Start point.
   -> Path e
longitudePath pt = rhumbPath pt _0