satnav.core.clj

(ns satnav.core
  (:require [clojure.string :as string]))

; Helpers
(def radius-earth 6371.0)
(defn degrees [rad] (* rad (/ 180.0 Math/PI)))
(defn radians [deg] (* deg (/ Math/PI 180.0)))
(defn normalize-lat [lat] (mod (+ (* 0.5 Math/PI) lat) (* 2 Math/PI)))
(defn normalize-lon [lon] (mod (+ Math/PI lon) (* 2 Math/PI)))
(defn hav [a] (/ (- 1 (Math/cos a)) 2))
(defn haversine [lat1 lon1 lat2 lon2]
  (+ (hav (- lat2 lat1)) (* (Math/cos lat1) (Math/cos lat2) (hav (- lon2 lon1)))))
(defn to-xyz [lat lon radius]
  (let [lat' (normalize-lat lat)
        lon' (normalize-lon lon)]
    [(* radius (Math/sin lat') (Math/cos lon')) (* radius (Math/sin lat') (Math/sin lon')) (* radius (Math/cos lat'))]))

(defn print-graph [graph]
  (let [edges (for [[k v] graph e (doall v)] (str k " -- " e))]
    (str "graph {" (clojure.string/join "\n" edges) "}")))

; Data parsing

(defn build-satellite [[id lat lon height]]
  {:id id
  :lat (radians (Float/parseFloat lat))
  :lon (radians (Float/parseFloat lon))
  :height (+ radius-earth (Float/parseFloat height))})

(defn build-route [[route from-lat from-lon to-lat to-lon]]
  {:from [(radians (Float/parseFloat from-lat)) (radians (Float/parseFloat from-lon))]
  :to [(radians (Float/parseFloat to-lat)) (radians (Float/parseFloat to-lon))]})

(defn get-data [resource]
  (let [[route & satellites] (->> (string/split (slurp resource) #"\n")
                                  (drop 1)
                                  (map (fn [row]
                                         (let [data (string/split row #",")]
                                           (condp re-matches (first data)
                                             #"SAT\d+" (build-satellite data)
                                             #"ROUTE" (build-route data)))))
                                  (reverse))]
    [(into [] (reverse satellites)) route]))

; Calculations
(defn closest [satellites lat lon]
  (apply min-key #(haversine (:lat %1) (:lon %1) lat lon) satellites))

(defn discriminant [[x1 y1 z1] [x2 y2 z2]]
  (let [dx (- x2 x1)
        dy (- y2 y1)
        dz (- z2 z1)
        a (+ (* dx dx) (* dy dy) (* dz dz))
        b (* 2 (+ (* dx x1) (* dy y1) (* dz z1)))
        c (+ (* x1 x1) (* y1 y1) (* z1 z1) (- (* radius-earth radius-earth)))]
    (- (* b b) (* 4 a c))))

(defn visible? [{id1 :id height1 :height lat1 :lat lon1 :lon} {id2 :id height2 :height lat2 :lat lon2 :lon}]
  (if-not (= id1 id2)
      (< (discriminant (to-xyz lat1 lon1 height1) (to-xyz lat2 lon2 height2)) 0)
      false))

(defn visible-satellites [satellite satellites] (filter (fn [candidate] (visible? satellite candidate)) satellites))

(defn build-graph [satellites]
  (->> (map #(vector (:id %) (map :id (visible-satellites % satellites))) satellites)
       (into {})))

; Simple depth-first search, the problem space does not deserve anything fancier :)
(defn iter-route [graph node last-sat edges visited]
  (let [last? (when (= last-sat node) node)
        first? (when (not (contains? visited (first edges)))
                (iter-route graph (first edges) last-sat (get graph (first edges)) (conj visited node)))
        rest? (when (not (empty? edges))
                (iter-route graph node last-sat (rest edges) (conj visited node)))]
    (if last? [last?]
        (if first? (apply conj [node] first?)
           (if rest? rest?
              nil)))))

(defn find-route [satellites {[from-lat from-lon] :from [to-lat to-lon] :to}]
  (let [first-sat (:id (closest satellites from-lat from-lon))
        last-sat (:id (closest satellites to-lat to-lon))
        graph (build-graph satellites)]
    (iter-route graph first-sat last-sat (get graph first-sat) #{})))

(defn -main [& args]
  (let [[satellites endpoints] (get-data "resources/test.txt")
        graph (build-graph satellites)]
  (spit "/tmp/satellites.dot" (print-graph graph))
  (println (clojure.string/join "," (find-route satellites endpoints)))))