#define enable1 12
#define microStep1 11
#define step1 10
#define dir1 9

#define enable2 8
#define microStep2 7
#define step2 6
#define dir2 5

bool ran = false;
bool lastState1 = false;
bool lastState2 = false;

double mot1Steps = 0;
double mot2Steps = 0;
double a1,a2;
double anglex = 0;
double angley = 0;

int arm1Len = 5;
int arm2Len = 5;

String SerialInput;
int coords [2];

void setup() {
  pinMode(enable1,OUTPUT);
  pinMode(microStep1,OUTPUT);
  pinMode(step1,OUTPUT);
  pinMode(dir1,OUTPUT);
  pinMode(enable2,OUTPUT);
  pinMode(microStep2,OUTPUT);
  pinMode(step2,OUTPUT);
  pinMode(dir2,OUTPUT);
  Serial.begin(9600);
  Serial.println();
}

void loop() {
  if(mot1Steps >= angleToSteps(deg(a1))*-1) {
    mStepOne(0);
  }
  if(deg(a2) > 0){
    if(mot2Steps <= angleToSteps(deg(a2))) {
      mStepTwo(1);
    }
  }
  else {
    if(mot2Steps >= angleToSteps(deg(a2))) {
      mStepTwo(0);
    }
  }
  int i = 0;
  while( Serial.available() != 0) {
    SerialInput = Serial.readString();
  }
  


  // Serial.println(angleToSteps(deg(a1))*-1);
  // Serial.println(deg(a2));
  // Serial.println();
  angles(coords[0],coords[1]);
}

void mStepOne(int dir) {
  if(dir == 0) {
    mot1Steps -= 0.5;
  }
  if(dir == 1) {
    mot1Steps += 0.5;
  }
  digitalWrite(enable1, LOW);
  digitalWrite(microStep1, HIGH);
  digitalWrite(dir1, dir);
  for(int i = 0; i < 2; i++){
    if(!lastState1) {
    digitalWrite(step1,HIGH);
    }
    if(lastState1) {
      digitalWrite(step1,LOW);
    }
    lastState1 = digitalRead(step1);
  }
  delay(10);
}

void mStepTwo(int dir) {
  if(dir == 0) {
    mot2Steps -= 0.5;
  }
  if(dir == 1) {
    mot2Steps += 0.5;
  }
  digitalWrite(enable2, LOW);
  digitalWrite(microStep2, HIGH);
  digitalWrite(dir2, dir);
  for(int i = 0; i < 2; i++) {
    if(!lastState2) {
      digitalWrite(step2,HIGH);
    }
    if(lastState2) {
      digitalWrite(step2,LOW);
    }
    lastState2 = digitalRead(step2);
  }
  delay(10);
}

void angles(double x, double y) {
  double dist = distance(x,y);
  double D1 = atan2(y,x);
  double D2 = lawOfCosines(dist,arm1Len,arm2Len);
  a1 = D1+D2;
  a2 = lawOfCosines(arm1Len,arm2Len,dist);
}

double lawOfCosines(double a, double b, double c) {
  return acos(((a*a)+(b*b)-(c*c))/(2*a*b));
}

double distance(double x, double y) {
  return sqrt((x*x)+(y*y));
}

double deg(float rad) {
  int tempAngle = rad*(180/PI);
  if(tempAngle < 0) {
    return tempAngle + 360;
  }
  else {
    return tempAngle;
  }
}

double stepsToAngle(double steps) {
  double angle = steps*1.8;
  return angle;
}

int angleToSteps(double angle) {
  int steps = angle/1.8;
  return steps;
}