// Back Scratcher Plotter
// Vije Miller

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

// Scratcher
const int stepsrev = 32;
Stepper scratch(stepsrev, 2, 4, 3, 5);

// For Bluetooth Data
String readString, xaxisdata, yaxisdata;

// X Axis
Servo xaxis;
// Y Axis
Servo yaxis;

int xpos = 90;
int ypos = 90; // Stall
int stall = 90;
int yup = 75;
int ydown = 105;
int pos = 90;

void setup() {
  Serial.begin(9600);
  // Max(?) Speed
  scratch.setSpeed(800);
  xaxis.attach(6);
  yaxis.attach(7);
  xaxis.write(xpos);
  yaxis.write(ypos);
}

void loop() {

  // Bluetooth Data X-Y Axis String?
  while (Serial.available()) {
    delay(10);
    if (Serial.available() > 0) {
      char c = Serial.read();
      readString += c;
    }
  }

  if (readString.length() > 0) {
    // Serial.println(readString);
    // X-Y Axis Combined Sent (ex. 368124)
    // First 3 Digits
    xaxisdata = readString.substring(0, 3);
    // Next 3 Digits
    yaxisdata = readString.substring(4, 6);

    // Convert String to Int
    int prexpos = xaxisdata.toInt();
    int preypos = yaxisdata.toInt();

    // X-Y Based on 480x800 Phone Dimension & MIT App
    // X 100 to 350 High 170 to 320 Low
    // Y 220 to 560
    // Map X to xPos (Actual)
    xpos = map(prexpos, 100, 350, 120, 60);
    // Map Y to yPos (Delay)
    ypos = map(preypos, 220, 560, 0, 6300);

  }

  // Final Routine
  if (readString.length() > 0) {
    if (xpos > 90) {
      for (pos = 90; pos <= xpos; pos += 1) {
        xaxis.write(pos);
        delay(40);
      }
    }
    else if (xpos < 90) {
      for (pos = 90; pos >= xpos; pos -= 1) {
        xaxis.write(pos);
        delay(40);
      }
    }
    yaxis.write(yup);
    delay(ypos);
    yaxis.write(stall);
    // Scratch 2048 Steps = 1 Revolution (2 Up/Down)
    // Used Stepper bcz Spare Parts -- Sigh!
    // 2048 x 7 Seconds = 14336 Steps
    scratch.step(14336);

    // Return to Start
    if (xpos > 90) {
      for (pos = xpos; pos >= xpos; pos -= 1) {
        xaxis.write(pos);
        delay(40);
      }
    }
    else if (xpos < 90) {
      for (pos = xpos; pos <= xpos; pos += 1) {
        xaxis.write(pos);
        delay(40);
      }
    }
    yaxis.write(ydown);
    delay(ypos);
    yaxis.write(stall);
  }

  // Reset
  readString = "";
}