//============================================================
//                          LCD
#define RS 5
#define EN 18

// Pines de LCD RS, EN, D4, D5, D6, D7
byte pines[] = { RS, EN, 19, 21, 22, 23 };

//============================================================

#define MENU 0
#define SERVO 1
#define M_PASOS 2
#define GIRANDO_SERVO 3
#define GIRANDO_M_PASOS 4
#define ERROR 5

byte estado;

byte servoPin = 13;
int angulo = 90;
int anguloActual = 0;

byte b1 = 15, b2 = 2, b3 = 4, b4 = 12;


unsigned long previusMillis = 0, previusMillisServo = 0;
unsigned long periodo = 200, periodoServo = 500;

byte FilasPin[] = { 14, 27, 26, 25 };
byte ColumnasPin[] = { 33, 32, 16, 17 };

byte teclas[4][4] = {
  { 1, 2, 3, 10 },
  { 4, 5, 6, 11 },
  { 7, 8, 9, 12 },
  { 14, 0, 15, 13 }
};

byte iFil = 0, iCol = 0, valorObtenido = 0;

byte digitos[] = { 16, 16, 16 };
int digito = 0;
byte huecos = 0;
byte escribir = false, negativo = false;


void setup() {
  // Pin PWM para servo
  pinMode(servoPin, OUTPUT);


  // Pines de las bobinas del motor a pasos
  pinMode(b1, OUTPUT);
  pinMode(b2, OUTPUT);
  pinMode(b3, OUTPUT);
  pinMode(b4, OUTPUT);

  // Pines para teclado matricial
  for (byte i = 0; i < 4; i++) {
    pinMode(FilasPin[i], OUTPUT);
    pinMode(ColumnasPin[i], INPUT_PULLUP);
    digitalWrite(FilasPin[i], HIGH);
  }

  // Pines de pantalla LCD
  for (byte i = 0; i < 6; i++) {
    pinMode(pines[i], OUTPUT);
  }

  // Inicialización de la pantalla LCD en modo de 4 bits
  EscribeComando(0x02);  // Inicializar en modo de 4 bits
  EscribeComando(0x28);  // Configurar el modo de 2 líneas y 5x8
  EscribeComando(0x0C);  // Limpiar pantalla
}

void loop() {
  digito = 0;
  huecos = 0;

  if (LeerTeclado() && millis() - previusMillis >= periodo) {
    previusMillis = millis();
    valorObtenido = teclas[iFil][iCol];

    if (valorObtenido == 14 && escribir) BorraUno();
    else if (valorObtenido == 15 && escribir) BorraTodo();
    else if (escribir) ActualizaDigitos();
  }

  for (byte i = 0; i < 3; i++) {
    if (digitos[i] == 16) huecos++;
  }

  for (byte i = 0; i < 3; i++) {
    if (digitos[i] != 16) digito += digitos[i] * pow(10, 2 - i - huecos);
  }

  switch (estado) {
    case MENU:
      escribir = false;
      ImprimirEnLCD("A->SERVO", 0, 4);
      ImprimirEnLCD("B->MOTOR A PASOS", 1, 0);

      if (valorObtenido == 10) {
        estado = SERVO;
        EscribeComando(0x01);  // Limpiar pantalla
      }

      if (valorObtenido == 11) {
        estado = M_PASOS;
        EscribeComando(0x01);  // Limpiar pantalla
      }

      break;
    case SERVO:
      escribir = true;
      ImprimirEnLCD("ANGULO:", 0, 0);
      ImprimirEnLCD("C->MENU", 1, 0);
      ImprimirEnLCD(digito, 0, 8);
      
      if (digito < 10) {
        ImprimirEnLCD("  ", 0, 9);
      }
      else if(digito < 100){
        ImprimirEnLCD(" ", 0, 10);
      }


      if (valorObtenido == 12) {
        estado = MENU;
        EscribeComando(0x01);  // Limpiar pantalla
      }

      if (valorObtenido == 13) {
        if (digito <= 180 && digito >= 0) {
          EscribeComando(0x01);  // Limpiar pantalla
          estado = GIRANDO_SERVO;
          previusMillisServo = millis();
        }
        else {
          EscribeComando(0x01);  // Limpiar pantalla
          estado = ERROR;
          previusMillisServo = millis();
        }
      }
      break;
    case GIRANDO_SERVO:
      escribir = false;
      GirarServo(digito);
      if (millis() - previusMillisServo >= periodoServo) {
        ImprimirEnLCD("FIN.", 0, 0);

        escribir = true;

        ImprimirEnLCD("MENU->C", 0, 7);
        ImprimirEnLCD("VOLVER->A", 1, 3);

        if (valorObtenido == 10) {
          estado = SERVO;
          EscribeComando(0x01);  // Limpiar pantalla
        }

        if (valorObtenido == 12) {
          estado = MENU;
          EscribeComando(0x01);  // Limpiar pantalla
        }
      }
      break;
    case ERROR:
      escribir = false;
      if (millis() - previusMillisServo >= periodoServo * 4) {
        escribir = true;

        ImprimirEnLCD("VOLVER->A", 0, 0);
        ImprimirEnLCD("MENU->C", 1, 0);

        if (valorObtenido == 10) {
          estado = SERVO;
          EscribeComando(0x01);  // Limpiar pantalla
        }

        if (valorObtenido == 12) {
          estado = MENU;
          EscribeComando(0x01);  // Limpiar pantalla
        }
      } else {
        ImprimirEnLCD("ERROR FUERA DE", 0, 0);
        ImprimirEnLCD("RANGO [0 - 180]", 1, 0);
      }
      break;
    case M_PASOS:
      escribir = true;
      ImprimirEnLCD("ANGULO:", 0, 0);
      ImprimirEnLCD("C->MENU", 1, 0);
      ImprimirEnLCD("A=+", 0, 13);
      ImprimirEnLCD("B=-", 1, 13);
      if (negativo) ImprimirEnLCD("-", 0, 7);
      else ImprimirEnLCD(" ", 0, 7);

      ImprimirEnLCD(digito, 0, 8);

      if (digito < 10){
        ImprimirEnLCD("  ", 0, 9);
      }
      else if(digito < 100){
        ImprimirEnLCD(" ", 0, 10);
      }

      if (valorObtenido == 10) negativo = false;
      if (valorObtenido == 11) negativo = true;

      if (valorObtenido == 12) {
        estado = MENU;
        EscribeComando(0x01);  // Limpiar pantalla
      }

      if (valorObtenido == 13) {
        EscribeComando(0x01);  // Limpiar pantalla
        estado = GIRANDO_M_PASOS;
        escribir = false;
      }
      break;
    case GIRANDO_M_PASOS:
      if (escribir == false) ImprimirEnLCD("ANGULO:", 0, 0);

      if (negativo) GirarMotorAPasos(-digito);
      else GirarMotorAPasos(digito);

      escribir = true;
      ImprimirEnLCD("FIN.", 0, 0);

      ImprimirEnLCD("MENU->C", 0, 8);
      ImprimirEnLCD("VOLVER->A", 1, 3);

      if (valorObtenido == 10) {
        estado = M_PASOS;
        EscribeComando(0x01);  // Limpiar pantalla
      }

      if (valorObtenido == 12) {
        estado = MENU;
        EscribeComando(0x01);  // Limpiar pantalla
      }
      break;
  }
}

void Adelante() {
  BobinasEstado(1, 0, 0, 0);
  delay(10);
  BobinasEstado(1, 1, 0, 0);
  delay(10);
  BobinasEstado(0, 1, 0, 0);
  delay(10);
  BobinasEstado(0, 1, 1, 0);
  delay(10);
  BobinasEstado(0, 0, 1, 0);
  delay(10);
  BobinasEstado(0, 0, 1, 1);
  delay(10);
  BobinasEstado(0, 0, 0, 1);
  delay(10);
  BobinasEstado(1, 0, 0, 1);
  delay(10);
}

void Atras() {
  BobinasEstado(0, 0, 0, 1);
  delay(10);
  BobinasEstado(0, 0, 1, 1);
  delay(10);
  BobinasEstado(0, 0, 1, 0);
  delay(10);
  BobinasEstado(0, 1, 1, 0);
  delay(10);
  BobinasEstado(0, 1, 0, 0);
  delay(10);
  BobinasEstado(1, 1, 0, 0);
  delay(10);
  BobinasEstado(1, 0, 0, 0);
  delay(10);
  BobinasEstado(1, 0, 0, 1);
  delay(10);
}


void BobinasEstado(byte a, byte b, byte c, byte d) {
  digitalWrite(b1, a);
  digitalWrite(b2, b);
  digitalWrite(b3, c);
  digitalWrite(b4, d);
}


void GirarServo(int angulo) {
  int pwmValue = map(angulo, 0, 180, 500, 2400);

  digitalWrite(servoPin, HIGH);
  delayMicroseconds(pwmValue);
  digitalWrite(servoPin, LOW);

  delayMicroseconds(20000 - pwmValue);
}


void GirarMotorAPasos(int angulo) {
  int pasosGiro;
  bool adelante;

  if (angulo > anguloActual) {
    pasosGiro = map(abs(angulo - anguloActual), 0, 360, 0, 512);
    adelante = true;
  } else {
    pasosGiro = map(abs(anguloActual - angulo), 0, 360, 0, 512);
    adelante = false;
  }

  for (int i = 0; i < pasosGiro; i++) {
    if (adelante) {
      Adelante();
      ImprimirEnLCD(map(i, 0, 512, 0, 360) + anguloActual, 0, 8);
    } else {
      Atras();
      ImprimirEnLCD(-map(i, 0, 512, 0, 360) + anguloActual, 0, 8);
    }
  }

  anguloActual = angulo;
}

bool LeerTeclado() {
  for (byte f = 0; f < 4; f++) {
    digitalWrite(FilasPin[f], LOW);


    for (byte c = 0; c < 4; c++) {
      if (digitalRead(ColumnasPin[c]) == LOW) {
        iFil = f;
        iCol = c;

        return true;
      }
    }
    digitalWrite(FilasPin[f], HIGH);
  }
  return false;
}

void ActualizaDigitos() {
  // Evita que el primer dígito sea cero.
  if (digitos[0] == 16 && (valorObtenido == 0 || valorObtenido > 9)) return;
  else if (digitos[0] == 16) {
    digitos[0] = valorObtenido;
    return;
  }

  for (byte i = 1; i < 3; i++) {
    if (digitos[i] == 16 && valorObtenido < 10) {
      digitos[i] = valorObtenido;
      break;
    }
  }
}

void BorraUno() {
  for (int i = 1; i < 3; i++) {
    if (digitos[2] != 16) {
      digitos[2] = 16;
      break;
    }
    if (digitos[i] == 16) digitos[i - 1] = 16;
  }
}

void BorraTodo() {
  for (int i = 0; i < 3; i++) {
    digitos[i] = 16;
  }
}