#define pinPWM 5
#define pinIN1 6
#define pinIN2 7
#define INTERVALO 100 //200 ms
#define PWM_Inicial 150
#define HORARIO 1
#define ANTIHORARIO 2
#define PWM_MINIMO 95
#define PWM_MAXIMO 255

char comando = '\0';  
char datos_Seriales[4] = {'\0'}; //Como máximo se requiere 4 caracteres

uint64_t TiempoActual;
uint64_t TiempoAnterior;
uint16_t PWM;

bool Recorrido;
bool control;
bool Girando;
bool Detenido;
uint16_t sentido;
bool subiendoPWM;

void setup() {
  Serial.begin(115200);
  Serial.println("Empezando programa...");

  //Configuración
  pinMode(pinIN1,OUTPUT);
  pinMode(pinIN2,OUTPUT);

  DetenerMotor();
  
  Girando = false;
  Detenido = true;
  Recorrido = false;
  TiempoAnterior = millis();
  TiempoActual = TiempoAnterior; 
  PWM = PWM_Inicial;

  Serial.println("Configuración completa!");
  Serial.println("Comandos:");
  Serial.println("Iniciar: G\nDetener: E\nHorario: D\nAntihorario:G\nValor PWM (95-255)");
}

void loop() {
  
  TiempoActual = millis();
  if(Recorrido && TiempoActual - TiempoAnterior > INTERVALO){
    TiempoAnterior = TiempoActual; 
    GesiontarRecorrido();
  }

  if(ComandoValidoRecibido()){
    if(control){
      switch(comando){
        case 'G':
          if(Girando){
            Serial.println("Ya se inició el programa");
          }else{
            Girando = true;
            Detenido = false;
            sentido = HORARIO;

            MoverMotor(PWM,sentido);
          }
          break;
        case 'E':
          if(Detenido){
            Serial.println("Ya se detuvo el programa");
          }else{
            Girando = false;
            Detenido = true;
            Recorrido = false;

            DetenerMotor();
          }
          break;
        
        case 'D':
          if(!Girando){
            Serial.println("Se debe iniciar el giro primero");
          }else if(Recorrido){
            Serial.println("No se puede activar un comando durante el recorrido");
          }else{
            sentido = HORARIO;
            MoverMotor(PWM,sentido);
          }
          break;
        case 'A':
          if(!Girando){
            Serial.println("Se debe iniciar el giro primero");
          }else if(Recorrido){
            Serial.println("No se puede activar un comando durante el recorrido");
          }else{
            sentido = ANTIHORARIO;
            MoverMotor(PWM,ANTIHORARIO);
          }
          break;
        case 'R':
          if(!Girando){
            Serial.println("Se debe iniciar el giro primero");
          }else{
            if(Recorrido){
              Serial.println("Terminando recorrido");
              Recorrido = false;
            }else{
              Serial.println("Iniciando recorrido");
              Recorrido = true;
              subiendoPWM = true;
            }
          }
          break;
        default:
          Serial.printf("Comando '%c' no procesado correctamente",comando);
          break;
      }
    }else{ //PWM
      if(Girando && !Recorrido){
        analogWrite(pinPWM, PWM);
        Serial.printf("Valor PWM actual: %d\n",PWM);
      }else if(!Girando){
        Serial.println("Se debe iniciar el giro primero");
      }else if(Recorrido){
        Serial.println("No se puede ajustar el valor PWM durante el recorrido");
      }
    }
  } 

  delay(50);
}

void LimpiarBuffer(){
  while(Serial.available()>0){
    Serial.read();
  }
}

void GesiontarRecorrido(){
  
  if(subiendoPWM && PWM >= PWM_MAXIMO){
    subiendoPWM = false;
  }else if(!subiendoPWM && PWM <= PWM_MINIMO){
    subiendoPWM = true;
  }

  if(subiendoPWM){
    PWM++;
  }else{
    PWM--;
  }

  /*
  if(subiendoPWM){
    if(PWM < PWM_MAXIMO){
      PWM++;
    }else{
      subiendoPWM = false;
      PWM--;
    }
  }else{
    if(PWM > PWM_MAXIMO){
      PWM-;
    }else{
      subiendoPWM = true;
      PWM++;
    }
  }
  */

  if(sentido == HORARIO && PWM == PWM_MINIMO){
    sentido = ANTIHORARIO;
  }else if(sentido == ANTIHORARIO && PWM == PWM_MINIMO){
    sentido = HORARIO;
  }
  MoverMotor(PWM,sentido);
  //Serial.printf("%s, PWM: %d\n",sentido == HORARIO ? "Horario" : "Antihorario",PWM);
}

bool ComandoValidoRecibido(){
  bool ComandoValido = false;

  if(Serial.available()>4){
    Serial.println("Ingrese un comando válido");
    LimpiarBuffer();
  }else if(Serial.available()>0){
    VaciarArreglo(datos_Seriales);

    int NumeroCaracteres = Serial.readBytesUntil('\n',datos_Seriales, Serial.available() - 1); //Cada byte indica un caracter
    int numero = atoi(datos_Seriales); //Revisar por qué los datos transciende del anterior comando si se limpia el arreglo con '\0'

    if(NumeroCaracteres == cantidadDigitos(numero)){
      
      //Se recibió un número PWM válido
      if(numero < PWM_MINIMO || numero > PWM_MAXIMO){
        Serial.printf("Los valores PWM van desde %d hasta %d\n",PWM_MINIMO,PWM_MAXIMO);
      }else{
        //Valor PWM en un rango válido
        PWM = numero;
        control = false;
        ComandoValido = true;
      }
    }else{ //Se podría haber recibido un comando de control
      if(NumeroCaracteres == 1 && (datos_Seriales[0] == 'G' || datos_Seriales[0] == 'E' || datos_Seriales[0] == 'D' || datos_Seriales[0] == 'A' || datos_Seriales[0] == 'R')){
        //Comando válido
        comando = datos_Seriales[0];
        control = true;
        ComandoValido = true;
        //Serial.printf("Comando recibido: %c\n",comando);
      }else{
        Serial.println("Ingrese un comando válido");
      }
    }
    LimpiarBuffer();
  }

  return ComandoValido;
}

int cantidadDigitos(int numero){
  if(numero == 0){
    return 0;
  }
  return (int)log10(numero) + 1;
}

void DetenerMotor(){
    digitalWrite(pinIN1,LOW);
    digitalWrite(pinIN2,LOW);
    analogWrite(pinPWM,0);
    Serial.println("Motor detenido");
 }

void MoverMotor(int PWM_local,int sentido_local){
  if(sentido_local == HORARIO){
    digitalWrite(pinIN1,HIGH);
    digitalWrite(pinIN2,LOW);
    analogWrite(pinPWM,PWM_local);
    Serial.printf("Girando en sentido horario, PWM: %d\n", PWM_local);
  }else if(sentido_local == ANTIHORARIO){
    digitalWrite(pinIN1,LOW);
    digitalWrite(pinIN2,HIGH);
    Serial.printf("Girando en sentido antihorario, PWM: %d\n", PWM_local);
    analogWrite(pinPWM,PWM_local);
  }else{
    Serial.printf("Sentido incorrecto al mover el motor: %d\n",sentido);
  }
}

void VaciarArreglo(char* arreglo){
  for(int i = 0; i < strlen(arreglo); i++){
    arreglo[i] = '\n';
  }
}

/*
  Comando stop: E
  Comando para inciar movimiento: G
    Comando para girar horario: H
    Comando para girar antihorario: A
    Comando para cambiar la velocidad: Valor int entre 95 y 255 para el PWM
*/