(ns cortex.touch
  "Simulate the sense of touch in jMonkeyEngine3. Enables any Geometry
  to be outfitted with touch sensors with density determined by a UV
  image. In this way a Geometry can know what parts of itself are
  touching nearby objects. Reads specially prepared blender files to
  construct this sense automatically."
  {:author "Robert McIntyre"}
  (:use (cortex world util sense))
  (:import (com.jme3.scene Geometry Node Mesh))
  (:import com.jme3.collision.CollisionResults)
  (:import com.jme3.scene.VertexBuffer$Type)
  (:import (com.jme3.math Triangle Vector3f Vector2f Ray Matrix4f)))
(defn tactile-sensor-profile
  "Return the touch-sensor distribution image in BufferedImage format,
   or nil if it does not exist."
  [#^Geometry obj]
  (if-let [image-path (meta-data obj "touch")]
    (load-image image-path)))

(defn tactile-scale
  "Return the length of each feeler. Default scale is 0.01
  jMonkeyEngine units."
  [#^Geometry obj]
  (if-let [scale (meta-data obj "scale")]
    scale 0.1))
(defn vector3f-seq [#^Vector3f v]
  [(.getX v) (.getY v) (.getZ v)])

(defn triangle-seq [#^Triangle tri]
  [(vector3f-seq (.get1 tri))
   (vector3f-seq (.get2 tri))
   (vector3f-seq (.get3 tri))])

(defn ->vector3f 
  ([coords] (Vector3f. (nth coords 0 0)
                       (nth coords 1 0)
                       (nth coords 2 0))))

(defn ->triangle [points]
  (apply #(Triangle. %1 %2 %3) (map ->vector3f points)))
(in-ns 'cortex.touch)
(defn triangle
  "Get the triangle specified by triangle-index from the mesh."
  [#^Geometry geo triangle-index]
  (triangle-seq
   (let [scratch (Triangle.)]
     (.getTriangle (.getMesh geo) triangle-index scratch) scratch)))

(defn triangles
  "Return a sequence of all the Triangles which comprise a given
   Geometry." 
  [#^Geometry geo]
  (map (partial triangle geo) (range (.getTriangleCount (.getMesh geo)))))

(defn triangle-vertex-indices
  "Get the triangle vertex indices of a given triangle from a given
   mesh."
  [#^Mesh mesh triangle-index]
  (let [indices (int-array 3)]
    (.getTriangle mesh triangle-index indices)
    (vec indices)))

(defn vertex-UV-coord
  "Get the UV-coordinates of the vertex named by vertex-index"
  [#^Mesh mesh vertex-index]
  (let [UV-buffer
        (.getData
         (.getBuffer
          mesh
          VertexBuffer$Type/TexCoord))]
    [(.get UV-buffer (* vertex-index 2))
     (.get UV-buffer (+ 1 (* vertex-index 2)))]))

(defn pixel-triangle [#^Geometry geo image index]
  (let [mesh (.getMesh geo)
        width (.getWidth image)
        height (.getHeight image)]
    (vec (map (fn [[u v]] (vector (* width u) (* height v)))
              (map (partial vertex-UV-coord mesh)
                   (triangle-vertex-indices mesh index))))))

(defn pixel-triangles 
  "The pixel-space triangles of the Geometry, in the same order as
   (triangles geo)"
  [#^Geometry geo image]
  (let [height (.getHeight image)
        width (.getWidth image)]
    (map (partial pixel-triangle geo image)
         (range (.getTriangleCount (.getMesh geo))))))
(in-ns 'cortex.touch)

(defn triangle->matrix4f
  "Converts the triangle into a 4x4 matrix: The first three columns
   contain the vertices of the triangle; the last contains the unit
   normal of the triangle. The bottom row is filled with 1s."
  [#^Triangle t]
  (let [mat (Matrix4f.)
        [vert-1 vert-2 vert-3]
        (mapv #(.get t %) (range 3))
        unit-normal (do (.calculateNormal t)(.getNormal t))
        vertices [vert-1 vert-2 vert-3 unit-normal]]
    (dorun 
     (for [row (range 4) col (range 3)]
       (do
         (.set mat col row (.get (vertices row) col))
         (.set mat 3 row 1)))) mat))

(defn triangles->affine-transform
  "Returns the affine transformation that converts each vertex in the
   first triangle into the corresponding vertex in the second
   triangle."
  [#^Triangle tri-1 #^Triangle tri-2]
  (.mult 
   (triangle->matrix4f tri-2)
   (.invert (triangle->matrix4f tri-1))))
(defn convex-bounds
  "Returns the smallest square containing the given vertices, as a
   vector of integers [left top width height]."
  [verts]
  (let [xs (map first verts)
        ys (map second verts)
        x0 (Math/floor (apply min xs))
        y0 (Math/floor (apply min ys))
        x1 (Math/ceil (apply max xs))
        y1 (Math/ceil (apply max ys))]
    [x0 y0 (- x1 x0) (- y1 y0)]))

(defn same-side?
  "Given the points p1 and p2 and the reference point ref, is point p
  on the same side of the line that goes through p1 and p2 as ref is?" 
  [p1 p2 ref p]
  (<=
   0
   (.dot 
    (.cross (.subtract p2 p1) (.subtract p p1))
    (.cross (.subtract p2 p1) (.subtract ref p1)))))

(defn inside-triangle?
  "Is the point inside the triangle?"
  {:author "Dylan Holmes"}
  [#^Triangle tri #^Vector3f p]
  (let [[vert-1 vert-2 vert-3] [(.get1 tri) (.get2 tri) (.get3 tri)]]
    (and
     (same-side? vert-1 vert-2 vert-3 p)
     (same-side? vert-2 vert-3 vert-1 p)
     (same-side? vert-3 vert-1 vert-2 p))))
(in-ns 'cortex.touch)

(defn feeler-pixel-coords
 "Returns the coordinates of the feelers in pixel space in lists, one
  list for each triangle, ordered in the same way as (triangles) and
  (pixel-triangles)."
 [#^Geometry geo image]
 (map 
  (fn [pixel-triangle]
    (filter
     (fn [coord]
       (inside-triangle? (->triangle pixel-triangle)
                         (->vector3f coord)))
       (white-coordinates image (convex-bounds pixel-triangle))))
  (pixel-triangles geo image)))

(defn feeler-world-coords 
 "Returns the coordinates of the feelers in world space in lists, one
  list for each triangle, ordered in the same way as (triangles) and
  (pixel-triangles)."
 [#^Geometry geo image]
 (let [transforms
       (map #(triangles->affine-transform
              (->triangle %1) (->triangle %2))
            (pixel-triangles geo image)
            (triangles geo))]
   (map (fn [transform coords]
          (map #(.mult transform (->vector3f %)) coords))
        transforms (feeler-pixel-coords geo image))))

(defn feeler-origins
  "The world space coordinates of the root of each feeler."
  [#^Geometry geo image]
   (reduce concat (feeler-world-coords geo image)))

(defn feeler-tips
  "The world space coordinates of the tip of each feeler."
  [#^Geometry geo image]
  (let [world-coords (feeler-world-coords geo image)
        normals
        (map
         (fn [triangle]
           (.calculateNormal triangle)
           (.clone (.getNormal triangle)))
         (map ->triangle (triangles geo)))]

    (mapcat (fn [origins normal]
              (map #(.add % normal) origins))
            world-coords normals)))

(defn touch-topology
  "touch-topology? is not a function."
  [#^Geometry geo image]
  (collapse (reduce concat (feeler-pixel-coords geo image))))
(in-ns 'cortex.touch)

(defn set-ray [#^Ray ray #^Matrix4f transform
               #^Vector3f origin #^Vector3f tip]
  ;; Doing everything locally reduces garbage collection by enough to
  ;; be worth it.
  (.mult transform origin (.getOrigin ray))
  (.mult transform tip (.getDirection ray))
  (.subtractLocal (.getDirection ray) (.getOrigin ray))
  (.normalizeLocal (.getDirection ray)))

(import com.jme3.math.FastMath)
 
(defn touch-kernel
  "Constructs a function which will return tactile sensory data from
   'geo when called from inside a running simulation"
  [#^Geometry geo]
  (if-let
      [profile (tactile-sensor-profile geo)]
    (let [ray-reference-origins (feeler-origins geo profile)
          ray-reference-tips (feeler-tips geo profile)
          ray-length (tactile-scale geo)
          current-rays (map (fn [_] (Ray.)) ray-reference-origins)
          topology (touch-topology geo profile)
          correction (float (* ray-length -0.2))]

      ;; slight tolerance for very close collisions.
      (dorun
       (map (fn [origin tip]
              (.addLocal origin (.mult (.subtract tip origin)
                                       correction)))
            ray-reference-origins ray-reference-tips))
      (dorun (map #(.setLimit % ray-length) current-rays))
      (fn [node]
        (let [transform (.getWorldMatrix geo)]
          (dorun
           (map (fn [ray ref-origin ref-tip]
                  (set-ray ray transform ref-origin ref-tip))
                current-rays ray-reference-origins
                ray-reference-tips))
          (vector
           topology
           (vec
            (for [ray current-rays]
              (do
                (let [results (CollisionResults.)]
                  (.collideWith node ray results)
                  (let [touch-objects
                        (filter #(not (= geo (.getGeometry %)))
                                results)
                        limit (.getLimit ray)]
                    [(if (empty? touch-objects)
                       limit
                       (let [response
                             (apply min (map #(.getDistance %)
                                             touch-objects))]
                         (FastMath/clamp
                          (float 
                           (if (> response limit) (float 0.0)
                               (+ response correction)))
                           (float 0.0)
                           limit)))
                     limit])))))))))))

(defn touch! 
  "Endow the creature with the sense of touch. Returns a sequence of
   functions, one for each body part with a tactile-sensor-profile,
   each of which when called returns sensory data for that body part."
  [#^Node creature]
  (filter
   (comp not nil?)
   (map touch-kernel
        (filter #(isa? (class %) Geometry)
                (node-seq creature)))))
(in-ns 'cortex.touch)

(defn touch->gray
  "Convert a pair of [distance, max-distance] into a gray-scale pixel." 
  [distance max-distance]
  (gray (- 255 (rem (int (* 255 (/ distance max-distance))) 256))))

(defn view-touch 
  "Creates a function which accepts a list of touch sensor-data and
   displays each element to the screen."
  []
  (view-sense
   (fn [[coords sensor-data]]
     (let [image (points->image coords)]
       (dorun
        (for [i (range (count coords))]
          (.setRGB image ((coords i) 0) ((coords i) 1)
                   (apply touch->gray (sensor-data i)))))
     image))))