# Dr. Khaled Eskaf
#Robotics WorkShop - 1
#06-2024


# Import necessary classes for the robot control
from controller import Robot, Motor, DistanceSensor

# Initialize the Robot instance
robot = Robot()

# Define the time step of the robot's control loop
TIME_STEP = 64

# Initialize motor devices
leftMotor = robot.getDevice('left wheel motor')
rightMotor = robot.getDevice('right wheel motor')

# Set to infinite position mode
leftMotor.setPosition(float('inf'))
rightMotor.setPosition(float('inf'))

# Initial velocity
leftMotor.setVelocity(0.0)
rightMotor.setVelocity(0.0)

# Speed constants
FORWARD_SPEED = 5.0

# Initialize multiple distance sensors by their names
sensors = ['ps0', 'ps7', 'ps1', 'ps6']
sensorObjects = []

for sensorName in sensors:
    sensor = robot.getDevice(sensorName)  # Get the sensor device
    sensor.enable(TIME_STEP)  # Enable the sensor with the defined time step
    sensorObjects.append(sensor)  # Append the sensor object to the list

# Control loop: execute at each simulation step
while robot.step(TIME_STEP) != -1:
    # Read sensor values into a list
    sensorValues = [sensor.getValue() for sensor in sensorObjects]

    # Basic obstacle detection and navigation strategy
    if sensorValues[0] < 70 or sensorValues[1] < 70:  # ps0 or ps7 detects an obstacle in front
        # Obstacle detected in front: stop the robot
        leftMotor.setVelocity(0)
        rightMotor.setVelocity(0)
    elif sensorValues[2] < 70:  # ps1 detects an obstacle in the front-right
        # Obstacle detected in front-right: turn left
        leftMotor.setVelocity(-FORWARD_SPEED)
        rightMotor.setVelocity(FORWARD_SPEED)
    elif sensorValues[3] < 70:  # ps6 detects an obstacle in the front-left
        # Obstacle detected in front-left: turn right
        leftMotor.setVelocity(FORWARD_SPEED)
        rightMotor.setVelocity(-FORWARD_SPEED)
    else:
        # No obstacle detected: move forward
        leftMotor.setVelocity(FORWARD_SPEED)
        rightMotor.setVelocity(FORWARD_SPEED)