#!/usr/bin/env roseus

(load "package://euslisp/jskeus/irteus/kalmanlib.l")
(ros::load-ros-manifest "roseus")
(ros::load-ros-manifest "posedetection_msgs")
(ros::roseus "marker-publish")
(ros::roseus-add-msgs "posedetection_msgs")

(defvar *detection-topic* "/openni/rgb/ObjectDetection")
(defvar *base-frame-id* "/base_footprint")
(defvar *map-id* "/map")
(defvar *solve-tf* t)

(defvar map-frame-objectdetection (instance posedetection_msgs::ObjectDetection :init))
(send map-frame-objectdetection :header :frame_id *map-id*)

(defmethod coordinates
  (:stamp (&optional s) (if s (setf (get self :stamp) s)) (get self :stamp))
  (:frame_id (&optional f) (if f (setf (get self :frame_id) f)) (get self :frame_id)))

(defun detection-interface-objectdetection (force target-obj msg)
  (let ((obj-lst (send msg :objects))
        ;;(av (send *pr2* :angle-vector))
        (frame (send msg :header :frame_id))
        (tp (get target-obj :type))
        tmp-obj-lst stamp trans)
    (ros::ros-info "object detection callback target ~A(~A), frame ~A, object ~A"
                   tp (stringp tp) frame target-obj)
    (dolist (obj obj-lst)
      (when (or (not (stringp tp))
                (substringp tp (send obj :type))) ;; found target
        (setq stamp (if force (ros::time 0) (send msg :header :stamp)))
        (cond
         (*solve-tf*
          (unless (boundp '*tfl*)
            (setq *tfl* (instance ros::transform-listener :init)))
          (unless (send *tfl* :wait-for-transform *map-id* frame stamp 1)
            (ros::ros-error "could not solve ~A to ~A" *map-id* frame))
          (setq trans (send *tfl* :lookup-transform *map-id* frame stamp))
          (ros::ros-info "trans ~A" trans))
         (t (setq trans (make-coords))))

        (when trans
          (let ((tmp-obj (instance posedetection_msgs::Object6DPose :init)))
            (ros::ros-info "object detected ~A ~A ~20,9f" (send obj :type)
                           (ros::tf-pose->coords (send obj :pose))
                           (send (send msg :header :stamp) :to-sec))
            (send target-obj :name (send obj :type))
            (send target-obj :stamp (send msg :header :stamp))
            (send target-obj :frame_id (send msg :header :frame_id))
            ;; (send *pr2* :angle-vector (send *ri* :state :potentio-vector))
            (send target-obj :reset-coords)
            (send target-obj :move-to trans :local)
            (send target-obj :move-to (ros::tf-pose->coords (send obj :pose)) :local)
            (send tmp-obj :pose (ros::coords->tf-pose target-obj))
            (send tmp-obj :type (send obj :type))
            (setq tmp-obj-lst (append tmp-obj-lst (list tmp-obj)))
            (when (find-method target-obj :update-descendants)
              (send target-obj :update-descendants) ;; clean me
              )
            (unless (boundp '*tfb*)
              (setq *tfb* (instance ros::transform-broadcaster :init)))
            (send *tfb* :send-transform (ros::tf-pose->coords (send obj :pose)) frame (send target-obj :name))
            ;; target-obj should be on *base-frame-id* coordinates
            ;; (send *pr2* :angle-vector av)
          ))))
    (send map-frame-objectdetection :objects tmp-obj-lst)
    (send map-frame-objectdetection :header :stamp (send msg :header :stamp))
    ))

(defun publish-marker (target-obj marker-life &key (publish-name "/object_detection_marker_array")
                                  (frame *base-frame-id*))
  (let ((mf-obj-lst (send map-frame-objectdetection :objects))
        (tmp-tgtobj (make-cube 60 60 60))
        (tp (get target-obj :type)))
    (dolist (obj mf-obj-lst)
      (when (or (not (stringp tp))
                (substringp tp (send obj :type))) ;; found target
        (send tmp-tgtobj :move-to (ros::tf-pose->coords (send obj :pose)) :local)
        (ros::publish publish-name
                      (instance visualization_msgs::MarkerArray :init
                                :markers
                                (list (text->marker-msg
                                       (send obj :type)
                                       (send (send (ros::tf-pose->coords (send obj :pose)) :copy-worldcoords) :translate #f(-100 0 100))
                                       (send map-frame-objectdetection :header)
                                       :ns (format nil "object_text_~A" (send obj :type)) :color #f(1 0 0)
                                       :scale 100 :lifetime marker-life :id 0)
                                      (coords->marker-msg
                                       (send (ros::tf-pose->coords (send obj :pose)) :copy-worldcoords)
                                       (send map-frame-objectdetection :header)
                                       :ns (format nil "object_coords_~A" (send obj :type))
                                       ;; :size 200 :width 20
                                       :lifetime marker-life :id 1)
                                      (object->marker-msg
                                       tmp-tgtobj
                                       (send map-frame-objectdetection :header)
                                       :ns (format nil "object_body_~A" (send obj :type))
                                       :lifetime marker-life :id 2)
                                      )))))))


(defun calc-kalman-filter (target-obj kfp kfr0 kfr1 kfr2)
  (let* ((coords (send target-obj :copy-worldcoords))
         (rot (send coords :rot))
         (pose (ros::coords->tf-pose coords))
         (x (* 1000 (send pose :position :x)))
         (y (* 1000 (send pose :position :y)))
         (z (* 1000 (send pose :position :z)))
         (p (float-vector x y z))
         (p^ (send kfp :proc p))
         (np (norm (float-vector (abs (- x (elt p^ 0))) (abs (- y (elt p^ 1))) (abs (- z (elt p^ 2))))))

         (rot0 (float-vector (aref rot 0 0) (aref rot 0 1) (aref rot 0 2)))
         (rot1 (float-vector (aref rot 1 0) (aref rot 1 1) (aref rot 1 2)))
         (rot2 (float-vector (aref rot 2 0) (aref rot 2 1) (aref rot 2 2)))
         (rot0^ (send kfr0 :proc rot0))
         (rot1^ (send kfr1 :proc rot1))
         (rot2^ (send kfr2 :proc rot2))
         (nr (norm (float-vector (- (aref rot 0 0) (elt rot0^ 0)) (- (aref rot 0 1) (elt rot0^ 1)) (- (aref rot 0 2) (elt rot0^ 2))
                                 (- (aref rot 1 0) (elt rot1^ 0)) (- (aref rot 1 1) (elt rot1^ 1)) (- (aref rot 1 2) (elt rot1^ 2))
                                 (- (aref rot 2 0) (elt rot2^ 0)) (- (aref rot 2 1) (elt rot2^ 1)) (- (aref rot 2 2) (elt rot2^ 2)))))
         (filter-obj (make-cube 60 60 60))
         (rot-mat (make-array (list 3 3)))
         )
    (ros::ros-info "error:~A" (send kfp :error))
    (ros::ros-info "coords:~A  pose:~A position:~A" coords pose (send pose :position))
    (ros::ros-info " raw:(~A ~A ~A)" x y z)
    (ros::ros-info "  kf:(~A ~A ~A)" (elt p^ 0) (elt p^ 1) (elt p^ 2))
    (ros::ros-info "diff:(~A ~A ~A)" (abs (- x (elt p^ 0))) (abs (- y (elt p^ 1))) (abs (- z (elt p^ 2))))
    (ros::ros-info "norm pose:~A" np)
    (ros::publish "/norm/pose" (instance std_msgs::Float32 :init :data np))

    (ros::ros-info "rot0:~A rot1:~A rot2:~A" rot0 rot1 rot2)
    (ros::ros-info "rot0^:~A rot1^:~A rot2^:~A" rot0^ rot1^ rot2^)
    (ros::ros-info "norm rot:~A" nr)
    (ros::publish "/norm/rot" (instance std_msgs::Float32 :init :data nr))

    (send filter-obj :reset-coords)
    (setf (get filter-obj :type) (format t "filtered ~A" (get filter-obj :type)))
    (send filter-obj :name "filtered")
    (send filter-obj :stamp (send target-obj :stamp))
    (send filter-obj :frame_id (send target-obj :frame_id))
    (setf (aref rot-mat 0 0) (elt rot0^ 0))
    (setf (aref rot-mat 0 1) (elt rot0^ 1))
    (setf (aref rot-mat 0 2) (elt rot0^ 2))
    (setf (aref rot-mat 1 0) (elt rot1^ 0))
    (setf (aref rot-mat 1 1) (elt rot1^ 1))
    (setf (aref rot-mat 1 2) (elt rot1^ 2))
    (setf (aref rot-mat 2 0) (elt rot2^ 0))
    (setf (aref rot-mat 2 1) (elt rot2^ 1))
    (setf (aref rot-mat 2 2) (elt rot2^ 2))
    (send filter-obj :newcoords rot-mat p^)
    (ros::ros-info "pos : ~A" (send filter-obj :pos))
    (ros::ros-info "rot : ~A" (send filter-obj :rot))
    (ros::ros-info "filter name:~A stamp:~A frame_id:~A world-coords:~A" (send filter-obj :name) (send filter-obj :stamp) (send filter-obj :frame_id) (send filter-obj :copy-worldcoords))
    filter-obj
    ))

(defun calc-distribution (x sigma xa)
  (let* ((x-xa (v- x xa))
         (tt (* -0.5 (v. x-xa (transform (inverse-matrix sigma) x-xa))))
         (det (abs (- (* (aref sigma 0 0) (aref sigma 1 1)) (* (aref sigma 0 1) (aref sigma 1 0)))))
         (p (/ (exp tt) (* (* 2 pi) (expt det 0.5))))
         )
    ;; (ros::ros-info "inverse:~A exptt:~A exptdet:~A x-xa:~A tt:~A det:~A p:~A" (inverse-matrix sigma) (exp tt) (expt det 0.5)  x-xa tt det p)
    ;; (ros::ros-info "x-xa:~A tt:~A det:~A p:~A" x-xa tt det p)
    p
    )
  )

(defun calc-probability (x sigma xa)
  (let ((N 10)
        (S 0)
        (tmpx 0)
        (tmpy 0)
        )
    (dotimes (i N nil)
      (setf tmpx (- (+ (elt x 0) (/ (* 1.0 i) N)) 0.5))
      (dotimes (j N nil)
        (setf tmpy (- (+ (elt x 1) (/ (* 1.0 j) N)) 0.5))
        (setf S (+ S (/ (/ (calc-distribution (float-vector tmpx tmpy) sigma xa) (* N 1.0)) N)))
        ;;        (ros::ros-info "x:~A y:~A calcdist:~A" tmpx tmpy (calc-distribution (float-vector tmpx tmpy) sigma xa))
        )
      )
    S
    )
  )

(defun publish-2Dpointcloud (kfp)
  (let* ((P (cdr (assoc 'p_k (send kfp :slots))))
         (sigma (make-matrix 2 2))
         (xa (float-vector (elt (cdr (assoc 'x_k-1 (send kfp :slots))) 0) (elt (cdr (assoc 'x_k-1 (send kfp :slots))) 1)))
         )
    (ros::ros-info "P:~A xa:~A" P xa)
    (setf (aref sigma 0 0) (aref P 0 0))
    (setf (aref sigma 0 1) (aref P 0 1))
    (setf (aref sigma 1 0) (aref P 1 0))
    (setf (aref sigma 1 1) (aref P 1 1))
    (ros::ros-info "sigma: ~A ~A ~A ~A" (aref sigma 0 0) (aref sigma 0 1) (aref sigma 1 0) (aref sigma 1 1))
    (dotimes (y 3 nil)
      (let ((a nil))
        (dotimes (x 3 nil)
          (let ((xy (float-vector (+ (elt xa 0) (- x 1)) (+ (elt xa 1) (- y 1)))))
            (setq a (append a (list (calc-probability xy sigma xa))))
            )
          )
        (ros::ros-info "~A ~A ~A" (elt a 0) (elt a 1) (elt a 2))
        )
      )
    )
  )


(defun pub-cb (target-obj marker-life force kfp kfr0 kfr1 kfr2 msg)
  (let ((filter-obj))
    (detection-interface-objectdetection force target-obj msg)
    (cond
     ((send target-obj :stamp);;for tf error
      ;;(setq filter-obj (calc-kalman-filter target-obj kfp kfr0 kfr1 kfr2))
      (publish-marker target-obj marker-life :frame *map-id*)
      ;;       (publish-marker filter-obj marker-life :publish-name "/filtered")
      ;;       (publish-2Dpointcloud kfp)
      ;; (ros::ros-info "cur-coords:(~A ~A ~A)"
      ;;                (send (send (send cur-coords :pose) :position) :x)
      ;;                (send (send (send cur-coords :pose) :position) :y)
      ;;                (send (send (send cur-coords :pose) :position) :z))
      )
     (t
      nil)
     )
    )
  )

(defun target-obj-cb (req)
  (let ((m (send req :response))
        (cds (make-coords))
        (tmp-coords (instance posedetection_msgs::Object6DPose :init))
        ts)
    (send tmp-coords :pose :orientation :w 1)
    (send tmp-coords :prob 0.5)

    (unless (boundp '*tfl*)
      (setq *tfl* (instance ros::transform-listener :init)))
    (unless (send *tfl* :wait-for-transform *base-frame-id* *map-id* (ros::time 0) 1)
      (ros::ros-error "could not solve ~A to ~A" *base-frame-id* *map-id*))
    (setq ts (send *tfl* :lookup-transform *base-frame-id* *map-id* (ros::time 0)))
    (ros::ros-info "ts ~A" ts)
    (send cds :reset-coords)
    (cond
     (ts
      (send cds :move-to ts :local)
      (let ((mf-obj-lst (send map-frame-objectdetection :objects))
            (tp (send req :type)))
        (dolist (obj mf-obj-lst)
          (when (or (not (stringp tp))
                    (substringp tp (send obj :type))) ;; found target
            (send cds :move-to (ros::tf-pose->coords (send obj :pose)) :local)
            (send tmp-coords :pose (ros::coords->tf-pose (send cds :copy-worldcoords)))
            (send tmp-coords :type (send obj :type))
            (ros::ros-info "in cb tmp-coords:(~A ~A ~A)"
                           (send (send (send tmp-coords :pose) :position) :x)
                           (send (send (send tmp-coords :pose) :position) :y)
                           (send (send (send tmp-coords :pose) :position) :z))))))
     (t
      nil))
    (ros::ros-info "cds:~A" (send cds :worldcoords))
    (send m :objpose tmp-coords)
    (ros::ros-info "prob:~A type:~A" (send (send m :objpose) :prob) (send (send m :objpose) :type))
    m))


(defun only-perception (&key ((:type atype) nil) (tf-force nil)
                             (publish-objectdetection-marker t)
                             (marker-life 30)
                             (detection-topic *detection-topic*)
                             ((:target-object target-obj) (make-cube 60 60 60))
                             )
  (let ((tgt-cds (send target-obj :copy-worldcoords))
        ;;        (kfp (instance kalman-filter :init :state-dim 3 :measure-dim 3 :r-variance 0.001))
        (kfp (instance kalman-filter :init :state-dim 6 :measure-dim 3 :r-variance 1))
        (kfr0 (instance kalman-filter :init :state-dim 3 :measure-dim 3 :r-variance 0.001))
        (kfr1 (instance kalman-filter :init :state-dim 3 :measure-dim 3 :r-variance 0.001))
        (kfr2 (instance kalman-filter :init :state-dim 3 :measure-dim 3 :r-variance 0.001))
        )
    (send kfp :A #2f((1 0 0 1 0 0) (0 1 0 0 1 0) (0 0 1 0 0 1) (0 0 0 1 0 0) (0 0 0 0 1 0) (0 0 0 0 0 1)))
    (send target-obj :reset-coords)
    (setf (get target-obj :type) atype)
    (ros::subscribe detection-topic
                    posedetection_msgs::ObjectDetection
                    #'pub-cb target-obj marker-life tf-force kfp kfr0 kfr1 kfr2))
  (ros::rate 10)
  (ros::advertise "/object_detection_marker_array" visualization_msgs::MarkerArray 5)
  (ros::advertise "/filtered" visualization_msgs::MarkerArray 5)
  (ros::advertise "/norm/pose" std_msgs::Float32 1)
  (ros::advertise "/norm/rot" std_msgs::Float32 1)
  (ros::advertise-service "targetobj" posedetection_msgs::TargetObj #'target-obj-cb)
  (ros::spin)
  )

;(only-perception :type "ge_handle_upper")
(only-perception)


