;;;; geo-utils.scm
;;;; Kon Lovett, May '17
;;;; Kon Lovett, Sep '17

(module geo-utils

(;export
  ;
  pythagorean-distance pythagorean-distance*
  spherical-surface-distance
  great-circle-distance great-circle-distance-radians
  straight-line-distance
  approximate-ellipsoid-distance
  ;
  great-circle-azimuth
  ;
  great-circle-position)

(import scheme chicken)
(use
  numbers
  fp-utils
  geopoint)

(include "geo-constants")

;;

(: pythagorean-distance (number number number number --> number))
(define (pythagorean-distance lat1 lon1 lat2 lon2)
  (sqrt (pythagorean-distance* lat1 lon1 lat2 lon2)) )

(: pythagorean-distance* (number number number number --> number))
(define (pythagorean-distance* lat1 lon1 lat2 lon2)
  (let (
    (a (- lat1 lat2) )
    (b (- lon1 lon2) ) )
    ;
    (+ (* a a) (* b b)) ) )

;;

(: spherical-surface-distance (float float float float #!optional float --> float))
(define (spherical-surface-distance lat1 lon1 lat2 lon2 #!optional (R EARTH-RADIUS-KILOMETERS))
  ;haversine formula : https://en.wikipedia.org/wiki/Haversine_formula
  (let (
    (dlat (fpdegree->radian (fpabs (fp- lat2 lat1))) )
    (dlon (fpdegree->radian (fpabs (fp- lon2 lon1))) )
    (lat1 (fpdegree->radian lat1) )
    (lat2 (fpdegree->radian lat2) ) )
    ;
    (let* (
      (a
        (fp+
          (fpsqr (fpsin (fp/ dlon 2.0)))
          (fp*
            (fp* (fpcos lat1) (fpcos lat2))
            (fpsqr (fpsin (fp/ dlat 2.0))))) )
      (c (fp* 2.0 (fpasin (fpmin 1.0 (fpsqrt a)))) ) )
      ;
      (fp* c R) ) ) )

;;

(: great-circle-distance (float float float float #!optional float --> float))
(define (great-circle-distance lat1 lon1 lat2 lon2 #!optional (R EARTH-RADIUS-KILOMETERS))
  (let (
    (lat1 (fpdegree->radian lat1) )
    (lon1 (fpdegree->radian lon1) )
    (lat2 (fpdegree->radian lat2) )
    (lon2 (fpdegree->radian lon2) ) )
    ;
    (great-circle-distance-radians lat1 lon1 lat2 lon2 R) ) )

; https://en.wikipedia.org/wiki/Great-circle_distance
(: great-circle-distance-radians (float float float float #!optional float --> float))
(define (great-circle-distance-radians lat1 lon1 lat2 lon2 #!optional (R EARTH-RADIUS-KILOMETERS))
  (let (
    (d
      (fp+
        (fp* (fpsin lat1) (fpsin lat2))
        (fp*
          (fp* (fpcos lat1) (fpcos lat2))
          (fpcos (fp- lon1 lon2)))) ) )
    ;
    (fp* (fpacos d) R) ) )

#| ;???
  (atan2
    (fp/
      (sqrt ((sqr (cos lat2)) + (sqr ((cos lat1) • ((sin lon2) - (sin lon1)) • (cos (fpabs (lon2 - lon1)))))
|#

;;

(: straight-line-distance (float float float float float float #!optional float --> float))
(define (straight-line-distance lat1 lon1 h1 lat2 lon2 h2 #!optional (R EARTH-RADIUS-KILOMETERS))
  ;filler
  (pythagorean-distance lat1 lon1 lat2 lon2) )

;;

(: approximate-ellipsoid-distance (float float float float #!optional float float --> float))
(define (approximate-ellipsoid-distance lat1 lon1 lat2 lon2
          #!optional
          (R EARTH-RADIUS-KILOMETERS)
          (F EARTH-FLATTENING))
  (let (
    (lat1 (fpdegree->radian lat1) )
    (lon1 (fpdegree->radian lon1) )
    (lat2 (fpdegree->radian lat2) )
    (lon2 (fpdegree->radian lon2) ) )
    ;
    (let (
      (f (fp/ (fp+ lat1 lat2) 2.0) )
      (g (fp/ (fp- lat1 lat2) 2.0) )
      (l (fp/ (fp- lon1 lon2) 2.0) ) )
      ;
      (let (
        (sing (fpsin g) )
        (cosl (fpcos l) )
        (cosf (fpcos f) )
        (sinl (fpsin l) )
        (sinf (fpsin f) )
        (cosg (fpcos g) ) )
        ;
        (let (
          (sing2 (fpsqr sing) )
          (cosf2 (fpsqr cosf) )
          (cosg2 (fpsqr cosg) )
          (sinf2 (fpsqr sinf) ) )
          ;
          (let* (
            (s
              (fp+
                (fp* sing2 (fpsqr cosl))
                (fp* cosf2 (fpsqr sinl))) )
            (c
              (fp+
                (fp* cosg2 (fpsqr cosl))
                (fp* sinf2 (fpsqr sinl))) )
            (w (fpatan2 (fpsqrt s) (fpsqrt c)) )
            (r (fpsqrt (fp/ (fp* s c) w)) )
            (h1 (fp/ (fp* 3.0 (fp- r 1.0)) (fp* 2.0 c)) )
            (h2 (fp/ (fp* 3.0 (fp+ r 1.0)) (fp* 2.0 s)) )
            (d (fp* 2.0 (fp* w R)) ) )
            ;
            (fp*
              d
              (fp+
                1.0
                (fp-
                  (fp*
                    (fp* h1 F)
                    (fp* sinf2 cosg2))
                  (fp*
                    (fp* h2 F)
                    (fp* cosf2 sing2))))) ) ) ) ) ) )

;;

(: great-circle-azimuth (float float float float #!optional (or fixnum float) --> float))
(define (great-circle-azimuth lat1 lon1 lat2 lon2 #!optional (prec 5))
  ;
  (define precfact
    (fp* 360.0 (fpprecision-factor prec)) )
  ;
  (define (clamp n)
    (inexact->exact (fpround (fp* n precfact))) )
  ;
  (let (
    (ilat1 (clamp lat1) )
    (ilon1 (clamp lon1) )
    (ilat2 (clamp lat2) )
    (ilon2 (clamp lon2) ) )
    ;
    (cond
      ((fx= ilon1 ilon2)
        ; going up?
        (if (fx> ilat1 ilat2)
          180.0
          ; going down or nowhere
          0.0 ) )
      (else
        (let (
          (lat1 (fpdegree->radian lat1) )
          (lon1 (fpdegree->radian lon1) )
          (lat2 (fpdegree->radian lat2) )
          (lon2 (fpdegree->radian lon2) ) )
          ;
          (let* (
            (c (great-circle-distance-radians lat1 lon1 lat2 lon2) )
            (a
              (fpasin
                (fp/
                  (fp* (fpcos lat2) (fpsin (fp- lon2 lon1)))
                  (fpsin c))) )
            (r (fpradian->degree a) ) )
            ;
            (cond
              ((fx> ilat2 ilat1)
                (cond
                  #; ;see else
                  ((fx> ilon2 ilon1)
                    r )
                  ((fx< ilon2 ilon1)
                    (fp+ r 360.0) )
                  (else
                    r ) ) )
              ((fx< ilat2 ilat1)
                (cond
                  ((fx< ilon2 ilon1)
                    (fp- 180.0 r) )
                  ((fx> ilon2 ilon1)
                    (fp- 180.0 r) )
                  (else
                    r ) ) )
              (else
                r ) ) ) ) ) ) ) )

;;

(: great-circle-position (float float float float #!optional float --> float float))
(define (great-circle-position lat lon dis azi #!optional (R EARTH-RADIUS-KILOMETERS))
  (let (
    (dlat (fpdegree->radian (fp- 90.0 lat)) )
    (lat (fpdegree->radian lat) )
    (lon (fpdegree->radian lon) )
    (azi (fpdegree->radian azi)) )
    ;
    (let* (
      (b (fp/ dis R) )
      (sinb (fpsin b) )
      (a
        (fpacos
          (fp+
            (fp* (fpcos b) (fpcos dlat))
            (fp* sinb (fp* (fpsin dlat) (fpcos azi))))) )
      (b (fpasin (fp/ (fp* sinb (fpsin azi)) (fpsin a))) ) )
      ;
      (values (fp- 90.0 (fpradian->degree a)) (fpradian->degree (fp+ b lon))) ) ) )

) ;geo-utils