;m is threshold for pin 0
;n is threshold for pin1
;pi2t is threshold for pin 2
;pin a3 left
;pin a1 right
;pin a2 center
;pin a0, b3 is white
;pin a5 is triangulation

global [whitet cthresh lthresh rthresh tthresh]

to test
  ;port 0 is right, port 1 left, port 2 is front
  clearbit 0 portb-ddr
  clearbit 1 portb-ddr
  clearbit 2 portb-ddr
  clearbit 3 portb-ddr
  clearbit 4 portb-ddr

  setbit 0 portb
  setbit 1 portb
  setbit 2 portb
  setbit 3 portb
  setbit 4 portb  
  
  setcthresh 1
  setrthresh 1
  setlthresh 1
  settthresh 1

  setm read-ad 3 ;auto-threshold right sensor
  setn read-ad 1 ;auto-threshold left sensor
  setwhitet read-ad 0 ;auto-threshold white line dealy.

  ;could enqueue candle-sensing the first time around? how to dequeue it though?
  ;also, may be important to setbit before clearbit to avoid accidental snuffing!

  ;turned threshold WAY up 4-8 to prevent accidental candle-seeing. Had been 200.
  loop[
    setrthresh read-ad 3
    setlthresh read-ad 1
    ifelse (((rthresh) < (m - 300)) or ((lthresh) < (n - 300)))
      [;One of the side sensors saw the candle - tell Perseus to stop turning to look for it.
       clearbit 0 portb]
      [setbit 0 portb] ;make use of this in HandyLogo code to take care of getting off course while candle-searching
    ifelse (((lthresh) - (rthresh)) > 7) ;left > right, go right!
      [setbit 2 portb
       clearbit 1 portb] ;set B to 1 and C to 0
      [ifelse (((rthresh) - (lthresh)) > 7) ;right > left, go left! Was 20 for some reason? otherwise goes left for most of the time
	[setbit 1 portb
	 clearbit 2 portb] ;set B to 0 and C to 1
        [setcthresh read-ad 2
         settthresh read-ad 5
         ifelse ((cthresh) < 40) ;If time to snuff, do so. upped to 30 to prevent short-candle-doom-ness.
          [clearbit 1 portb
	   clearbit 2 portb] ;Snuff!
	  [setbit 1 portb
 	   setbit 2 portb] ;go straight!
         ]
       ]
    ifelse ((read-ad 0) < (whitet + 200))
      [clearbit 3 portb wait 2]
      [setbit 3 portb]
  ]
end

to startup ;used to be powerup
  test
end