// Adafruit Motor shield library
// copyright Adafruit Industries LLC, 2009
// this code is public domain, enjoy!

#include <AFMotor.h>
#include <SoftwareSerial.h>
#include <Servo.h>

AF_DCMotor motor1(2);
AF_DCMotor motor2(1);
AF_DCMotor motor3(3);
AF_DCMotor motor4(4);

volatile int contador = 0; // Declaramos como 'volatile' las variables que participan dentro y fuera de la interrupción.
int  n = 0;                // Variable auxiliar para notar cambios en el contador.
int vueltas = 0;
//int paso = 128;           // Flancos de subida y bajada necesarios para que el encoder de una vuelta completa.
//int paso = 64;              // Flancos de subidoa o bajada necesarios para que el encoder de una vuelta completa.
int paso = 32;              // Cantidad de dientes. ¿Dará solo una vuelta por fin?
int pasosvuelta = 42;      // Pasos del engranaje grande (blanco) para dar una vuelta completa.
int vueltacompleta = paso * pasosvuelta; // Cuenta para detectar vuelta completa.
int girarcuello = vueltacompleta;

int luztrasera = 13;
int luzdelantera = A1;
int mibocina = A0;

Servo servo1;
Servo servo2;
int angservo1;
int angservo2;

boolean buzzer = false;
boolean motores = false;

SoftwareSerial BT1(0, 1); // RX | TX

char comando = 'S';
char prevComando = 'A';
int velocidad = 255;
unsigned long timer0 = 2000;  //Stores the time (in millis since execution started)
unsigned long timer1 = 0;  //Stores the time when the last command was received from the phone
//int val;
int minimo = 650;
int maximo = 2550;

void setup() {
  //  Serial.begin(9600);           // set up Serial library at 9600 bps
  pinMode(mibocina, OUTPUT);      // Bocina
  pinMode(luzdelantera, OUTPUT);      // Luz delantera
  pinMode(luztrasera, OUTPUT);      // Luz trasera

  servo1.attach(9, minimo, maximo);     // 1000 y 2000 son valores para el SG90
  servo2.attach(10, minimo, maximo);
  angservo1 = maximo;
  angservo2 = 0;
  servo1.write(angservo1);
  servo2.write(angservo2);

  // turn on motor
  motor1.setSpeed(velocidad);       // Rueda
  motor2.setSpeed(velocidad);       // Rueda
  motor3.setSpeed(velocidad);       // Cuello
  motor4.setSpeed(velocidad);       // Pinza

  motor1.run(RELEASE);
  motor2.run(RELEASE);
  motor3.run(RELEASE);
  motor4.run(RELEASE);

  BT1.begin(9600);

  attachInterrupt(0, EncoderCuello, RISING);    // Sensor del encoder del motor de la cabeza.

}

void loop() {
  if (BT1.available()) { // Usar si control por bluetooth.
    //  if(Serial.available()>0) { // Usar si control por puerto serie.
    timer1 = millis();
    prevComando = comando;
    comando = BT1.read();
    //    Serial.write(comando);
    //  comando = Serial.read(); // Usar si control por puerto serie.
    if (comando != prevComando) {
      switch (comando) {

        // Código para los servos. Minúsculas para servo 1 y mayúsuculas para servo2
        case 'a':
          servo1.writeMicroseconds(minimo);
          angservo1 = minimo;
          break;
        case 'c':
          servo1.write(minimo+(maximo - minimo)/4);
          angservo1 = minimo+(maximo - minimo)/4;
          break;
        case 'e':
          servo1.writeMicroseconds(minimo+(maximo - minimo)/2);
          angservo1 = minimo+(maximo - minimo)/2;
          break;
        case 'p':
          servo1.write(minimo+(maximo - minimo)*3/4);
          angservo1 = minimo+(maximo - minimo)*3/4;
          break;
        case 'z':
          servo1.writeMicroseconds(maximo);
          angservo1 = maximo;
          break;
        case 'A':
          servo2.write(minimo);
          angservo2 = minimo;
          break;
        case 'C':
          servo2.write(minimo+(maximo - minimo)/4);
          angservo2 = minimo+(maximo - minimo)/4;
          break;
        case 'E':
          servo2.write(minimo+(maximo - minimo)/2);
          angservo2 = minimo+(maximo - minimo)/2;
          break;
        case 'P':
          servo2.write(minimo+(maximo - minimo)*3/4);
          angservo2 = minimo+(maximo - minimo)*3/4;
          break;
        case 'Z':
          servo2.write(maximo);
          angservo2 = maximo;
          break;
        case 't':
          angservo1 = angservo1 + (maximo - minimo)/180;
          if (angservo1 > maximo) {
            angservo1 = maximo;
          };
          servo1.write(angservo1);
          break;
        case 'T':
          angservo2 = angservo2 + (maximo - minimo)/180;
          if (angservo2 > maximo) {
            angservo2 = maximo;
          };
          servo2.write(angservo2);
          break;
        case 'y':
          angservo1 = angservo1 + (maximo - minimo)/18;
          if (angservo1 > maximo) {
            angservo1 = maximo;
          };
          servo1.write(angservo1);
          break;
        case 'Y':
          angservo2 = angservo2 + (maximo - minimo)/18;
          if (angservo2 > maximo) {
            angservo2 = maximo;
          };
          servo2.write(angservo2);
          break;
        case 'k':
          angservo1 = angservo1 + (maximo - minimo)/4;
          if (angservo1 > maximo) {
            angservo1 = maximo;
          };
          servo1.write(angservo1);
          break;
        case 'K':
          angservo2 = angservo2 + (maximo - minimo)/4;
          if (angservo2 > maximo) {
            angservo2 = maximo;
          };
          servo2.write(angservo2);
          break;
        case 'm':
          angservo1 = angservo1 -(maximo - minimo)/180;
          if (angservo1 < minimo) {
            angservo1 = minimo;
          };
          servo1.write(angservo1);
          break;
        case 'M':
          angservo2 = angservo2 - (maximo - minimo)/180;
          if (angservo2 < minimo) {
            angservo2 = minimo;
          };
          servo2.write(angservo2);
          break;
        case 'n':
          angservo1 = angservo1 - (maximo - minimo)/18;
          if (angservo1 < minimo) {
            angservo1 = minimo;
          };
          servo1.write(angservo1);
          break;
        case 'N':
          angservo2 = angservo2 - (maximo - minimo)/18;
          if (angservo2 < minimo) {
            angservo2 = minimo;
          };
          servo2.write(angservo2);
          break;
        case 'o':
          angservo1 = angservo1 - (maximo - minimo)/4;
          if (angservo1 < minimo) {
            angservo1 = minimo;
          };
          servo1.write(angservo1);
          break;
        case 'O':
          angservo2 = angservo2 - (maximo - minimo)/4;
          if (angservo2 < minimo) {
            angservo2 = minimo;
          };
          servo2.write(angservo2);
          break;


        // Código para bocina.
        case 'V':
          buzzer = true;
          break;
        case 'v':
          buzzer = false;
          break;
        // Código para motores.
        case 'F':
          if (motores) {
            motor3.run(FORWARD);
          }
          else {
            motor1.run(FORWARD);
            motor2.run(FORWARD);
          };
          break;
        case 'f':
          motor3.run(FORWARD);
          break;
        case 'B':
          if (motores) {
            motor3.run(BACKWARD);
          } else {
            motor1.run(BACKWARD);
            motor2.run(BACKWARD);
          };
          break;
        case 'b':
          motor3.run(BACKWARD);
          break;
        case 'L':
          if (motores) {
            motor4.run(FORWARD);
          } else {
            motor2.run(FORWARD);
            motor1.run(BACKWARD);
          };
          break;
        case 'l':
          motor4.run(FORWARD);
          break;
        case 'R':
          if (motores) {
            motor4.run(BACKWARD);
          } else {
            motor1.run(FORWARD);
            motor2.run(BACKWARD);
          };
          break;
        case 'r':
          motor4.run(BACKWARD);
          break;
        case 'S':               // Solo detiene ruedas
          motor2.run(RELEASE);
          motor1.run(RELEASE);
          break;
        case 'D':              // Detiene todos los motores
          motor2.run(RELEASE);
          motor1.run(RELEASE);
          motor3.run(RELEASE);
          motor4.run(RELEASE);
          break;
        // Código para luces.
        case 'W':
          digitalWrite(luzdelantera, HIGH);
          break;
        case 'w':
          digitalWrite(luzdelantera, LOW);
          break;
        case 'U':
          digitalWrite(luztrasera, HIGH);
          break;
        case 'u':
          digitalWrite(luztrasera, LOW);
          break;
        // Códigos para cuello.
        case 'Q':
          contador = 0;
          break;
        case 'x':
          motores = false;
          break;
        case 'X':
          motores = true;
          break;
        case 'q':
          girarcuello = vueltacompleta;
          contador = 0;
          break;
        case '9':
          girarcuello = vueltacompleta;
          contador = 0;
          break;
        case '8':
          girarcuello = vueltacompleta / 2;
          contador = 0;
          break;
        case '7':
          girarcuello = vueltacompleta / 2;
          contador = 0;
          break;
        case '6':
          girarcuello = vueltacompleta / 3;
          contador = 0;
          break;
        case '5':
          girarcuello = vueltacompleta / 3;
          contador = 0;
          break;
        case '4':
          girarcuello = vueltacompleta / 4;
          contador = 0;
          break;
        case '3':
          girarcuello = vueltacompleta / 4;
          contador = 0;
          break;
        case '2':
          girarcuello = 32;
          contador = 0;
          break;
        case '1':
          girarcuello = 32;
          contador = 0;
          break;
      }
    }
  }
  else {
    timer0 = millis();  //Get the current time (millis since execution started).
    //Check if it has been 500ms since we received last command.
    if ((timer0 - timer1) > 500) {
      //More tan 500ms have passed since last command received, car is out of range.
      //Therefore stop the car and turn lights off.
      motor2.run(RELEASE);
      motor1.run(RELEASE);
    }
  }

  if (buzzer) {
    if ((millis() % 2) == 1) {
      digitalWrite(mibocina, HIGH);
    }
    else {
      digitalWrite(mibocina, LOW);
    }
  }
  if (contador > girarcuello) {
    contador = 0;
    motor3.run(RELEASE);
    vueltas++;
  }
}

void EncoderCuello() {
  contador++;
}