;; Author Max Sobell
;; Based off of a skeleton by Yann LeCun
;; For NYU V22.0480 'Robotics' course
;; 20090330

(load "rovio-lib")
(defvar r (new rovio "admin" "r0b0t0" "192.168.1.10" 1))

(defvar rgb-yuv
  [f[  0.299  0.587  0.114 ] 
    [ -0.147 -0.289  0.437 ] 
    [  0.615 -0.515 -0.100 ]])

(de yuv-distance (v1 v2)
  (let ((d1 (- (v1 1) (v2 1)))
	(d2 (- (v1 2) (v2 2))))
    (+ (* d1 d1) (* d2 d2))))

#? (detect-color <ball color> <post color> <threshold> <in> <ballout> <postout> <map>)
;; sweep through all pixel in <in>.
;; and write the corrnnnesponding pixel
;; in out with 255 if the <in> pixel
;; matches <color> (square distance less
;; than <threshold> in RGB space.
(de detect-color (ballc postc threshold in ballo posto dmapball dmappost)
  (idx-bloop ((in in) (ballo ballo) (posto posto) (dmapball dmapball) (dmappost dmappost))
    (idx-bloop ((in in) (ballo ballo) (posto posto) (dmapball dmapball) (dmappost dmappost))
      (let ((bdist (yuv-distance in ballc))
	    (pdist (yuv-distance in postc)))
	(dmapball bdist)
	(dmappost pdist)
	;; (print in colour dist)
	(if (< pdist threshold)
	    (posto 255) (posto 0))
	(if (< bdist threshold)
	    (ballo 255) (ballo 0))
	))))

#? (dist <y>)
;; given a distance in pixels, <y>
;; returns the distance from the
;; rovio, where height of the camera
;; is 8.4 cm, and y is the height
;; from the top of the screen (?)
(de dist (y k f h)
  (* h (/ (- (* k f) y) (+ (* k y) f))))

(de circright (obj dur)
  (for (i 1 dur)
       (==> obj straight-right 10))
  (sleep .5)
  (for (i 1 (- dur 2))
       (==> obj rotate-left 6))
  (sleep .5))
  
(de circleft (obj dur)
  (for (i 1 dur)
       (==> obj straight-left 10))
  (sleep .5)
  (for (i 1 (- dur 2))
       (==> obj rotate-right 6))
  (sleep .5))

#? (getangle <x> <y>)
;; returns the angle from the middle of the screen
(de getangle (x y)
  (* (/ 180 3.14) (atan (/ (- x 160) (- 120 y)))))

(de turn-5-left (obj)
  (==> obj rotate-left 5)
  (sleep .05)
  (==> obj move 0 0)
  (sleep .1))

(de turn-5-right (obj)
  (==> obj rotate-right 5)
  (sleep .05)
  (==> obj move 0 0)
  (sleep .1))

(de turn-test (obj)
;; should turn 45 degrees
  (for (i 0 18)
       (turn-5-left obj)))

(de rot-left (obj)
  (for (i 0 2)
       (==> r straight-left 10)
       (sleep .1)
       (==> r move 0 0)))

(de rot-right (obj)
  (for (i 0 2)
       (==> r straight-right 10)
       (sleep .1)
       (==> r move 0 0)))

(de turn-45 (obj)
  (for (i 0 7)
       (turn-5-right r)
       (sleep .5)))

(de takeandmask ()
  (libload "libimage/morpho")
  (libload "libimage/cca")
  (when (not window) (new-window 670 310))
  (let ((connected 1) ;are we on ylcnet?
	(postcrgb [f 173 40 9]) ;;rgb
	(ballcrgb [f 155 189 102]);; in rgb
	(postc (float-matrix 3))
	(ballc (float-matrix 3))
	(rgbmatrix (float-matrix 140 320 3))
	(yuvmatrix (float-matrix 140 320 3))
	(dmapball (float-matrix 140 320))
	(dmappost (float-matrix 140 320))
	(m ())
	(q ())
	(thres 600))
    (==> r grab)
    (==> r grab)
    (==> r grab)
    (==> r grab) ;to make sure we have the latest
    (if (= 1 connected) (setq m (==> r grab)))
    (save-matrix m "test_image_6.dat")
    ;; for when we're not connected to rovio
    (if (= 0 connected) (setq m (load-matrix "test_image_5.dat")))
    ;;narrow down the m to only the floor (bottom half)
    (setq m (narrow m 0 140 99))
    ;;copy the grabbed matrix into a float matrix
    (idx-copy m rgbmatrix)
    ;;use formula to find post color and ball color
    (setq postc (idx-m2dotm1 rgb-yuv postcrgb))
    (setq ballc (idx-m2dotm1 rgb-yuv ballcrgb))
    ;;and dot the float matrix into a yuv matrix
    (idx-bloop ((rgbmatrix rgbmatrix) (yuvmatrix yuvmatrix))
      (idx-bloop ((rgbmatrix rgbmatrix) (yuvmatrix yuvmatrix))
	(idx-m2dotm1 rgb-yuv rgbmatrix yuvmatrix)))
    ;;draw the original picture
    (rgb-draw-matrix 10 10 m)
    ;;and draw a greyscale of the luminance of the yuv matrix
    ;;(gray-draw-matrix 0 150 (select yuvmatrix 2 0) 0 255)
    ;;use threshold to find the ball
    (detect-color ballc postc thres yuvmatrix ballo posto dmapball dmappost)
    (ubim-erosion ballo 4)
    (ubim-erosion posto 3)
    (ubim-dilation ballo 1)
    (ubim-dilation posto 10)
    ;;find the min and max of the dmap ball matrix (distance map)
    (let ((vmin ((idx-inf dmapball))) (vmax ((idx-sup dmapball))))
      (print vmin vmax)
      )
    ;;find the ... of the dmappost post
    (let ((vmin ((idx-inf dmappost))) (vmax ((idx-sup dmappost))))
      (print vmin vmax)
      )
    ;;draw the ball matrix (the result)nnn
    (rgb-draw-matrix 10 160 ballo)
    (rgb-draw-matrix 340 160 posto)
    ))

(de soccer ()
  (let ((ballo (ubyte-matrix 140 320))
	(posto (ubyte-matrix 140 320))
	(x 0)
	(y 0)
	(p1x 0)
	(p1y 0)
	(p2x 0)
	(p2y 0)
	(ball 0)
	(post 0)
	(done 0)
	(temp 0)
	(maxpost 0)
	(atball 0)
	(ballstat 0) ;0 for not on screen, 1 for
	             ;to the left, 2 in the middle
	             ;3 to the right
	(robostat 0) ;0 for not sure, 1 for left, 3 for right. no 2.
	(ballangle 0)
	(roboangle 0)
	(dir 0) ;0 for left 1 for right
	(numobjs ())
	;;(ballc [f 106.19 -33.03 -15.08])
	(result ())
	(disttoball 0)
	(disttopost 0)
	(k (/ 500 8))
	(f 380)
	(h 8.4)
	(biggest ())
	(rot-count 0)
	(rot-changes 0)
	(rot-dir 0) ;0 for left 1 for right
	(anglechecked 0)
	(timesleft 0)
	(timesright 0)
	(biggest-goal 0)
	(goal-size 0))
    (while (= done 0)
      (takeandmask)
      ;;;;;;;;;;;;
      ;;analysis;;
      ;;;;;;;;;;;;
      ;(find-xy ballo)
      (setq ccball (new CCAnalyzer ballo))
      (==> ccball run-analysis 100)
      (if (< (car (idx-dim :ccball:runs)) 512)
	  (progn
	    (==> ccball cc-analysis)
	    (setq x (:ccball:ccdesc 0 6) y (:ccball:ccdesc 0 7))
;	    (pause "matrix analysis")
	    (setq y (- y 20)) ;;count from the middle of the screen
	    (setq disttoball (dist y k f h))
	    (printf "Middle of the ball: (%d, %d) \n" x y)
	    (setq ball 1))
	(progn
	  (printf "There is no ball!\n")
	  (setq ball 0)))
      (setq timesum 0)
      (setq cc (new CCAnalyzer posto))
      (==> cc run-analysis 100)
      (if (< (car (idx-dim :cc:runs)) 512)
	  (progn
	    (==> cc cc-analysis)
	    (setq post (car (idx-dim :cc:ccdesc))))
	(setq post 0))
;      (pause "examine 'cc:ccdesc'")
      ;set the x's and y's of the post cc's
      (if (< 0 post)
	  (progn
	    (setq p1x (:cc:ccdesc 0 6))
	    (setq p1y (:cc:ccdesc 0 7))))
      (if (= post 0)
	  (progn
	    (setq p1x 0 p1y 0 p2x 0 p2y 0)
	    (setq disttopost 0))
	(setq disttopost (dist (- p1y 20) k f h))) ;distance to the posts 
      ;set ball's status
      (if (= 1 ball)
	  (setq ballstat 4)) ;there _is_ a ball
      (if (and (= 1 ball) (> p1x x) (< 0 p1x) (< 0 x))
	  (progn
	    (printf "The ball is to the left of the goal.\n")
	    (setq ballstat 1)))
      (if (and (= 1 ball) (< p1x x) (< 0 p1x) (< 0 x))
	  (progn
	    (printf "The ball is to the right of the goal.\n")
	    (setq ballstat 3)))
      (if (and (= 1 ball) (> p2x x) (< p1x x) (= post 2))
	  (progn
	    (printf "SUCCESS: The ball is inbetween the posts.\n")
	    (setq ballstat 2)))
      ;set robot's status
      (if (= 0 p1y)
	  (progn
	    (printf "We don't see any posts.\n")
	    (setq robostat 0)))
      (setq goal-size (* (/ disttopost 75) (:cc:ccdesc 0 3))) ;enough of the goal is in sight
      (if (< biggest-goal goal-size)
	  (setq biggest-goal goal-size)) ; keep track of the biggest
      (if (> goal-size 6500) ;enough of the goal is in sight
	  (progn
	    (printf "There is enough goal to shoot.\n")
	    (setq robostat 2)))
      (printf "Dist to ball: %d\n" disttoball)
      (printf "Average distance to posts: %d\n" disttopost)
      (if (< 0 ballstat) ;the ball is somewhere on the screen
	  (progn
	    (setq ballangle (getangle x y)) ;angle from the middle of the screen
	    ; and count y from the ROBOT, not the top of the screen
	    (printf "Angle to ball: %d\n" ballangle)))
      (if (< 0 robostat)
	  (progn
	    (setq roboangle (getangle p1x p1y))
	    (printf "Angle to goal: %d\n" roboangle)))
      ;;;;;;;;;;;;;;;;;;;;;;;;
      ;;now go for the ball!;;
      ;;;;;;;;;;;;;;;;;;;;;;;;
      ;first, find the ball
      (printf "ballstat: %d\nrobostat %d\n" ballstat robostat)
;      (pause "lets see if we could find the ball...")
      (sleep .5)
      ;we see nothing
      (if (and (= 0 ballstat) (= 0 atball))
	  (for (i 0 9)
	       (printf "we're turning left in search of the ball.\n")
	       (turn-5-left r))) ;rotate in a circle
      ;we see the ball not the posts
      (if (> rot-count 5)
	  (progn
	    (setq rot-count 0)
	    (setq atball 0)
	    (setq anglechecked 0)
	    (for (i 1 40)
		 (==> r backward 5)))) ;start over
;      (pause "eval goal-size and biggest-goal")
      (if (= 1 atball)
	  (progn
	    (incr rot-count)
	    (if (= 2 robostat) ; if there's enough goal...
		(for (i 1 (* .40 disttopost))
		     (==> r forward 5)) ; shoot!
	      (if (> biggest-goal goal-size) ; if we're going the wrong way...
		  (progn
		    (setq biggest-goal 0)
		    (if (= rot-dir 0)
		      (setq rot-dir 1)
		    (setq rot-dir 0))))
	      (if (= rot-dir 0)
		  (rot-left r)
		(rot-right r)))))
      (if (and (< 0 ballstat) (= 0 atball))
	  (progn
	    (if (< ballangle 0)
		(progn
		     (printf "we're turning left towards the ball.\n")
		     (turn-5-left r)
		     (incr timesleft))
	      (printf "we're turning right towards the ball.\n")
	      (turn-5-right r)
	      (incr timesright))
	    ;;now we're facing the goal	  
	    (printf "angle checked: %d\n" anglechecked)
	    (if (= 1 ball)
		(progn
		  (if (and (> 10 ballangle) (< -10 ballangle))
		      (setq anglechecked 1))
		  (if (= 0 anglechecked)
		      (takeandmask))
		  (if (= 1 anglechecked)
		      (progn
			(printf "we're here!\n")
			(for (i 1 (* .23 disttoball)) ;go to the ball
			     (printf "we're going to the ball.\n")
			     (==> r forward 5))
			(setq atball 1)
			(setq biggest-goal 0)))
		  ))))
      (printf "resetting ballstat and robostat \n")
      (setq ballstat 0 robostat 0)
      (sleep .5)
      )))