 (ns calc-angle.core
- (:require [clojure.string :as string]
-           [clojure.core.reducers :as r])
- (:gen-class))
+  (:require [clojure.string :as string]
+            [clojure.core.reducers :as r])
+  (:gen-class))
 
 (declare radian-to-degree)
 

          (when (= line "// Insert Arrays Here")
            (.write fout 
                    (with-out-str 
                      (println (str "var betas = [" (string/join "," (map #(str (first %)) alpha-beta-degree-list)) "];"))
                      (println (str "var alphas = [" (string/join "," (map #(str (second %)) alpha-beta-degree-list)) "];"))
                      (println (str "var delta_thetas = [" (string/join "," (map #(str (get-base-angle (second %))) r-theta-list)) "];"))))
                      ;(println (str "const int betas[] PROGMEM = {" (string/join "," (map #(str (first %)) alpha-beta-degree-list)) "};"))
                      ;(println (str "const int alphas[] PROGMEM = {" (string/join "," (map #(str (second %)) alpha-beta-degree-list)) "};"))
                      ;(println (str "const int delta_thetas[] PROGMEM = {" (string/join "," (map #(str (get-base-angle (second %))) r-theta-list)) "};"))))
            (.write fout "\n")))))))

;(defn -main1

  (int (+ ang 0.5)))

(defn get-first-angle [ang] 
  (map-angle first-angle-map ang))

(defn get-second-angle [ang] 
  (map-angle second-angle-map ang))

(defn get-base-angle [ang]
  (map-angle base-angle-map ang))

(defn normalize-angle [ang]
  (loop [a ang]
    (if (and (not (neg? a)) (< a (* 2 PI)))
      a
      (recur (+ a (if (neg? a) (* 2 PI) (* -2 PI)))))))

(defn calc-cos-beta [r z]
  (let [z2 (* z z)
        r2 (* r r)
        a (/ 
            (+ (- (* first-arm first-arm) (* second-arm second-arm)) z2 r2) 
            (* 2 first-arm))
        sqrt (Math/sqrt 
               (- 
                 (+ z2 r2) 
                 (* a a)))]
    (/ (- (* a r) (* z sqrt)) (+ z2 r2))))

(defn radian-to-degree [ang]
  (/ (* 180. ang) PI))

(defn degree-to-radian [ang]
  (/ (* PI ang) 180.))

(defn calc-alpha-beta [r z]
  (let [cos-beta (calc-cos-beta r z)
        sin-beta (Math/sqrt (- 1 (* cos-beta cos-beta)))
        cos-gamma (/ (- (* first-arm sin-beta) z) second-arm)]
    (list (- (/ PI 2.) (Math/acos cos-gamma)) (Math/acos cos-beta))))

(defn display-alpha-beta [alpha beta]
  (println (str "first-arm*cos(beta)=" (* first-arm (Math/cos beta))))
  (println (str "second-arm*cos(alpha)=" (* second-arm (Math/cos alpha))))
  (println (str "first-arm*sin(beta)=" (* first-arm (Math/sin beta))))
  (println (str "second-arm*sin(alpha)=" (* second-arm (Math/sin alpha)))))

(defn calc-r-theta [x y]
  (let [r (Math/sqrt (+ (* x x) (* y y)))]
    (list r (radian-to-degree (Math/asin (/ (- x) r))))))

;;; Code used to calculate the map between servo angle and arm angle
;(def exper-data {40 {60 [13.7 11.1]   65 [13.95 9.95]  70 [13.84 8.35]  75 [13.75 6.7]   80 [13.65 5.25]  85 [13.55 4.1]} 
