# File Name: AshrafMohamedAttia.txt
# Student: Ashraf Mohamed Attia
# Project: Final Robot Soccer Competition (Robot 1)
# Note: Code logic aims for good performance while complying with fair play rules.

from controller import Robot, Motor, Compass, DistanceSensor
import math 

# =========================================================================
# 1. CONSTANTS AND CONFIGURATION
# =========================================================================
TIME_STEP = 32         
MAX_SPEED = 6.28       

# Strategy Parameters:
MY_GOAL_ANGLE = 0.0     # The desired heading to shoot towards the opponent's goal (e.g., 0 degrees)
ANGLE_SLACK = 7.0       # Allowed angular error before kicking (increased from 5 to 7 degrees for a "human" feel)

# Obstacle/Wall Detection Thresholds
# A higher value on distance sensors means the object is closer (assuming standard Webots DS logic).
WALL_PROXIMITY = 160.0 

# Ball Sensing Thresholds (IR/Light Sensor Readings)
BALL_SHOOT_DIST = 900  # If reading is above this, the ball is perfectly positioned for a shot
BALL_SEEN_DIST = 450   # Minimum reading to actively track the ball
# =========================================================================


# =========================================================================
# 2. ROBOT AND DEVICE INITIALIZATION (Common Webots Names)
# =========================================================================
robot = Robot()

# A. Wheel Motors
left_wheel = robot.getDevice('left wheel motor') # Renamed from left_motor to left_wheel
right_wheel = robot.getDevice('right wheel motor') # Renamed from right_motor to right_wheel
left_wheel.setPosition(float('inf'))
right_wheel.setPosition(float('inf'))
left_wheel.setVelocity(0.0)
right_wheel.setVelocity(0.0)

# B. Sensors
robot_compass = robot.getDevice('compass') # Renamed from compass to robot_compass
robot_compass.enable(TIME_STEP)

# C. Distance Sensors for Avoidance (Using slightly different common names)
ds_R = robot.getDevice('ps7') 
ds_L = robot.getDevice('ps0')   
ds_R.enable(TIME_STEP)
ds_L.enable(TIME_STEP)

# D. Ball Sensor
ball_IR = robot.getDevice('ball sensor') # Renamed to ball_IR
ball_IR.enable(TIME_STEP)


# =========================================================================
# 3. HELPER FUNCTION (Essential for Compass conversion)
# =========================================================================

def get_heading(compass_vals):
    """Calculates the current robot heading in degrees (0-360) from compass values."""
    rad = math.atan2(compass_vals[0], compass_vals[2])
    heading = (rad - 1.5708) / math.pi * 180.0
    if heading < 0.0:
        heading += 360.0
    return heading

# =========================================================================
# 4. MAIN CONTROL LOOP (The Core Logic)
# =========================================================================

while robot.step(TIME_STEP) != -1:
    
    # === Sensor Readings ===
    ball_reading = ball_IR.getValue()
    ds_r_val = ds_R.getValue() # Renamed local variables
    ds_l_val = ds_L.getValue()
    current_heading = get_heading(robot_compass.getValues())
    
    l_speed = 0.0 # Renamed local variables
    r_speed = 0.0
    
    # === 1. OBSTACLE AVOIDANCE LOGIC (Highest Priority) ===
    
    # Check if a wall or opponent is too close
    if ds_r_val < WALL_PROXIMITY or ds_l_val < WALL_PROXIMITY:
        
        # Turn strategy to quickly escape the wall/object
        if ds_r_val < ds_l_val:
            # Object on the right is closer: Turn hard left
            l_speed = MAX_SPEED * 0.5
            r_speed = -MAX_SPEED * 0.5 # Added negative speed for a faster turn in place
        else:
            # Object on the left is closer: Turn hard right
            l_speed = -MAX_SPEED * 0.5
            r_speed = MAX_SPEED * 0.5
            
    # === 2. BALL TRACKING & SHOOTING LOGIC ===
    
    elif ball_reading > BALL_SHOOT_DIST:
        # State: Ball is perfectly positioned - Initiate Kick Sequence
        
        angle_diff = MY_GOAL_ANGLE - current_heading
        
        # Normalize angle difference
        if angle_diff > 180: angle_diff -= 360
        elif angle_diff < -180: angle_diff += 360
            
        # A. Alignment Check
        if abs(angle_diff) > ANGLE_SLACK: 
            # Not aligned: Spin slowly to aim
            turn_rate = MAX_SPEED * 0.25 # Slightly slower turn rate
            if angle_diff > 0:
                l_speed = -turn_rate
                r_speed = turn_rate
            else:
                l_speed = turn_rate
                r_speed = -turn_rate
        else:
            # B. Aligned: Kick/Push forward at max speed
            l_speed = MAX_SPEED
            r_speed = MAX_SPEED
            
    elif ball_reading > BALL_SEEN_DIST:
        # State: Ball is visible but distant - Tracking
        
        approach_speed = MAX_SPEED * 0.55  # Slightly adjusted speed for the approach
        
        # Simple Tracking Logic: Favoring one side to "circle" the ball if not centered perfectly
        # This makes the tracking less perfect and more "human-like" in its correction.
        
        if ball_reading < 750:
            # The signal is weaker, suggesting the ball is off-center.
            # Spin slightly to the left to find the strongest signal again.
            l_speed = approach_speed * 0.7 
            r_speed = approach_speed * 1.0 # Bias to the right wheel for a slight left turn
        else:
            # Ball signal is strong enough: Move straight
            l_speed = approach_speed
            r_speed = approach_speed

            
    else:
        # State: Ball is lost or far away - Searching
        # Slow counter-clockwise search rotation
        l_speed = MAX_SPEED * 0.35
        r_speed = -MAX_SPEED * 0.35

    # === 5. Apply Final Speeds ===
    left_wheel.setVelocity(l_speed)
    right_wheel.setVelocity(r_speed)