#include "Stepper.h"
#include "Servo.h"
//#include "Parser.h
#include "AccelStepper.h"
// #include <Parser.h>

// #include <AccelStepper.h>
// Define Constants  Connections to A4988
const int dirPin_x = 2;  // Direction 1
const int stepPin_x = 3; // Step 1
const int dirPin_y = 7;  // Direction 2
const int stepPin_y = 6; // Step 2
// Motor steps per rotation
const int STEPS_PER_REV = 200;
const int STEPS_BY_USER = 80;

int direction_x=1;
int direction_y=1;
char choice;
int x_axis = 0; int y_axis = 0;
int arr_index=0;
int plate_data[2]={}; // x_count,y_count

void setup()
{ 
  Serial.begin(9600);
  pinMode(5, INPUT);
  pinMode(stepPin_x, OUTPUT);
  pinMode(dirPin_x, OUTPUT);
  pinMode(stepPin_y, OUTPUT);
  pinMode(dirPin_y, OUTPUT);
  Serial.println("m -for start. s -for stop. h- home position" );
  // Serial.flush();
  //work_out:  
  // Setup the pins as Outputs
  // Serial.end();
}

void _delay()
{
  for (long i=0; i<900000; i++) { asm(""); }
}

void loop()
{
  set_operation();
  
  // if (plate_data[0]!=0 & plate_data[1]!=0 )
  //   { motors(plate_data); }
  choice=' ';
  Serial.flush();

}

void moves_home(int steps_x, int steps_y )
{
  Serial.println(plate_data[0]);
  Serial.print(plate_data[1]);
  for (int x = 0; x < steps_x*plate_data[0]; x++) 
  {
          digitalWrite(stepPin_x, HIGH);
          delayMicroseconds(1000);
          digitalWrite(stepPin_x, LOW);
          delayMicroseconds(5000); // Speed run !!!!
  }
  for (int y = 0; y < steps_y*plate_data[0]*plate_data[1]; y++) 
  {
          digitalWrite(stepPin_y, HIGH);
          delayMicroseconds(1000);
          digitalWrite(stepPin_y, LOW);
          delayMicroseconds(5000); // Speed run !!!!
  }
}
void moves(int motor_pin, int motor_step, int delay_ms)
{
  for (int x = 0; x < motor_step; x++) 
  {
          digitalWrite(motor_pin, HIGH);
          delayMicroseconds(1000);
          digitalWrite(motor_pin, LOW);
          delayMicroseconds(delay_ms); // Speed run !!!!
  }
}

void motors (int direction) 
{
 // Set motor direction clockwise
  digitalWrite(dirPin_y, direction_y);
  digitalWrite(dirPin_x, direction_x);
  int x_count=0; int y_count=0;
  int home_move_delay=50;
    while (x_count < plate_data[0])
      {
        // Spin motor one rotation slowly
        if (direction==0) { moves(stepPin_x,STEPS_BY_USER, home_move_delay);}
        else { moves(stepPin_x,STEPS_BY_USER, 10000); }

        
        x_count++; _delay(); 
        x_axis=x_axis+STEPS_BY_USER;
        Serial.print("  step_x # "); Serial.println(x_count);
           
        while( y_count < plate_data[1])
        {
          moves(stepPin_y, STEPS_PER_REV, 3000);
          
          y_count++; y_axis=y_axis+STEPS_PER_REV;
          _delay();
          Serial.print("step_y # "); Serial.println(y_count);

        }//while y_axis
        y_count=0;
          
      } // while x_axis  
    // if (x_count>plate_data[0]) break;   
 
  //delay(7000);
} //motors

void home_direction()
{
  direction_y^=1;
  direction_x^=1;
  digitalWrite(dirPin_y, direction_y);
  digitalWrite(dirPin_x, direction_x);
  //Serial.println((x_axis)); Serial.println((y_axis));
}

void set_operation()
{
    while(choice!='s' & choice!='m' & choice!='h') 
    {
      if (Serial.available()) 
      { choice = Serial.read(); Serial.println(choice); }
    }
    arr_index=0; 
    Serial.print("Your choice:"); Serial.println(choice);
    switch (choice) 
    {
      case 'm': // read custom array data
        plate_data[0]=0; plate_data[1]=0;
        while(arr_index<=1) 
        {
          if (Serial.available()) 
          {
            char val = Serial.read();
            plate_data[arr_index]=val-'0'; 
            Serial.println(plate_data[arr_index]);
            arr_index++;
          }
        }
        motors(direction_x);
       break;

      case 's': 
             Serial.println(choice); 
              // if (Serial.available()) 
              // {
              //   char val = Serial.read();
              //   //-'0' convert char to int
              //   plate_data[arr_index]=val-'0'; 
              //   Serial.println(plate_data[arr_index]);
              // }
              break;
      case 'h': { int x=0; 
                  Serial.println(choice);  
                  home_direction();
                  moves_home(STEPS_BY_USER,STEPS_PER_REV);
                  home_direction();
                  Serial.println(direction_x);
                }
        break;
    } // switch 
} // set_operation