#include <Servo.h>

Servo baseServo;
Servo shoulderServo;
Servo armServo;

const int recBtn = A0;
const int baseAnalog = A1;
const int shoulderAnalog = A2;
const int armAnalog = A3;

String commandReceived;

int positions[100][3] = {};
int positionLine = 0;
int playLine = 0;

int baseCurrPos = 0;
int shoulderCurrPos = 0;
int armCurrPos = 0;

bool nextPos = false;
bool basePosOk = false, shoulderPosOk = false, armPosOk = false;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);

  //Attachments of Servo
  baseServo.attach(3);
  shoulderServo.attach(5);
  armServo.attach(6);
  
  pinMode(recBtn, INPUT_PULLUP);
  pinMode(baseAnalog, INPUT);
  pinMode(shoulderAnalog, INPUT);
  pinMode(armAnalog, INPUT);

  Serial.println("Machine initialized...");
}

void loop() {
  // put your main code here, to run repeatedly:
  if(Serial.available()) {
    commandReceived = Serial.readStringUntil('\n');
    Serial.print("Command Received: ");
    Serial.println(commandReceived); 
  }

  //for moving servos
  if (commandReceived == "record") {
    //saves all position to array
    positions[positionLine][0] = analogRead(baseAnalog);
    positions[positionLine][1] = analogRead(shoulderAnalog);
    positions[positionLine][2] = analogRead(armAnalog);

    //posts it to serial
    Serial.print("Positions: ");
    Serial.print(positions[positionLine][0]);
    Serial.print(" : ");
    Serial.print(positions[positionLine][1]);
    Serial.print(" : ");
    Serial.println(positions[positionLine][2]);

    //increments the position
    positionLine+=1;

    //resets the command
    commandReceived = "";
  }
  else if (commandReceived == "play") {
    for (playLine = 0; playLine <= positionLine; playLine++) {
      nextPos = false;
      basePosOk = false;
      shoulderPosOk = false;
      armPosOk = false;
      
      Serial.print("Playing: ");
      Serial.print(positions[playLine][0]);
      Serial.print(" : ");
      Serial.print(positions[playLine][1]);
      Serial.print(" : ");
      Serial.println(positions[playLine][2]);

      while (!nextPos) {
        if (baseCurrPos != positions[playLine][0]) {
          baseCurrPos+=1;
          baseServo.write(baseCurrPos);
        }
        else if (baseCurrPos == positions[playLine][0]) {
          basePosOk = true;
        }

        if (shoulderCurrPos != positions[playLine][1]) {
          shoulderCurrPos+=1;
          shoulderServo.write(shoulderCurrPos);
        }
        else if (shoulderCurrPos == positions[playLine][1]) {
          shoulderPosOk = true;
        }

        if (armCurrPos != positions[playLine][2]) {
          armCurrPos+=1;
          armServo.write(armCurrPos);
        }
        else if (armCurrPos == positions[playLine][2]) {
          armPosOk = true;
        }

        if (basePosOk && shoulderPosOk && armPosOk) {
          nextPos = true;
        }
      }
    }
  }
  else if (commandReceived == "rst") {
    playLine = 0;
    positionLine = 0;
    memset(positions, 0, sizeof(positions));
  }
}