(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))))

(in-ns 'cortex.test.body)

(def debug-control 
  {"key-h" (fn [world val]
             (if val (enable-debug world)))})
  
(defn test-hand-3
  ([] (test-hand-3 false))
  ([record?]
     (world 
      (nodify
       [(doto (hand)
          (physical!)
          (joints!))
        (floor)])
      (merge standard-debug-controls debug-control
             gravity-control)
      (comp
       #(Capture/captureVideo
         % (File. "/home/r/proj/cortex/render/body/3"))
       #(do (set-gravity % Vector3f/ZERO) %)
       setup)
      no-op)))