;Goals as of 4-4-07
;- Should safeguard against case where it thinks it sees the candle but didn't. Could edge-trigger it on the front sensor
;  to not let it go more than 360 deg.
;- We still have an occasional issue with infinite 360s in check-for-candle b/c too far from wall.

;the two errors being, no safeguard on the second sweep for the candle, 
;and the staircase problem. Hopefully is now solved. Probably not.

menu 1 [navigate]

menu 2 [put-out-candle]

global [kill-drive kill-candle over-white
        front-left-val back-left-val
        front-right-val back-right-val front-val front-val-2
	candle-found front-white-thr turn-count turn-count-2
	time time-up backup-count] 


to navigate
  setcandle-found 0
  setkill-drive 0
  setkill-candle 1
  setover-white 0
  setturn-count 1
  setturn-count-2 1
  settime 0
  setbackup-count 0
  setkill-drive 0
  setfront-val front-sensor
  setfront-val-2 front-sensor-2
  setfront-white-thr front-white
  setfront-left-val front-left-sensor
  setfront-right-val front-right-sensor
  setback-left-val back-left-sensor
  setback-right-val back-right-sensor
  settime-up 0
  both-motors thisway setpower 8 on
  setkill-drive 0
  forever [drive]
  waituntil [(front-wall)] ;used to be in drive
  forever [watch-for-white]
end

to drive
  ifelse kill-drive 
    []
    [ifelse (front-too-close) 
      [ifelse and (front-sensor-2 > 40) (front-sensor < 35)
         [left-motor setpower 5 both-motors thatway onfor 4 both-motors setpower 8 off
          left-motor thisway right-motor thatway
          launch [time-limit 30]
          both-motors on wait 5
          waituntil [or time-up (not front-wall-2)]
   	  align-with-hallway
         ]
  	 [backup]
      ]
      [ifelse miss-left
        [turn-left]
        [ifelse (miss-right)
          [wait 2
           if (left-and-front)
      	   [right-90] ;Changed to right-90 from turn-right by Sarah 4-3-07. 
          ]
 	  [align-with-hallway]
        ]
      ]
    ]
end

to left-and-front
  output and (front-left-sensor > 40) (front-wall)
end

to watch-for-white
  if (not over-white)
    [if (white-sensor)
	[setover-white 1]
    ];enqueue seeing white
  if over-white ;SLIGHTLY FUNKY 4-12
    [
     setkill-drive 1
     go-forward
     waituntil [or front-too-close (not white-sensor)] ;added (re-added?) not 4-11.
     setover-white 0
     room-left
     check-for-candle
    ]
end

to check-for-candle
  both-motors off
  setkill-drive 1
  both-motors thisway onfor 2        ;USED TO BE 3
  setkill-candle 0
  launch [find-candle]
  if (front-too-close) [backup]
  right-motor setpower 2 thisway on
  left-motor setpower 2 thatway on
  waituntil [front-wall]
  right-motor thatway
  left-motor thisway
  wait 10
  waituntil [right-aligned]
  both-motors setpower 8 off 
  setkill-candle 1
  ifelse candle-found [put-out-candle]
	[follow-right]
end

to turn-left
  setturn-count 1
  right-motor setpower 8 on
  left-motor setpower 5 on
  loop [
    if (kill-drive) [stop] ;added 3/30
    if (turn-count > 25) [both-motors thatway onfor 4 both-motors setpower 8 off left-motor thisway right-motor thatway launch [time-limit 30]
                          waituntil [or time-up (not front-wall-2)] align-with-hallway stop]
    setfront-left-val front-left-sensor
    setback-left-val back-left-sensor
    if front-too-close [ 
      setfront-val front-sensor
      ifelse (front-val < 35)
         [left-motor setpower 5 both-motors thatway onfor 4 both-motors setpower 8 off
          left-motor thisway right-motor thatway
          launch [time-limit 30]
          both-motors on wait 5
          waituntil [or time-up (not front-wall-2)]
          align-with-hallway
          stop]
  	 [reverse-right-motor 1 wait 1] ;CHANGED 4-10 for circling center.
      ]
    reverse-left-motor 3
    wait 2   ;UP FROM 1 4-7-07 BY SARAH
    if (or (front-left-val > 40) (back-left-val > 40)) [both-motors setpower 8 stop] ;NEW MOTORS
    setturn-count (turn-count + 1)
    if (turn-count \\ 3 = 0) [wait 3] ;note: the order of these last two lines is IMPORTANT.
  ]
end

to time-limit :time-end
  settime 0
  settime-up 0
  loop [
    wait 1
    settime (time + 1)
    if (time = :time-end) [settime-up 1 stop]
   ]
end

to front-wall-basic
  setfront-val front-sensor
  output (front-val > 55)
end

to room-left
  setkill-drive 1
  go-forward
  setturn-count 1
  loop[
    if (turn-count \\ 5 = 0) [pause-right-motor 1]
    pause-left-motor 2
    wait 1
    if (front-white < (front-white-thr + 20)) [both-motors off beep wait 1 beep backup put-out-candle] ;SLIGHTLY FUNKY
    if (front-left-sensor > 57) [both-motors off stop]
    if (front-sensor > 85) [left-motor off reverse-right-motor 6 go-forward]
    setturn-count (turn-count + 1)
    ]
end

to right-90
  left-motor thisway on
  right-motor thatway on
  waituntil [(not front-wall)] 
  right-motor thisway
end

to align-with-hallway
  if (kill-drive) [stop]
  setfront-left-val front-left-sensor
  setback-left-val back-left-sensor
  if (and (front-left-val < 100) (back-left-val < 100)) ;ALIGNING WITH LEFT SORTA
    [if (back-left-val - front-left-val) > 7 [pause-left-motor 1] 
     if (front-left-val - back-left-val) > 7 [pause-right-motor 1]
    ]
  setfront-right-val front-right-sensor
  setback-right-val back-right-sensor
  if (and (front-right-val > 50) (back-right-val > 50)) ;ALIGNING WITH right SORTA
    [if (back-right-val - front-right-val) > 7 [pause-right-motor 1] 
     if (front-right-val - back-right-val) > 7 [pause-left-motor 1]
    ]
  if (and (front-right-val > 35) (front-left-val > 35))
    [if not (between (front-right-val - front-left-val) (0 - 30) 30)
      [if front-left-val > front-right-val [pause-right-motor 1]
       if front-right-val > front-left-val [pause-left-motor 1]
      ]
    ]
end

;USED IN ROOMS ONLY.
to follow-right
  setbackup-count 0
  go-forward
  loop [
    if (not over-white)
      [if (white-sensor)
	[setover-white 1]
      ];enqueue seeing white
    if (over-white)
        [
         waituntil [not white-sensor]
	 setover-white 0 
         waituntil [front-too-close-2] ;get a little further out of the room
	 left-motor thatway ;start 90-degree left turn
	 waituntil [not front-wall] ;stop turning when you have turned 90 degrees
         left-motor thisway ;start going straight again
	 loop [
           setfront-left-val front-left-sensor
	   if (front-left-val > 40) [wait 1 if (front-left-sensor > 40) [setkill-drive 0 stop]] 
	   if (and (front-right-sensor > 35) (front-right-sensor - back-right-sensor > 7)) [pause-left-motor 1]
      	   if (and (front-right-sensor > 35) (back-right-sensor - front-right-sensor > 7)) [pause-right-motor 1]
         ]
    ]
    if (front-right-sensor > 35) 
    [
      if front-right-sensor - back-right-sensor > 8 [pause-left-motor 1]
      if back-right-sensor - front-right-sensor > 8 [pause-right-motor 1]
      if (front-too-close) [
        ifelse (backup-count < 3) 
          [both-motors thatway wait 10 both-motors thisway setbackup-count (backup-count + 1)]
 	  [setover-white 1]
      ]
    ]
  ]
end

to find-candle
  loop[
    if (not candle-found) [
      if (candle-A)
        [setcandle-found 1]
    ]
    if kill-candle [stop]
  ]
end

to put-out-candle
  setturn-count 1
  setturn-count-2 1
  setkill-drive 1
  right-motor setpower 3 thisway on ;reverse 180 till see the candle.
  left-motor setpower 3 thatway on
  setcandle-found 1
  launch [candle-check]
  waituntil [or (not candle-found) (candle-A)]
  both-motors off wait 1
  if (not candle-found) 
    [left-motor thisway on right-motor thatway on 
     both-motors setpower 8 wait 10 waituntil [right-aligned] follow-right stop]

  ;both B and C means snuff
  ;B but not C means go left
  ;C but not B means go right
  ;neither B nor C means go straight!

  loop [
    if (or (turn-count > 70) (turn-count-2 > 70)) [panic-exit stop]
    ifelse (and candle-B candle-C) ;snuff!
      [setkill-drive 1 
	ifelse (white-sensor)
	[ ;are over the white circle.
	  both-motors thatway on
	  waituntil [not white-sensor]
	  both-motors off
	  wait 2
	  both-motors setpower 2 thisway on ;SETPOWER NEW; MAYBE DON'T NEED
	  waituntil [white-sensor]
	  wait 2
	  both-motors off
	  b, thisway setpower 4 on
	  waituntil [switch 11] 
          b, off
          wait 20
	  c, thisway onfor 70
	  wait 20
	  c, thatway onfor 78
          b, setpower 3 onfor 7
	  both-motors off
          setkill-drive 1
	  wait 1000 beep wait 1 beep wait 1 beep wait 1 beep
	  both-motors off stoprules stop
         ]
	[ ;are not over the white circle. try again!
	  both-motors thisway onfor 1 wait 1
	]
      ]
      [ifelse (or candle-B candle-C) ;can't be both since we are in the first else
	[if candle-C [right-motor thisway onfor 1 wait 1 setturn-count (turn-count + 1)] 
	 if candle-B [left-motor thisway onfor 1 wait 1 setturn-count-2 (turn-count-2 + 1)]
	 ] 
    	[both-motors onfor 1 wait 1] ;go straight!
      ]
  ]
end

;added 4-13
to candle-check
  wait 10
  loop [
    if (candle-A) [stop]
    if (left-aligned) [setcandle-found 0 stop]
  ]
end

to panic-exit
  right-motor thatway setpower 8 on
  left-motor thisway setpower 8 on
  waituntil [right-aligned]
  follow-right
end

to right-aligned
  ifelse (back-right-sensor > 54) ;lowering to 50 or below messes up 180 degree turn.
    [output right-align-step]
    [output 0]
end

to right-align-step
  output between (front-right-sensor - back-right-sensor) (0 - 7) 7
end

to left-aligned
  ifelse (back-left-sensor > 45) ;To prevent 90-degree right turn that then doesn't see the wall. 4-4-07
    [output left-align-step]
    [output 0]
end

to left-align-step
  output between (front-left-sensor - back-left-sensor) (0 - 7) 7
end

to pause-left-motor :wait-time
  left-motor off
  wait :wait-time
  left-motor on
end

to pause-right-motor :wait-time
  right-motor off
  wait :wait-time
  right-motor on
end

to reverse-left-motor :wait-time
  left-motor thatway
  wait :wait-time
  left-motor thisway
end

to reverse-right-motor :wait-time
  right-motor thatway
  wait :wait-time
  right-motor thisway
end

to backup
  go-backward 3
  go-forward
end

to go-backward :time
  both-motors thatway onfor :time
end

to go-forward
  both-motors thisway on
end

to between :num :min :max
  output and (:num > :min) (:num < :max)
end

to miss-left
  setfront-left-val front-left-sensor
  output (front-left-val < 35)
end

to miss-right
  setfront-right-val front-right-sensor
    output (front-right-val < 35)
end

to front-wall
  setfront-val front-sensor
  setfront-val-2 front-sensor-2
  ifelse (front-val > 33)
    [output (front-val > 55)]
    [output (front-val-2 > 60)]
end

to front-wall-2
  setfront-val-2 front-sensor-2
  output (front-val-2 > 60)
end

to front-too-close
  setfront-val front-sensor
  setfront-val-2 front-sensor-2
  ifelse (front-val > 32)
    [output (front-val > 77)] ;Down from 90 3/17/07
    [output (front-val-2 > 100)] ;need to actually check this value. DIFF VAL
end

to front-too-close-2
  setfront-val-2 front-sensor-2
  output (front-val-2 > 100) ;DIFF VAL
end

;MOTORS
to right-motor
  a,
end

to left-motor
  d,
end

to both-motors
  ad,
end

;SENSORS

to candle-A
  output switch 7 ;see candle?
end

to candle-B
  output switch 8 ;left!
end

to candle-C
  output switch 9 ;right!
end

to front-right-sensor
  output sensor 0
end

to back-right-sensor
  output sensor 1
end

to front-left-sensor
  output sensor 2
end

to back-left-sensor
  output sensor 4
end

to front-sensor
  output sensor 3
end

to front-sensor-2
  output sensor 5
end

to white-sensor
  output switch 10
end

to front-white
  output sensor 6
end