Option Explicit

public sub RunMaze()
dim count as byte
dim CornerCount as byte
dim LastBump as byte
dim t as single
dim s as string

	cleardisplay
	call display("3  ")
	delay(1.0)
	call display("2  ")
	delay(1.0)
	call display("1  ")
	delay(1.0)
	cleardisplay
	call display("Activate...")
	running = 1
	CornerCount = 0
	LastBump = 0
	if Mode = modeRun then
		FloorSensorsOn
	end if
	startmotors
	call PutTime(0,0,0.0)							'use the timer for cornerCounts
	ptr = 0
	do
		if BothBumpersPressed then
			StopMotors
			Backwards
			delay(0.3)
			TurnLeft
			delay(0.6)
			StartMotors
			CornerCount = CornerCount + 1
			if LastBump = Left then
				LastBump = Right
			else
				LastBump = Left
			end if
			call PutTime(0,0,0.0)					'reset the timer
		elseif LeftBumperPressed then
			StopMotors
			backwards
			delay(0.2)
			TurnRight
			delay(0.5)
			startmotors
			if LastBump = right then				'test to see if we're in a corner
				LastBump = Left
				CornerCount = CornerCount + 1
			else
				LastBump = Left
				CornerCount = 0
			end if
			call PutTime(0,0,0.0)					'reset the timer
		elseif RightBumperPressed then
			StopMotors
			backwards
			delay(0.2)
			TurnLeft
			delay(0.5)
			startmotors
			if LastBump = left then				'test to see if we're in a corner
				LastBump = right
				CornerCount = CornerCount + 1
			else
				LastBump = right
				CornerCount = 0
			end if
			call PutTime(0,0,0.0)					'reset the timer
		end if

		if mode = modeRun then		
			if LeftFloor then							'is the floor white?
				delay(0.5)							'go forward a few inches
				if LeftFloor then						'check the floor again
					calltask "RedBlink",LEDStack		'start blinking the LED
					if (LeftBumperPressed) or (RightBumperPressed) then
						backwards						'move back a little
						delay(0.3)
						stopmotors
					end if
					TurnRight							'victory spin
					cleardisplay
					call display("Victory  ")
					delay(1.0)
					call display("Victory")
					delay(1.0)
					call setcursor(1,0)
					call display("Victory  ")
					delay(1.0)
					call display("Victory")
					exit do	
				end if
			end if
		end if
			
		t = Timer()
		if t > 0.6 then							'if 1 second has elasped since a bumper hit
			CornerCount = 0						'reset the counter to avoid false triggers
			SaveTime(0)		
			call PutTime(0,0,0.0)					'reset the timer
		end if

		if CornerCount >= 6 then
			backwards								'we must be in a corner so turn around
			delay(0.3)
			TurnLeft
			delay(0.7)							'turn 180 degrees
			startmotors
			CornerCount = 0						'reset the counter
			LastBump = 0
			call PutTime(0,0,0.0)					'reset the timer
		end if
	loop

	stopmotors
	running = 0
	FloorSensorsOff
	delay(3.0)
end sub

'------ Commands you can use ------------------------------------------------

'	StartMotors			both motors go forward
'	Backwards				both motors go backwards
'	StopMotors			both motors stop
'	TurnLeft				robot spins left
'	TurnRight				robot spins right
'	Delay				program delays the number of seconds you specify
'	ClearDisplay			clears the display
'	Display				sends a message to the display
'	LeftBumperPressed		tests the left bumper
'	RightBumperPressed		tests the right bumper
'	BothBumpersPressed		tests for both bumpers


sub SaveTime(byval bump as byte)
	if ptr >= 20 then
		ptr = 1						'wrap the pointer
	else
		ptr = ptr + 1					'inc the pointer
	end if
	data(ptr) = timer()				'get the elasped time
	whichBump(ptr) = bump				'save bumper
end sub

sub DumpTime()
dim a as integer
dim s as string
	for a = 1 to ptr
		select case WhichBump(a)
			case Left
				Debug.Print "Left   ";
			case Right
				Debug.Print "Right  ";
			case Both
				Debug.Print "Both   ";
			case 0
				Debug.Print "Reset  ";
		end select
		s = CStr(data(a))
		Debug.Print s 
	next
end sub