# robot1.py

# Import necessary Webots libraries
from controller import Robot, Compass, GPS, Motor, Emitter, Receiver, DistanceSensor

# Define constants
TIME_STEP = 32
MAX_SPEED = 6.28
BALL_THRESHOLD = 0.5  # Distance threshold for the ball to be considered "close"
GOAL_X_BLUE = -0.7  # X coordinate of the blue team's goal
GOAL_X_YELLOW = 0.7  # X coordinate of the yellow team's goal

# Initialize robot and sensors
robot = Robot()

# Get the name of the robot to determine its team
robot_name = robot.getName()
is_blue_team = 'blue' in robot_name

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

# Initialize ball sensor (IR distance sensor)
ball_sensor = robot.getDevice('ball sensor')
ball_sensor.enable(TIME_STEP)

# Initialize GPS
gps = robot.getDevice('gps')
gps.enable(TIME_STEP)

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

# Initialize sonar sensors for obstacle avoidance
sonar_names = ['sonar_0', 'sonar_1', 'sonar_2', 'sonar_3', 'sonar_4', 'sonar_5', 'sonar_6', 'sonar_7']
sonars = [robot.getDevice(name) for name in sonar_names]
for sonar in sonars:
    sonar.enable(TIME_STEP)

# Function to calculate robot's bearing
def get_bearing_in_degrees():
    # Read compass values
    north = compass.getValues()
    # Calculate bearing
    rad = math.atan2(north[0], north[2])
    bearing = (rad - 1.5708) / math.pi * 180.0
    if bearing < 0.0:
        bearing += 360.0
    return bearing

# Function to get ball's relative position
def get_ball_position():
    # The ball sensor gives distance to the ball
    distance_to_ball = ball_sensor.getValue()
    # The direction of the ball is determined by which sonar sensors are triggered
    # (This is a simplified approach, a more advanced bot would use a camera)
    # Here, we'll just use the ball sensor distance
    return distance_to_ball

# Main control loop
while robot.step(TIME_STEP) != -1:
    
    # Get robot's current position and bearing
    robot_pos = gps.getValues()
    
    # Get distance to the ball
    ball_distance = get_ball_position()

    # Obstacle avoidance check
    front_obstacle = False
    for i in range(2, 6):  # Sonars 2-5 cover the front area
        if sonars[i].getValue() < 0.1:  # Check for a close obstacle
            front_obstacle = True
            break
            
    # Basic logic for the robot's behavior
    if ball_distance < BALL_THRESHOLD and not front_obstacle:
        # The ball is close, try to score!
        left_speed = MAX_SPEED
        right_speed = MAX_SPEED
        # The robot just drives forward to push the ball
        
    else:
        # Ball is not close, try to find and approach it
        if front_obstacle:
            # An obstacle is in front, turn to avoid it
            left_speed = -MAX_SPEED * 0.5
            right_speed = MAX_SPEED * 0.5
        else:
            # No obstacle, drive towards the ball
            # This is a basic "seek" behavior. A more advanced bot would use a camera.
            left_speed = MAX_SPEED
            right_speed = MAX_SPEED
            
    # Set motor velocities
    left_motor.setVelocity(left_speed)
    right_motor.setVelocity(right_speed)