Option Explicit
' Going above the threshold detects a line
Public EdgeSensorLeftThreshHold as single
Public EdgeSensorRightThreshHold as single
Public EdgeDetected As boolean
DIM Stack1(1 TO 100) As Byte
DIM Stack2(1 TO 50) As Byte
PUBLIC Const samples As Integer = 3
Public LeftEye(1 to samples) As Byte
Public RightEye(1 to samples) as Byte
'Public EyeSampleCount As Byte
Public CurrentHeadPosition As Integer
Public Direction As Integer
Public Count As Integer
Public ScanCount As Integer
Public Const MaxScanCount as Integer=1

Public Sub Main()
    ScanCount = 0
    DIM diff as integer
		Dim bot as integer
    call delay(1.0)

    call init()
Call PutPin(StartPinNum , bxInputPullup)
    EdgeDetected = false
'    EyeSampleCount = 0
    CALL InitializePWM(4)
    ' Center the head
    CurrentHeadPosition = HeadMotorCenter 
    '.print "Start"
    Count =0
    Direction=1
    ' delay for 2 seconds
    CALLTASK "UpdateServo", Stack2

		bot = WaitForStart()
    call delay(1.0)

    ' delay for 2.5 seconds
    call delay(1.6)

    ' sample the edge sensors and set the threshold values
    Call FindThreshHold()

    ' Start the edge detector tasks
    CALLTASK "EdgeTest", Stack1
    
    ' Start off by going forward
    Call SetMotors(70,70,fwd)
    CurrentHeadPosition = HeadMotorCenter 
		If bot = 1 then
			Sleep(1.0)
			Call RotateHardRight()

		End If
		If bot = 2 Then
			Sleep(1.0)
			Call RotateHardLeft()

		End If
    do
      If EdgeDetected = False Then
        Call IRDistance()
       If IRDist > 90 Then
'          If IRDist > 110 Then
            'Debug.Print "H=" & cstr(CurrentHeadPosition) & ":D=" & cstr(IRDist)
            ScanCount = -1
            If (CurrentHeadPosition < (HeadMotorCenter - 200)) And (EdgeDetected = False) Then
              ' Spotted something on the left
              If EdgeDetected = False Then
                If HeadMotorCenter - CurrentHeadPosition > 400 Then
                  ' Debug.Print "Hard Right"
                  CurrentHeadPosition = HeadMotorCenter 
                  Call RotateLeft()
                Else
                  ' Debug.Print "Soft Right"
                  CurrentHeadPosition = HeadMotorCenter 
                  Call RotateSmallLeft()
                End If
                Call SetMotors(100,100,fwd)
              End If
            ElseIf (CurrentHeadPosition > HeadMotorCenter + 200) And (EdgeDetected = False) Then
              'Spotted something on the right
               If EdgeDetected = False Then
                  If CurrentHeadPosition - HeadMotorCenter > 400 Then
                    'Debug.Print "Hard Right"
                    CurrentHeadPosition = HeadMotorCenter 
                    Call RotateRight()
                  Else
                    'Debug.Print "Soft Right"
                    CurrentHeadPosition = HeadMotorCenter 
                    Call RotateSmallRight()
                  End If
               End If
               Call SetMotors(100,100,fwd)
            Else
              'Spotted something Straight Ahead
              If EdgeDetected = False Then
                'Debug.Print "Ram"
                Call SetMotors(100,100,fwd)
              End If
            End If
          Else
            If ScanCount = -1 Then
              ScanCount = MaxScanCount
            End If
            If EdgeDetected = False then
              Call SetMotors(70,70,fwd)
            End If
            Call Scan
          End if
        End If
    loop
End Sub

Public Function IsEdge() As Byte

End Function

Public Sub Scan()
    If ScanCount = -1 Then
        Exit Sub
    End If
    ScanCount = ScanCount + 1
    If ScanCount >= MaxScanCount Then
        ScanCount = 0
        CurrentHeadPosition = CurrentHeadPosition + (HeadMotorInc * Direction)
'debug.print cstr(CurrentHeadPosition)
        If CurrentHeadPosition >= HeadMotorMax Then
            Direction=-1
        End If
        If CurrentHeadPosition <= HeadMotorMin Then
            Direction=1
        End If 
    End If
'Debug.Print cstr(CurrentHeadPosition)
End Sub
Public Sub MoveServo( _
    ByVal ServoPin As Byte, _
    ByVal Position As Integer)
End Sub

Public Sub EdgeTest()
    ' This sub is designed to be run as a task.  Whenever the sensors detect the edge of the ring the 
    ' robot imediatly backs up and turns.  This process takes highest priority so that the robot does not
    ' run off the edge of the ring.
    Dim ResValue1 as single
    Dim ResValue2 as single
		count = 0
    Do
        ' Sample the Left Sensor
        CALL GETADC(EdgeSensorLeft,ResValue1)
        
        ' Sample the Right Sensor
        CALL GETADC(EdgeSensorRight,ResValue2)

        ' Test both sensors to determine if evasive action must be taken
        If (ResValue1 > EdgeSensorLeftThreshHold) or (ResValue2 > EdgeSensorRightThreshHold) Then
            count = count + 1
            If Count>2 Then
	       EdgeDetected = True
  	       Call Brake()
  	       If ResValue1 > EdgeSensorLeftThreshHold Then
     	         'debug.print "Left"
		 CurrentHeadPosition=HeadMotorCenter + 300
	         Call Backup()
  		 Call RotateRight()
  	       End If
     	       If ResValue2 > EdgeSensorRightThreshHold Then
       	         'debug.print "Right"
		 CurrentHeadPosition=HeadMotorCenter + 300
	         Call Backup()
	         Call RotateRight()
  		 Call RotateRight()
  	         'Call RotateLeft()
	       End If
            End If  
            EdgeDetected = False
                  
	Else
	    Count=0
            EdgeDetected = False
'           Call MoveServo(HeadMotor,CurrentHeadPosition)
        End If
        Sleep(0)
    Loop
End Sub

Public Sub Brake()
    Call SetMotors(100,100,rev)
    Call delay(0.25)
    Call SetMotors(0,0,rev)
End Sub

Public Sub BackUp()
    Call SetMotors(100,100,rev)
    Call delay(0.5)
    Call SetMotors(0,0,rev)
End Sub

Public Sub RotateRight()
    Call SetMotors(50,50,left)
    Call delay(0.25)
    Call SetMotors(0,0,left)
End Sub

Public Sub RotateLeft()
    Call SetMotors(50,50,right)
    Call delay(0.25)
    Call SetMotors(0,0,right)
End Sub
Public Sub RotateHardRight()
    Call SetMotors(50,50,left)
    Call delay(0.5)
    Call SetMotors(0,0,left)
End Sub

Public Sub RotateHardLeft()
    Call SetMotors(50,50,right)
    Call delay(0.5)
    Call SetMotors(0,0,right)
End Sub

Public Sub RotateSmallRight()
    Call SetMotors(50,50,left)
    Call delay(0.15)
    Call SetMotors(0,0,left)
End Sub

Public Sub RotateSmallLeft()
    Call SetMotors(50,50,right)
    Call delay(0.15)
    Call SetMotors(0,0,right)
End Sub

Public Sub UpdateServo()
    Do
        Call PulseOut(HeadMotor, CurrentHeadPosition, 1)
        call sleep(0.02)
    Loop
End Sub

Public Sub FindThreshHold()
    Dim x as integer
    Dim LeftSum as single
    Dim RightSum as single
    Dim ResValue1 as single
    Dim ResValue2 as single
    LeftSum = 0.0
    RightSum = 0.0
    For x = 1 to 20
        CALL GETADC(EdgeSensorLeft,ResValue1)
        LeftSum = LeftSum + ResValue1
        CALL GETADC(EdgeSensorRight,ResValue2)
        RightSum = RightSum + ResValue2
        debug.print "left=" & cstr(ResValue1) & ":" & "right=" & cstr(ResValue2)
    Next
    ' Find the Average
    EdgeSensorLeftThreshHold = LeftSum / 20.0
    EdgeSensorRightThreshHold = RightSum / 20.0
    Debug.Print "leftAvg=" & cstr(EdgeSensorLeftThreshHold) & ":" & "rightAvg=" & cstr(EdgeSensorRightThreshHold)

    ' Set the Threshold 50% higher
    	'EdgeSensorRightThreshHold = EdgeSensorRightThreshhold + EdgeSensorRightThreshhold * 0.5
		'EdgeSensorLeftThreshHold = EdgeSensorLeftThreshhold + EdgeSensorLeftThreshhold * 0.5
    ' Set the Threshold 50% higher
    EdgeSensorRightThreshHold = EdgeSensorRightThreshhold + EdgeSensorRightThreshhold * 0.9
    EdgeSensorLeftThreshHold = EdgeSensorLeftThreshhold + EdgeSensorLeftThreshhold * 0.9
    Debug.Print "leftThreshHold=" & cstr(EdgeSensorLeftThreshHold) & ":" & "rightThreshHold=" & cstr(EdgeSensorRightThreshHold)

End Sub

Public Sub IRSample()
    Dim x as integer
    Call IRDistance()
'   For x = 1 to samples - 1
'       LeftEye(x+1) = LeftEye(x)
'       RightEye(x+1) = RightEye(x) 
'   Next 
'   RightEye(1) = RightIRDist
'   LeftEye(1) = LeftIRDist
'    debug.print "sensor=" & cstr(IRDist)
End Sub

Public Sub GetAverage(byRef LeftValue as byte,byRef RightValue as byte)
    Dim LeftSum as Long
    Dim RightSum as Long
    LeftSum =0
    RightSum =0
    Dim x as integer
    For x = 1 to samples
        LeftSum = LeftSum + clng(LeftEye(x))
        RightSum = RightSum + clng(RightEye(x))
    Next
    'debug.print "leftsum=" & cstr(LeftSum)
    LeftValue = cbyte(LeftSum \ clng(samples))
    RightValue = cbyte(RightSum \ clng(samples))
End Sub

Public Sub EdgeTest2()
    Dim ResValue1 as single
    Dim ResValue2 as single
    CALL GETADC(EdgeSensorLeft,ResValue1)
    CALL GETADC(EdgeSensorRight,ResValue2)

    'debug.print "left=" & cstr(ResValue1) & ":" & "right=" & cstr(ResValue2)
End Sub

Public Function WaitForStart() As Integer
	  WaitForStart=0
    Do While (getpin(StartPinNum) = 1)
	     ' Look for opponent
       Call IRDistance()
debug.print "d=" ; cstr(IRDist)
       If IRDist > 90 Then
	 ScanCount=-1
	 if WaitForStart=0 Then
	   If CurrentHeadPosition>HeadMotorCenter Then
	     WaitForStart=1
	   Else
	     WaitForStart=2
	   End If
	 End If
       Else
         If ScanCount = -1 Then
           'ScanCount = MaxScanCount
         End If
         Call Scan
'call sleep(1.0)
       End If
    Loop
    Exit Function	  
End Function