(ns cortex.body
  "Assemble a physical creature using the definitions found in a
   specially prepared blender file. Creates rigid bodies and joints so
   that a creature can have a physical presence in the simulation."
  {:author "Robert McIntyre"}
  (:use (cortex world util sense))
  (:import
   (com.jme3.math Vector3f Quaternion Vector2f Matrix3f)
   (com.jme3.bullet.joints
    SixDofJoint Point2PointJoint HingeJoint ConeJoint)
   com.jme3.bullet.control.RigidBodyControl
   com.jme3.collision.CollisionResults
   com.jme3.bounding.BoundingBox
   com.jme3.scene.Node
   com.jme3.scene.Geometry
   com.jme3.bullet.collision.shapes.HullCollisionShape))
(defn physical!
  "Iterate through the nodes in creature and make them real physical
   objects in the simulation."
  [#^Node creature]
  (dorun
   (map
    (fn [geom]
      (let [physics-control
            (RigidBodyControl.
             (HullCollisionShape.
              (.getMesh geom))
             (if-let [mass (meta-data geom "mass")]
               (do
                 ;;(println-repl
                 ;; "setting" (.getName geom) "mass to" (float mass))
                 (float mass))
               (float 1)))]
        (.addControl geom physics-control)))
    (filter #(isa? (class %) Geometry )
            (node-seq creature)))))
(def
  ^{:doc "Return the children of the creature's \"joints\" node."
    :arglists '([creature])}
  joints
  (sense-nodes "joints"))
(defn joint-targets
  "Return the two closest two objects to the joint object, ordered
  from bottom to top according to the joint's rotation."
  [#^Node parts #^Node joint]
  (loop [radius (float 0.01)]
    (let [results (CollisionResults.)]
      (.collideWith
       parts
       (BoundingBox. (.getWorldTranslation joint)
                     radius radius radius) results)
      (let [targets
            (distinct
             (map  #(.getGeometry %) results))]
        (if (>= (count targets) 2)
          (sort-by
           #(let [joint-ref-frame-position
                  (jme-to-blender
                   (.mult
                    (.inverse (.getWorldRotation joint))
                    (.subtract (.getWorldTranslation %)
                               (.getWorldTranslation joint))))]
              (.dot (Vector3f. 1 1 1) joint-ref-frame-position))                  
           (take 2 targets))
          (recur (float (* radius 2))))))))
(defmulti joint-dispatch
  "Translate blender pseudo-joints into real JME joints."
  (fn [constraints & _] 
    (:type constraints)))

(defmethod joint-dispatch :point
  [constraints control-a control-b pivot-a pivot-b rotation]
  ;;(println-repl "creating POINT2POINT joint")
  ;; bullet's point2point joints are BROKEN, so we must use the
  ;; generic 6DOF joint instead of an actual Point2Point joint!

  ;; should be able to do this:
  (comment 
    (Point2PointJoint.
     control-a
     control-b
     pivot-a
     pivot-b))

  ;; but instead we must do this:
  ;;(println-repl "substituting 6DOF joint for POINT2POINT joint!")
  (doto
      (SixDofJoint.
       control-a
       control-b
       pivot-a
       pivot-b
       false)
    (.setLinearLowerLimit Vector3f/ZERO)
    (.setLinearUpperLimit Vector3f/ZERO)))

(defmethod joint-dispatch :hinge
  [constraints control-a control-b pivot-a pivot-b rotation]
  ;;(println-repl "creating HINGE joint")
  (let [axis
        (if-let
            [axis (:axis constraints)]
          axis
          Vector3f/UNIT_X)
        [limit-1 limit-2] (:limit constraints)
        hinge-axis
        (.mult
         rotation
         (blender-to-jme axis))]
    (doto
        (HingeJoint.
         control-a
         control-b
         pivot-a
         pivot-b
         hinge-axis
         hinge-axis)
      (.setLimit limit-1 limit-2))))

(defmethod joint-dispatch :cone
  [constraints control-a control-b pivot-a pivot-b rotation]
  (let [limit-xz (:limit-xz constraints)
        limit-xy (:limit-xy constraints)
        twist    (:twist constraints)]
    
    ;;(println-repl "creating CONE joint")
    ;;(println-repl rotation)
    ;;(println-repl
    ;; "UNIT_X --> " (.mult rotation (Vector3f. 1 0 0)))
    ;;(println-repl
    ;; "UNIT_Y --> " (.mult rotation (Vector3f. 0 1 0)))
    ;;(println-repl
    ;; "UNIT_Z --> " (.mult rotation (Vector3f. 0 0 1)))
    (doto
        (ConeJoint.
         control-a
         control-b
         pivot-a
         pivot-b
         rotation
         rotation)
      (.setLimit (float limit-xz)
                 (float limit-xy)
                 (float twist)))))

(defn connect
  "Create a joint between 'obj-a and 'obj-b at the location of
  'joint. The type of joint is determined by the metadata on 'joint.

   Here are some examples:
   {:type :point}
   {:type :hinge  :limit [0 (/ Math/PI 2)] :axis (Vector3f. 0 1 0)}
   (:axis defaults to (Vector3f. 1 0 0) if not provided for hinge joints)

   {:type :cone :limit-xz 0]
                :limit-xy 0]
                :twist 0]}   (use XZY rotation mode in blender!)"
  [#^Node obj-a #^Node obj-b #^Node joint]
  (let [control-a (.getControl obj-a RigidBodyControl)
        control-b (.getControl obj-b RigidBodyControl)
        joint-center (.getWorldTranslation joint)
        joint-rotation (.toRotationMatrix (.getWorldRotation joint))
        pivot-a (world-to-local obj-a joint-center)
        pivot-b (world-to-local obj-b joint-center)]
   
    (if-let [constraints
             (map-vals
              eval
              (read-string
               (meta-data joint "joint")))]
      ;; A side-effect of creating a joint registers
      ;; it with both physics objects which in turn
      ;; will register the joint with the physics system
      ;; when the simulation is started.
      (do
        ;;(println-repl "creating joint between"
        ;;              (.getName obj-a) "and" (.getName obj-b))
        (joint-dispatch constraints
                        control-a control-b
                        pivot-a pivot-b
                        joint-rotation))
      ;;(println-repl "could not find joint meta-data!")
      )))
(defn joints!
  "Connect the solid parts of the creature with physical joints. The
   joints are taken from the \"joints\" node in the creature."
  [#^Node creature]
  (dorun
   (map
    (fn [joint]
      (let [[obj-a obj-b] (joint-targets creature joint)]
        (connect obj-a obj-b joint)))
    (joints creature))))
(defn body!
  "Endow the creature with a physical body connected with joints.  The
   particulars of the joints and the masses of each body part are
   determined in blender."
  [#^Node creature]
  (physical! creature)
  (joints! creature))