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

(load "rovio-lib")

(defparameter ip "192.168.1.13")
(defvar x 0)
(defvar y 0)

(de square-distance (v1 v2)
  (let ((s 0))
    (idx-bloop ((v1 v1) (v2 v2))
      (let ((d (- (v1) (v2))))
	(incr s (* d d))))
    s))

#? (detect-color <color> <threshold> <in> <out>)
;; sweep through all pixel in <in>.
;; and write the corresponding pixel
;; in out with 255 if the <in> pixel
;; matches <color> (square distance less
;; than <threshold> in RGB space.
(de detect-color (color threshold in out)
  (idx-bloop ((in in) (out out))
    (idx-bloop ((in in) (out out))
      (if (< (square-distance in color) threshold)
	  (out 255) (out 0)))) ())


#? (find-xy <in>)
;; sweep through all pixel in <in>
;; and find the middle of a white blob
;; in an otherwise black image by summing
;; the index of each white pixel in <in>
;; in xsum and ysum, and then averaging
;; to find the middle.
(de find-xy (in)
  (let ((xsum 0)
	(ysum 0)
	(xused 0)
	(yused 0))
  (idx-gloop ((row in)(r))
    (idx-gloop ((col in)(c))
      (when (> (in r c) 0)
	  (progn
;;	    (print r c)
	    (incr ysum r)
	    (incr yused)
	    (incr xsum c)
	    (incr xused))))) ()
  (setq x (/ xsum xused) y (/ ysum yused))))

#? (dist <y>)
;; given a distance in pixels, <y>
;; returns the distance from the
;; rovio, where height of the camera
;; is 8.4 cm and alpha is 89.08
;; degrees = 1.5547 radians
(de dist (y)
  (let ((len 0))
    (setq len (* 8.4 (tan (- 1.5547 (atan (/ y -371))))))
    len))

(de test (ip thres)
  (libload "libimage/morpho")
  (when (not window) (new-window))
  (let (;;(r (new rovio "admin" "r0b0t0" ip 1))
	(m ())
	(q ())
	(z (ubyte-matrix 240 320))
	(color [u 39 71 0])
	(result ())
	(length 0))
;;    (setq m (==> r grab))
;;    (save-matrix m "test-image.dat")
;;    (load-matrix m "test-image.dat")
    (setq q (load-matrix "test_image.dat"))
    (rgb-draw-matrix 0 0 q)
    (detect-color color thres q z)
    (ubim-erosion z 3)
    (ubim-dilation z 3)
    (find-xy z)
    (setq y (- y 120)) ;;count from the middle of the screen
    (setq length (dist y))
    (rgb-draw-matrix 0 244 z)
    (print "x:" x "y:" y "len:" length "cm")
    ))