//Sketch name: plotter X_Y serial in 0

// Plotter limits definitions
#define limitX_L 9
#define limitX_R 10
#define limitY_U 11
#define limitY_D 12

// X defenisions *****
#define Dir_X 5
#define Step_X 2
unsigned long X_Time;
unsigned long X_TimeOLD;
int dirX = 0;
int speedX = 0;
int jsX = 0;
int jsIN_X = 0;
byte X_Status = 0; //Signal for X steps
//int jsIN_X=80; // Joystick input X (temp)<--------

// Y defenisions *****
#define Dir_Y 6
#define Step_Y 3
unsigned long Y_Time;
unsigned long Y_TimeOLD;
int dirY = 0;
int speedY = 0;
int jsY = 0;
int jsIN_Y = 0;
byte Y_Status = 0; //Signal for Y steps
//int jsIN_Y=-50; // Joystick input Y (temp) <-------

#define pen_U_D 11 
#define Enable_X_Y 8

int speedMax = 129;
int penStat = 0;
String jsIN; //<--------- Input from joystick
int JSvalues[3] = {0, 0, 0};
byte i = 0;

void setup() {
  pinMode(Dir_X, OUTPUT); // DIR X
  pinMode(Step_X, OUTPUT); // STEP X
  pinMode(Dir_Y, OUTPUT); // DIR Y
  pinMode(Step_Y, OUTPUT); // STEP Y
  pinMode(Enable_X_Y, OUTPUT); // ENABLE X, Y
  pinMode(pen_U_D, OUTPUT); 
  pinMode (LED_BUILTIN, OUTPUT); // For pen_U_D indication only 
  digitalWrite(Dir_X, LOW);
  digitalWrite(Dir_Y, LOW);
  digitalWrite(Enable_X_Y, LOW); //ENABLE X,Y
  digitalWrite(LED_BUILTIN, LOW);
  X_Time = micros();
  X_TimeOLD = X_Time;
  Y_Time = micros();
  Y_TimeOLD = Y_Time;
  Serial.begin(9600);
  Serial.println("Speed X, Speed Y, Pen status:");
}

void loop() {
  // Enter parameters from Bluetooth
  do
  {
    if (Serial.available())
    {
      jsIN = Serial.readStringUntil('\n');
      JSvalues[i] = jsIN.toInt();
      ++i;
    }

    jsIN_X = JSvalues[0];
    jsIN_Y = JSvalues[1];
    penStat = JSvalues[2];

    jsX = jsIN_X;
    jsY = jsIN_Y;

// Diseble steppers if the joystick in home position
    if(jsIN_X==0 && jsIN_Y==0)
      {
        digitalWrite(Enable_X_Y, HIGH);
        digitalWrite(pen_U_D, LOW);
        digitalWrite(LED_BUILTIN, LOW); 
      } else digitalWrite(Enable_X_Y, LOW);
              
      

    //xxxxxxxxx X speed and direction xxxxxxxxxxxxxxxxxxxx
    if (jsX != 0)
    {
      if (jsIN_X < 0)
      {
        dirX = LOW;
        jsX = jsX * (-1);
      }
      else dirX = HIGH ;

      digitalWrite(Dir_X, dirX);
      //speedX = (speedMax - jsX)*18;
      speedX = sq(speedMax - jsX);

      if ((X_Time - X_TimeOLD) > speedX)
      {
        X_Status = !X_Status;
        digitalWrite(Step_X, X_Status);
        X_TimeOLD = X_Time;
      }
      X_Time = micros();

    }

    //xxxxxxxxx Y speed and direction xxxxxxxxxxxxxxxxxxxx


    if (jsY != 0)
    {
      if (jsIN_Y < 0)
      {
        dirY = HIGH;
        jsY = jsY * (-1);
      }
      else dirY = LOW;
      digitalWrite(Dir_Y, dirY);
      //speedY = (speedMax - jsY)*18;
      speedY = sq(speedMax - jsY);

      if ((Y_Time - Y_TimeOLD) > speedY)
      {
        Y_Status = !Y_Status;
        digitalWrite(Step_Y, Y_Status);
        Y_TimeOLD = Y_Time;
      }
      Y_Time = micros();
    }

// ******* Pen UP or DOWN *******     
    if (penStat == 1)
    {
      digitalWrite(LED_BUILTIN, HIGH);
      digitalWrite(pen_U_D, HIGH); // Pen touch the paper
    }
    else
    {
       digitalWrite(LED_BUILTIN, LOW);
       digitalWrite(pen_U_D, LOW); // Pen is up
    }


  } while (i <= 2);
  i = 0;
}