/*

  RC Robot - Arduino Uno + L298N

  Commands from serial (USB from Raspberry Pi):

    W = forward

    S = backward

    A = turn left (pivot)

    D = turn right (pivot)

    P = stop

    I = grip toggle (open/close)

    K = lift toggle (down/up)

    J = rotate servo left (step)

    L = rotate servo right (step)

*/

#include <Servo.h>

const int IN1 = 8;   // right motor dir 1

const int IN2 = 9;   // right motor dir 2

const int IN3 = 10;  // left motor dir 1

const int IN4 = 11;  // left motor dir 2

const int SERVO_GRIP_PIN = 2;

const int SERVO_LIFT_PIN = 3;

const int SERVO_ROTATE_PIN = 4;

const bool REVERSE_RIGHT = false;

const bool REVERSE_LEFT = false;

Servo gripServo;

Servo liftServo;

Servo rotateServo;

const int GRIP_OPEN_DEG = 20;

const int GRIP_CLOSE_DEG = 90;

const int LIFT_DOWN_DEG = 40;

const int LIFT_UP_DEG = 120;

const int ROTATE_MIN_DEG = 20;

const int ROTATE_MAX_DEG = 160;

const int ROTATE_STEP_DEG = 8;

bool gripClosed = false;

bool liftUp = false;

int rotateDeg = 90;

unsigned long lastCommandMs = 0;

const unsigned long FAILSAFE_MS = 1200;

void setup() {

  pinMode(IN1, OUTPUT);

  pinMode(IN2, OUTPUT);

  pinMode(IN3, OUTPUT);

  pinMode(IN4, OUTPUT);

  stopMotors();

  gripServo.attach(SERVO_GRIP_PIN);

  liftServo.attach(SERVO_LIFT_PIN);

  rotateServo.attach(SERVO_ROTATE_PIN);

  gripServo.write(GRIP_OPEN_DEG);

  liftServo.write(LIFT_DOWN_DEG);

  rotateServo.write(rotateDeg);

  Serial.begin(115200);

  Serial.setTimeout(20);

  lastCommandMs = millis();

}

void loop() {

  if (Serial.available() > 0) {

    String msg = Serial.readStringUntil('\n');

    msg.trim();

    if (msg.length() > 0) {

      handleCommand(msg);

      lastCommandMs = millis();

    }

  }

  if (millis() - lastCommandMs > FAILSAFE_MS) {

    stopMotors();

  }

}

void setMotorRight(int dir) {

  // dir: 1=forward, -1=backward, 0=stop

  if (REVERSE_RIGHT) {

    dir = -dir;

  }

  if (dir > 0) {

    digitalWrite(IN1, HIGH);

    digitalWrite(IN2, LOW);

  } else if (dir < 0) {

    digitalWrite(IN1, LOW);

    digitalWrite(IN2, HIGH);

  } else {

    digitalWrite(IN1, LOW);

    digitalWrite(IN2, LOW);

  }

}

void setMotorLeft(int dir) {

  // dir: 1=forward, -1=backward, 0=stop

  if (REVERSE_LEFT) {

    dir = -dir;

  }

  if (dir > 0) {

    digitalWrite(IN3, HIGH);

    digitalWrite(IN4, LOW);

  } else if (dir < 0) {

    digitalWrite(IN3, LOW);

    digitalWrite(IN4, HIGH);

  } else {

    digitalWrite(IN3, LOW);

    digitalWrite(IN4, LOW);

  }

}

void moveForward() {

  setMotorRight(1);

  setMotorLeft(1);

}

void moveBackward() {

  setMotorRight(-1);

  setMotorLeft(-1);

}

void turnLeftPivot() {

  setMotorRight(1);

  setMotorLeft(-1);

}

void turnRightPivot() {

  setMotorRight(-1);

  setMotorLeft(1);

}

void stopMotors() {

  setMotorRight(0);

  setMotorLeft(0);

}

void toggleGrip() {

  gripClosed = !gripClosed;

  gripServo.write(gripClosed ? GRIP_CLOSE_DEG : GRIP_OPEN_DEG);

}

void toggleLift() {

  liftUp = !liftUp;

  liftServo.write(liftUp ? LIFT_UP_DEG : LIFT_DOWN_DEG);

}

void rotateLeftStep() {

  rotateDeg = max(ROTATE_MIN_DEG, rotateDeg - ROTATE_STEP_DEG);

  rotateServo.write(rotateDeg);

}

void rotateRightStep() {

  rotateDeg = min(ROTATE_MAX_DEG, rotateDeg + ROTATE_STEP_DEG);

  rotateServo.write(rotateDeg);

}

void handleCommand(const String &cmdRaw) {

  String cmd = cmdRaw;

  cmd.toUpperCase();

  char c = cmd.charAt(0);

  switch (c) {

    case 'W':

      moveForward();

      break;

    case 'S':

      moveBackward();

      break;

    case 'A':

      turnLeftPivot();

      break;

    case 'D':

      turnRightPivot();

      break;

    case 'P':

      stopMotors();

      break;

    case 'I':

      toggleGrip();

      break;

    case 'K':

      toggleLift();

      break;

    case 'J':

      rotateLeftStep();

      break;

    case 'L':

      rotateRightStep();

      break;

    default:

      // Unknown command -> stop for safety

      stopMotors();

      break;

  }

}