#"Integrating Neural Networks for Color-Based Decision Making in Robotics with Webots"
#"ColorNN: Neural Network for Color-Driven Robot Control in Webots"

# Dr. Khaled Eskaf

# Define and Train the Model
# First, we import necessary libraries.
from controller import Robot
import numpy as np
from keras.models import Sequential, load_model
from keras.layers import Dense
import os   #is used to check if the neural network model file (my_model.h5) already exists in the filesystem.


# We define a function called 'train_model' that will handle the training of our neural network.
def train_model():
    """
    This function trains a simple neural network on some predefined data.
    """

    # We have some example training data. Each item in this list represents the average
    # color (in RGB) of some objects our robot might see. The colors are in the format [R, G, B].
    # The "/ 255.0" part is to normalize these values, so they are between 0 and 1, which is
    # a format neural networks work well with.
    X_train = np.array([[120, 40, 40], [40, 40, 120], [80, 110, 90]]) / 255.0

    # The labels for our training data. These tell the network what action to take for each color:
    # [1, 0, 0] means "Turn Right" for the first color, [0, 0, 1] means "Move Backward" for the second color,
    # and [0, 1, 0] means "Move Forward" for the third color. This is called "one-hot encoding."
    y_train = np.array([[1, 0, 0], [0, 0, 1], [0, 1, 0]])

    # Next, we define our neural network's structure. It's a simple one with one input layer,
    # one hidden layer, and one output layer.
    model = Sequential([
        Dense(64, activation='relu', input_dim=3),  # The input layer has 3 neurons (for R, G, B), and uses ReLU activation.
        Dense(64, activation='relu'),              # The hidden layer also has 64 neurons and uses ReLU activation.
        Dense(3, activation='softmax')             # The output layer has 3 neurons (for our actions) and uses softmax activation.
    ])

    # Now we compile our model. This step configures the model for training.
    model.compile(optimizer='adam',               # The Adam optimizer is a popular choice.
                  loss='categorical_crossentropy',# This loss function is good for classification tasks like ours.
                  metrics=['accuracy'])          # We'll track accuracy during training.

    # We train (fit) our model on our dataset. We do this for 100 "epochs," which means
    # the model will see the entire dataset 100 times. We don't print out progress (verbose=0).
    model.fit(X_train, y_train, epochs=100, verbose=0)

    # After training, we save our model to a file so we can use it later without having
    # to retrain it every time we start our Webots simulation.
    model.save('my_model.h5')

#Step 1: Initialize the Webots Robot
# Create an instance of the Robot class. This is necessary to interact with the robot and its devices.
robot = Robot()

#Step 2: Check for an Existing Model
# Before we use the neural network, we check if we've already trained and saved it.
# This is to avoid retraining the network every time we start the simulation, saving time and resources.
if not os.path.exists('my_model.h5'):
    # If the model doesn't exist, we call the function `train_model()` to train and save our model.
    train_model()

#Step 3: Load the Trained Model
# Once we have our model (either by just training it or because it was already saved), we load it.
# This model will be used to make decisions based on the robot's camera input.
model = load_model('my_model.h5')

#Step 4: Initialize Camera and Motors
# To make decisions based on what the robot "sees," we need to initialize and enable the camera.
camera = robot.getDevice('camera')
camera.enable(64)  # The camera is enabled with a time step of 64 milliseconds.

# We also initialize the motors. These motors control the robot's wheels, allowing it to move.
left_motor = robot.getDevice('left wheel motor')
right_motor = robot.getDevice('right wheel motor')

# To control the robot's movement through velocity, we set the motor position to infinity.
left_motor.setPosition(float('inf'))
right_motor.setPosition(float('inf'))

# Initially, we don't want the robot to move, so we set the velocity of both motors to 0.
left_motor.setVelocity(0)
right_motor.setVelocity(0)

#Step 5: Define a Function to Process Camera Images and Make Decisions
def process_image_and_decide():
    """
    This function grabs the current view from the robot's camera, processes it to
    understand the colors it sees, and decides on an action based on those colors.
    """
    # We get the RGB array from the camera. This is a 3D array with dimensions corresponding to the width,
    # height of the image, and the RGB color channels.
    image = camera.getImageArray()
    width = camera.getWidth()
    height = camera.getHeight()

    # Calculate the average RGB values across the entire image.
    avg_r = avg_g = avg_b = 0
    for x in range(width):
        for y in range(height):
            avg_r += image[x][y][0]
            avg_g += image[x][y][1]
            avg_b += image[x][y][2]
    avg_r /= (width * height)
    avg_g /= (width * height)
    avg_b /= (width * height)

    # Normalize these averages to be between 0 and 1.
    input_data = np.array([[avg_r, avg_g, avg_b]]) / 255.0

    # Use the neural network model to predict the best action based on these colors.
    prediction = model.predict(input_data)
    action = np.argmax(prediction)  # We choose the action with the highest predicted score.
    
    return action

#Step 6: Main Simulation Loop
# This loop runs continuously for the duration of the simulation. In each iteration, the robot
# processes its camera input, makes a decision on how to move, and then executes that movement.
while robot.step(64) != -1:
    action = process_image_and_decide()

    # Depending on the action decided by the neural network, we set the velocities of the motors.
    # This causes the robot to move forward, turn right, or move backward.
    if action == 0:  # Move Forward
        left_motor.setVelocity(5.0)
        right_motor.setVelocity(5.0)
    elif action == 1:  # Turn Right
        left_motor.setVelocity(5.0)
        right_motor.setVelocity(-5.0)
    elif action == 2:  # Move Backward
        left_motor.setVelocity(-5.0)
        right_motor.setVelocity(-5.0)






