#include <Servo.h>
#include <math.h>
Servo myservo;

void move();
void moveXYZ();
// Variable Decleration
int acc_dec_distance = 600;
int initial_final_speed = 2011;
int tm = 150;  // Delay for stepper movement

int on = 1000;  // Delay after order in ms
int down = 30;  // Servo angle for lowering
int x = 0;      // Current X position
int y = 0;      // Current Y position
int z = 0;      // Servo control (0 - off, 1 - down, 2 - down without delay)
int x1 = 0;     // Target X position
int y1 = 0;     // Target Y position
int x_new = 0;  // New target X position
int y_new = 0;  // New target Y position
int z_new = 0;  // New target servo control
int R = 1;      // Flag for page 1
int W = 1;      // Flag for page 2
int o1 = 1;     // Unused flag
int T = 50;     // Step delay for both axes

// Pin Assignment
int limetx = 8;  // X-axis limit switch
int limety = 9;  // Y-axis limit switch
int step1 = 3;   // Stepper X Pul+
int step2 = 5;   // Stepper Y Clk+
int dir1 = 2;    // Stepper X Dir+
int dir2 = 4;    // Stepper Y Cw+
int x2 = A3;     // Joystick X-axis input
int y2 = A4;     // Joystick Y-axis input
int valx = 0;    // Joystick X-axis value
int valy = 0;    // Joystick Y-axis value
int timex = 1;   // Time delay for X-axis movement
int timey = 1;   // Time delay for Y-axis movement

void setup() {
  Serial1.begin(9600);  //for the screen and motors
  Serial.begin(9600);   // for python
  myservo.attach(10);   //Servo motor
  pinMode(limetx, INPUT_PULLUP);
  pinMode(limety, INPUT_PULLUP);
  pinMode(step1, OUTPUT);
  pinMode(step2, OUTPUT);
  pinMode(dir1, OUTPUT);
  pinMode(dir2, OUTPUT);
  digitalWrite(dir1, LOW);
  myservo.write(0);
  /*  while (digitalRead(limetx) == HIGH) {
    digitalWrite(dir1, HIGH);
    digitalWrite(step1, LOW);
    delayMicroseconds(500);
    digitalWrite(step1, HIGH);
    delayMicroseconds(500);
  }
  delay(1000);
    for (int i = 0; i <= 100; i++) {
    digitalWrite(dir1, LOW);
    digitalWrite(step1, LOW);
    delayMicroseconds(1000);
    digitalWrite(step1, HIGH);
    delayMicroseconds(1000);
  }

  while (digitalRead(limety) == HIGH) {
    digitalWrite(dir2, HIGH);
    digitalWrite(step2, LOW);
    delayMicroseconds(500);
    digitalWrite(step2, HIGH);
    delayMicroseconds(500);
  }
  delay(1000);
      for (int i = 0; i <= 100; i++) {
    digitalWrite(dir2, LOW);
    digitalWrite(step2, LOW);
    delayMicroseconds(1000);
    digitalWrite(step2, HIGH);
    delayMicroseconds(1000);
  }
  */
}
void loop() {
  Serial.println("going to 1");
  x = 3000;
  y = 4000;
  z = 1;
  moveXYZ();
  delay(500);

  Serial.println("back to 0");
  x = 0;
  y = 0;
  z = 0;
  moveXYZ();
  delay(500);

  Serial.println("going to 1 second time");
  x = 300;
  y = 400;
  z = 2;
  moveXYZ();
  delay(500);

  Serial.println("back to 0 again");
  x = 0;
  y = 0;
  z = 0;
  moveXYZ();
  delay(500);
}
void moveXYZ() {
  // int acc_dec_distance = 500;
  // int initial_final_speed = 1150;
  // int tm = 150;   // Delay for stepper movement
  // int acceleration = 2

  //Direction
  int xDirection = (x + 1 >= x1) ? LOW : HIGH;
  int yDirection = (y + 1 >= y1) ? LOW : HIGH;
  digitalWrite(dir1, xDirection);
  Serial.println(xDirection);
  digitalWrite(dir2, yDirection);
  Serial.println(yDirection);

  // Distance
  int remainingStepsX = abs(x1 - x);
  int accdecStepsX = min(int(remainingStepsX) / 2, acc_dec_distance);
  int constStepsX = (remainingStepsX > 2 * acc_dec_distance) ? remainingStepsX - 2 * acc_dec_distance : 0;
  int remainingStepsY = abs(y1 - y);
  int accdecStepsY = min(int(remainingStepsY) / 2, acc_dec_distance);
  int constStepsY = (remainingStepsY > 2 * acc_dec_distance) ? remainingStepsY - 2 * acc_dec_distance : 0;

  // Speed
  int currentSpeedX = initial_final_speed;
  int currentSpeedY = initial_final_speed;
/*  Serial.print(remainingStepsX);
  Serial.print("-");
  Serial.print(accdecStepsX);
  Serial.print("-");
  Serial.println(constStepsX);*/

  for (int i = 0; i <= remainingStepsX + 1; i++) {
    //Serial.print("i: "); Serial.print(i);
    //Serial.print(". Curr Speed: ");Serial.print(currentSpeedX);
    //Acceleration X
    if (i < accdecStepsX) {
      //Serial.print(". Doing ACC");
      if ((i+1 % 10) == 0){
      currentSpeedX -= i/10;
      }
    } else if (i >= accdecStepsX + constStepsX ) {
      if ((i+1 % 10) == 0){
      currentSpeedX += remainingStepsX - i/10;
      }
      
      //Serial.print(". Doing Dec");
    } else {
      currentSpeedX = tm;    
      //Serial.print(". Doing Const");
    }
    digitalWrite(step1, LOW);
    delayMicroseconds(currentSpeedX);
    digitalWrite(step1, HIGH);
    delayMicroseconds(currentSpeedX);
    if (x1 < x) {
      x1 += 1;
    } else {
      x1 -= 1;
    }
    //Serial.println("\n-----------");
  }

  for (int i = 0; i <= remainingStepsY + 1; i++) {
    //Acceleration X
    if (i < accdecStepsY) {
      currentSpeedY -= i;
    } else if (i > accdecStepsY + constStepsY ) {
      currentSpeedY += i;
    } else {
      currentSpeedY = tm;    
    }
    digitalWrite(step2, LOW);
    delayMicroseconds(currentSpeedY);
    digitalWrite(step2, HIGH);
    delayMicroseconds(currentSpeedY);
    if (y1 < y) {
      y1 += 1;
    } else {
      y1 -= 1;
    }
  }

  // Z Servo Control
  if (z == 1) {
    myservo.write(down);
    delay(500);
    myservo.write(0);
  }
  if (z == 2) { myservo.write(down); }
  if (z == 0) { myservo.write(0); }
  delay(on);
}


/*




*/