#include<EEPROM.h>

//definicion de pines
int resetButton = 27;
int motorPWM = 14; // Pin del PWM para controlar la velocidad del motor
int motorIN1 = 13; // Pin de dirección 1 del motor
int motorIN2 = 12; // Pin de dirección 2 del motor
int idlec = 32;
int potDeseadaPin = 34; // Pin del potenciómetro para la posición deseada
int potActualPin = 35; // Pin del potenciómetro para la posición actual
int buttonMax = 25;
int buttonMin = 26;

//pid variables
double kp = 1.0; // Ganancia proporcional del controlador PID
double ki = 0.35; // Ganancia integral del controlador PID
double kd = 0.15; // Ganancia derivativa del controlador PID
unsigned long lastUpdateTime = 0; // Variable para el tiempo de la última actualización
int lastError = 0; // Variable para almacenar el error en el ciclo anterior
double integral = 0; // Variable para la componente integral del controlador PID

//variables de calibracion
int minValueDes; //valor minimo del potenciometro calibrado 
//int minb = digitalRead(buttonMin);
//int mincal = false;
int maxValueDes; //valor maximo del potenciometro calibrado 
//int maxb = digitalRead(buttonMax);
//int maxcal = false;
int minValue; //valor minimo del potenciometro calibrado
//int minvalcal = false;
//int maxvalcal = false; 
int maxValue; //valor maximo del potenciometro calibrado 
bool calibrated = false; //flag para saber si el potenciometro ha sido calibrado 

void guardarCalibracionActual(){
  EEPROM.begin(512);  // Inicializar la EEPROM
  int address = 0;  // Dirección en la EEPROM donde se almacenarán los datos

  // Guardar los valores en la EEPROM
  EEPROM.put(address, minValueDes);  // Almacena minValueDes en la dirección address
  address += sizeof(minValueDes); // Mover la dirección al siguiente espacio
  EEPROM.put(address, maxValueDes);
  address += sizeof(maxValueDes);
  EEPROM.put(address, minValue);
  address += sizeof(minValue);
  EEPROM.put(address, maxValue);
  address += sizeof(maxValue);
  EEPROM.put(address, calibrated);

  EEPROM.commit();  // Es importante llamar a esta función para escribir los datos en la EEPROM físicamente
}

void cargarCalibracionActual(){
  EEPROM.begin(512);  // Inicializar la EEPROM
  int address = 0;  // Dirección en la EEPROM donde se almacenarán los datos

  // Leer los valores desde la EEPROM
  EEPROM.get(address, minValueDes);  // Leer el valor de minValueDes en la dirección address
  address += sizeof(minValueDes); // Mover la dirección al siguiente espacio
  EEPROM.get(address, maxValueDes);
  address += sizeof(maxValueDes);
  EEPROM.get(address, minValue);
  address += sizeof(minValue);
  EEPROM.get(address, maxValue);
  address += sizeof(maxValue);
  EEPROM.get(address, calibrated);
}

void setup() {
  pinMode(motorPWM, OUTPUT);
  pinMode(motorIN1, OUTPUT);
  pinMode(motorIN2, OUTPUT);
  pinMode(potDeseadaPin, INPUT);
  pinMode(potActualPin, INPUT);
  pinMode(resetButton,INPUT_PULLUP);
  pinMode(buttonMin,INPUT_PULLUP);
  pinMode(buttonMax,INPUT_PULLUP);
  Serial.begin(9600);
  cargarCalibracionActual(); // Verifica si hay valores almacenados en la EEPROM y carga los valores calibrados
  if (calibrated==0){
    calibrarPotenciometroActual();
    guardarCalibracionActual(); // Guarda los valores calibrados en la EEPROM
  }
}


void calibrarPotenciometroActual() {
  int minVal = 4095; // Inicializar minVal con un valor alto
  int maxVal = 0; // Inicializar maxVal con un valor bajo
  int minValDes = 4095;
  int maxValDes = 0;
  int valDes = analogRead(potDeseadaPin);
  Serial.println("presione el boton min para establecer el minimo de app");
  while (digitalRead(buttonMin) == HIGH){
    delay(100);
    }
    minValDes = analogRead(potDeseadaPin);
    Serial.print("valor minimo de app");
    Serial.println(minValueDes);

  Serial.println("presione el boton max para establecer el maximo de app");
  while (digitalRead(buttonMax) == HIGH){
    delay(100);
    }
    maxValDes = analogRead(potDeseadaPin);
    Serial.print("valor maximo de app");
    Serial.println(maxValueDes);

  digitalWrite(motorIN1, LOW); // IN1 = 0
  digitalWrite(motorIN2, HIGH); // IN2 = 1
  analogWrite(motorPWM, 160); // PWM = 160

  delay(500); // Esperar un segundo para que el potenciómetro se estabilice

  for (int i = 0; i < 100; i++) {

    int val = analogRead(potActualPin);

    if (val < minVal) {
      minVal = val;
    }

    delay(10);
  }

  digitalWrite(motorIN1, HIGH); // IN1 = 1
  digitalWrite(motorIN2, LOW); // IN2 = 0
  analogWrite(motorPWM, 200); 

  delay(500); // Esperar un segundo para que el potenciómetro se estabilice

  for (int i = 0; i < 100; i++) {

    int val = analogRead(potActualPin);

    if (val > maxVal) {
      maxVal = val;
    }

    delay(10);
  }
  minValueDes = minValDes;
  minValueDes = maxValDes;

  minValue = minVal;
  // Set minValue as the minimum value obtained
  
  maxValue = maxVal;
  // Set maxValue as the maximum value obtained
  calibrated = 1;
  
}

void loop(){  

  if (digitalRead(resetButton) == HIGH) { // Si se presiona el botón
    EEPROM.write(0, 0);  // Escribe el valor 0 en todas las posiciones de la EEPROM
    calibrated = 0; // Establecer calibrated como falso para indicar que el potenciómetro debe calibrarse nuevamente
    Serial.println("Valores de la EEPROM restablecidos");
    delay(1000); // Esperar un segundo para evitar múltiples restablecimientos con un solo botón
  }
  if (calibrated==0){
    calibrarPotenciometroActual();
    guardarCalibracionActual(); // Guarda los valores calibrados en la EEPROM
  }
  if (millis() - lastUpdateTime >= 50) {
    lastUpdateTime = millis();
    int posicionDeseada = map(analogRead(potDeseadaPin), maxValueDes, minValueDes, 0, 255);
    int posicionActual = map(analogRead(potActualPin), minValue, maxValue, 0, 255);
    
    double error = posicionDeseada - posicionActual;
    integral = integral + error;
    double derivativa = error - lastError;
    lastError = error;
    
    double correccion = (kp * error) + (ki * integral) + (kd * derivativa);
    int pwmValue = abs(correccion);
    
    if (correccion > 0) {
      analogWrite(motorPWM, pwmValue);
      digitalWrite(motorIN1, HIGH);
      digitalWrite(motorIN2, LOW);
    } else if (correccion < 0) {
      analogWrite(motorPWM, pwmValue);
      digitalWrite(motorIN1, LOW);
      digitalWrite(motorIN2, HIGH);
    } else {
      analogWrite(motorPWM, 0);
      digitalWrite(motorIN1, LOW);
      digitalWrite(motorIN2, LOW);
    }
    Serial.print("tps: ");
    Serial.print(posicionActual);
    Serial.print("|app: ");
    Serial.print(posicionDeseada);    
    Serial.print("|pid: ");
    Serial.println(pwmValue);
  }
}