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

(module geo-utils

(;export
  ;
  earth-flattening
  earth-radius-miles
  earth-radius-kilometers
  ;
  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)

(use numbers)

(use fp-utils)

(use geopoint)

(include "geo-constants")

;;

(define earth-flattening EARTH-FLATTENING)

(define earth-radius-miles EARTH-RADIUS-MILES)
(define earth-radius-kilometers EARTH-RADIUS-KILOMETERS)

;;

(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-MILES))
  (let ((dlat (fpdegree->radian (fp- lat2 lat1)))
        (dlon (fpdegree->radian (fp- lon2 lon1)))
        (lat1 (fpdegree->radian lat1))
        (lat2 (fpdegree->radian lat2)) )
    (let* ((sdlon (fpsin (fp/ dlon 2.0)))
           (sdlat (fpsin (fp/ dlat 2.0)))
           (a (fp+ (fpsqr sdlat)
                   (fp* (fpsqr sdlon) (fp* (fpcos lat1) (fpcos lat2)))))
           (c (fp* 2.0 (fpasin (fpmin 1.0 (fpsqrt a))))) )
      (fp* c R) ) ) )

;;

(define (great-circle-distance lat1 lon1 lat2 lon2 #!optional (R EARTH-RADIUS-MILES))
  (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) ) )

(define (great-circle-distance-radians lat1 lon1 lat2 lon2 #!optional (R EARTH-RADIUS-MILES))
  (let ((d (fp+ (fp* (fpsin lat1) (fpsin lat2))
                (fp* (fpcos lat1) (fp* (fpcos lat2) (fpcos (fp- lon1 lon2)))))))
    (fp* (fpacos d) R) ) )

;;

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

;;

(define (approximate-ellipsoid-distance lat1 lon1 lat2 lon2 #!optional (R EARTH-RADIUS-MILES))
  (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 EARTH-FLATTENING)
                                (fp* sinf2 cosg2))
                           (fp* (fp* h2 EARTH-FLATTENING)
                                (fp* cosf2 sing2))))) ) ) ) ) ) )

;;

(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 ) ) ) ) ) ) ) )

;;

(define (great-circle-position lat lon dis azi #!optional (R EARTH-RADIUS-MILES))
  (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