#include <Stepper.h>
#include <Servo.h>

String serialData;

Servo shoulder;
Servo elbow;
Servo wrist;
Servo fingers;

int distanceThreshold = 0;
int cm = 0;
int inches = 0;

int in1 = 6;
int in2 = 7;
int in3 = 5;
int in4 = 4;

int shoulder_angle = shoulder.read();
int elbow_angle = elbow.read();
int wrist_angle = wrist.read();
int travel_delay;
int i;

int hypotenuse;


long readUltrasonicDistance(int triggerPin, int echoPin)
{
  pinMode(triggerPin, OUTPUT);  // Clear the trigger
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  // Sets the trigger pin to HIGH state for 10 microseconds
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);
  pinMode(echoPin, INPUT);
  // Reads the echo pin, and returns the sound wave travel time in microseconds
  return pulseIn(echoPin, HIGH);
}


long distance() {
  distanceThreshold = 350;
  // measure the ping time in cm
  cm = 0.01723 * readUltrasonicDistance(2, 3);
  return cm;
}


void forward() {
  digitalWrite(in1, 0);
  digitalWrite(in2, 1);
  digitalWrite(in3, 1);
  digitalWrite(in4, 0);
}

void backward() {
  digitalWrite(in1, 1);
  digitalWrite(in2, 0);
  digitalWrite(in3, 0);
  digitalWrite(in4, 1);
}

void left() {
  digitalWrite(in1, 1);
  digitalWrite(in2, 0);
  digitalWrite(in3, 1);
  digitalWrite(in4, 0);
}

void right() {
  digitalWrite(in1, 0);
  digitalWrite(in2, 1);
  digitalWrite(in3, 0);
  digitalWrite(in4, 1);
}

void stop_car() {
  digitalWrite(in1, 0);
  digitalWrite(in2, 0);
  digitalWrite(in3, 0);
  digitalWrite(in4, 0);
}

String getValue(String data, char separator, int index)
{
    int found = 0;
    int strIndex[] = { 0, -1 };
    int maxIndex = data.length() - 1;

    for (int i = 0; i <= maxIndex && found <= index; i++) {
        if (data.charAt(i) == separator || i == maxIndex) {
            found++;
            strIndex[0] = strIndex[1] + 1;
            strIndex[1] = (i == maxIndex) ? i+1 : i;
        }
    }
    return found > index ? data.substring(strIndex[0], strIndex[1]) : "";
}


void setup() {
  Serial.begin(9600);

  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);

  shoulder.attach(11);
  elbow.attach(10);
  wrist.attach(9);
  fingers.attach(8);
  shoulder.write(45);
  elbow.write(45);
  wrist.write(30);
}

void loop() {
  if (Serial.available() > 0) {
    serialData = Serial.readString();
    Serial.println(serialData);
    if (serialData == "distang") {
      int hypotenuse = distance();
      wrist_angle = wrist.read();
      Serial.println(String(hypotenuse));
      Serial.println(wrist_angle);
    }
    
    if (serialData.substring(0, 7) == "forward") {
      travel_delay = serialData.substring(8).toInt();
    } else if (serialData.substring(0, 8) == "backward") {
      travel_delay = serialData.substring(9).toInt();
    } else if (serialData.substring(0, 4) == "left") {
      left();
    } else if (serialData.substring(0, 5) == "right") {
      right();
    } else if (serialData.substring(0, 4) == "grab") {
      for (i = 0; i > 3; i --) {
        fingers.write(i);
        delay(10);
      }
    } else if (serialData.substring(0, 7) == "release") {
      for (i = 0; i < 100; i ++) {
        fingers.write(i);
        delay(10);
      }
    } else if (serialData.substring(0, 5) == "wrist") {
      wrist.write(serialData.substring(6).toInt());
    } else if (serialData.substring(0, 9) == "shoulder") {
      shoulder.write(serialData.substring(10).toInt());
    } else if (serialData.substring(0, 6) == "elbow") {
      shoulder.write(serialData.substring(7).toInt());
    } else if (serialData.substring(0, 4) == "stop") {
      stop_car();
    }
    if (serialData.substring(0, 11) == "wrist_track") {
      int angle = serialData.substring(12).toInt();
      int between_delay = travel_delay/pow(pow(wrist_angle-angle, 2), 0.5);
      forward();
      if (angle < 50) {
        for (i = wrist_angle; i > angle; i --) {
          wrist.write(i);
          delay(between_delay);
        }
      } else if (angle > 50) {
        for (i = wrist_angle; i < angle; i ++) {
          wrist.write(i);
          delay(between_delay);
        }
      }
      stop_car();
    }
  }
}