#include <Arduino.h>

/**
 * ================== ESP32 Command-Based Motor Control ==================
 * 
 * This ESP32 program processes serial commands to control:
 *   - Left Motor (Blue LED)
 *   - Right Motor (Green LED)
 *   - Nerf Gun (Red LED)
 * 
 * The LEDs simulate motor movement and gun firing, which will later be
 * replaced with actual motor and gun control logic.
 * 
 * ================== Command Format ==================
 * The ESP32 listens for commands sent over Serial in the following format:
 * 
 *      <command_number> <units>
 * 
 * - <command_number>: Determines what action to take.
 * - <units>: The parameter associated with that action.
 * 
 * Example input (for Wokwi testing):
 * 
 *      2 35000
 * 
 * This means command `2` (Left Motor) with a `units` value of `35000`, which
 * would make the left motor blink (simulating movement).
 * 
 * ================== How Commands Are Parsed ==================
 * 
 * 1. **In Wokwi (Testing Mode)**:
 *    - Commands are entered **as plain text** in the Serial Monitor.
 *    - The ESP32 reads this using `Serial.readStringUntil('\n')`.
 *    - We use `sscanf()` to split the command into two values.
 *    - The values are processed in `processCommand()`.
 * 
 * 2. **In Actual Deployment with Raspberry Pi**:
 *    - The Raspberry Pi will send **binary-encoded data** instead of text.
 *    - The ESP32 will read raw bytes using `Serial.readBytes()`.
 *    - The command will be stored in a **binary struct** (`CommandPacket`).
 *    - The ESP32 will then interpret the binary data and execute the command.
 * 
 * **IMPORTANT:**  
 * Since **Wokwi doesn’t support sending binary data easily**, we modified this 
 * ESP32 code to accept **text-based commands** for testing. In the real setup, 
 * the Raspberry Pi will send binary data instead of text.
 *
 * ================== List of Commands & Behavior ==================
 * 
 * | Command  | Description                 | Units Value & Behavior |
 * |----------|----------------------------|------------------------|
 * | `0 0`    | Stop everything             | **All LEDs OFF**       |
 * | `1 1`    | Fire Nerf Gun               | **Red LED stays ON**   |
 * | `1 0`    | Stop Firing                 | **Red LED turns OFF**  |
 * | `2 32768`| Left Motor (Neutral)        | **Blue LED OFF**       |
 * | `2 Any`  | Left Motor Moves (Fwd/Bwd)  | **Blue LED Blinks**    |
 * | `3 32768`| Right Motor (Neutral)       | **Green LED OFF**      |
 * | `3 Any`  | Right Motor Moves (Fwd/Bwd) | **Green LED Blinks**   |

 *
 * ================== Proposal for Raspberry Pi Integration ==================
 * 
 * I propose that instead of handling variable speeds directly on the ESP32,
 * we set predefined speed levels that will be parsed and interpreted by the
 * Raspberry Pi. This would allow for more precise motor control when we
 * transition to actual hardware.
 * 
 * Proposed speed mappings for Raspberry Pi to recognize:
 * 
 * | Command  | Speed Label | ESP32 Units Value |
 * |----------|------------|-------------------|
 * | `2`      | **Neutral** | `32768` (Motor OFF) |
 * | `2`      | **Slow Fwd** | `35000` |
 * | `2`      | **Fast Fwd** | `40000` |
 * | `2`      | **Slow Bwd** | `32000` |
 * | `2`      | **Fast Bwd** | `30000` |
 * | `3`      | **Neutral** | `32768` (Motor OFF) |
 * | `3`      | **Slow Fwd** | `35000` |
 * | `3`      | **Fast Fwd** | `40000` |
 * | `3`      | **Slow Bwd** | `32000` |
 * | `3`      | **Fast Bwd** | `30000` |
 *
 * **Why This Matters for the Raspberry Pi:**  
 * - The ESP32 simply receives the commands and toggles LEDs.
 * - The Raspberry Pi will eventually send these values to control the actual motors.
 * - Keeping these numbers consistent ensures that commands remain the same when we move to real motor control.
 * 
 * ================== ESP32 vs Raspberry Pi Differences ==================
 * This is how we handle parsing and execution on the ESP32, but it will be 
 * slightly different when we move this system to a Raspberry Pi.
 * 
 * - **ESP32 (Wokwi Testing Mode - Text-Based Commands):**  
 *   - Reads commands as **plain text** (e.g., `2 35000`).
 *   - Uses `Serial.readStringUntil('\n')` to read input.
 *   - Parses input with `sscanf()` into `commandNumber` and `units`.
 *   - Controls LEDs directly using `digitalWrite()`.  
 * 
 * - **ESP32 (Final Code for Raspberry Pi - Binary-Based Commands):**  
 *   - Reads commands **as binary data** sent from the Raspberry Pi.
 *   - Uses `Serial.readBytes()` to read a structured `CommandPacket`.
 *   - Parses commands directly from **raw binary** instead of text.
 *   - Executes commands based on the parsed binary values.
 * 
 * - **Raspberry Pi (Actual Code Setup):**  
 *   - Uses Python (`pyserial`) or C++ to send commands over serial.
 *   - Sends structured **binary packets** instead of text-based commands.
 *   - May forward commands to **motor controllers** instead of toggling LEDs directly.
 *
 * ================== Next Steps ==================
 * - Implement structured **binary packet parsing** to remove reliance on text.
 * - Replace LEDs with actual **motor driver control**.
 * - Establish serial communication between ESP32 and Raspberry Pi.
 * - Implement feedback mechanisms (sensors, encoders, etc.).
 * 
 * ================== Team Notes ==================
 * - If testing commands in **Wokwi**, use the Serial Monitor to send inputs.
 * - If testing on a **real ESP32**, use Arduino Serial Monitor or a Python script.
 * - If debugging command parsing, check the `Serial.println()` outputs.
 * 
 * Written by: Hamza Jalil  
 * Date: 02/15/2025 
 */






// Define GPIO pins for motors and Nerf gun (simulated with LEDs)
#define MOTOR_LEFT_PIN 26   // Blue LED (Left Motor)
#define MOTOR_RIGHT_PIN 27  // Green LED (Right Motor)
#define GUN_TRIGGER_PIN 25  // Red LED (Nerf Gun)

struct CommandPacket {
    uint8_t commandNumber;  // 8-bit command ID
    uint16_t units;         // 16-bit command value (e.g., speed)
    uint8_t terminator;     // Must be '\n' for validity
};

/**
 * Reads and parses a command from the Serial Monitor.
 * Expected format: "<command_number> <units>\n"
 * Example: "1 20" -> Command 1 (Fire Gun) with parameter 20
 */
bool readCommandPacket(CommandPacket& cmd) {
    if (Serial.available()) // Check if data is available
    {  
        Serial.println("Data received!");

        String input = Serial.readStringUntil('\n');  // Read text input
        Serial.print("Received: ");
        Serial.println(input);

        int commandNumber, units;
        if (sscanf(input.c_str(), "%d %d", &commandNumber, &units) == 2) 
        {
            cmd.commandNumber = commandNumber;
            cmd.units = units;
            cmd.terminator = '\n';  // Simulate proper termination
            Serial.println("Parsed command successfully!");
            return true;
        } 
        
        else 
        {
            Serial.println("ERROR: Invalid input format! Use: <command_number> <units>");
            return false;
        }
    }

    return false;
}

/**
 * Processes received command and executes corresponding actions.
0 0	Stop Everything	All LEDs OFF
1 1	Fire Nerf Gun	🔴 Red LED ON
1 0	Stop Firing	🔴 Red LED OFF
2 32768	Left Motor (Neutral Position)	🔵 Blue LED Blinks
2 35000	Move Left Motor Forward	🔵 Blue LED Blinks
3 32768	Right Motor (Neutral Position)	🟢 Green LED Blinks
3 35000	Move Right Motor Forward	🟢 Green LED Blinks
 */

void processCommand(const CommandPacket& cmd) 
{
    Serial.print("Executing Command: ");
    Serial.print(cmd.commandNumber);
    Serial.print(" | Units: ");
    Serial.println(cmd.units);

    switch (cmd.commandNumber) 
    {
        case 0: // Stop Everything
            Serial.println("Stopping all LEDs...");
            digitalWrite(MOTOR_LEFT_PIN, LOW);
            digitalWrite(MOTOR_RIGHT_PIN, LOW);
            digitalWrite(GUN_TRIGGER_PIN, LOW);
            break;

        case 1: // Toggle Nerf Gun (Simulated with Red LED)
            if (cmd.units == 1) 
            {
                Serial.println("Firing Nerf Gun (Red LED ON)");
                digitalWrite(GUN_TRIGGER_PIN, HIGH);
            } 
            
            else 
            {
                Serial.println("Stopping Nerf Gun (Red LED OFF)");
                digitalWrite(GUN_TRIGGER_PIN, LOW);
            }
            break;

        case 2: // Set Left Motor speed (Simulated with Blue LED)
            Serial.println("Activating Left Motor (Blue LED)");
            digitalWrite(MOTOR_LEFT_PIN, HIGH);
            delay(500);
            digitalWrite(MOTOR_LEFT_PIN, LOW);
            break;

        case 3: // Set Right Motor speed (Simulated with Green LED)
            Serial.println("Activating Right Motor (Green LED)");
            digitalWrite(MOTOR_RIGHT_PIN, HIGH);
            delay(500);
            digitalWrite(MOTOR_RIGHT_PIN, LOW);
            break;

        default:
            Serial.println("ERROR: Unknown Command Received!");
            break;
    }
}

/**
 * Setup function: Runs once at startup
 */
void setup() {
    Serial.begin(115200);  // Initialize Serial communication

    // Configure LED pins as outputs
    pinMode(MOTOR_LEFT_PIN, OUTPUT);
    pinMode(MOTOR_RIGHT_PIN, OUTPUT);
    pinMode(GUN_TRIGGER_PIN, OUTPUT);

    Serial.println("ESP32 Ready. Enter commands as: <command_number> <units>");
}

/**
 * Main loop: Continuously checks for and processes incoming commands.
 */
void loop() {
    Serial.println("Waiting for input...");
    delay(1000);

    CommandPacket cmd;
    if (readCommandPacket(cmd)) {
        processCommand(cmd);
    }
}






/**
 * Since there is no free Pi and ESP32 virturalizer we can use this
 * code on a rasberry pi connected to an ESP32 

import serial  # Import the PySerial library for UART communication
import struct  # Import the struct library to handle binary data packing
import time    # Import time library to add delays between commands

# Define the serial port and baud rate for communication with the ESP32
SERIAL_PORT = "/dev/ttyUSB0"  # Change to the correct port (e.g., "/dev/serial0" on Raspberry Pi)
BAUD_RATE = 115200  # Must match the ESP32's baud rate

# Function to send a command to the ESP32
def send_command(command_number, units):
    """
    This function sends a formatted binary command packet to the ESP32.
    
    Packet Format:
    - 1 byte: Command number (uint8)
    - 2 bytes: Units/parameters (uint16)
    - 1 byte: Newline ('\n') (uint8)

    :param command_number: The ID of the command to send (e.g., 0 = Stop, 1 = Fire Gun)
    :param units: The parameter value (e.g., speed for motors, 1 to fire gun, 0 to stop firing the gun)
    """
    
    # Pack the data into a binary format (8-bit command, 16-bit units, 8-bit newline)
    packet = struct.pack("BHB", command_number, units, ord('\n'))
    
    # Send the packet to the ESP32 over the serial connection
    ser.write(packet)
    
    # Print the command being sent for debugging purposes
    print(f"Sent Command {command_number} with Units {units}")

# Open a serial connection with the ESP32
ser = serial.Serial(SERIAL_PORT, BAUD_RATE, timeout=1)  
"""
- SERIAL_PORT: The UART port where ESP32 is connected.
- BAUD_RATE: Must be the same on both ESP32 and Raspberry Pi.
- timeout=1: Sets a 1-second timeout for reading responses (if needed).
"""

try:
    # Send test commands to the ESP32

    # Command 0: Stop Everything (Units = 0)
    send_command(0, 0)
    time.sleep(1)  # Wait 1 second before sending the next command

    # Command 1: Fire the Nerf Gun (Units = 1 to fire)
    send_command(1, 1)
    time.sleep(1)

    # Command 1: Stop Firing the Nerf Gun (Units = 0 to stop)
    send_command(1, 0)
    time.sleep(1)

    # Command 2: Move Left Stepper at 0 mm/s (middle range of 32768 encoding)
    send_command(2, 32768)
    time.sleep(1)

    # Command 2: Move Left Stepper forward (e.g., velocity = 35000)
    send_command(2, 35000)
    time.sleep(1)

    # Command 2: Move Left Stepper backward (e.g., velocity = 30000)
    send_command(2, 30000)
    time.sleep(1)

    # Command 3: Move Right Stepper at 0 mm/s
    send_command(3, 32768)
    time.sleep(1)

except KeyboardInterrupt:
    """
    If the user presses CTRL+C, exit the loop safely.
    """
    print("Stopped by user")

finally:
    """
    Always close the serial connection when done.
    This ensures resources are released properly.
    """
    ser.close()
    print("Serial connection closed.")


**/





/**


CODE FOR REGULAR USE ON ESP32

#include <Arduino.h>  // Include the Arduino library for serial communication & GPIO control

// Define serial baud rate (communication speed)
#define BAUD_RATE 115200  // 115200 bits per second (fast and reliable for ESP32)

// Define GPIO pins for motors and Nerf gun
#define MOTOR_LEFT_PIN 5   // Left motor control pin
#define MOTOR_RIGHT_PIN 6  // Right motor control pin
#define GUN_TRIGGER_PIN 7  // Nerf gun trigger control pin

// Structure representing the command packet received from Raspberry Pi
struct CommandPacket {
    uint8_t commandNumber;  // 8-bit unsigned integer for command ID
    uint16_t units;         // 16-bit unsigned integer for command parameter (e.g., speed)
    uint8_t terminator;     // Should always be '\n' (marks end of a valid packet)
};


//Function to process received command that executes appropriate actions based on command number

void processCommand(const CommandPacket& cmd) {
    // Debugging: Print received command and parameter value
    Serial.print("Received Command: ");
    Serial.print(cmd.commandNumber);
    Serial.print(" | Units: ");
    Serial.println(cmd.units);

    switch (cmd.commandNumber) {
        case 0: // Stop Everything
            Serial.println("Executing: STOP EVERYTHING");
            digitalWrite(MOTOR_LEFT_PIN, LOW);   // Turn off left motor
            digitalWrite(MOTOR_RIGHT_PIN, LOW);  // Turn off right motor
            digitalWrite(GUN_TRIGGER_PIN, LOW);  // Turn off Nerf gun
            break;

        case 1: // Toggle Nerf Gun
            if (cmd.units == 1) {  // If '1', fire the gun
                Serial.println("Executing: FIRE GUN");
                digitalWrite(GUN_TRIGGER_PIN, HIGH);
            } else {  // If '0', stop firing
                Serial.println("Executing: STOP FIRING");
                digitalWrite(GUN_TRIGGER_PIN, LOW);
            }
            break;

        case 2: // Set Left Stepper Motor speed
            {
                int velocity = cmd.units - 32768; // Convert unsigned to signed velocity
                Serial.print("Executing: SET LEFT STEPPER at velocity ");
                Serial.println(velocity);

                if (velocity == 0) {
                    digitalWrite(MOTOR_LEFT_PIN, LOW);
                } else {
                    digitalWrite(MOTOR_LEFT_PIN, HIGH);
                    delay(500);  // Simulated movement
                    digitalWrite(MOTOR_LEFT_PIN, LOW);
                }
            }
            break;

        case 3: // Set Right Stepper Motor speed
            {
                int velocity = cmd.units - 32768; // Convert unsigned to signed velocity
                Serial.print("Executing: SET RIGHT STEPPER at velocity ");
                Serial.println(velocity);

                if (velocity == 0) {
                    digitalWrite(MOTOR_RIGHT_PIN, LOW);
                } else {
                    digitalWrite(MOTOR_RIGHT_PIN, HIGH);
                    delay(500);  // Simulated movement
                    digitalWrite(MOTOR_RIGHT_PIN, LOW);
                }
            }
            break;

        default: // Handle unknown commands
            Serial.println("ERROR: Unknown Command Received!");
            break;
    }
}


// Function to read and parse a command packet from serial and returns true if a valid packet is received, false otherwise.

bool readCommandPacket(CommandPacket& cmd) {
    if (Serial.available() >= sizeof(CommandPacket)) {  // Ensure we have enough data
        Serial.readBytes(reinterpret_cast<uint8_t*>(&cmd), sizeof(CommandPacket));  // Read into struct
        
        if (cmd.terminator == '\n') {  // Validate termination character
            return true;  // Valid command received
        } else {
            Serial.println("ERROR: Invalid packet format!");  // Debugging message
            return false;
        }
    }
    return false;  // Not enough data received yet
}


// Setup function: Runs once at startup that initializes serial communication and configures GPIO pins
void setup() {
    Serial.begin(BAUD_RATE);  // Initialize serial communication with defined baud rate
    pinMode(MOTOR_LEFT_PIN, OUTPUT);   // Configure left motor pin as output
    pinMode(MOTOR_RIGHT_PIN, OUTPUT);  // Configure right motor pin as output
    pinMode(GUN_TRIGGER_PIN, OUTPUT);  // Configure Nerf gun pin as output

    Serial.println("ESP32 Ready to Receive Binary Commands from Raspberry Pi.");
}

// Main loop: Continuously checks for and processes incoming commands

void loop() {
    CommandPacket cmd;
    if (readCommandPacket(cmd)) {  // Check if a valid command is received
        processCommand(cmd);       // Execute the corresponding action
    }
}



**/