# Dr.Khaled Eskaf
# Optimized & Enhanced by ShahadAli (IT Student)

import math
import utils
from rcj_soccer_robot import RCJSoccerRobot, TIME_STEP
from controller import Robot

robot_api = Robot()
robot = RCJSoccerRobot(robot_api)

GOAL_X = -0.8  # left goal
CENTER_POS = [0, 0]  # mid field position

def distance(a, b):
    return math.sqrt((a[0] - b[0])**2 + (a[1] - b[1])**2)

while robot_api.step(TIME_STEP) != -1:
    if robot.is_new_ball_data():
        ball_data = robot.get_new_ball_data()
    else:
        robot.left_motor.setVelocity(0)
        robot.right_motor.setVelocity(0)
        print("⛔ Ball not detected - stopping.")
        continue

    heading = robot.get_compass_heading()
    robot_pos = robot.get_gps_coordinates()
    sonar = robot.get_sonar_values()
    ball_pos = ball_data["coordinates"]
    dist_to_ball = distance(robot_pos, ball_pos)

    # --- obstacle detection ---
    obstacle_left = sonar[0] > 80 or sonar[1] > 80
    obstacle_right = sonar[6] > 80 or sonar[7] > 80

    # --- default speed ---
    left_speed = 6
    right_speed = 6

    # --- avoid opponent ---
    if obstacle_left:
        left_speed, right_speed = 6, -3
        print("🟡 Obstacle left → Avoiding RIGHT")
    elif obstacle_right:
        left_speed, right_speed = -3, 6
        print("🟡 Obstacle right → Avoiding LEFT")

    else:
        # --- direction to ball ---
        angle_to_ball = math.atan2(ball_pos[1] - robot_pos[1], ball_pos[0] - robot_pos[0])
        angle_diff = angle_to_ball - heading

        # normalize angle
        angle_diff = (angle_diff + math.pi) % (2 * math.pi) - math.pi

        if abs(angle_diff) > 0.2:
            # rotate smoothly toward ball
            turn_speed = 5 * angle_diff
            left_speed = 6 - turn_speed
            right_speed = 6 + turn_speed
            print("🎯 Turning toward ball")
        else:
            # go forward
            left_speed = right_speed = 8 if dist_to_ball > 0.3 else 4
            print(f"🚀 Moving toward ball | Dist: {dist_to_ball:.2f}")

        # --- align & shoot ---
        if dist_to_ball < 0.25:
            goal_angle = math.atan2(0 - robot_pos[1], GOAL_X - robot_pos[0])
            goal_diff = goal_angle - heading
            goal_diff = (goal_diff + math.pi) % (2 * math.pi) - math.pi

            if abs(goal_diff) > 0.25:
                left_speed = 6 - 6 * goal_diff
                right_speed = 6 + 6 * goal_diff
                print("🎯 Aligning with goal")
            else:
                left_speed = right_speed = 10
                print("⚡ Shooting toward goal!")

    # --- apply speed ---
    robot.left_motor.setVelocity(max(-10, min(10, left_speed)))
    robot.right_motor.setVelocity(max(-10, min(10, right_speed)))

    # --- send team data ---
    robot.send_data_to_team(robot.player_id)