#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <math.h>
#include <avr/interrupt.h>

#define BUTTON_PCINT 0     // PCINT0 → PB0 → Arduino D8
#define LED_PIN    9       // LED indicador
#define POT_PIN    A0      // Potenciómetro
#define DIP0_PIN   3       // Dip Switch: metros
#define DIP1_PIN   4       // Dip Switch: pies
#define DIP2_PIN   5       // Dip Switch: pulgadas

LiquidCrystal_I2C lcd(0x27, 16, 2);

// Máquina de estados
enum Estado { ESPERANDO_BOTON, MIDiendo, PAUSADO };
volatile Estado estado = ESPERANDO_BOTON;
volatile bool boton_flag = false;

// Variables auxiliares
typedef unsigned long ul;
ul lastBlink = 0, lastPCINT = 0;
const ul blinkInterval = 500, debounce = 200;
bool ledState = false;
float medidaCongelada = 0;

// --- Polinomios de calibración ---
// Rango cercano: 0–3 mm → voltaje 0.01–3.1 V
float funcion0(float v) {
  return 0.1839 * pow(v,6) - 1.5651 * pow(v,5) + 4.7103 * pow(v,4)
       - 5.7343 * pow(v,3) + 2.3552 * pow(v,2) + 0.4217 * v - 0.0024;
}
// Rango lejano: 3–30 mm → voltaje 0.4–3.1 V
float funcion1(float v) {
  return 3.8697 * pow(v,6) - 42.279 * pow(v,5) + 184.25 * pow(v,4)
       - 410.28 * pow(v,3) + 497.79 * pow(v,2) - 325.53 * v + 105.37;
}

// Lee ADC y convierte a voltaje con AREF externa = 3.3V
float leerVoltaje() {
  int raw = analogRead(POT_PIN);
  return (raw * 3.3) / 1023.0;
}
// Aplica polinomios para obtener distancia en mm
float calcularDistanciaMM(float v) {
  float mm = (v > 3.0 ? funcion1(v) : funcion0(v)) * 100.0;
  return mm < 0 ? 0 : mm;
}

// ISR: Pin Change Interrupt para PB0 (PCINT0_vect)
ISR(PCINT0_vect) {
  ul now = millis();
  if (now - lastPCINT > debounce) {
    boton_flag = true;
    lastPCINT = now;
  }
}

void setup() {
  // LCD inicial
  lcd.init(); lcd.backlight(); lcd.clear();

  // Pines
  pinMode(LED_PIN, OUTPUT); digitalWrite(LED_PIN, LOW);
  pinMode(POT_PIN, INPUT);
  pinMode(DIP0_PIN, INPUT_PULLUP);
  pinMode(DIP1_PIN, INPUT_PULLUP);
  pinMode(DIP2_PIN, INPUT_PULLUP);

  // Botón en PB0 con pull-up interno
  DDRB  &= ~(1 << DDB0);    // PB0 como entrada
  PORTB |=  (1 << PORTB0);  // PB0 pull-up

  // Habilita Pin Change Interrupt en PCINT0 (PB0)
  PCICR  |= (1 << PCIE0);   // grupo PCINT[7:0]
  PCMSK0 |= (1 << PCINT0);  // solo PB0
  sei();                    // interrupciones globales

  // ADC: referencia AREF externa, prescaler 128
  ADMUX  &= ~((1<<REFS1)|(1<<REFS0));
  ADCSRA |=  (1<<ADEN)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0);
}

void loop() {
  // Lectura DIP switch: LOW = activado
  bool dip0 = digitalRead(DIP0_PIN) == LOW;
  bool dip1 = digitalRead(DIP1_PIN) == LOW;
  bool dip2 = digitalRead(DIP2_PIN) == LOW;
  int sel = dip0 + dip1 + dip2;

  // Override de pantalla si error múltiple o sin unidad
  if (sel > 1) {
    lcd.clear();
    lcd.setCursor(0,0); lcd.print("Error: multiple");
    lcd.setCursor(0,1); lcd.print("unidades");
    digitalWrite(LED_PIN, LOW);
    delay(100);
    return;
  }
  if (sel == 0) {
    lcd.clear();
    lcd.setCursor(0,0); lcd.print("Seleccione unidad");
    digitalWrite(LED_PIN, LOW);
    delay(100);
    return;
  }

  // Procesa botón: alterna ESPERANDO_BOTON→MIDiendo→PAUSADO
  if (boton_flag) {
    boton_flag = false;
    if (estado == ESPERANDO_BOTON) {
      estado = MIDiendo;
    } else if (estado == MIDiendo) {
      medidaCongelada = calcularDistanciaMM(leerVoltaje());
      estado = PAUSADO;
      lcd.clear();
    } else if (estado == PAUSADO) {
      estado = MIDiendo;
      lcd.clear();
    }
  }

  // Ejecuta comportamiento según estado
  switch (estado) {
    case ESPERANDO_BOTON:
      digitalWrite(LED_PIN, LOW);
      // pantalla en blanco
      break;

    case MIDiendo: {
      float v  = leerVoltaje();
      float mm = calcularDistanciaMM(v);
      // parpadeo LED
      if (millis() - lastBlink >= blinkInterval) {
        ledState = !ledState;
        digitalWrite(LED_PIN, ledState);
        lastBlink = millis();
      }
      // conversión unidad
      float val; const char* u;
      if (dip0)      { val = mm/1000.0; u = " m"; }
      else if (dip1) { val = mm/304.8;  u = " ft"; }
      else           { val = mm/25.4;   u = " in"; }
      // muestra en LCD
      lcd.setCursor(0,0);
      lcd.print(val,2); lcd.print(u); lcd.print("   ");
      break;
    }

    case PAUSADO:
      digitalWrite(LED_PIN, HIGH);
      { float mm = medidaCongelada;
        float val; const char* u;
        if (dip0)      { val = mm/1000.0; u = " m"; }
        else if (dip1) { val = mm/304.8;  u = " ft"; }
        else           { val = mm/25.4;   u = " in"; }
        lcd.setCursor(0,0);
        lcd.print(val,2); lcd.print(u); lcd.print("   ");
        lcd.setCursor(0,1);
        lcd.print("Medida tomada"); }
      break;
  }

  delay(50);
}