// https://forum.arduino.cc/t/issues-controlling-6-servo-arduino-robot-arm-using-serial-communication/1228888/6

// https://docs.arduino.cc/retired/getting-started-guides/Braccio/
// https://github.com/arduino-libraries/Braccio
// https://github.com/arduino-libraries/Arduino_Braccio_plusplus
// https://github.com/WLaney/tinkerkit-braccio?tab=readme-ov-file
// Author: Stefan Str�mberg

/*
  Axis 1 – It is located at the base of a robot, and helps it to rotate from left to right.
  Axis 2 – It helps the lower arm of a robot to move in an up and down motion.
  Axis 3 – It allows the upper arm of a robot to move forward and backward.
  Axis 4 – This axis is known as wrist roll, and it rotates the upper arm of a robot in a circular movement.
  Axis 5 – It permits the wrist of the robot’s arm to raise and lower.
  Axis 6 – It allows the wrist of the robot’s arm to rotate freely in a circular motion.
*/

// normal: b30, s60, e90, v120, r150, g60
// test: ,b,30,s,60,e,90,v,120,r,150,g,60,

#include <Servo.h>
#include <Braccio.h>
#include <NeoSWSerial.h>

#define SERVO_PIN_BASE      11
#define SERVO_PIN_SHOULDER  10
#define SERVO_PIN_ELBOW      9
#define SERVO_PIN_WRIST_VER  6
#define SERVO_PIN_WRIST_ROT  5
#define SERVO_PIN_GRIPPER    3

#define START_BASE       90 // M1 range   0 to 180
#define START_SHOULDER   45 // M2 range  15 to 165
#define START_ELBOW     180 // M3 range 180 to   0
#define START_WRIST_VER 180 // M4 range 180 to   0
#define START_WRIST_ROT  90 // M5 range   0 to 180
#define START_GRIPPER    10 // M6 range  10 to  73 (10 = open, 73 = closed)

NeoSWSerial mySerial(2, 4); // RX = 2, TX = 4

Servo base;
Servo shoulder;
Servo elbow;
Servo wrist_ver;
Servo wrist_rot;
Servo gripper;

uint8_t idx = 0;
uint8_t value_idx = 0;
char value[4] = "";

void reach_goal(Servo& motor, String motorN,  int goal) {
  if (goal >= motor.read()) {
    for (int pos = motor.read(); pos <= goal; pos += 1) {
      mySerial.print("Current goal: ");
      mySerial.println(goal);
      mySerial.print("Current position: ");
      mySerial.println(pos);
      mySerial.println("Servo used: " + motorN);
      motor.write(pos);
      delay(5);
    }
  } else {
    for (int pos = motor.read(); pos >= goal; pos -= 1) {
      //mySerial.print("Current goal: ");
      //mySerial.println(goal);
      //mySerial.print("Current position: ");
      //mySerial.println(pos);
      //mySerial.println("Servo used:" + motorN);

      motor.write(pos);
      delay(5);
    }
  }
}

void setup() {
  Braccio.begin();

  base.attach(SERVO_PIN_BASE);
  shoulder.attach(SERVO_PIN_SHOULDER);
  elbow.attach(SERVO_PIN_ELBOW);
  wrist_ver.attach(SERVO_PIN_WRIST_VER);
  wrist_rot.attach(SERVO_PIN_WRIST_ROT);
  gripper.attach(SERVO_PIN_GRIPPER);

  base.write     (START_BASE);
  shoulder.write (START_SHOULDER);
  elbow.write    (START_ELBOW);
  wrist_ver.write(START_WRIST_VER);
  wrist_rot.write(START_WRIST_ROT);
  gripper.write  (START_GRIPPER);

  Serial.begin(115200);
  Serial.println();
  Serial.setTimeout(1);
  //Serial.println("Initial value:");
  //Serial.print(value);

  //mySerial.begin(9600);
  //delay(1000);
  //mySerial.println("Connection to Elegoo Arduino initiated");
}

void loop() {
  while (Serial.available() > 0) {
    //mySerial.println("Reading serial data");

    char chr = Serial.read();
    //Serial.println(chr);

    if (chr == 'b') {
      idx = 0;
      value_idx = 0;
    }
    else if (chr == 's') {
      idx = 1;
      value_idx = 0;
    }
    else if (chr == 'e') {
      idx = 2;
      value_idx = 0;
    }
    else if (chr == 'r') {
      idx = 3;
      value_idx = 0;
    }
    else if (chr == 'v') {
      idx = 4;
      value_idx = 0;
    }
    else if (chr == 'g') {
      idx = 5;
      value_idx = 0;
    }
    else if (chr == ',') {
      //Serial.print("Initial value before atoi");
      //Serial.println(value);
      int val = strtol(value, NULL, 0);

      if (idx == 0) {
        //Serial.print("value0: ");
        //Serial.println(val);
        reach_goal(base, "base", val);
      }
      else if (idx == 1) {
        //Serial.print("value1: ");
        //Serial.println(val);
        reach_goal(shoulder, "shoulder", val);
      }
      else if (idx == 2) {
        //Serial.print("value2: ");
        //Serial.println(val);
        reach_goal(elbow, "elbow", val);
      }
      else if (idx == 3) {
        //Serial.print("value3: ");
        //Serial.println(val);
        reach_goal(wrist_rot, "wrist_rot", val);
      }
      else if (idx == 4) {
        //Serial.print("value4: ");
        //Serial.println(val);
        reach_goal(wrist_ver, "wrist_ver", val);
      }
      else if (idx == 5) {
        //Serial.print("value5: ");
        //Serial.println(val);
        reach_goal(gripper, "gripper", val);
      }

      value[0] = 0;
      value[1] = 0;
      value[2] = 0;
      value[3] = '\0"';
    } else {
      value[value_idx] = chr;
      value_idx++;

      //String current_pos = String(base.read()) + "," + String(shoulder.read()) + "," + String(elbow.read()) + "," + String(wrist_ver.read()) + "," + String(wrist_rot.read()) + "," + String(gripper.read());
      //mySerial.println("The current position of each servo is (in order):" + current_pos);
      //delay(1000);
    }
  }
}