(** Coordinate pair module *)

type t = {
  lat : Lat.t;
  lon : Lon.t;
}

let create ~lat ~lon = {
  lat = Lat.create lat;
  lon = Lon.create lon;
}

let of_lat_lon lat lon = { lat; lon }

let lat t = t.lat
let lon t = t.lon
let lat_float t = Lat.to_float t.lat
let lon_float t = Lon.to_float t.lon

(** Constants *)
let earth_radius_km = 6371.0
let deg_to_rad = Float.pi /. 180.0
let rad_to_deg = 180.0 /. Float.pi

(** Distance calculations *)

let distance_haversine p1 p2 =
  let lat1 = lat_float p1 *. deg_to_rad in
  let lat2 = lat_float p2 *. deg_to_rad in
  let lon1 = lon_float p1 *. deg_to_rad in
  let lon2 = lon_float p2 *. deg_to_rad in

  let dlat = lat2 -. lat1 in
  let dlon = lon2 -. lon1 in

  let a =
    (sin (dlat /. 2.0) ** 2.0) +.
    cos lat1 *. cos lat2 *. (sin (dlon /. 2.0) ** 2.0) in

  let c = 2.0 *. atan2 (sqrt a) (sqrt (1.0 -. a)) in
  earth_radius_km *. c

let distance_vincenty p1 p2 =
  (* WGS84 ellipsoid constants *)
  let a = 6378.137 in  (* semi-major axis in km *)
  let b = 6356.752 in  (* semi-minor axis in km *)
  let f = 1.0 /. 298.257223563 in  (* flattening *)

  let lat1 = lat_float p1 *. deg_to_rad in
  let lat2 = lat_float p2 *. deg_to_rad in
  let lon1 = lon_float p1 *. deg_to_rad in
  let lon2 = lon_float p2 *. deg_to_rad in

  let l = lon2 -. lon1 in
  let u1 = atan ((1.0 -. f) *. tan lat1) in
  let u2 = atan ((1.0 -. f) *. tan lat2) in
  let sin_u1 = sin u1 in
  let cos_u1 = cos u1 in
  let sin_u2 = sin u2 in
  let cos_u2 = cos u2 in

  let rec iterate lambda _prev_lambda iter =
    if iter > 100 then
      (* Failed to converge, fall back to haversine *)
      distance_haversine p1 p2
    else
      let sin_lambda = sin lambda in
      let cos_lambda = cos lambda in
      let sin_sigma =
        sqrt ((cos_u2 *. sin_lambda) ** 2.0 +.
              (cos_u1 *. sin_u2 -. sin_u1 *. cos_u2 *. cos_lambda) ** 2.0) in

      if sin_sigma = 0.0 then 0.0  (* coincident points *)
      else
        let cos_sigma = sin_u1 *. sin_u2 +. cos_u1 *. cos_u2 *. cos_lambda in
        let sigma = atan2 sin_sigma cos_sigma in
        let sin_alpha = cos_u1 *. cos_u2 *. sin_lambda /. sin_sigma in
        let cos_sq_alpha = 1.0 -. sin_alpha ** 2.0 in
        let cos_2sigma_m =
          if cos_sq_alpha = 0.0 then 0.0  (* equatorial line *)
          else cos_sigma -. 2.0 *. sin_u1 *. sin_u2 /. cos_sq_alpha in

        let c = f /. 16.0 *. cos_sq_alpha *. (4.0 +. f *. (4.0 -. 3.0 *. cos_sq_alpha)) in
        let lambda' = l +. (1.0 -. c) *. f *. sin_alpha *.
                      (sigma +. c *. sin_sigma *.
                       (cos_2sigma_m +. c *. cos_sigma *.
                        (-1.0 +. 2.0 *. cos_2sigma_m ** 2.0))) in

        if abs_float (lambda' -. lambda) < 1e-12 then
          (* Converged *)
          let u_sq = cos_sq_alpha *. (a ** 2.0 -. b ** 2.0) /. (b ** 2.0) in
          let cap_a = 1.0 +. u_sq /. 16384.0 *.
                      (4096.0 +. u_sq *. (-768.0 +. u_sq *. (320.0 -. 175.0 *. u_sq))) in
          let cap_b = u_sq /. 1024.0 *. (256.0 +. u_sq *. (-128.0 +. u_sq *. (74.0 -. 47.0 *. u_sq))) in
          let delta_sigma = cap_b *. sin_sigma *.
                            (cos_2sigma_m +. cap_b /. 4.0 *.
                             (cos_sigma *. (-1.0 +. 2.0 *. cos_2sigma_m ** 2.0) -.
                              cap_b /. 6.0 *. cos_2sigma_m *.
                              (-3.0 +. 4.0 *. sin_sigma ** 2.0) *.
                              (-3.0 +. 4.0 *. cos_2sigma_m ** 2.0))) in

          b *. cap_a *. (sigma -. delta_sigma)
        else
          iterate lambda' lambda (iter + 1)
  in

  iterate l 0.0 0

(** Bearing calculations *)

let bearing p1 p2 =
  let lat1 = lat_float p1 *. deg_to_rad in
  let lat2 = lat_float p2 *. deg_to_rad in
  let lon1 = lon_float p1 *. deg_to_rad in
  let lon2 = lon_float p2 *. deg_to_rad in

  let dlon = lon2 -. lon1 in
  let y = sin dlon *. cos lat2 in
  let x = cos lat1 *. sin lat2 -. sin lat1 *. cos lat2 *. cos dlon in

  let bearing = atan2 y x *. rad_to_deg in
  mod_float (bearing +. 360.0) 360.0

let bearing_final p1 p2 =
  (* Final bearing is the reverse of the initial bearing from p2 to p1 *)
  let reverse_bearing = bearing p2 p1 in
  mod_float (reverse_bearing +. 180.0) 360.0

(** Geographic operations *)

let offset t ~heading ~distance =
  let lat1 = lat_float t *. deg_to_rad in
  let lon1 = lon_float t *. deg_to_rad in
  let brng = heading *. deg_to_rad in
  let d = distance /. earth_radius_km in

  let lat2 = asin (sin lat1 *. cos d +. cos lat1 *. sin d *. cos brng) in
  let lon2 = lon1 +. atan2 (sin brng *. sin d *. cos lat1)
                           (cos d -. sin lat1 *. sin lat2) in

  create ~lat:(lat2 *. rad_to_deg) ~lon:(lon2 *. rad_to_deg)

let midpoint p1 p2 =
  let lat1 = lat_float p1 *. deg_to_rad in
  let lat2 = lat_float p2 *. deg_to_rad in
  let lon1 = lon_float p1 *. deg_to_rad in
  let lon2 = lon_float p2 *. deg_to_rad in

  let dlon = lon2 -. lon1 in
  let bx = cos lat2 *. cos dlon in
  let by = cos lat2 *. sin dlon in

  let lat3 = atan2 (sin lat1 +. sin lat2)
                   (sqrt ((cos lat1 +. bx) ** 2.0 +. by ** 2.0)) in
  let lon3 = lon1 +. atan2 by (cos lat1 +. bx) in

  create ~lat:(lat3 *. rad_to_deg) ~lon:(lon3 *. rad_to_deg)

let interpolate p1 p2 fraction =
  if fraction <= 0.0 then p1
  else if fraction >= 1.0 then p2
  else
    let lat1 = lat_float p1 in
    let lat2 = lat_float p2 in
    let lon1 = lon_float p1 in
    let lon2 = lon_float p2 in

    (* Simple linear interpolation in degrees *)
    let lat = lat1 +. (lat2 -. lat1) *. fraction in
    let lon = lon1 +. (lon2 -. lon1) *. fraction in

    create ~lat ~lon

(** Comparison *)

let equal p1 p2 =
  Lat.equal p1.lat p2.lat && Lon.equal p1.lon p2.lon

let ( = ) = equal

let almost_equal ?(epsilon = 0.000001) p1 p2 =
  distance_haversine p1 p2 < epsilon

(** Formatting *)

let to_string t =
  Printf.sprintf "%s, %s" (Lat.to_string t.lat) (Lon.to_string t.lon)

let to_dms_string t =
  Printf.sprintf "%s, %s" (Lat.to_dms_string t.lat) (Lon.to_dms_string t.lon)

let format ?(lat_precision = 6) ?(lon_precision = 6) t =
  Printf.sprintf "%s, %s"
    (Lat.format ~precision:lat_precision t.lat)
    (Lon.format ~precision:lon_precision t.lon)