//Definizione dei PIN
const int pin_dir = 5;
const int pin_step = 2;

const int pin_limit_switch_under = 4;
const int pin_limit_switch_over = 6;

const int num_step_motore = 200; //E' il numero di step per rotazione del motore (potrebbe differire in base al modello)
//const long velocita = 1000; //Si tratta di microsecondi tra un impulso e l'altro sul pin STEP
const int step_per_mm = 80;

const int slider_length = 1000;

const int pin_led = 13;

const int jog_y = A0;
const int jog_x = A1;
const int pin_speed_override = A2;

int speed_override = 0;


enum Direction{
  UP, DOWN, RIGHT, LEFT, OVER, UNDER
};

void execute_steps(int num_step, Direction direction, int velocita){
  if(direction == OVER){
    digitalWrite(pin_dir, HIGH);
  }else if(direction == UNDER){
    digitalWrite(pin_dir, LOW);
  }
  

  for (int x = 0; x < num_step; x++) {
    digitalWrite(pin_step, HIGH);
    delayMicroseconds(velocita);
    digitalWrite(pin_step, LOW);
    delayMicroseconds(velocita);
  }
}

void motion(Direction direction, int velocita){

  int limit_switch_value = 0;
  int limit_switch_pin = 0;

  if(direction == OVER){
    limit_switch_pin = pin_limit_switch_under;
    
  }else if(direction == UNDER){
    limit_switch_pin = pin_limit_switch_over;
  }

  //first check
  limit_switch_value = digitalRead(limit_switch_pin);

  while(limit_switch_value == 0){

    execute_steps(1, direction, velocita);

    limit_switch_value = digitalRead(limit_switch_pin);
  }
}

void move_x_axis(Direction direction, int velocita, int steps){
  execute_steps(steps, direction, velocita);
}

void init_slider(){
  motion(OVER, 600);
}

void setup() {
  //inizializzo i PIN come OUTPUT
  pinMode(pin_step, OUTPUT);
  pinMode(pin_dir, OUTPUT);
  pinMode(pin_led, OUTPUT);
  pinMode(pin_limit_switch_over, INPUT);
  pinMode(pin_limit_switch_under, INPUT);


  Serial.begin(9600);
  //init_slider();

}

void motor1(){
  speed_override = map(analogRead(pin_speed_override), 0, 1023, 0, 400);
  int steps = 1;
  int x = analogRead(jog_x);
  int y = analogRead(jog_y);

  //Serial.println("value:" + String(x));


  if(x != 512){
    Direction x_direction;
    int speed = 1000;
    if(x<512){
      x_direction = UNDER;
      //speed = speed - map(x, 512, 0, 0, 200);
      steps = steps + map(x, 512, 0, 0, 10);
    }else{
      x_direction = OVER;
      //speed = speed - map(x, 512, 1023, 0, 200);
      steps = steps + map(x, 512, 1023, 0, 10);
    }

    if(speed_override > 0){
      speed = speed - speed_override;
      //steps = steps + (speed_override/10);
    } 

    Serial.println("Speed: " + String(speed));

    move_x_axis(x_direction, speed, steps);
  }
}

void motor2(){

}

void loop() {

  motor1();
  
  
}