;;
;; library to evaluate IK quality like reachability
;;


;; ik-grid-cell
;; ik-grid-cell is a class to represent each cell of
;; ik-grid.
(defclass ik-grid-cell
  :super propertied-object
  :slots (center-pos indices value time-value dimension))

(defmethod ik-grid-cell
  (:init (acenter-pos aindices adimension)
    (setq center-pos acenter-pos)
    (setq indices aindices)
    (setq dimension adimension))
  (:center-pos () center-pos)
  (:indices () indices)
  (:set-value (v) (setq value v))
  (:set-time-value (v) (setq time-value v))
  (:get-value () value)
  (:get-time-value () time-value)
  (:cube (&optional (max-value 1.0))
    "instantiate cube for this cell"
    (let ((c (make-cube dimension dimension dimension)))
      (send c :locate center-pos :world)
      ;;(setf (get c :face-color) :green)
      (setf (get c :face-color)
            (scale (/ 1.0 255.0)
                   (coerce (his2rgb (* 180 (/ (or value 0.0) max-value)) 1.0 0.8)
                           float-vector)))
      (gl::transparent c 0.5)
      (send c :worldpos)
      c))
  (:face (&optional (max-value 1.0))
    (let* ((d/2 (/ dimension 2.0))
           (-d/2 (- d/2))
           (vertices (list
                      (v+ center-pos (float-vector d/2 d/2 0))
                      (v+ center-pos (float-vector -d/2 d/2 0))
                      (v+ center-pos (float-vector -d/2 -d/2 0))
                      (v+ center-pos (float-vector d/2 -d/2 0)))))
      (let ((f (instance face :init :vertices vertices)))
        (let ((fs (instance faceset :init :faces (list f))))
          (setf (get fs :face-color)
                (scale (/ 1.0 255.0)
                       (coerce
                        (his2rgb (* 180 (/ value max-value)) 1.0 0.8)
                        float-vector)))
          (gl::transparent fs 0.5)
          fs))))
  (:clone ()
    (let ((new (instance ik-grid-cell :init center-pos indices dimension)))
      new))
  (:sphere ()
    (let ((s (make-sphere (/ dimension 2.0))))
      (send s :locate center-pos :world)
      (send s :worldpos)
      s)
    )
  )

(defclass ik-grid
  :super propertied-object
  :slots (min-x max-x min-y max-y min-z max-z grid-step cells
                numx numy numz origin))

(defmethod ik-grid
  (:init (min-max-x min-max-y min-max-z
          &key ((:grid-step agrid-step) 100) ((:origin aorigin) (make-coords)))
    (setq min-x (car min-max-x))
    (setq max-x (cdr min-max-x))
    (setq min-y (car min-max-y))
    (setq max-y (cdr min-max-y))
    (setq min-z (car min-max-z))
    (setq max-z (cdr min-max-z))
    (setq grid-step agrid-step)
    (setq origin aorigin)
    (setq numx (ceiling (/ (- max-x min-x) grid-step)))
    (setq numy (ceiling (/ (- max-y min-y) grid-step)))
    (setq numz (ceiling (/ (- max-z min-z) grid-step)))
    (send self :initialize-cells)
    )
  (:initialize-cells ()
    ;; TODO: using `loop' is better implementation
    (dotimes (i numx)
      (dotimes (j numy)
        (dotimes (k numz)
          (let ((x (+ (* (+ 0.5 i) grid-step) min-x))
                (y (+ (* (+ 0.5 j) grid-step) min-y))
                (z (+ (* (+ 0.5 k) grid-step) min-z)))
            (let ((cell (instance ik-grid-cell :init
                                  (float-vector x y z)
                                  (list i j k)
                                  grid-step)))
              (setq cells (append cells (list cell)))
              ))))))
  (:cells () cells)
  (:valid-cells () (remove-if #'(lambda (x) (= (send x :get-value) 0)) cells))
  (:dump-to-csv (fname)
    (with-open-file (f fname :direction :output)
      (format f "x,y,z,i,j,k,value~%")
      (dolist (cell cells)
        (format f "~A,~A,~A,~A,~A,~A,~A~%"
                (elt (send cell :center-pos) 0)
                (elt (send cell :center-pos) 1)
                (elt (send cell :center-pos) 2)
                (elt (send cell :indices) 0)
                (elt (send cell :indices) 1)
                (elt (send cell :indices) 2)
                (send cell :get-value))
        )
      )
    )
  (:normalize-cells ()
    (let ((sum (reduce #'+ (send-all cells :get-value))))
      (dolist (cell cells)
        (send cell :set-value (/ (send cell :get-value) (float sum))))
      ))
  (:get-cell (indices)
    (let ((i (car indices))
          (j (cadr indices))
          (k (caddr indices)))
      (let ((index (+ k (* j numz) (* i numz numy))))
        (if (and (> index 0) (< index (length cells)))
            (elt cells index)
          nil))))
  (:get-cell-value (indices)
    (let ((c (send self :get-cell indices)))
      (if c (send c :get-value)
        0)))
  (:max-value ()
    (apply #'max (send-all cells :get-value)))
  (:get-value-for-coordinates-set (cs offset-coords)
    (reduce #'+ (mapcar #'(lambda (c)
                            (let ((offsetted-coords (send (send offset-coords :inverse-transformation)
                                                          :transform (send c :copy-worldcoords))))

                            ;; (let ((offsetted-coords (send (send c :copy-worldcoords)
                            ;;                                :transform (send offset-coords :inverse-transformation))))
                              ;;(send offsetted-coords :draw-on :flush t)
                              (send self :get-cell-value
                                    (send self :pos-to-indices (send offsetted-coords :worldpos)))))
                        (send cs :coordinates))))
  (:sum-filter ()
    ;; Update destructively
    (let ((new-cells nil))
      (dolist (cell cells)
        (if (not (= (send cell :get-value) 0))
            (let* ((cell-indices  (send cell :indices))
                   (i (car cell-indices))
                   (j (cadr cell-indices))
                   (k (caddr cell-indices))
                   (sum 0))
              (let ((neighbors (list (list (1+ i) j k)
                                     (list i j k)
                                     (list (1- i) j k)
                                     (list i (1+ j) k)
                                     (list i (1- j) k)
                                     (list i j (1+ k))
                                     (list i j (1- k)))))
                (dolist (neighbor-index neighbors)
                  (let ((neigbor-cell (send self :get-cell neighbor-index)))
                    (if neigbor-cell
                        (setq sum (+ sum (send neigbor-cell :get-value)))))))
              ;;(send cell :set-value sum)
              (let ((new-cell (send cell :clone)))
                (send new-cell :set-value sum)
                (setq new-cells (append new-cells (list new-cell))))
              )
          (setq new-cells (append new-cells (list cell)))
          ))
      (setq cells new-cells)
      (send self :normalize-cells)
      t))
  (:cell-edges ()
    "return edges to visualize cells to compute"
    (flatten (send-all (send-all cells :cube) :edges)))
  (:visualize-objects ()
    ;; Slice in z
    (let ((faces nil)
          (max-value (send self :max-value)))
      (dotimes (k numz)
        (dotimes (i numx)
          (dotimes (j numy)
            (let ((c (send self :get-cell (list i j k))))
              (if (and c (> (send c :get-value) 0.0))
                  (let ((f (send (send self :get-cell (list i j k)) :face max-value)))
                    (send f :transform origin)
                    (setq faces (append faces (list f)))
                    )))))
          )
      (append (send self :max-cells-per-z-lines) faces)))
  (:max-cell ()
    (let ((max-value 0)
          (max-cell nil))
      (dolist (cell cells)              ;linear search
        (when (> (send cell :get-value) max-value)
          (setq max-value (send cell :get-value))
          (setq max-cell cell))
        )
      max-cell))
  (:max-cells-per-z ()
    (let ((cells nil))
      (dotimes (k numz)
        (let ((c (send self :max-cell-in-z-plane k)))
          (if c
              (setq cells (append cells (list c)))))
        )
      cells))
  (:max-cell-in-z-plane (k)
    (let ((max-value 0)
          (max-cell nil))
      (dotimes (i numx)
        (dotimes (j numy)
          (let ((c (send self :get-cell (list i j k))))
            (when (and c (> (send c :get-value) max-value))
              (setq max-value (send c :get-value))
              (setq max-cell c))
            )))
      max-cell))
  (:max-cells-per-z-lines ()
    (let ((lines nil))
      (let ((cells (send self :max-cells-per-z)))
        (dotimes (i (1- (length cells)))
          (push (instance line :init
                          :pvertex (send origin :transform-vector (send (elt cells i) :center-pos))
                          :nvertex (send origin :transform-vector (send (elt cells (1+ i)) :center-pos)))
                lines))
        lines)))
  (:dump-to-file (fname)
    (dump-object fname self))
  (:pos-to-indices (pos)
    (list (floor (/ (- (elt pos 0) min-x) grid-step))
          (floor (/ (- (elt pos 1) min-y) grid-step))
          (floor (/ (- (elt pos 2) min-z) grid-step)))
    )
  (:indices-to-pos (index)
    (float-vector (+ (* grid-step (car index)) min-x)
                  (+ (* grid-step (cadr index)) min-y)
                  (+ (* grid-step (caddr index)) min-z)))
  (:move-origin (org)
    (setq origin org))
  (:lookup-stand-location (cset
                           &key
                           (analysis-level :center) ; :center or :cet
                           (collision nil) ;take into account collision or not. not implemented yet
                           (theta-range (deg2rad 90))
                           (theta-num 10)
                           (x-range 1000.0)
                           (y-range 1000.0)
                           (x-num 10)
                           (y-num 10))
    "compute transform of cset to maximize cell value"
    ;; x-y-theta
    (let ((midpoint (send cset :midpoint)))
      (let ((z-index (caddr (send self :pos-to-indices midpoint))))
        (let ((initial-transform (make-coords :pos
                                              (vz0 (v- midpoint
                                                       (send (send self :max-cell-in-z-plane z-index)
                                                             :center-pos))))))
          (print initial-transform)
          (let ((max-evaluate 0)
                (max-evaluate-offset initial-transform)
                (dx (/ x-range x-num))
                (dy (/ y-range y-num))
                (dtheta (/ theta-range theta-num)))
            (dotimes (ix x-num)
              (dotimes (iy y-num)
                (dotimes (it theta-num)
                  ;; TODO
                  (let ((offset
                         (make-coords :pos
                                      (float-vector (* dx (- ix (/ x-num 2.0)))
                                                    (* dy (- iy (/ y-num 2.0)))
                                                    0))))
                    (send offset :rotate (* dtheta (- it (/ theta-num 2.0))) :z :local)
                    (setq offset (send (send initial-transform :copy-worldcoords)
                                       :transform offset))
                    ;;(send *robot* :newcoords (send offset :copy-worldcoords))
                    (let ((v (send self :get-value-for-coordinates-set cset offset)))
                      (when (> v max-evaluate)
                        (setq max-evaluate v)
                        (setq max-evaluate-offset offset)
                        ;;(send *max-robot* :newcoords (send offset :copy-worldcoords))
                        ))
                    ;;(send *irtviewer* :draw-objects)
                    ;; (send-all (send cset :coordinates) :draw-on :flush t)
                    ))))
          ;;initial-transform
            max-evaluate-offset)))))
  (:draw (vwer)
      (gl::glPushAttrib gl::GL_ALL_ATTRIB_BITS)
      (gl::glDisable gl::GL_LIGHTING)
      (gl::glColor3fv (gl::find-color (get self :face-color)))
      ;; x-y
      (gl::glBegin gl::GL_LINES)
      (dotimes (i (1+ numx))
        (dotimes (j (1+ numy))
          (let ((from (send self :indices-to-pos (list (- i 0.5) (- j 0.5) -0.5)))
                (to (send self :indices-to-pos (list (- i 0.5) (- j 0.5) (- numz 0.5)))))
            (gl::glvertex3fv from)
            (gl::glvertex3fv to)
            )))
      ;; y-z
      (dotimes (i (1+ numy))
        (dotimes (j (1+ numz))
          (let ((from (send self :indices-to-pos (list -0.5 (- i 0.5) (- j 0.5))))
                (to (send self :indices-to-pos (list (- numx 0.5) (- i 0.5) (- j 0.5)))))
            (gl::glvertex3fv from)
            (gl::glvertex3fv to)
            )))
      ;; z-x
      (dotimes (i (1+ numz))
        (dotimes (j (1+ numx))
          (let ((from (send self :indices-to-pos (list (- j 0.5) -0.5 (- i 0.5) )))
                (to (send self :indices-to-pos (list (- j 0.5) (- numy 0.5) (- i 0.5) ))))
            (gl::glvertex3fv from)
            (gl::glvertex3fv to)
            )))
      (gl::glEnd)
      (gl::glEnable gl::GL_LIGHTING)
      (gl::glPopAttrib)
      )
  (:vertices ()
    "Return vertices of bounding box of the grid"
    (list (send self :indices-to-pos (list 0 0 0))
          (send self :indices-to-pos (list 0 0 numz))
          (send self :indices-to-pos (list 0 numy 0))
          (send self :indices-to-pos (list 0 numy numz))
          (send self :indices-to-pos (list numx 0 0))
          (send self :indices-to-pos (list numx 0 numz))
          (send self :indices-to-pos (list numx numy 0))
          (send self :indices-to-pos (list numx numy numz))))
  (:worldcoords ()
    origin)
  )
  

(defun vz0 (vec)
  "force to set z element of `vec' as 0"
  (vzset vec 0))

(defun vzset (vec z)
  "replace z element of `vec' with `z'"
  (float-vector (elt vec 0) (elt vec 1) z))

(defun random-coordinates (num mean sigma
                           &key
                           (fixed-z nil)
                           (rotation (unit-matrix)))
  (let ((points nil))
    (dotimes (i num)
      (if fixed-z
          (setq points
                (append (list (vzset (gaussian-random 3 mean sigma) fixed-z))
                        points))
        (setq points
              (append (list (gaussian-random 3 mean sigma)) points))))
    (mapcar #'(lambda (p) (make-coords :pos p :rot (copy-matrix rotation)))
            points)))
  

(defun load-ik-grid-from-file (fname)
  "Load and instantiate ik-grid object from dumped file `fname'"
  (with-open-file (f fname)
    (let ((tmp (read f)))
      (send tmp :move-origin (make-coords)) ;reset origin
      tmp)))

(defclass coordinates-set
  :super propertied-object
  :slots (coordinates-list))

(defmethod coordinates-set
  (:init (initial-set)
    (setq coordinates-list initial-set))
  (:add (c)
    (setq coordinates-list (append coordinates-list (list c))))
  (:transform-copy (c)
    (instance coordinates-set (send-all (send-all coordinates-list :copy-worldcoords) :transrofm c)))
  (:coordinates ()
    coordinates-list)
  (:midcoords ()
    (make-coords :pos (send self :midpoint)))
  (:midpoint ()
    (scale (/ 1.0 (length coordinates-list)) (reduce #'v+ (send-all coordinates-list :worldpos))))
  )

(defun distance2d (a b)
  (norm (v- (float-vector (elt a 0) (elt a 1))
            (float-vector (elt b 0) (elt b 1)))))

(defun ik-evaluation (robot grid limb ik-args
                      &key
                      (update-view nil)
                      (rotation (unit-matrix))
                      (min-distance 0.0))
  (send robot :fix-leg-to-coords (make-coords))
  (let ((counter 0))
  (dolist (cell (send grid :cells))
    (send robot :reset-pose)
    (if (< (distance2d (send robot :worldpos) (send cell :center-pos)) min-distance)
        (progn
          (warn "skip because it violtates min-distance~%")
          (send cell :set-value 0))
      (progn
        (if (send* robot limb :inverse-kinematics
                   (make-coords :pos (send cell :center-pos)
                                :rot rotation)
                   :dump-command nil
                   :warnp nil
                   ik-args)
            (send cell :set-value 1)
          (send cell :set-value 0))
        (when update-view
          (if (= (send cell :get-value) 1)
              (objects (send cell :cube)))
          (send *irtviewer* :draw-objects)
          (x::window-main-one))
        (incf counter)
        (when (= (mod counter 10) 0)
          (format t "~c~0,4d/~A" #x0d counter (length (send grid :cells)))
          (let* ((percentage (/ (* 100 counter) (length (send grid :cells))))
                 (max-bar-length 60)
                 (bar-num (/ (* max-bar-length percentage) 100)))
            (dotimes (i bar-num)
              (format t "=")))
          (finish-output))
        )))
  (send grid :normalize-cells)
  ))

(defun save-4-views (file-prefix)
    ;; slant
  (send *irtviewer* :viewpoint (float-vector 5000 5000 5000))
  (send *irtviewer* :look1 (float-vector 0 0 800) 30 30)
  (send *irtviewer* :draw-objects)
  (send *irtviewer* :viewer :viewsurface :write-to-image-file (format nil "~A-slant.png" file-prefix))
  ;; up
  (send *irtviewer* :viewpoint (float-vector 0 0 10000))
  (send *irtviewer* :look1 (float-vector 0 0 800) 180 90)
  (send *irtviewer* :draw-objects)
  (send *irtviewer* :viewer :viewsurface :write-to-image-file (format nil "~A-up.png" file-prefix))
  ;; side
  (send *irtviewer* :viewpoint (float-vector 0 10000 0))
  (send *irtviewer* :look1 (float-vector 0 0 800) 90 0)
  (send *irtviewer* :draw-objects)
  (send *irtviewer* :viewer :viewsurface :write-to-image-file (format nil "~A-side.png" file-prefix))

  ;; front
  (send *irtviewer* :viewpoint (float-vector 10000 0 0))
  (send *irtviewer* :look1 (float-vector 0 0 800) 0 0)
  (send *irtviewer* :draw-objects)
  (send *irtviewer* :viewer :viewsurface :write-to-image-file (format nil "~A-front.png" file-prefix))
 )

(defun generate-ik-grid-for-robot (robot file-prefix min-max-x min-max-y min-max-z grid-step
                                         ik-arg
                                         rot-matrix
                                         output-directory
                                         &key (min-distance 0))
  (format t "Generate reachability map for ~A with rotation ~A~%" (send robot :name) rot-matrix)
  (setq *grid* (instance ik-grid :init
                         min-max-x min-max-y min-max-z
                         :grid-step grid-step))
  (ik-evaluation *robot* *grid* :larm ik-arg :update-view nil
                 :rotation rot-matrix
                 :min-distance min-distance)
  (send *grid* :dump-to-csv (format nil "~A/~A-input.csv" output-directory file-prefix))
  (send *grid* :dump-to-file (format nil "~A/~A-input.l" output-directory file-prefix))
  (progn (objects (append (send *grid* :visualize-objects)
                          (list *robot*)))
         t)
  (save-4-views (format nil "~A/~A-input" output-directory file-prefix))
  (dotimes (i 10)
    (send *grid* :sum-filter)
    (progn (objects (append (send *grid* :visualize-objects)
                            (list *robot*)))
           t)
    (save-4-views (format nil "~A/~A-iterate~0,3d" output-directory file-prefix i))
    (send *grid* :dump-to-csv (format nil "~A/~A-iterate~0,3d.csv" output-directory file-prefix i))
    (send *grid* :dump-to-file (format nil "~A/~A-iterate~0,3d.l" output-directory file-prefix i))
    )
  )

(defun test-with-jaxon-yup ()
  (require "package://hrpsys_ros_bridge_tutorials/euslisp/jaxon-interface.l")
  (setq *robot* (make-robot-model-from-name "jaxon"))
  (objects (list *robot*))
  (setq *grid* (instance ik-grid :init
                         '(0.0 . 1000.0) '(-1000.0 . 2000.0) '(800.0 . 2000.0)
                         :grid-step 100))
  (format t "Start test~%")
  (ik-evaluation *robot* *grid* :larm '(:rotation-axis :z) :update-view t
                 :rotation (send (make-coords :rpy (list 0 0 pi/2)) :worldrot))
  ;;(send *grid* :normalize-cells)
  (objects (append (send-all (send *grid* :valid-cells) :cube (send *grid* :max-value)) (list *robot*)))
  (send (send (send *irtviewer* :viewer) :viewsurface)
          :write-to-image-file "input.png")
  t)


(defun test-with-hrp2jsknt-yup ()
  (require "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-interface.l")
  (setq *robot* (make-robot-model-from-name "hrp2jsknt"))
  (objects (list *robot*))
  (setq *grid* (instance ik-grid :init
                         '(0.0 . 700.0) '(-1000.0 . 2000.0) '(800.0 . 2000.0)
                         :grid-step 100))
  (format t "Start test~%")
  (ik-evaluation *robot* *grid* :larm '(:rotation-axis :z) :update-view t
                 :rotation (send (make-coords :rpy (list 0 0 pi/2)) :worldrot))
  ;;(send *grid* :normalize-cells)
  (objects (append (send-all (send *grid* :valid-cells) :cube (send *grid* :max-value)) (list *robot*)))
  (send (send (send *irtviewer* :viewer) :viewsurface)
          :write-to-image-file "input.png")
  t)

(defun test-with-hrp2jsknt-zup ()
  (require "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-interface.l")
  (setq *robot* (make-robot-model-from-name "hrp2jsknt"))
  (objects (list *robot*))
  (setq *grid* (instance ik-grid :init
                         '(0.0 . 700.0) '(-1000.0 . 2000.0) '(800.0 . 2000.0)
                         :grid-step 100))
  (format t "Start test~%")
  (ik-evaluation *robot* *grid* :larm '(:rotation-axis :z) :update-view t)
  ;;(send *grid* :normalize-cells)
  (objects (append (send-all (send *grid* :valid-cells) :cube (send *grid* :max-value)) (list *robot*)))
  (send (send (send *irtviewer* :viewer) :viewsurface)
          :write-to-image-file "input.png")
  t)

(defun test-with-jaxon-zup ()
  (require "package://hrpsys_ros_bridge_tutorials/euslisp/jaxon-interface.l")
  (setq *robot* (make-robot-model-from-name "jaxon"))
  (objects (list *robot*))
  (setq *grid* (instance ik-grid :init
                         '(0.0 . 1000.0) '(-1000.0 . 2000.0) '(800.0 . 2000.0)
                         :grid-step 100))
  (format t "Start test~%")
  (ik-evaluation *robot* *grid* :larm '(:rotation-axis :z) :update-view t)
  ;;(send *grid* :normalize-cells)
  (objects (append (send-all (send *grid* :valid-cells) :cube (send *grid* :max-value)) (list *robot*)))
  (send (send (send *irtviewer* :viewer) :viewsurface)
          :write-to-image-file "input.png")
  t)

(defun test-with-jaxon-xup ()
  (require "package://hrpsys_ros_bridge_tutorials/euslisp/jaxon-interface.l")
  (setq *robot* (make-robot-model-from-name "jaxon"))
  (objects (list *robot*))
  (setq *grid* (instance ik-grid :init
                         '(0.0 . 1000.0) '(-1000.0 . 2000.0) '(800.0 . 2000.0)
                         :grid-step 100))
  (format t "Start test~%")
  (ik-evaluation *robot* *grid* :larm '(:rotation-axis :z) :update-view t
                 :rotation (send (make-coords :rpy (list 0 -pi/2 0)) :worldrot))
  ;;(send *grid* :normalize-cells)
  (objects (append (send-all (send *grid* :valid-cells) :cube (send *grid* :max-value)) (list *robot*)))
  (send (send (send *irtviewer* :viewer) :viewsurface)
          :write-to-image-file "input.png")
  t)


(defun test-with-pr2-zup ()
  (require "package://pr2eus/pr2.l")
  (setq *robot* (make-robot-model-from-name "pr2"))
  (objects (list *robot*))
  (setq *grid* (instance ik-grid :init
                         '(0.0 . 1000.0) '(-1000.0 . 2000.0) '(000.0 . 1500.0)
                         :grid-step 100))
  (format t "Start test~%")
  (ik-evaluation *robot* *grid* :larm '(:rotation-axis :z) :update-view t)
  ;;(send *grid* :normalize-cells)
  (objects (append (send-all (send *grid* :valid-cells) :cube (send *grid* :max-value))
                   (list *robot*)))
  (send (send (send *irtviewer* :viewer) :viewsurface)
          :write-to-image-file "input.png")
  t)


(defun iterate-and-save-images ()
  (dotimes (i 10)
    (send *grid* :sum-filter)
    (progn (objects (append (send *grid* :visualize-objects)
                            (list *robot*)))
           t)
    (send (send (send *irtviewer* :viewer) :viewsurface)
          :write-to-image-file (format nil "iterate~0,3d.png" i))
    (send *grid* :dump-to-csv (format nil "iterate~0,3d.csv" i))
    ))

(defun test-pos-to-index ()
  (dolist (cell (send *grid* :cells))
    (assert (equal (send *grid* :pos-to-indices (send cell :center-pos))
                   (send cell :indices)))
    ))

(defun test-stand-location-planning (target-coordinates)
  (let ((arrow (instance arrow-object :init :pos (send target-coordinates :worldpos)
                         :rot (send target-coordinates :worldrot))))
    (let ((pose (bench (send *grid* :lookup-stand-location (instance coordinates-set :init (list target-coordinates))))))
      (send *robot* :reset-pose)
      (send *robot* :fix-leg-to-coords pose)
      (send *grid* :move-origin pose)
      (send *robot* :larm :inverse-kinematics target-coordinates :rotation-axis :z)
      (progn
        (send *irtviewer* :objects
              (append ;;(send *grid* :visualize-objects)
                      (list *robot* arrow)))
        (send *irtviewer* :viewpoint (float-vector 5000 5000 5000))
        (send *irtviewer* :look1 (float-vector 0 0 800) 130 30)
        (send *irtviewer* :draw-objects)
        t)
      )))

(defun test-stand-location-planning2 ()
  (let ((target (instance coordinates-set :init (random-coordinates 10 1000 30 :fixed-z 900))))
    (setq *grid* (load-ik-grid-from-file (ros::resolve-ros-path "package://jsk_ik_server/data/jaxon-xup-iterate009.l")))
    (let ((pose (bench (send *grid* :lookup-stand-location target))))
      
      (send-all (send target :coordinates) :draw-on :flush t)
      (send *robot* :reset-pose)
      (send *robot* :fix-leg-to-coords pose)
      (send *grid* :move-origin pose)
      (send *robot* :larm :inverse-kinematics (send target :midcoords) :rotation-axis :z)
      (progn
        (send *irtviewer* :objects *robot*)
        ;; (send *irtviewer* :viewpoint (float-vector 5000 5000 5000))
        ;; (send *irtviewer* :look1 (float-vector 0 0 800) 130 30)
        (send *irtviewer* :draw-objects)
        (x::window-main-one)
        t)
      )))

;; (load "ik-evaluation.l")
;; (require "package://hrpsys_ros_bridge_tutorials/euslisp/jaxon-interface.l")
;; (setq *robot* (make-robot-model-from-name "jaxon"))
;; (objects (list *robot*))
;; (progn (test-with-hrp2jsknt-zup) (iterate-and-save-images))
;; (progn (test-with-hrp2jsknt-yup) (iterate-and-save-images))
;; (progn (test-with-jaxon-yup) (iterate-and-save-images))
;; (progn (test-with-jaxon-zup) (iterate-and-save-images))
;; (progn (test-with-jaxon-xup) (iterate-and-save-images))
;; (progn (test-with-pr2-zup) (iterate-and-save-images))
;; (progn     (send *grid* :sum-filter) (objects (append (send *grid* :visualize-objects) (list *robot*))) t)
;; (do-until-key (test-stand-location-planning (make-coords :pos (float-vector (random 1000.0) (random 1000.0) (+ 900 (random 1000.0))))))
;; (do-until-key (test-stand-location-planning2))
