//Librería que nos ayuda a controlar el movimiento del motor(JOYSTICK)
#include <Stepper.h>

//Declaramos las variables del joystick, pines analógicos
#define Y_PIN A0
#define X_PIN A1
#define STAT_PIN 2

//Declaramos las variables para los LED
int LED_DERECHA_FRENTE = 4;
int LED_IZQUIERDA_FRENTE = 5;
int LED_DERECHA_ATRAS = 3;
int LED_IZQUIERDA_ATRAS = 15;

//Revoluciones, pasos de los motores
const int stepsPerRevolution = 200;

const int vel_normal = 40;
const int vel_alta = 100;

// Declara y define los motores con sus pines.
Stepper motor_izquierda_frente(stepsPerRevolution, 13, 12, 11, 10);
Stepper motor_derecha_frente(stepsPerRevolution, 9, 8, 7, 6);
Stepper motor_izquierda_atras(stepsPerRevolution, A2, A3, A4, A5);
Stepper motor_derecha_atras(stepsPerRevolution, A6, A7, A8, A9);

void setup() {
  pinMode(Y_PIN, INPUT);
  pinMode(X_PIN, INPUT);
  pinMode(STAT_PIN, INPUT);

  pinMode(LED_DERECHA_FRENTE, OUTPUT);
  pinMode(LED_IZQUIERDA_FRENTE, OUTPUT);
  pinMode(LED_DERECHA_ATRAS, OUTPUT);
  pinMode(LED_IZQUIERDA_ATRAS, OUTPUT);

  // initialize the serial port:
  Serial.begin(9600);
}

void giroIzquierda() {
  // deposita en el 'pin' un valor HIGH (alto)
  digitalWrite(LED_IZQUIERDA_FRENTE, HIGH);
  digitalWrite(LED_IZQUIERDA_ATRAS, HIGH);

  //Establece la velocidad del motor
  motor_izquierda_frente.setSpeed(vel_normal);
  motor_izquierda_atras.setSpeed(vel_normal);
  motor_derecha_frente.setSpeed(vel_alta);
  motor_derecha_atras.setSpeed(vel_alta);

  for (int i = 0; i < stepsPerRevolution; i++) {
      //Gira el motor 
      motor_izquierda_frente.step(-1);
      motor_izquierda_atras.step(1);
      motor_derecha_frente.step(-2);
      motor_derecha_atras.step(2);
   
  }

  // deposita en el 'pin' un valor LOW (bajo)
  digitalWrite(LED_IZQUIERDA_FRENTE, LOW);
  digitalWrite(LED_IZQUIERDA_ATRAS, LOW);
}

void turnRight() {
  digitalWrite(LED_DERECHA_FRENTE, HIGH);
  digitalWrite(LED_DERECHA_ATRAS, HIGH);

  motor_derecha_frente.setSpeed(vel_normal);
  motor_derecha_atras.setSpeed(vel_normal);

  motor_izquierda_frente.setSpeed(vel_alta);
  motor_izquierda_atras.setSpeed(vel_alta);


  for (int i = 0; i < stepsPerRevolution; i++) {
    motor_derecha_frente.step(-1);
    motor_derecha_atras.step(1);
     motor_izquierda_frente.step(-2);
    motor_izquierda_atras.step(2);
    
  }

  digitalWrite(LED_DERECHA_FRENTE, LOW);
  digitalWrite(LED_DERECHA_ATRAS, LOW);
}

void delante() {
   digitalWrite(LED_IZQUIERDA_FRENTE, HIGH);
  digitalWrite(LED_DERECHA_FRENTE, HIGH);
  digitalWrite(LED_IZQUIERDA_ATRAS, LOW);
  digitalWrite(LED_DERECHA_ATRAS, LOW);


  motor_izquierda_atras.setSpeed(vel_normal);
  motor_izquierda_frente.setSpeed(vel_normal);
  motor_derecha_frente.setSpeed(vel_normal);
  motor_derecha_atras.setSpeed(vel_normal);


  for (int i = 0; i < stepsPerRevolution; i++) {
    motor_izquierda_frente.step(-1);
    motor_derecha_frente.step(-1);
    motor_izquierda_atras.step(1);
    motor_derecha_atras.step(1);
  }


  digitalWrite(LED_IZQUIERDA_FRENTE, LOW);
  digitalWrite(LED_DERECHA_FRENTE, LOW);
  digitalWrite(LED_IZQUIERDA_ATRAS, LOW);
  digitalWrite(LED_DERECHA_ATRAS, LOW);
}


void atras() {
  digitalWrite(LED_IZQUIERDA_FRENTE, LOW);
  digitalWrite(LED_DERECHA_FRENTE, LOW);
  digitalWrite(LED_IZQUIERDA_ATRAS, HIGH);
  digitalWrite(LED_DERECHA_ATRAS, HIGH);

  motor_izquierda_frente.setSpeed(vel_normal);
  motor_izquierda_atras.setSpeed(vel_normal);
  motor_derecha_frente.setSpeed(vel_normal);
  motor_derecha_atras.setSpeed(vel_normal);

  for (int i = 0; i < stepsPerRevolution; i++) {
    motor_izquierda_frente.step(1);
    motor_derecha_frente.step(1);
    motor_izquierda_atras.step(-1);
    motor_derecha_atras.step(-1);
  }

  digitalWrite(LED_IZQUIERDA_FRENTE, LOW);
  digitalWrite(LED_DERECHA_FRENTE, LOW);
  digitalWrite(LED_IZQUIERDA_ATRAS, LOW);
  digitalWrite(LED_DERECHA_ATRAS, LOW);
}
void loop() {
  int status_pin = digitalRead(STAT_PIN);
  int x_pin = analogRead(X_PIN);
  int y_pin = analogRead(Y_PIN);

  int x = map(x_pin, 0, 1023, 0, 180);
  int y = map(y_pin, 0, 1023, 0, 180);

  if (x == 0 && y == 90) {
    turnRight();
  }
  else if (x == 180 && y == 90) {
    giroIzquierda();
  }
  else if (x == 90 && y == 180) {
    delante();
  }
  else if (x == 90 && y == 0) {
    atras();
  }
}