;;;; 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
  (chicken base)
  (chicken type)
  (chicken flonum)
  geopoint)

;;

(define-type real (or integer ratnum float))

(: pythagorean-distance (number number number number --> number))
(: pythagorean-distance* (number number number number --> number))

(: spherical-surface-distance (real real real real #!optional real --> real))
(: great-circle-distance (real real real real #!optional real --> real))
(: great-circle-distance-radians (real real real real #!optional real --> real))
(: straight-line-distance (real real real real real real #!optional real --> real))
(: approximate-ellipsoid-distance (real real real real #!optional real real --> real))
(: great-circle-azimuth (real real real real #!optional real --> real))
(: great-circle-position (real real real real #!optional real --> real real))

;;(fp-tils)

(: sqr (number --> number))
(: precision-factor (fixnum #!optional fixnum --> integer))

(: degree->radian (real --> float))
(: radian->degree (real --> float))

(define (sqr n) (* n n))
(define (precision-factor p #!optional (base 10)) (expt base p))

(define-constant DEG/RAD 0.0174532925199432957692369076848861271344) ;pi/180

(define (degree->radian deg) (* deg DEG/RAD))
(define (radian->degree rad) (/ rad DEG/RAD))


;;

(include "geo-constants")

;;

(define (pythagorean-distance* lat1 lon1 lat2 lon2)
  (+ (sqr (- lat1 lat2)) (sqr (- lon1 lon2))) )

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

;;

(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))) (* (cos lat1) (cos lat2) (sqr (sin (/ dlat 2))))))
      (c  (* 2 (asin (min 1 (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))
      (g (/ (- lat1 lat2) 2))
      (l (/ (- lon1 lon2) 2)) )
      (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 (- r 1)) (* 2 c)))
            (h2 (/ (* 3 (+ r 1)) (* 2 s)))
            (d  (* 2 (* w R))) )
            (* d (+ 1 (- (* h1 F sinf2 cosg2) (* h2 F cosf2 sing2)))) ) ) ) ) ) )

;;

(define (great-circle-azimuth lat1 lon1 lat2 lon2 #!optional (prec 5))
  ;
  (define precfact (* 360 (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
          ; going down or nowhere
          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) )
                  (else
                    r ) ) )
              ((< ilat2 ilat1)
                (cond
                  ((< ilon2 ilon1)
                    (- 180 r) )
                  ((> ilon2 ilon1)
                    (- 180 r) )
                  (else
                    r ) ) )
              (else
                r ) ) ) ) ) ) ) )

;;

(define (great-circle-position lat lon dis azi #!optional (R EARTH-RADIUS-KILOMETERS))
  (let (
    (dlat (degree->radian (- 90 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 (radian->degree a)) (radian->degree (+ b lon))) ) ) )

) ;geo-utils