(ns cortex.proprioception
  "Simulate the sense of proprioception (ability to detect the
  relative positions of body parts with respect to other body parts)
  in jMonkeyEngine3. Reads specially prepared blender files to
  automatically generate proprioceptive senses."
  (:use (cortex world util sense body))
  (:import com.jme3.scene.Node)
  (:import java.awt.image.BufferedImage)
  (:import (com.jme3.math Vector3f Quaternion)))
(in-ns 'cortex.proprioception)

(defn right-handed?
  "true iff the three vectors form a right handed coordinate
   system. The three vectors do not have to be normalized or
   orthogonal."
  [vec1 vec2 vec3]
  (pos? (.dot (.cross vec1 vec2) vec3)))

(defn absolute-angle
  "The angle between 'vec1 and 'vec2 around 'axis. In the range 
   [0 (* 2 Math/PI)]."
  [vec1 vec2 axis]
  (let [angle (.angleBetween vec1 vec2)]
    (if (right-handed? vec1 vec2 axis)
      angle (- (* 2 Math/PI) angle))))
(defn proprioception-kernel
  "Returns a function which returns proprioceptive sensory data when
  called inside a running simulation."
  [#^Node parts #^Node joint]
  (let [[obj-a obj-b] (joint-targets parts joint)
        joint-rot (.getWorldRotation joint)
        x0 (.mult joint-rot Vector3f/UNIT_X)
        y0 (.mult joint-rot Vector3f/UNIT_Y)
        z0 (.mult joint-rot Vector3f/UNIT_Z)]
    (fn []
      (let [rot-a (.clone (.getWorldRotation obj-a))
            rot-b (.clone (.getWorldRotation obj-b))
            x (.mult rot-a x0)
            y (.mult rot-a y0)
            z (.mult rot-a z0)

            X (.mult rot-b x0)
            Y (.mult rot-b y0)
            Z (.mult rot-b z0)
            heading  (Math/atan2 (.dot X z) (.dot X x))
            pitch  (Math/atan2 (.dot X y) (.dot X x))

            ;; rotate x-vector back to origin
            reverse
            (doto (Quaternion.)
              (.fromAngleAxis
               (.angleBetween X x)
               (let [cross (.normalize (.cross X x))]
                 (if (= 0 (.length cross)) y cross))))
            roll (absolute-angle (.mult reverse Y) y x)]
        [heading pitch roll]))))

(defn proprioception!
  "Endow the creature with the sense of proprioception. Returns a
   sequence of functions, one for each child of the \"joints\" node in
   the creature, which each report proprioceptive information about
   that joint."
  [#^Node creature]
  ;; extract the body's joints
  (let [senses (map (partial proprioception-kernel creature)
                    (joints creature))]
    (fn []
      (map #(%) senses))))
(in-ns 'cortex.proprioception)

(defn draw-sprite [image sprite x y color ]
  (dorun
   (for [[u v] sprite]
     (.setRGB image (+ u x) (+ v y) color))))

(defn view-angle
  "create a debug view of an angle"
  [color]
  (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
        previous (atom [25 25])
        sprite [[0 0] [0 1]
                [0 -1] [-1 0] [1 0]]] 
    (fn [angle]
      (let [angle (float angle)]
        (let [position
              [(+ 25 (int (* 20 (Math/cos angle))))
               (+ 25 (int (* -20 (Math/sin angle))))]]
          (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
          (draw-sprite image sprite (position 0) (position 1) color)
          (reset! previous position))
        image))))

(defn proprioception-display-kernel
  "Display proprioception angles in a BufferedImage"
  [[h p r]]
  (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
        previous-heading (atom [25 25])
        previous-pitch   (atom [25 25])
        previous-roll    (atom [25 25])
        
        heading-sprite [[0 0] [0 1] [0 -1] [-1 0] [1 0]]
        pitch-sprite   [[0 0] [0 1] [0 -1] [-1 0] [1 0]]
        roll-sprite    [[0 0] [0 1] [0 -1] [-1 0] [1 0]]
        draw-angle
        (fn [angle sprite previous color]
          (let [angle (float angle)]
            (let [position
                  [(+ 25 (int (* 20 (Math/cos angle))))
                   (+ 25 (int (* -20 (Math/sin angle))))]]
              (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
              (draw-sprite image sprite (position 0) (position 1) color)
              (reset! previous position))
            image))]
    (dorun (map draw-angle
                [h p r]
                [heading-sprite pitch-sprite roll-sprite]
                [previous-heading previous-pitch previous-roll]
                [0xFF0000 0x00FF00 0xFFFFFF]))
    image))
                
(defn view-proprioception
  "Creates a function which accepts a list of proprioceptive data and
   display each element of the list to the screen as an image."
  []
  (view-sense proprioception-display-kernel))