;-----------------------
; Port Constants
;-----------------------

constants
   [[portb 6][portb-ddr $86]
    [portc 7][portc-ddr $87]]

;-----------------------
; Global Variables
;-----------------------
global [nn a b c blue red green slp_time]


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;;;   Print Functions taken from lctools    ;;;;;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

to prs :n
setnn :n
loop
  [if (read-rom nn) = 0 [stop]
   send read-rom nn
   setnn nn + 1]
end

to prh :n
prh-digit :n / 16
prh-digit :n % 16
cr
end

to prh-digit :n
ifelse :n < 10
   [send :n + 48]
   [send :n + 55]
end

to cr
send 13
end

to print :n
if :n < 0 [send 45 pn 0 - :n stop]
pn :n
end

to pn :n
if :n > 9999 [pdigit :n 10000]
if :n > 999 [pdigit :n 1000]
if :n > 99 [pdigit :n 100]
if :n > 9 [pdigit :n 10]
pdigit :n 1
cr
end

to pdigit :n :d
send ((:n / :d) % 10) + 48
end

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

;----------------------------
; Functions to flash the LED
;----------------------------
to start_green
   setbit 2 portc
end

to end_green
   clearbit 2 portc
end

to start_blue
   setbit 1 portc
end

to end_blue
   clearbit 1 portc
end

to start_red
   setbit 0 portc
end

to end_red
   clearbit 0 portc
end


;--------------------------------------------------------------
; Functions to flash and read 3 times for each color of the LED
; and then store away the measurement
;--------------------------------------------------------------
to set_green

	;------------------------------------
	; Green
	;------------------------------------
	start_green
	wait (slp_time * 2)
	seta read-ad 0
	wait slp_time
	setb read-ad 0
	wait slp_time
	setc read-ad 0
	setgreen (a + b + c) / 3
	end_green

end


to set_red

	;------------------------------------
	; Red
	;------------------------------------
	start_red
	wait (slp_time * 2)
	seta read-ad 0
	wait slp_time
	setb read-ad 0
	wait slp_time
	setc read-ad 0
	setred (a + b + c) / 3
	end_red
end


to set_blue

	;------------------------------------
	; Blue
	;------------------------------------
	start_blue
	wait (slp_time * 2)
	seta read-ad 0
	wait slp_time
	setb read-ad 0
	wait slp_time
	setc read-ad 0
	setblue (a + b + c) / 3
	end_blue
end

;---------------------------------------------------------
; Function to print out the 3 color values for the marble
;---------------------------------------------------------
to read_color

	set_green
		prs "Green
		cr
		print green
		cr
	set_red	
		prs "Red
		cr
		print red
		cr
	set_blue
		prs "Blue
		cr
		print blue
		cr	

end

;------------------------------------------------------------------
; After the color is read, test to see what it is and print it out
; to the screen
;------------------------------------------------------------------
to color_test
	
	
  ;------------------------------------------
  ; With this set of marbles, the red can be
  ; one of the main color determinates
  ;------------------------------------------
  if ((red > 990) and (blue < 1010)) [ setbit 1 portb prs "yellow]
  if (((red > 975) and (red < 1001)) and ((blue < 1012) and (blue > 1004)))[ setbit 2 portb prs "orange ]
  if (((red > 999) and (red < 1014)) and (blue < 1015)) [ setbit 3 portb prs "magenta]
  if (((red > 994) and (red < 1019)) and (blue < 1019)) [ setbit 4 portb prs "red]
  if (((red > 1019) and (red < 1021)) and (blue > 1014)) [ setbit 5 portb prs "green]
  if (((red > 1017) and (red < 1020)) and (blue > 1013)) [ setbit 6 portb prs "blue]

end


;--------------------------------------------------
; Function to initialize the register as an output
;--------------------------------------------------
to init
   setslp_time 4
   write portb-ddr 0
   write portc-ddr 0

   clearbit 6 portb
   clearbit 5 portb
   clearbit 4 portb
   clearbit 3 portb
   clearbit 2 portb 
   clearbit 1 portb

   tester
end

;------------------------------------
; The main function for this sensor
;------------------------------------
to tester
	read_color
	color_test
end

;--------------------------------------------------------------
;Now the main function - WAS NOT IMPLEMENTED AT BOTFEST!!!
;--------------------------------------------------------------
;to slave
;  print 1
;  waituntil [newbus?]
;  if brcv = $180 [test]
;  slave
;end