from controller import Robot

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

# Sensors
ir = robot.getDevice("ir_sensor")
ir.enable(timestep)

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

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

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

# Motors
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 detection
    ball_strength = ir.getValue()
    
    # Compass direction
    north = compass.getValues()
    heading = north[0]  # Simplified heading
    
    # GPS position
    position = gps.getValues()
    x = position[0]
    y = position[1]
    
    # Obstacle detection
    obstacle = sonar.getValue()
    
    # Movement logic
    if ball_strength > 500:
        # Ball is close
        left_motor.setVelocity(MAX_SPEED)
        right_motor.setVelocity(MAX_SPEED)
    elif obstacle < 800:
        # Avoid obstacle
        left_motor.setVelocity(-0.5 * MAX_SPEED)
        right_motor.setVelocity(0.5 * MAX_SPEED)
    else:
        # Search for ball
        left_motor.setVelocity(0.5 * MAX_SPEED)
        right_motor.setVelocity(-0.5 * MAX_SPEED)