const int PIN_SHARP = A0;

// ESP32 ADC configuration
const float VOLTAJE_REFERENCIA = 3.3;
const int ADC_MAX = 4095;

// Voltage divider:
// Sharp OUT ---- R1 4.7k ---- A0 ---- R2 10k ---- GND
const float R1 = 4700.0;
const float R2 = 10000.0;

// Adjust this interval for a faster or slower graph
const unsigned long INTERVALO_LECTURA = 50; // ms

unsigned long tiempoAnterior = 0;

// Simple average to stabilize reading
int leerPromedioADC(int pin, int muestras) {
  long suma = 0;

  for (int i = 0; i < muestras; i++) {
    suma += analogRead(pin);
    delay(2);
  }

  return suma / muestras;
}

// Convert the voltage at A0 back to the original Sharp sensor output voltage
float calcularVoltajeSensor(float voltajeA0) {
  float voltajeSensor = voltajeA0 * ((R1 + R2) / R2);
  return voltajeSensor;
}

float calcularDistanciaCM(float voltajeSensor) {
  if (voltajeSensor <= 0.1) {
    return -1;
  }

  // Approximate curve used for the Sharp sensor
  float distancia = 27.86 / (voltajeSensor - 0.1);

  return distancia;
}

void setup() {
  Serial.begin(115200);
  delay(3000);

  analogReadResolution(12); // 0 to 4095

  Serial.println("ADC\tVoltaje_A0\tVoltaje_Sensor\tDistancia_cm");
}

void loop() {
  unsigned long tiempoActual = millis();

  if (tiempoActual - tiempoAnterior >= INTERVALO_LECTURA) {
    tiempoAnterior = tiempoActual;

    int lecturaADC = leerPromedioADC(PIN_SHARP, 10);

    float voltajeA0 = lecturaADC * (VOLTAJE_REFERENCIA / ADC_MAX);
    float voltajeSensor = calcularVoltajeSensor(voltajeA0);
    float distanciaCM = calcularDistanciaCM(voltajeSensor);

    // Serial Plotter compatible format
    Serial.print("ADC:");
    Serial.print(lecturaADC);

    Serial.print("\tVoltaje_A0:");
    Serial.print(voltajeA0, 3);

    Serial.print("\tVoltaje_Sensor:");
    Serial.print(voltajeSensor, 3);

    Serial.print("\tDistancia_cm:");
    Serial.println(distanciaCM, 2);
  }
}