# Modified Robot 1 Code - Intelligent Soccer Bot
# Name: FirstNameLastName

from controller import Robot

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

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

# Sensors
ir = []
for i in range(7):
    sensor = robot.getDevice("ir" + str(i))
    sensor.enable(timestep)
    ir.append(sensor)

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

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

sonar_left = robot.getDevice("sonar_left")
sonar_right = robot.getDevice("sonar_right")
sonar_left.enable(timestep)
sonar_right.enable(timestep)

# Speed settings
MAX_SPEED = 6.0
TURN_SPEED = 3.0

def get_ball_direction():
    """Return ball direction index (0-6) or -1 if not seen"""
    max_val = 0
    idx = -1
    for i in range(len(ir)):
        val = ir[i].getValue()
        if val > max_val:
            max_val = val
            idx = i
    if max_val < 5:  # threshold to ignore noise
        idx = -1
    return idx

def get_heading():
    """Return heading in degrees (0 is north, clockwise positive)"""
    north = compass.getValues()
    rad = -1.5708 - (north[0] + north[2])  # adjust based on Webots compass orientation
    deg = (rad * 180.0 / 3.14159) % 360
    return deg

def set_speed(left, right):
    left_motor.setVelocity(max(min(left, MAX_SPEED), -MAX_SPEED))
    right_motor.setVelocity(max(min(right, MAX_SPEED), -MAX_SPEED))

while robot.step(timestep) != -1:
    ball_dir = get_ball_direction()
    heading = get_heading()

    # Basic obstacle avoidance
    if sonar_left.getValue() < 0.15:  # obstacle on left
        set_speed(-TURN_SPEED, TURN_SPEED)
        continue
    if sonar_right.getValue() < 0.15:  # obstacle on right
        set_speed(TURN_SPEED, -TURN_SPEED)
        continue

    # Ball tracking
    if ball_dir == -1:
        # Search for ball
        set_speed(TURN_SPEED, -TURN_SPEED)
    elif ball_dir in [3]:
        # Ball in front
        set_speed(MAX_SPEED, MAX_SPEED)
    elif ball_dir in [0, 1, 2]:
        # Ball to left
        set_speed(TURN_SPEED, -TURN_SPEED)
    elif ball_dir in [4, 5, 6]:
        # Ball to right
        set_speed(-TURN_SPEED, TURN_SPEED)

    # Optional: Align toward opponent goal when very close to ball
    # This assumes your robot is Yellow, facing north initially
    if ball_dir == 3 and sonar_left.getValue() > 0.25 and sonar_right.getValue() > 0.25:
        if heading > 10 and heading < 180:
            set_speed(TURN_SPEED, -TURN_SPEED)
        elif heading > 180 and heading < 350:
            set_speed(-TURN_SPEED, TURN_SPEED