# Dr.Khaled Eskaf
#Professor of Intelligent Systems
#khaled.eskaf@mail.mcgill.ca


# rcj_soccer_player controller - ROBOT B1
# وحدة التحكم بالروبوت B1

# ----------------------------
# Import the Webots Robot API
# استيراد واجهة برمجة التطبيقات الخاصة بـ Webots للتحكم بالروبوت
from controller import Robot, Motor, BallSensor, DistanceSensor, Compass, GPS
import math

# ----------------------------
# Import helper function to decide direction
# استيراد ملف utils الذي يحتوي على دالة تساعد الروبوت في تحديد اتجاه الكرة
# بما أننا لا نستخدم RCJSoccerRobot، سنقوم بتضمين المنطق مباشرة.
# import utils

# ----------------------------
# Import base robot configuration (sensors, motors, etc.)
# استيراد الكلاس RCJSoccerRobot الذي يربط بين الحساسات والمحركات
# واستيراد TIME_STEP لتحديد الزمن بين كل خطوة في الحلقة
# سنقوم بالاستغناء عن هذا الكلاس واستخدام الأجهزة مباشرة لضمان التوافق
# from rcj_soccer_robot import RCJSoccerRobot, TIME_STEP
TIME_STEP = 32

# ----------------------------
# Create an instance of the Robot class from Webots
# إنشاء كائن من كلاس Robot لتوصيل الكود مع الروبوت الموجود في المحاكاة
robot = Robot()

# ----------------------------
# Define constants and thresholds for a smarter robot
# تعريف الثوابت والعتبات لسلوك روبوت أكثر ذكاءً
MAX_SPEED = 10.0
FORWARD_SPEED = 8.0
TURN_SPEED = 4.0
KICK_SPEED = 10.0
SONAR_THRESHOLD = 0.5  # Sonar threshold in meters
AIMING_TOLERANCE = 0.2  # Tolerance for aiming in radians

# ----------------------------
# Get motor and sensor devices
# الحصول على المحركات وأجهزة الاستشعار
left_motor = robot.getDevice('left wheel motor')
right_motor = robot.getDevice('right wheel motor')
left_motor.setPosition(float('inf'))
right_motor.setPosition(float('inf'))

ball_sensor = robot.getDevice('ball sensor')
compass = robot.getDevice('compass')
gps = robot.getDevice('gps')
ps0 = robot.getDevice('ps0') # Sonar Front-right
ps7 = robot.getDevice('ps7') # Sonar Front-left

# ----------------------------
# Enable sensors to receive data
# تفعيل أجهزة الاستشعار لاستقبال البيانات
ball_sensor.enable(TIME_STEP)
compass.enable(TIME_STEP)
gps.enable(TIME_STEP)
ps0.enable(TIME_STEP)
ps7.enable(TIME_STEP)

# ----------------------------
# Helper functions for navigation logic
# دوال مساعدة لمنطق الملاحة
def get_compass_heading():
# الحصول على اتجاه الروبوت من البوصلة
compass_values = compass.getValues()
rad = math.atan2(compass_values[0], compass_values[2])
return rad

def get_angle_to_target(target_x, target_z):
# حساب الزاوية المطلوبة لمواجهة هدف معين
robot_position = gps.getValues()
vec_x = target_x - robot_position[0]
vec_z = target_z - robot_position[2]
angle_to_target = math.atan2(vec_x, vec_z)
return angle_to_target

def normalize_angle(angle):
# تطبيع الزاوية لتكون في نطاق [-pi, pi]
while angle > math.pi:
angle -= 2 * math.pi
while angle < -math.pi:
angle += 2 * math.pi
return angle

# ----------------------------
# Main control loop
# حلقة التحكم الرئيسية
while robot.step(TIME_STEP) != -1:

# -----------------------------------
# Read sensor data
# قراءة بيانات المستشعر
ball_contact = ball_sensor.getContact()
ps0_value = ps0.getValue()
ps7_value = ps7.getValue()
current_heading = get_compass_heading()

# Define goal coordinates (assuming Yellow team)
# تحديد إحداثيات مرمى الخصم (لنفترض أن الروبوت في الفريق الأصفر)
OPPONENT_GOAL_Z = -0.7
OPPONENT_GOAL_X = 0.0

# -----------------------------------
# Behavioral logic with a priority-based approach
# منطق السلوك القائم على الأولويات

# Priority 1: Smart Obstacle Avoidance
# الأولوية 1: تجنب العوائق بذكاء
if ps0_value < SONAR_THRESHOLD or ps7_value < SONAR_THRESHOLD:
# إذا تم اكتشاف عائق، انعطف بعيدًا عنه
if ps0_value < SONAR_THRESHOLD:
# عائق على اليمين، انعطف لليسار
left_motor.setVelocity(-TURN_SPEED)
right_motor.setVelocity(TURN_SPEED)
else:
# عائق على اليسار، انعطف لليمين
left_motor.setVelocity(TURN_SPEED)
right_motor.setVelocity(-TURN_SPEED)

# Priority 2: Ball Tracking and Scoring
# الأولوية 2: تتبع الكرة والتسديد
elif ball_contact > BALL_SENSOR_CONTACT_THRESHOLD:
# الروبوت لديه الكرة، الآن يهدف للمرمى
angle_to_goal = get_angle_to_goal(OPPONENT_GOAL_X, OPPONENT_GOAL_Z)
angle_diff = normalize_angle(angle_to_goal - current_heading)

if abs(angle_diff) < AIMING_TOLERANCE:
# إذا كان التصويب صحيحاً، قم بالتسديد بقوة
left_motor.setVelocity(KICK_SPEED)
right_motor.setVelocity(KICK_SPEED)
else:
# إذا لم يكن موجهًا، قم بالدوران نحو المرمى
if angle_diff < 0:
left_motor.setVelocity(TURN_SPEED)
right_motor.setVelocity(-TURN_SPEED)
else:
left_motor.setVelocity(-TURN_SPEED)
right_motor.setVelocity(TURN_SPEED)

# Priority 3: Search for Ball if no obstacles and no ball contact
# الأولوية 3: البحث عن الكرة إذا لم تكن هناك عوائق أو لم يتم لمس الكرة
else:
# يدور الروبوت ببطء للبحث عن الكرة
left_motor.setVelocity(TURN_SPEED / 2)
right_motor.setVelocity(-TURN_SPEED / 2)