;;;; 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 fixnum)
  (chicken flonum)
  (chicken type)
  ;fp-utils
  geopoint)

;;; FP Utils

;;

(: fpsqr (float --> float))
;
(define-inline (fpsqr n)
  (fp* n n) )

;;

(define-constant DEGREE 0.0174532925199432957692369076848861271344) ;pi/180

(: fpdegree->radian (float --> float))
;
(define-inline (fpdegree->radian deg)
  (fp* deg DEGREE) )

(: fpradian->degree (float --> float))
;
(define-inline (fpradian->degree rad)
  (fp/ rad DEGREE) )

;;

(: fpprecision-factor ((or float fixnum) #!optional float --> float))
;
(define-inline (fpprecision-factor p #!optional (base 10.0))
  (fpexpt base (exact->inexact p)) )

;;;

(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