'{$STAMP BS2}
'{$PBASIC 2.5}

right var bit
front var bit
left var bit
frequency con 38500

input 0
input 1
input 2
input 10
input 11

output 12
output 13
output 3
output 4
output 5

myCount var word
stackData var word

popNow var bit
prev var bit
new var bit
wheelCount var word

popNow = 0
myCount = 0
beat con 125
mini con 0
maxi con 1000

start:
  left = 0
  right = 0
  front = 1
  do while (left = 0 and right = 0 and front = 1)
	freqout 3, 1, frequency
	right = in0
	freqout 4, 1, frequency
	front = in1
	freqout 5, 1, frequency
	left = in2
	pulsout 12, 1
	pulsout 13, 1000
	pause 20
  loop
  
  if left = 1 and right = 1 and front = 1then:
	pulsout 12, 1
	pulsout 13, 1000
	pause 20
	stackData = 20
	gosub push
	stackData = 50
	gosub push
  endif

  if left = 0 and right = 1 and front = 1then:
	pulsout 12, 1
	pulsout 13, 1000
	pause 20
	stackData = 10
	gosub push
  endif

  if left = 1 and right = 0 and front = 1 then:
	pulsout 12, 1
	pulsout 13, 1000
	pause 20
	stackData = 20
	gosub push
  endif

  if left = 1 and right = 1 and front = 0 then:
	gosub turnLeft
	stackData = 50
	gosub push
  endif
  
  if left = 0 and right = 1 and front = 0then gosub turnRight
  
  if right = 0 and front = 0 then:
	gosub turnLeft
	freqout 4, 1, frequency
	front = in1
	if front = 0 then:
		popNow = 1
		gosub turnLeft
	endif
  endif
  
goto start



turnLeft:
  prev = in10
  wheelCount = 0
  do while (wheelCount < 7)
	pulsout 12, 1
	pulsout 13, 1
	new = in10
	if (new ^ prev) then:
		prev = new
		wheelCount = wheelCount + 1
	endif
	pause 20
  loop
return

turnRight:
  prev = in11
  wheelCount = 0
  do while (wheelCount < 7)
	pulsout 12, 1000 
	pulsout 13, 1000
	new = in11
	if (new ^ prev) then:
		prev = new
		wheelCount = wheelCount + 1
	endif
	pause 20
  loop
return

intersection:
  gosub pop
  if stackData = 10 then gosub turnLeft
  if stackData = 20 then gosub turnRight
return

peekTop:
  read myCount,stackData
return

pop:
  read myCount,stackData
  myCount = myCount-1
return

push:
  myCount = myCount+1
  write myCount,stackData
return



song:
FREQOUT 15, beat, 1760
FREQOUT 15, beat, 1760
FREQOUT 15, beat, 1760
FREQOUT 15, beat, 1976
FREQOUT 15, 250, 2093
PAUSE 250
FREQOUT 15, beat, 1760
FREQOUT 15, beat, 1760
FREQOUT 15, beat, 1760
FREQOUT 15, beat, 1976
FREQOUT 15, beat, 2093
FREQOUT 15, beat, 1976
FREQOUT 15, beat, 1760
pause beat
FREQOUT 15, beat, 1397
FREQOUT 15, beat, 1397
FREQOUT 15, beat, 1397
FREQOUT 15, beat, 1568
FREQOUT 15, 250, 1760
pause 250
FREQOUT 15, beat, 1397
FREQOUT 15, beat, 1397
FREQOUT 15, beat, 1397
FREQOUT 15, beat, 1568
FREQOUT 15, beat, 1760
FREQOUT 15, beat, 1568
FREQOUT 15, beat, 1397
pause beat
FREQOUT 15, beat, 1568
FREQOUT 15, beat, 1568
FREQOUT 15, beat, 1568
FREQOUT 15, beat, 1760
FREQOUT 15, 250, 1976
pause 250
FREQOUT 15, beat, 1568
FREQOUT 15, beat, 1568
FREQOUT 15, beat, 1568
FREQOUT 15, beat, 1760
FREQOUT 15, beat, 1976
FREQOUT 15, beat, 1760
FREQOUT 15, beat, 1568
pause beat
FREQOUT 15, beat, 1760
FREQOUT 15, beat, 1760
FREQOUT 15, beat, 1760
FREQOUT 15, beat, 1976
FREQOUT 15, 187, 2093
PAUSE 1500
GOTO start