# Webots Robot 1 Soccer Code (No classes/functions, no external libraries)
# Uses IR sensors for ball detection, compass for orientation, GPS for position, and sonar for obstacle avoidance

from controller import Robot

# Create the Robot instance
robot = Robot()

# Time step in milliseconds
time_step = int(robot.getBasicTimeStep())

# Initialize motors
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)

# IR sensors for ball detection
ir_sensors = []
for i in range(8):
    sensor = robot.getDevice(f"ir{i}")
    sensor.enable(time_step)
    ir_sensors.append(sensor)

# Compass
compass = robot.getDevice('compass')
compass.enable(time_step)

# GPS
gps = robot.getDevice('gps')
gps.enable(time_step)

# Sonar sensors for obstacle detection
sonar_sensors = []
for i in range(3):
    sonar = robot.getDevice(f"us{i}")
    sonar.enable(time_step)
    sonar_sensors.append(sonar)

# Movement parameters
max_speed = 6.28

# Main control loop
while robot.step(time_step) != -1:
    # Read IR sensors
    ir_values = [sensor.getValue() for sensor in ir_sensors]
    ball_detected = max(ir_values)
    ball_direction = ir_values.index(ball_detected)

    # Read compass
    compass_values = compass.getValues()
    angle = (180 / 3.14159) * (3.14159 - (3.14159 + compass_values[0]))

    # Read GPS
    position = gps.getValues()
    x = position[0]
    y = position[2]  # Webots uses x, y, z; y is vertical

    # Read sonar sensors
    front_distance = sonar_sensors[0].getValue()
    left_distance = sonar_sensors[1].getValue()
    right_distance = sonar_sensors[2].getValue()

    # Obstacle avoidance
    if front_distance < 800:
        left_motor.setVelocity(-0.5 * max_speed)
        right_motor.setVelocity(0.5 * max_speed)
        continue
    elif left_distance < 800:
        left_motor.setVelocity(0.5 * max_speed)
        right_motor.setVelocity(0.2 * max_speed)
        continue
    elif right_distance < 800:
        left_motor.setVelocity(0.2 * max_speed)
        right_motor.setVelocity(0.5 * max_speed)
        continue

    # Ball tracking logic
    if ball_detected > 50:
        if ball_direction in [0, 1]:
            left_motor.setVelocity(0.2 * max_speed)
            right_motor.setVelocity(0.5 * max_speed)
        elif ball_direction in [2, 3]:
            left_motor.setVelocity(0.5 * max_speed)
            right_motor.setVelocity(0.2 * max_speed)
        elif ball_direction in [4, 5]:
            left_motor.setVelocity(0.5 * max_speed)
            right_motor.setVelocity(0.5 * max_speed)
        elif ball_direction in [6, 7]:
            left_motor.setVelocity(0.5 * max_speed)
            right_motor.setVelocity(0.2 * max_speed)
    else:
        # Rotate to search for the ball
        left_motor.setVelocity(0.5 * max_speed)
        right_motor.setVelocity(-0.5 * max_speed)