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


# Title: Obstacle Detection and Navigation with e-puck Robot
# Description: This program controls the e-puck robot to move forward,
# turn right if a red obstacle (rock) is detected, and move backward if a blue obstacle is detected.

# Import the necessary modules from the Webots API
from controller import Robot, Camera, Motor

# Set the time step of the simulation
TIME_STEP = 64

# Create a robot instance to interact with the robot
robot = Robot()

# Get the camera device, enable it, and set the time step
camera = robot.getDevice('camera')
camera.enable(TIME_STEP)

# Get the motor devices
left_motor = robot.getDevice('left wheel motor')
right_motor = robot.getDevice('right wheel motor')

# Set the target position of the motors to infinity for velocity control
left_motor.setPosition(float('inf'))
right_motor.setPosition(float('inf'))

# Set the initial velocity of the motors to 0 (robot is stationary)
left_motor.setVelocity(0)
right_motor.setVelocity(0)

# Define color thresholds for detecting red and blue
RED_THRESHOLD = 100
GREEN_THRESHOLD = 45
BLUE_THRESHOLD = 45
BLUE_RED_THRESHOLD = 50  # Blue objects may still have some red component

# Main loop: this is where the robot's behavior is defined
while robot.step(TIME_STEP) != -1:
    # Retrieve the image captured by the camera
    image = camera.getImage()
    
    # Get the width and height of the image from the camera's properties
    width = camera.getWidth()
    height = camera.getHeight()

    # Calculate the central pixel's coordinates
    centerX = width // 2
    centerY = height // 2
    
    # Get the red, green, and blue color components of the central pixel
    red = camera.imageGetRed(image, width, centerX, centerY)
    green = camera.imageGetGreen(image, width, centerX, centerY)
    blue = camera.imageGetBlue(image, width, centerX, centerY)
    
    # Print the RGB color values of the central pixel
    print(f"Color of the central pixel: R={red}, G={green}, B={blue}")

    # Detect whether the central pixel is red enough (rock)
    if red > RED_THRESHOLD and green < GREEN_THRESHOLD and blue < BLUE_THRESHOLD:
        # If red enough, it means there's a red obstacle (rock) ahead, so turn right
        left_motor.setVelocity(2)  # positive speed turns the wheel forward
        right_motor.setVelocity(-2)  # negative speed turns the wheel backward
    
    # Detect whether the central pixel is blue enough
    elif blue > RED_THRESHOLD and green < GREEN_THRESHOLD and red < BLUE_RED_THRESHOLD:
        # If blue enough, it means there's a blue obstacle ahead, so move backward
        left_motor.setVelocity(-2)  # negative speed turns the wheel backward
        right_motor.setVelocity(-2)  # negative speed turns the wheel backward

    else:
        # If not red or blue enough, it means the path is clear, so move forward
        left_motor.setVelocity(5)
        right_motor.setVelocity(5)