#include <Stepper.h>
int enablePin = 13;

int zUpPin = 11;
int zDownPin = 10;

int feedPin = 12;
int retractPin = 2;

int x1Pin = 9;
int x2Pin = 8;
int x3Pin = 7;
int x4Pin = 6;

int y1Pin = 5;
int y2Pin = 4;
int y3Pin = 3;
int y4Pin = 2;

int glueon = 2;
int gluecycle = 145;
int gluetick = 0;

#define xSTEPS 20
#define ySTEPS 20

Stepper xmotor(xSTEPS, x1Pin, x2Pin, x3Pin, x4Pin);
Stepper ymotor(ySTEPS, y1Pin, y2Pin, y3Pin, y4Pin);

int stepdelay = 100;

int feedtime = 2;


/*
x is x pos of head
y is y pos
x1 is path start x
x2 is path end x
y1 and y2 are for y
dx is delta x, how far x is from where is should be at current step in path. dy is the same for y axis.
steps is steps in path, which is x1 + y1 - x2 -y2,
sx is y move over steps slopes and yx is y move over steps slope
wx is the wanted x position at count step in path, wy is wanted y
px is the distance from x to wx, or the pull of x. p is d upside down. py is the same for y.
*/

int x = 0;
int y = 0;
int x1 = 0;
int x2 = 0;
int y1 = 0;
int y2 = 0;
float dx = 0;
float dy = 0;
int steps = 0;
float sx = 0;
float sy = 0;
float wx = 0;
float wy = 0;
float px = 0;
float py = 0;

void setup()
{
  pinMode(x1Pin, OUTPUT);
  pinMode(x2Pin, OUTPUT);
  pinMode(x3Pin, OUTPUT);
  pinMode(x4Pin, OUTPUT);

  pinMode(y1Pin, OUTPUT);
  pinMode(y2Pin, OUTPUT);
  pinMode(y3Pin, OUTPUT);
  pinMode(y4Pin, OUTPUT);

  pinMode(feedPin, OUTPUT);
  pinMode(retractPin, OUTPUT);

  pinMode(zUpPin, OUTPUT);
  pinMode(zDownPin, OUTPUT);
  xmotor.setSpeed(100);
  ymotor.setSpeed(100);
  Serial.begin(115200);
}


void move()
{
  Serial.print("at coords:");
  Serial.print(x);
  Serial.print(",");
  Serial.println(y);
  x1=x;
  y1=y;
  dx = x2 - x1;
  dy = y2 - y1;
  Serial.print("x2,y2:");
  Serial.print(x2);
  Serial.print(",");
  Serial.println(y2);

  steps = abs(dx) + abs(dy);
  Serial.print("need to move:");
  Serial.print(dx);
  Serial.print(",");
  Serial.println(dy);
  Serial.print("steps required:");
  Serial.println(steps);

  sx = dx / steps;
  sy = dy / steps;

  Serial.print("slopes:");
  Serial.print(sx);
  Serial.print(",");
  Serial.println(sy);
  digitalWrite(enablePin,HIGH);
  Serial.println("----------------------------");
  for (int i=0; i < steps + 1; i++){

    gluetick++;
    if (gluetick < glueon){
      digitalWrite(feedPin,HIGH);
    }
    if (gluetick > glueon){
      digitalWrite(feedPin,LOW);
    }
    if (gluetick > gluecycle){
      gluetick=0;
    }
    wx = sx * i + x1 + sx;
    wy = sy * i + y1 + sy;
    px = wx - x;
    py = wy - y;

    /*Serial.print("we are at coords:");
    Serial.print(x);
    Serial.print(",");
    Serial.println(y);

    Serial.print("wanted for this step:");
    Serial.print(wx);
    Serial.print(",");
    Serial.println(wy);

    Serial.print("pull is:");
    Serial.print(px);
    Serial.print(",");
    Serial.println(py);
*/
    if (abs(px) >= abs(py)){
      if (px > 0){
        stepxforward();
        //Serial.println("Stepping x up");
        x = x + 1;
      }else{
        stepxback();
        //Serial.println("Stepping x down");
        x = x - 1;
      }
    }else{
      if (py > 0){
        stepyforward();
        //Serial.println("Stepping y up");
        y = y + 1;
      }else{
        stepyback();
        //Serial.println("Stepping y down");
        y = y - 1;
      }
    }
   Serial.println("-----------------------");
   delay(5);
  }
  digitalWrite(enablePin,LOW);

}
void loop(){

  x2=20;
  y2=50;
  move();
  delay(1000);
  x2=130;
  y2=50;
  move();
  delay(1000);
  x2=130;
  y2=220;
  move();
  delay(1000);
  x2=20;
  y2=220;
  move();
  delay(1000);
  digitalWrite(zUpPin,HIGH);
  delay(25);
  digitalWrite(zUpPin,LOW);
}


void stepxforward(){
  xmotor.step(1);
}
void stepyforward(){
  ymotor.step(1);
}
void stepxback(){
  xmotor.step(-1);
}
void stepyback(){
  ymotor.step(-1);
}