#!/usr/bin/env roseus
(setq *debug-view* t)

(setq *plane-height* 10)
(setq *block-width* (* 16 2.54 10))
(setq *block-height* (* 8 2.54 10))
(setq *block-depth* (* 6 2.54 10))
(setq *width* (* 12 *block-height*))

(if *debug-view* (make-irtviewer))

(setq *bodies* nil)
(setq *grounds* nil)
(setq *blocks* nil)
(setq *top-faces* nil)
(setq *mountain-blocks* nil)
(setq *sloped-mountain-blocks* nil)
(let ((slope (make-cube 1000.0 *width* *plane-height*)))
  (send slope :locate (float-vector 500 0 (- (/ *plane-height* 2.0))))
  (send slope :set-color :brown)
  (push slope *bodies*)
  (push slope *grounds*)
  (push (elt (send slope :faces) 1) *top-faces*)
  )

(let* ((slope-length (/ 500.0 (cos (deg2rad 15))))
       (slope1 (make-cube slope-length *width* *plane-height*))
       (slope2 (make-cube slope-length *width* *plane-height*)))
  (send slope1 :locate (float-vector (+ 1000.0 250) 0 (/ (* slope-length (sin (deg2rad 15))) 2.0)))
  (send slope1 :rotate (- (deg2rad 15)) :y)
  (send slope1 :set-color :green)
  (push slope1 *bodies*)
  (push slope1 *grounds*)
  (push (elt (send slope1 :faces) 1) *top-faces*)
  (send slope2 :locate (float-vector (+ 1000.0 250 500) 0 (/ (* slope-length (sin (deg2rad 15))) 2.0)))
  (send slope2 :rotate (deg2rad 15) :y)
  (send slope2 :set-color :green)
  (push slope2 *bodies*)
  (push (elt (send slope2 :faces) 1) *top-faces*)
  (push slope2 *grounds*)
  (send-all *bodies* :worldpos)
  )

(dotimes (i 11)                         ;7000
  (let ((slope (make-cube 1000.0 *width* *plane-height*)))
    (send slope :locate (float-vector (+ (* i 1000.0) 2500) 0 (- (/ *plane-height* 2.0))))
    (if (evenp i)
        (send slope :set-color (float-vector 0 0 1))
      (send slope :set-color (float-vector 0 1 0)))
    (push slope *bodies*)
    (send-all *bodies* :worldpos)
    (push slope *grounds*)
    ))



;; 6, 8, 16
(defun make-block ()
  (let ((blck (make-cube *block-width* *block-height* *block-depth*)))
    (send blck :translate-vertices (float-vector 0 0 (/ *block-depth* 2.0)))
    (send blck :set-color :gray80)
    (send blck :worldpos)
    blck))

(defun diagonal-block-pair (right-center)
  (let ((block-right (make-block))
        (block-left (make-block))
        (w (* 16 2.54 10))
        (h (* 8 2.54 10)))
    (send block-right :locate right-center)
    (send block-right :rotate (deg2rad -45) :z)
    (send block-left :locate (v+ right-center (float-vector (- (* h (sin (deg2rad 45))))
                                                            (- (* w (cos (deg2rad 45))))
                                                            0)))
    (send block-left :rotate (deg2rad 45) :z)
    (list block-right block-left)))

;; second hazard
(dotimes (i 3)
  (let ((pos (float-vector 2700 (+ -400 (* i 2 (* 16 2.54 10) (cos (deg2rad 45)))) 0)))
  ;;(let ((pos (float-vector 3700 (+ -400 (* i 2 (* 16 2.54 10) (cos (deg2rad 45)))) 0)))
    (let ((blocks (diagonal-block-pair pos)))
      (setq *bodies* (append blocks *bodies*))
      (setq *blocks* (append blocks *blocks*))
      (send-all *bodies* :worldpos)
      )
    ))
(send-all *blocks* :worldpos)
;;(objects *bodies*)
(let ((vertices1 (list
                  (elt (send (elt *blocks* 0) :vertices) 4)
                  (elt (send (elt *blocks* 0) :vertices) 5)
                  (elt (send (elt *blocks* 1) :vertices) 0)
                  (elt (send (elt *blocks* 1) :vertices) 4)
                  (elt (send (elt *blocks* 3) :vertices) 0)
                  (elt (send (elt *blocks* 3) :vertices) 4)
                  (elt (send (elt *blocks* 5) :vertices) 0)
                  (elt (send (elt *blocks* 5) :vertices) 4)
                  (elt (send (elt *blocks* 5) :vertices) 5))))
  (let ((vertices (append (list (float-vector 3000
                                              (/ *width* 2.0)
                                              0)
                                (float-vector 2000
                                              (/ *width* 2.0)
                                              0)
                                (float-vector 2000
                                              (- (/ *width* 2.0))
                                              0)
                                (float-vector 3000
                                              (- (/ *width* 2.0))
                                              0))
                          (reverse vertices1))))
    ;; 13
    (let ((indices '((0 1 12)
                     (12 1 11)
                     (11 1 9 10)
                     (9 1 7)
                     (9 7 8)
                     (7 1 2)
                     (7 2 5)
                     (7 5 6)
                     (5 2 4)
                     (4 2 3))))
      (let ((fs (mapcar #'(lambda (ids)
                            (instance face :init :vertices
                                      (mapcar #'(lambda (i)
                                                  (elt vertices i))
                                              ids)))
                        indices)))
        (setq *top-faces* (append *top-faces* fs))
        )))
  )

(let ((vertices1 (list
                  (elt (send (elt *blocks* 0) :vertices) 4)
                  (elt (send (elt *blocks* 0) :vertices) 0)
                  (elt (send (elt *blocks* 2) :vertices) 4)
                  (elt (send (elt *blocks* 2) :vertices) 0)
                  (elt (send (elt *blocks* 4) :vertices) 4)
                  (elt (send (elt *blocks* 4) :vertices) 0)
                  (elt (send (elt *blocks* 5) :vertices) 5))))
   (let ((vertices (append (list (float-vector 3000
                                               (- (/ *width* 2.0))
                                               0)
                                 (float-vector 3000
                                               (/ *width* 2.0)
                                               0))
                           vertices1)))
     ;; 9
     (let ((indices '((1 2 3)
                      (1 3 0)
                      (3 5 0)
                      (3 4 5)
                      (5 6 7)
                      (5 7 0)
                      (7 8 0))))
       (let ((fs (mapcar #'(lambda (ids)
                            (instance face :init :vertices
                                      (mapcar #'(lambda (i)
                                                  (elt vertices i))
                                              ids)))
                        indices)))
         (setq *top-faces* (append *top-faces* fs))
         )))
   )

;; third hazard
(defun make-mountain (represent-pos)
  (let ((w (* 16 2.54 10))
        (h (* 8 2.54 10))
        (d (* 6 2.54 10))
        (blocks nil))
    (dotimes (i 4)
      (let ((block-num (1+ i))
            (x-offset (* i w)))
        (let ((ablock (make-block)))
          (send ablock :locate (v+ represent-pos (float-vector x-offset
                                                               0
                                                               (* (1- block-num) d))))
          (push ablock blocks)
          )))
    (dotimes (i 3)
      (let ((block-num (- 3 i))
            (x-offset (* (+ 4 i) w)))
        (let ((ablock (make-block)))
          (send ablock :locate (v+ represent-pos (float-vector x-offset
                                                               0
                                                               (* (1- block-num) d))))
          (push ablock blocks)
          )))
    blocks))

(dotimes (i 6)
  (let ((x-offset (+ 3750 (* i *block-width*))))
    (dotimes (j 2)
      (let ((blocks (make-mountain (float-vector x-offset
                                                 (- (/ *width* 2.0) (/ *block-height* 2.0)
                                                    (* *block-height* (+ j (* i 2))))
                                                 0))))
        (setq *bodies* (append blocks *bodies*))
        (send-all *bodies* :worldpos)
        (setq *mountain-blocks* (append blocks *mountain-blocks*))
        (setq *blocks* (append blocks *blocks*)))
      
      )))

(let ((vertices (list (float-vector 3000 (/ *width* 2.0) 0)
                      (float-vector 3000 (- (/ *width* 2.0)) 0))))
  (dotimes (i (/ (length *mountain-blocks*) (* 2 7)))
    (let ((blocks (subseq *mountain-blocks* (* i 2 7) (* (1+ i) 2 7))))
      (let ((right-block (elt blocks 6))
            (left-block (elt blocks 13)))
        (setq vertices (append vertices (list (elt (send right-block :vertices) 5)
                                              (elt (send left-block :vertices) 4))))
        )
      ))
  (let ((v1 (list (elt vertices 0)
                  (elt vertices 1)
                  (elt vertices 2)))
        (v2 (list (elt vertices 2)
                  (elt vertices 3)
                  (elt vertices 4)))
        (v3 (list (elt vertices 4)
                  (elt vertices 5)
                  (elt vertices 6)))
        (v4 (list (elt vertices 6)
                  (elt vertices 7)
                  (elt vertices 8)))
        (v5 (list (elt vertices 8)
                  (elt vertices 9)
                  (elt vertices 10)))
        (v6 (list (elt vertices 10)
                  (elt vertices 11)
                  (elt vertices 12)))
        (v7 (list (elt vertices 12)
                  (elt vertices 13)
                  (elt vertices 0)))
        (v8 (list (elt vertices 0)
                  (elt vertices 2)
                  (elt vertices 4)))
        (v9 (list (elt vertices 0)
                  (elt vertices 4)
                  (elt vertices 6)))
        (v10 (list (elt vertices 0)
                  (elt vertices 6)
                  (elt vertices 8)))
        (v11 (list (elt vertices 0)
                  (elt vertices 8)
                  (elt vertices 10)))
        (v12 (list (elt vertices 0)
                  (elt vertices 10)
                  (elt vertices 12)))
        )
  ;; (setq vertices (append (cdr vertices)
  ;;                        (list (car vertices))))
    
  ;; (let ((f (instance face :init :vertices vertices)))
  ;;   (push f *top-faces*))
    (let ((f1 (instance face :init :vertices v1))
          (f2 (instance face :init :vertices v2))
          (f3 (instance face :init :vertices v3))
          (f4 (instance face :init :vertices v4))
          (f5 (instance face :init :vertices v5))
          (f6 (instance face :init :vertices v6))
          (f7 (instance face :init :vertices v7))
          (f8 (instance face :init :vertices v8))
          (f9 (instance face :init :vertices v9))
          (f10 (instance face :init :vertices v10))
          (f11 (instance face :init :vertices v11))
          (f12 (instance face :init :vertices v12)))
      (push f1 *top-faces*)
      (push f2 *top-faces*)
      (push f3 *top-faces*)
      (push f4 *top-faces*)
      (push f5 *top-faces*)
      (push f6 *top-faces*)
      (push f7 *top-faces*)
      (push f8 *top-faces*)
      (push f9 *top-faces*)
      (push f10 *top-faces*)
      (push f11 *top-faces*)
      (push f12 *top-faces*))
    ))

(defun make-sloped-mountain (represent direction)
  (let* ((blocks nil)
         (w *block-width*)
         (h *block-height*)
         (d *block-depth*)
         (l (sqrt (+ (* d d) (* w w))))
         (alpha (acos (/ w l)))
         (theta (deg2rad 15))
         (beta (+ alpha theta)))
    (let ((a (make-block))
          (b (make-block)))
      (send a :rotate (* direction (- theta)) :y :local)
      (send a :locate (v+ represent
                          (float-vector (* (/ w 2.0) (cos theta)) 0
                                        (* (/ w 2.0) (sin theta))))
            :world)
      (push a blocks)
      (send b :rotate (* direction (- theta)) :y :local)
      (send b :locate (v+ represent
                          (float-vector (* (/ w 2.0) (cos theta))
                                        (- h)
                                        (* (/ w 2.0) (sin theta))))
            :world)
      (push b blocks))
    (let ((a (make-block))
          (b (make-block)))
      (send a :rotate pi/2 :z :local)
      (send a :rotate (* direction (- theta)) :x :world)
      (send a :translate (v+ represent
                             (float-vector (+ (* w (cos theta))
                                              (/ h 2.0))
                                           (- (* (/ w 4.0) (cos theta)))
                                           (+ d (* (/ w 2.0) (sin theta)))))
            :world)
      (push a blocks)
      (send a :worldpos)
      (send b :rotate pi/2 :z :local)
      (send b :rotate (* direction (- theta)) :x :world)
      (send b :translate (v+ represent
                             (float-vector (+ (* w (cos theta))
                                              h
                                              (/ h 2.0))
                                           (- (* (/ w 4.0) (cos theta)))
                                           (+ d (* (/ w 2.0) (sin theta)))))
            :world)
      (push b blocks)
      )
    (let* ((a (make-block))
           (b (make-block)))
      (send a :rotate (* direction (- theta)) :y :local)
      (send a :translate (v+ represent
                             (float-vector 
                              (+ (* w (cos theta))
                                 (* 2.0 h)
                                 (* d (sin theta))
                                 (/ (* w (cos theta)) 2.0))
                              0
                              (+ (* d 2) (* (/ w 2.0) (sin theta)))))
            :world)
      (push a blocks)
      (send b :rotate (* direction (- theta)) :y :local)
      (send b :locate (v+ represent
                          (float-vector
                           (+ (* w (cos theta))
                              (* 2.0 h)
                              (* d (sin theta))
                              (/ (* w (cos theta)) 2.0))
                           (- h)
                           (+ (* d 2) (* (/ w 2.0) (sin theta)))))
            :world)
      (push b blocks)
      )
    (let ((a (make-block))
          (b (make-block)))
      (send a :rotate pi/2 :z :local)
      (send a :rotate (* direction (- theta)) :x :world)
      (send a :translate (v+ represent
                             (float-vector (+ (* w (cos theta))
                                              (* 2.0 h)
                                              (* d (sin theta))
                                              (* w (cos theta))
                                              (/ h 2.0))
                                           (- (* (/ w 4.0) (cos theta)))
                                           (+ (* d 2)
                                              (* w (sin theta))
                                              (* w (sin theta)))))
            :world)
      (push a blocks)
      (send a :worldpos)
      (send b :rotate pi/2 :z :local)
      (send b :rotate (* direction (- theta)) :x :world)
      (send b :translate (v+ represent
                             (float-vector (+ (* w (cos theta))
                                              (* 2.0 h)
                                              (* d (sin theta))
                                              (* w (cos theta))
                                              (/ h 2.0)
                                              h)
                                           (- (* (/ w 4.0) (cos theta)))
                                           (+ (* 2 d) (* w (sin theta))
                                              (* w (sin theta)))))
            :world)
      (push b blocks)
      )
    (let* ((a (make-block))
           (b (make-block)))
      (send a :rotate (* direction (- theta)) :y :local)
      (send a :translate (v+ represent
                             (float-vector 
                              (+ (* w (cos theta))
                                 (* 2.0 h)
                                 (* 2.0 h)
                                 (* w (cos theta))
                                 (* 2 (* d (sin theta)))
                                 (/ (* w (cos theta)) 2.0))
                              0
                              (+ (* d 2) (* (/ w 2.0) (sin theta)))))
            :world)
      (push a blocks)
      (send b :rotate (* direction (- theta)) :y :local)
      (send b :locate (v+ represent
                          (float-vector
                           (+ (* w (cos theta))
                              (* 2.0 h)
                              (* w (cos theta))
                              (* 2.0 h)
                              (* 2 (* d (sin theta)))
                              (/ (* w (cos theta)) 2.0))
                           (- h)
                           (+ (* d 2) (* (/ w 2.0) (sin theta)))))
            :world)
      (push b blocks)
      )
    (let ((a (make-block))
          (b (make-block)))
      (send a :rotate pi/2 :z :local)
      (send a :rotate (* direction (- theta)) :x :world)
      (send a :translate (v+ represent
                             (float-vector (+ (* w (cos theta))
                                              (/ h 2.0)
                                              (* h 2)
                                              (* h 2)
                                              (* 2 d (sin theta))
                                              (* w (cos theta))
                                              (* w (cos theta)))
                                           (- (* (/ w 4.0) (cos theta)))
                                           (+ d (* (/ w 2.0) (sin theta)))))
            :world)
      (push a blocks)
      (send a :worldpos)
      (send b :rotate pi/2 :z :local)
      (send b :rotate (* direction (- theta)) :x :world)
      (send b :translate (v+ represent
                             (float-vector (+ (* w (cos theta))
                                              (/ h 2.0)
                                              (* h 2)
                                              (* h 2)
                                              h
                                              (* 2 d (sin theta))
                                              (* w (cos theta))
                                              (* w (cos theta)))
                                           (- (* (/ w 4.0) (cos theta)))
                                           (+ d (* (/ w 2.0) (sin theta)))))
            :world)
      (push b blocks)
      )
    (let ((a (make-block))
          (b (make-block)))
      (send a :rotate (* direction (- theta)) :y :local)
      (send a :locate (v+ represent
                          (float-vector (+ (* w (cos theta))
                                           (* h 2)
                                           (* h 2)
                                           (* h 2)
                                           (* 2 d (sin theta))
                                           (* d (sin theta))
                                           (/ (* w (cos theta)) 2.0)
                                           (* w (cos theta))
                                           (* w (cos theta)))
                                        0
                                        (* (/ w 2.0) (sin theta))))
            :world)
      (push a blocks)
      (send b :rotate (* direction (- theta)) :y :local)
      (send b :locate (v+ represent
                          (float-vector (+ (* w (cos theta))
                                           (* h 2)
                                           (* h 2)
                                           (* h 2)
                                           (* 2 d (sin theta))
                                           (* d (sin theta))
                                           (/ (* w (cos theta)) 2.0)
                                           (* w (cos theta))
                                           (* w (cos theta)))
                                        (- h)
                                        (* (/ w 2.0) (sin theta))))
            :world)
      (push b blocks))
    blocks))

(dotimes (i 6)
  (let ((objs (make-sloped-mountain (float-vector (+ 7000 (* i *block-width* (cos (deg2rad 15))))
                                                  (- (/ *width* 2.0) (* i *block-height* 2) (/ *block-height* 2))
                                                  0)
                                    (if (evenp i) 1 -1))))
    (setq *bodies* (append objs *bodies*))
    (send-all *bodies* :worldpos)
    (setq *sloped-mountain-blocks* (append objs *sloped-mountain-blocks*))
    (setq *blocks* (append objs *blocks*))
  ))
;;(objects *bodies*)

(let ((vertices nil))
  (dotimes (i (/ (length *mountain-blocks*) (* 2 7)))
    (let ((blocks (subseq *mountain-blocks* (* i 2 7) (* (1+ i) 2 7))))
      (let ((right-block (elt blocks 0))
            (left-block (elt blocks 7)))
        (setq vertices (append 
                        (list (elt (send left-block :vertices) 0)
                              (elt (send right-block :vertices) 2))
                        vertices))
        )))
  (dotimes (i (/ (length *sloped-mountain-blocks*) (* 2 7)))
    (let ((blocks (subseq *sloped-mountain-blocks* (* i 2 7) (* (1+ i) 2 7))))
      (let ((right-block (elt blocks 12))
            (left-block (elt blocks 13)))
        (setq vertices (append vertices
                               (list
                                (elt (send right-block :vertices) 5)
                                (elt (send left-block :vertices) 4))))
        )
      ))
  (let ((indices '((0 1 2 22 23)
                   (2 3 4 20 21 22)
                   (4 5 6 18 19 20)
                   (6 7 8 16 17 18)
                   (8 9 10 14 15 16)
                   (10 11 12 13 14))))
    (let ((fs (mapcar #'(lambda (ids)
                            (instance face :init :vertices
                                      (mapcar #'(lambda (i)
                                                  (elt vertices i))
                                              ids)))
                        indices)))
      (setq *top-faces* (append *top-faces* fs))
      )))

(let ((vertices nil))
  (dotimes (i (/ (length *sloped-mountain-blocks*) (* 2 7)))
    (let ((blocks (subseq *sloped-mountain-blocks* (* i 2 7) (* (1+ i) 2 7))))
      (let ((right-block (elt blocks 0))
            (left-block (elt blocks 1)))
        (setq vertices (append (list 
                                (elt (send left-block :vertices) 0)
                                (elt (send right-block :vertices) 2))
                               vertices))
        )
      ))
  (setq vertices (append
                  (list (float-vector 13000
                                      (/ *width* 2)
                                      0))
                  vertices
                  (list (float-vector 13000
                                      (- (/ *width* 2))
                                      0))))
  (let ((f (instance face :init :vertices vertices)))
    (push f *top-faces*)
    )
  )

 (let ((top-faces (mapcar #'(lambda (b)
                              (elt (send b :faces) 1))
                          *blocks*)))
   (setq *top-faces* (append *top-faces* top-faces))
   )

(if *debug-view*
    (objects *bodies*))

(ros::roseus "task2_planes")
(ros::roseus-add-msgs "jsk_recognition_msgs")
(ros::roseus-add-msgs "pcl_msgs")
(ros::advertise "planes" jsk_recognition_msgs::PolygonArray 1)
(ros::advertise "coefficients" jsk_recognition_msgs::ModelCoefficientsArray 1)
(ros::rate 10)

(while (ros::ok)
  (let ((now (ros::time-now)))
    (let ((arr (instance jsk_recognition_msgs::PolygonArray :init)))
      (send arr :header :frame_id "/odom")
      (send arr :header :stamp now)

      (send arr :polygons (mapcar #'(lambda (f)
                                      (let ((p (instance geometry_msgs::PolygonStamped :init)))
                                        (send p :header :frame_id "/odom")
                                        (send p :header :stamp now)
                                        (send p :polygon :points
                                              (mapcar #'(lambda (point)
                                                          (let ((pp (instance geometry_msgs::Point32 :init)))
                                                            (send pp :x (* 0.001 (elt point 0)))
                                                            (send pp :y (* 0.001 (elt point 1)))
                                                            (send pp :z (* 0.001 (elt point 2)))
                                                            pp))
                                                      (cdr (send f :vertices))))
                                        p))
                                  *top-faces*))
      (ros::publish "planes" arr)
      (if *debug-view* (x::window-main-one))
      (ros::sleep)
      )
    (let ((arr (instance jsk_recognition_msgs::ModelCoefficientsArray :init)))
      (send arr :header :frame_id "/odom")
      (send arr :header :stamp now)
      (send arr :coefficients (mapcar #'(lambda (f)
                                          (let ((p (instance pcl_msgs::ModelCoefficients :init)))
                                            (send p :header :frame_id "/odom")
                                            (send p :header :stamp now)
                                            (send p :values (send f :normal))
                                            p))
                                      *top-faces*))
      (ros::publish "coefficients" arr)
      )
    ))
