;;;; geo-utils.scm  -*- Scheme -*-
;;;; Kon Lovett, Jul '18
;;;; Kon Lovett, Sep '17
;;;; Kon Lovett, May '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)
(import (chicken base))
(import (chicken type))
(import (chicken flonum))
;g-utils
(import geopoint)

;;

(: pythagorean-distance (number number number number --> number))
(: pythagorean-distance* (number number number number --> number))
(: spherical-surface-distance (float float float float #!optional float --> float))
(: great-circle-distance (float float float float #!optional float --> float))
(: great-circle-distance-radians (float float float float #!optional float --> float))
(: straight-line-distance (float float float float float float #!optional float --> float))
(: approximate-ellipsoid-distance (float float float float #!optional float float --> float))
(: great-circle-azimuth (float float float float #!optional (or fixnum float) --> float))
(: great-circle-position (float float float float #!optional float --> float float))

;;(fp-tils)

(: sqr (float --> float))
(: degree->radian (float --> float))
(: radian->degree (float --> float))
(: precision-factor ((or float fixnum) #!optional float --> float))

(define (sqr n) (* n n))

(define-constant DEGREE 0.0174532925199432957692369076848861271344) ;pi/180

(define (degree->radian deg) (* deg DEGREE))
(define (radian->degree rad) (/ rad DEGREE))

(define (precision-factor p #!optional (base 10.0)) (expt base (exact->inexact p)))

;;

(include "geo-constants")

;;

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

(define (pythagorean-distance* lat1 lon1 lat2 lon2)
  (let (
    (a (- lat1 lat2))
    (b (- lon1 lon2)) )
    (+ (* a a) (* b b)) ) )

;;

(define (spherical-surface-distance lat1 lon1 lat2 lon2 #!optional (R EARTH-RADIUS-KILOMETERS))
  ;haversine formula : https://en.wikipedia.org/wiki/Haversine_formula
  (let (
    (dlat (degree->radian (abs (- lat2 lat1))))
    (dlon (degree->radian (abs (- lon2 lon1))))
    (lat1 (degree->radian lat1))
    (lat2 (degree->radian lat2)) )
    (let* (
      (a
        (+
          (sqr (sin (/ dlon 2.0)))
          (*
            (* (cos lat1) (cos lat2))
            (sqr (sin (/ dlat 2.0))))))
      (c
        (* 2.0 (asin (min 1.0 (sqrt a))))) )
      (* c R) ) ) )

;;

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

; https://en.wikipedia.org/wiki/Great-circle_distance
;
(define (great-circle-distance-radians lat1 lon1 lat2 lon2 #!optional (R EARTH-RADIUS-KILOMETERS))
  (let (
    (d
      (+
        (* (sin lat1) (sin lat2))
        (*
          (* (cos lat1) (cos lat2))
          (cos (- lon1 lon2)))) ) )
    (* (acos d) R) ) )

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

;;

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

;;

(define (approximate-ellipsoid-distance lat1 lon1 lat2 lon2
          #!optional
          (R EARTH-RADIUS-KILOMETERS)
          (F EARTH-FLATTENING))
  (let (
    (lat1 (degree->radian lat1) )
    (lon1 (degree->radian lon1))
    (lat2 (degree->radian lat2))
    (lon2 (degree->radian lon2)) )
    (let (
      (f (/ (+ lat1 lat2) 2.0))
      (g (/ (- lat1 lat2) 2.0))
      (l (/ (- lon1 lon2) 2.0)) )
      (let (
        (sing (sin g))
        (cosl (cos l))
        (cosf (cos f))
        (sinl (sin l))
        (sinf (sin f))
        (cosg (cos g)) )
        (let (
          (sing2 (sqr sing))
          (cosf2 (sqr cosf))
          (cosg2 (sqr cosg))
          (sinf2 (sqr sinf)) )
          (let* (
            (s
              (+
                (* sing2 (sqr cosl))
                (* cosf2 (sqr sinl))))
            (c
              (+
                (* cosg2 (sqr cosl))
                (* sinf2 (sqr sinl))))
            (w
              (fpatan2 (sqrt s) (sqrt c)))
            (r
              (sqrt (/ (* s c) w)))
            (h1
              (/ (* 3.0 (- r 1.0)) (* 2.0 c)))
            (h2
              (/ (* 3.0 (+ r 1.0)) (* 2.0 s)))
            (d
              (* 2.0 (* w R))) )
            (*
              d
              (+
                1.0
                (-
                  (*
                    (* h1 F)
                    (* sinf2 cosg2))
                  (*
                    (* h2 F)
                    (* cosf2 sing2))))) ) ) ) ) ) )

;;

(define (great-circle-azimuth lat1 lon1 lat2 lon2 #!optional (prec 5))
  ;
  (define precfact
    (* 360.0 (precision-factor prec)) )
  ;
  (define (clamp n)
    (inexact->exact (round (* n precfact))) )
  ;
  (let (
    (ilat1 (clamp lat1))
    (ilon1 (clamp lon1))
    (ilat2 (clamp lat2))
    (ilon2 (clamp lon2)) )
    (cond
      ((= ilon1 ilon2)
        ; going up?
        (if (> ilat1 ilat2)
          180.0
          ; going down or nowhere
          0.0 ) )
      (else
        (let (
          (lat1 (degree->radian lat1))
          (lon1 (degree->radian lon1))
          (lat2 (degree->radian lat2))
          (lon2 (degree->radian lon2)) )
          (let* (
            (c
              (great-circle-distance-radians lat1 lon1 lat2 lon2))
            (a
              (asin
                (/
                  (* (cos lat2) (sin (- lon2 lon1)))
                  (sin c))))
            (r
              (radian->degree a)) )
            (cond
              ((> ilat2 ilat1)
                (cond
                  #; ;see else
                  ((> ilon2 ilon1)
                    r )
                  ((< ilon2 ilon1)
                    (+ r 360.0) )
                  (else
                    r ) ) )
              ((< ilat2 ilat1)
                (cond
                  ((< ilon2 ilon1)
                    (- 180.0 r) )
                  ((> ilon2 ilon1)
                    (- 180.0 r) )
                  (else
                    r ) ) )
              (else
                r ) ) ) ) ) ) ) )

;;

(define (great-circle-position lat lon dis azi #!optional (R EARTH-RADIUS-KILOMETERS))
  (let (
    (dlat (degree->radian (- 90.0 lat)))
    (lat (degree->radian lat))
    (lon (degree->radian lon))
    (azi (degree->radian azi)) )
    (let* (
      (b
        (/ dis R))
      (sinb
        (sin b))
      (a
        (acos
          (+
            (* (cos b) (cos dlat))
            (* sinb (* (sin dlat) (cos azi))))))
      (b
        (asin (/ (* sinb (sin azi)) (sin a)))) )
      ;
      (values (- 90.0 (radian->degree a)) (radian->degree (+ b lon))) ) ) )

) ;geo-utils