#define PWM1 4
#define PWM2 0
#define PWM3 16
#define PWM4 2

// Define delay duration for movement
const int MOVE_DURATION = 5000;

// Define possible directions
enum Direction {
    FORWARD,
    REVERSE,
    TURN_LEFT,
    TURN_RIGHT
};

void setup() {
    pinMode(PWM1, OUTPUT);
    pinMode(PWM2, OUTPUT);
    pinMode(PWM3, OUTPUT);
    pinMode(PWM4, OUTPUT);
    Serial.begin(115200);
    Serial.println("4 Wheel Mobile Robot Simulation");
}

void loop() {
    // Test each movement
    controlRobot(FORWARD);
    controlRobot(REVERSE);
    controlRobot(TURN_LEFT);
    controlRobot(TURN_RIGHT);
    
    // Stop the motors
    stopMotors();
    Serial.println("STOP");
    delay(5000);
}

void controlRobot(Direction dir) {
    switch (dir) {
        case FORWARD:
            Serial.println("Move Forward");
            digitalWrite(PWM1, LOW);
            digitalWrite(PWM2, HIGH);
            digitalWrite(PWM3, LOW);
            digitalWrite(PWM4, HIGH);
            break;

        case REVERSE:
            Serial.println("Reverse");
            digitalWrite(PWM1, HIGH);
            digitalWrite(PWM2, LOW);
            digitalWrite(PWM3, HIGH);
            digitalWrite(PWM4, LOW);
            break;

        case TURN_LEFT:
            Serial.println("Turn Left");
            digitalWrite(PWM1, LOW);
            digitalWrite(PWM2, HIGH);
            digitalWrite(PWM3, LOW);
            digitalWrite(PWM4, LOW);
            break;

        case TURN_RIGHT:
            Serial.println("Turn Right");
            digitalWrite(PWM1, LOW);
            digitalWrite(PWM2, LOW);
            digitalWrite(PWM3, LOW);
            digitalWrite(PWM4, HIGH);
            break;
    }
    delay(MOVE_DURATION);
    stopMotors();  // Stop after each action
}

void stopMotors() {
    digitalWrite(PWM1, LOW);
    digitalWrite(PWM2, LOW);
    digitalWrite(PWM3, LOW);
    digitalWrite(PWM4, LOW);
}