from controller import Robot
import math

robot = Robot()
TIME_STEP = 64

left_motor = robot.getDevice('left wheel motor')
right_motor = robot.getDevice('right wheel motor')
left_motor.setPosition(float('inf'))
right_motor.setPosition(float('inf'))
left_motor.setVelocity(0.0)
right_motor.setVelocity(0.0)

ball_sensor = robot.getDevice('ball sensor')
ball_sensor.enable(TIME_STEP)

sonar = robot.getDevice('sonar')
sonar.enable(TIME_STEP)

compass = robot.getDevice('compass')
compass.enable(TIME_STEP)

gps = robot.getDevice('gps')
gps.enable(TIME_STEP)

while robot.step(TIME_STEP) != -1:
    ball_value = ball_sensor.getValue()
    distance = sonar.getValue()
    position = gps.getValues()
    direction = compass.getValues()
    angle = math.atan2(direction[0], direction[1])
    
    if ball_value > 600:
        left_motor.setVelocity(6.0)
        right_motor.setVelocity(6.0)
    elif ball_value > 300:
        left_motor.setVelocity(4.0)
        right_motor.setVelocity(2.5)
    else:
        if angle > 0:
            left_motor.setVelocity(2.5)
            right_motor.setVelocity(-2.5)
        else:
            left_motor.setVelocity(-2.5)
            right_motor.setVelocity(2.5)

    if distance < 700:
        left_motor.setVelocity(-3.0)
        right_motor.setVelocity(3.0)

    if position[0] > 0.8:
        left_motor.setVelocity(-3.0)
        right_motor.setVelocity(3.0)
    if position[0] < -0.8:
        left_motor.setVelocity(3.0)
        right_motor.setVelocity(-3.0)

    if ball_value > 700 and abs(position[1]) < 0.2:
        left_motor.setVelocity(6.0)
        right_motor.setVelocity(6.0)