# HG changeset patch
# User Robert McIntyre <rlm@mit.edu>
# Date 1395259186 14400
# Node ID 634795361af8f018602043d93bd21cf8321f083e
# Parent  54ef2e06c3ef41eb72123734f7b3e945e59c76af
working on wiggling? stream predicate.

diff -r 54ef2e06c3ef -r 634795361af8 org/worm_learn.clj
--- a/org/worm_learn.clj	Wed Mar 19 14:24:13 2014 -0400
+++ b/org/worm_learn.clj	Wed Mar 19 15:59:46 2014 -0400
@@ -81,17 +81,17 @@
 ;; motions that causes the worm to form a circle.
 
 (def curl-script
-  [[370 :d-up 40]
-   [600 :d-up 0]])
+  [[370 :d-flex 40]
+   [600 :d-flex 0]])
 
 (def period 18)
 
 (def worm-muscle-labels
-  [:base-down :base-up
-   :a-down :a-up
-   :b-down :b-up
-   :c-down :c-up
-   :d-down :d-up])
+  [:base-ex :base-flex
+   :a-ex :a-flex
+   :b-ex :b-flex
+   :c-ex :c-flex
+   :d-ex :d-flex])
 
 (defn gen-wiggle [[flexor extensor :as muscle-pair] time-base]
   (let [period period
@@ -102,7 +102,7 @@
      [(+ time-base (+ (* 2 period) 2))  extensor 0]]))
   
 (def wiggle-script
-  (mapcat gen-wiggle (repeat 4000 [:a-down :a-up])
+  (mapcat gen-wiggle (repeat 4000 [:a-ex :a-flex])
                      (range 100 1000000 (+ 3 (* period 2)))))
 
 
@@ -171,18 +171,49 @@
       (Math/abs)))
 
 (defn fft [nums]
-  (.transform
-   (FastFourierTransformer. DftNormalization/UNITARY)
-   (double-array nums) TransformType/FORWARD))
+  (map
+   #(.getReal %)
+   (.transform
+    (FastFourierTransformer. DftNormalization/STANDARD)
+    (double-array nums) TransformType/FORWARD)))
 
 (def indexed (partial map-indexed vector))
 
+(defn max-indexed [s]
+  (first (sort-by (comp - second) (indexed s))))
+
+
 (defn wiggling?
   "Is the worm wiggling?"
   [experiences]
-  (map (comp first :muscle) (vector:last-n experiences 200))
-  
-)
+  (let [analysis-interval 0x40]
+    (when (> (count experiences) analysis-interval)
+      (let [a-flex 3
+            a-ex   2
+            muscle-activity
+            (map :muscle (vector:last-n experiences analysis-interval))
+            base-activity
+            (map #(- (% a-flex) (% a-ex)) muscle-activity)]
+        (= 2
+           (first
+            (max-indexed (map #(Math/abs %) (take 20 (fft base-activity))))))))))
+
+        ;; (println-repl
+        ;;  (apply format "%d  %.2f"
+        ;;         (first (sort-by
+        ;;                 (comp -  second)
+        ;;                 (indexed (take 20 ))))))))))
+
+    ;;     (println-repl
+    ;;      (apply
+    ;;       format
+    ;;       (apply str (repeat analysis-interval "%5.1f"))
+    ;;       (fft base-activity)))
+
+    ;; ;;(println-repl (last base-activity)) 
+    ;;   )))
+
+
 
 (def standard-world-view
   [(Vector3f. 4.207176, -3.7366982, 3.0816958)
@@ -249,7 +280,7 @@
        (fn [world tpf]
          (if (and end-frame (> (.getTime timer) end-frame))
            (.stop world))
-         (let [muscle-data (motor-control muscles)
+         (let [muscle-data (vec (motor-control muscles))
                proprioception-data (prop)
                touch-data (map #(% (.getRootNode world)) touch)]
            (when experiences
@@ -262,7 +293,7 @@
              ;; (println-repl
              ;;  (apply format "%.2f %.2f %.2f %.2f %.2f\n"
              ;;         (map contact touch-data)))
-             
+             (wiggling? @experiences)
              )
            (muscle-display
             muscle-data