from controller import Robot

robot = Robot()
timestep = int(robot.getBasicTimeStep())

# Devices
ir = robot.getDevice("ir")
ir.enable(timestep)

compass = robot.getDevice("compass")
compass.enable(timestep)

gps = robot.getDevice("gps")
gps.enable(timestep)

sonar = robot.getDevice("sonar")
sonar.enable(timestep)

left_motor = robot.getDevice("left_motor")
right_motor = robot.getDevice("right_motor")
left_motor.setPosition(float('inf'))
right_motor.setPosition(float('inf'))
left_motor.setVelocity(0.0)
right_motor.setVelocity(0.0)

# Constants
MAX_SPEED = 6.28

while robot.step(timestep) != -1:
    ball_direction = ir.getValue()
    compass_values = compass.getValues()
    gps_values = gps.getValues()
    sonar_value = sonar.getValue()

    # Basic obstacle avoidance
    if sonar_value < 800:
        left_motor.setVelocity(-0.5 * MAX_SPEED)
        right_motor.setVelocity(0.5 * MAX_SPEED)
        continue

    # Ball tracking logic
    if ball_direction < 512:
        left_motor.setVelocity(0.5 * MAX_SPEED)
        right_motor.setVelocity(1.0 * MAX_SPEED)
    elif ball_direction > 512:
        left_motor.setVelocity(1.0 * MAX_SPEED)
        right_motor.setVelocity(0.5 * MAX_SPEED)
    else:
        left_motor.setVelocity(1.0 * MAX_SPEED)
        right_motor.setVelocity(1.0 * MAX_SPEED)

    # Field boundary check
    x = gps_values[0]
    y = gps_values[1]
    if abs(x) > 0.7 or abs(y) > 0.5:
        left_motor.setVelocity(-0.5 * MAX_SPEED)
        right_motor.setVelocity(-0.5 * MAX_SPEED)