#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Fonts/FreeMonoBold9pt7b.h>
#include <Servo.h>

// ── Pines ─────────────────────────────────────────────────────
#define DIR      2
#define STEP     3
#define SERVO_PIN 6
#define IR1      7
#define IR2      8
#define IR3      9
#define ENCENDER 10
#define LED1     11   // LED rojo
#define LED2     12   // LED verde
#define APAGAR   13

// ── Motor a pasos ─────────────────────────────────────────────
// Motor: 2048 pasos/revolución
// Velocidad objetivo: 512 pasos/segundo
// Periodo por paso: 1 000 000 µs / 512 = 1953.125 µs
// Semiciclo (HIGH + LOW): 1953 / 2 = 976 µs
#define STEP_DELAY_US   976
#define DIR_HORARIO     HIGH
#define DIR_ANTIHORARIO LOW

// ── Display OLED ──────────────────────────────────────────────
#define SCREEN_WIDTH  128
#define SCREEN_HEIGHT  64
#define OLED_RESET    -1
#define OLED_ADDRESS  0x3C

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// ── Servo ─────────────────────────────────────────────────────
Servo servo;
#define SERVO_REPOSO  90   // Posición de reposo
#define SERVO_ACTIVO   0   // Posición de dispensado
#define SERVO_ESPERA_MS 5000  // 5 segundos en posición activa

// ── Bitmap logo UNHEVAL ───────────────────────────────────────
static const unsigned char PROGMEM image_paint_12_bits[] = {
  0x02,0x00, 0x04,0x00, 0x0c,0x00, 0x0e,0x00,
  0x5f,0x40, 0xf5,0xe0, 0x55,0x40, 0x55,0x40, 0xae,0xa0
};

// ── Máquina de estados ────────────────────────────────────────
typedef enum {
  ESTADO_DETENIDO,
  ESTADO_FUNCIONANDO
} EstadoSistema;

EstadoSistema estadoActual = ESTADO_DETENIDO;

// ── Variables de control interno ──────────────────────────────
bool motorActivo        = false;
bool esperandoIR1       = false;   // true: sistema listo, esperando objeto en IR1
bool esperandoIR2       = false;   // true: motor avanzando hacia IR2
bool esperandoIR3       = false;   // true: motor avanzando hacia IR3
bool servoEnActivo      = false;
unsigned long tiempoServo = 0;

// ── LED verde parpadeo 1 Hz ───────────────────────────────────
unsigned long tiempoLed     = 0;
bool          estadoLed2    = false;
#define LED_INTERVALO_MS  500      // 500ms ON + 500ms OFF = 1Hz

// ── Anti-rebote botones ───────────────────────────────────────
#define DEBOUNCE_MS 50
bool          lastEncender     = HIGH;
bool          lastApagar       = HIGH;
unsigned long lastTimeEncender = 0;
unsigned long lastTimeApagar   = 0;

// =============================================================
//  Función: drawEllipse (no existe en Adafruit GFX)
// =============================================================
void drawEllipse(int16_t cx, int16_t cy, int16_t rx, int16_t ry, uint16_t color) {
  int32_t x   = 0, y = ry;
  int32_t rx2  = (int32_t)rx * rx;
  int32_t ry2  = (int32_t)ry * ry;
  int32_t px   = 0;
  int32_t py   = 2 * rx2 * y;

  int32_t p = (int32_t)(ry2 - rx2 * ry + 0.25f * rx2);
  while (px < py) {
    display.drawPixel(cx+x, cy+y, color); display.drawPixel(cx-x, cy+y, color);
    display.drawPixel(cx+x, cy-y, color); display.drawPixel(cx-x, cy-y, color);
    x++; px += 2 * ry2;
    if (p < 0) { p += ry2 + px; }
    else       { y--; py -= 2*rx2; p += ry2 + px - py; }
  }
  p = (int32_t)(ry2*(x+0.5f)*(x+0.5f) + rx2*(y-1)*(y-1) - rx2*ry2);
  while (y >= 0) {
    display.drawPixel(cx+x, cy+y, color); display.drawPixel(cx-x, cy+y, color);
    display.drawPixel(cx+x, cy-y, color); display.drawPixel(cx-x, cy-y, color);
    y--; py -= 2*rx2;
    if (p > 0) { p += rx2 - py; }
    else       { x++; px += 2*ry2; p += rx2 - py + px; }
  }
}

// =============================================================
//  Display: elementos comunes a ambas pantallas
// =============================================================
void drawCommon(void) {
  display.drawRect(3,  2, 59, 17, SSD1306_WHITE);
  display.drawRect(64, 2, 59, 17, SSD1306_WHITE);

  display.setFont(&FreeMonoBold9pt7b);
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);
  display.setCursor(5,  14); display.print(F("START"));
  display.setCursor(72, 14); display.print(F("STOP"));

  display.drawLine(2, 22, 123, 22, SSD1306_WHITE);

  display.setFont(&FreeMonoBold9pt7b);
  display.setCursor(39, 48);
  display.print(F("UNHEVAL"));

  drawEllipse(27, 44, 8, 9, SSD1306_WHITE);
  drawEllipse(27, 44, 7, 8, SSD1306_WHITE);
  display.drawBitmap(22, 39, image_paint_12_bits, 11, 9, SSD1306_WHITE);

  display.setFont(NULL);
  display.setTextSize(1);
  display.setCursor(5, 55);
  display.print(F("Llenadora \"Kamilita\""));
}

// =============================================================
//  Pantalla 1 — Estado: Funcionando
// =============================================================
void drawScreen_1(void) {
  display.clearDisplay();
  drawCommon();
  display.setFont(NULL);
  display.setTextSize(1);
  display.setCursor(8, 26);
  display.print(F("Estado: Funcionando"));
  display.display();
  Serial.println(F("[DISPLAY] Pantalla 1 — Estado: Funcionando"));
}

// =============================================================
//  Pantalla 2 — Estado: Detenido  (por defecto al iniciar)
// =============================================================
void drawScreen_2(void) {
  display.clearDisplay();
  drawCommon();
  display.setFont(NULL);
  display.setTextSize(1);
  display.setCursor(19, 26);
  display.print(F("Estado: Detenido"));
  display.display();
  Serial.println(F("[DISPLAY] Pantalla 2 — Estado: Detenido"));
}

// =============================================================
//  Motor a pasos: dar UN pulso
//  Llamar en cada iteración del loop cuando motorActivo == true
// =============================================================
void pulsarMotor(void) {
  digitalWrite(STEP, HIGH);
  delayMicroseconds(STEP_DELAY_US);
  digitalWrite(STEP, LOW);
  delayMicroseconds(STEP_DELAY_US);
}

// =============================================================
//  Anti-rebote con detección de flanco único
// =============================================================
bool botonPresionado(uint8_t pin, bool &lastState, unsigned long &lastTime) {
  bool actual = digitalRead(pin);
  if (actual != lastState) {
    lastTime  = millis();
    lastState = actual;
  }
  if ((millis() - lastTime) >= DEBOUNCE_MS && actual == LOW && lastState == LOW) {
    lastState = HIGH;
    return true;
  }
  return false;
}

// =============================================================
//  Activar estado DETENIDO
// =============================================================
void activarDetenido(void) {
  estadoActual  = ESTADO_DETENIDO;
  motorActivo   = false;
  esperandoIR1  = false;
  esperandoIR2  = false;
  esperandoIR3  = false;
  servoEnActivo = false;

  servo.write(SERVO_REPOSO);

  digitalWrite(LED1, HIGH);   // LED rojo fijo
  digitalWrite(LED2, LOW);    // LED verde apagado
  estadoLed2 = false;

  drawScreen_2();
  Serial.println(F("[ESTADO] -> DETENIDO | Servo: 90deg | LED rojo ON"));
}

// =============================================================
//  Activar estado FUNCIONANDO
//  El motor NO arranca aquí — espera que IR1 detecte primero
// =============================================================
void activarFuncionando(void) {
  estadoActual  = ESTADO_FUNCIONANDO;
  motorActivo   = false;   // Motor detenido hasta que IR1 dispare
  esperandoIR1  = true;    // Fase 0: aguardando objeto en IR1
  esperandoIR2  = false;
  esperandoIR3  = false;
  servoEnActivo = false;

  digitalWrite(DIR, DIR_HORARIO);
  digitalWrite(LED1, LOW);  // LED rojo apagado

  drawScreen_1();
  Serial.println(F("[ESTADO] -> FUNCIONANDO | Motor DETENIDO | Esperando IR1..."));
}

// =============================================================
//  Setup
// =============================================================
void setup() {
  Serial.begin(115200);
  Serial.println(F("=== Llenadora Kamilita — UNHEVAL ==="));

  // Pines de salida
  pinMode(STEP,  OUTPUT);
  pinMode(DIR,   OUTPUT);
  pinMode(LED1,  OUTPUT);
  pinMode(LED2,  OUTPUT);
  // SERVO_PIN lo maneja la librería Servo

  // Pines de entrada con pull-up interno
  // (botones conectados entre PIN y GND → LOW = presionado)
  pinMode(IR1,      INPUT_PULLUP);
  pinMode(IR2,      INPUT_PULLUP);
  pinMode(IR3,      INPUT_PULLUP);
  pinMode(ENCENDER, INPUT_PULLUP);
  pinMode(APAGAR,   INPUT_PULLUP);

  // NOTA: NO hacer pinMode de SDA(A4) y SCL(A5)
  // La librería Wire los configura internamente

  // Servo
  servo.attach(SERVO_PIN);
  servo.write(SERVO_REPOSO);

  // Display
  if (!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDRESS)) {
    Serial.println(F("[ERROR] SSD1306 no encontrado. Verificar I2C."));
    while (true);
  }
  Serial.println(F("[OK] Display SSD1306 inicializado"));

  // Estado inicial: DETENIDO
  activarDetenido();

  Serial.println(F("Listo. Presiona ENCENDER (Pin 10) para iniciar."));
}

// =============================================================
//  Loop
// =============================================================
void loop() {

  unsigned long ahora = millis();

  // ── Botón ENCENDER (pin 10) ────────────────────────────────
  if (botonPresionado(ENCENDER, lastEncender, lastTimeEncender)) {
    Serial.println(F("[BTN] ENCENDER presionado (Pin 10)"));
    if (estadoActual == ESTADO_DETENIDO) {
      activarFuncionando();
    } else {
      Serial.println(F("[INFO] Sistema ya en funcionamiento"));
    }
  }

  // ── Botón APAGAR (pin 13) ──────────────────────────────────
  if (botonPresionado(APAGAR, lastApagar, lastTimeApagar)) {
    Serial.println(F("[BTN] APAGAR presionado (Pin 13)"));
    if (estadoActual == ESTADO_FUNCIONANDO) {
      activarDetenido();
    } else {
      Serial.println(F("[INFO] Sistema ya detenido"));
    }
  }

  // ── Lógica de funcionamiento ───────────────────────────────
  if (estadoActual == ESTADO_FUNCIONANDO) {

    // ── Parpadeo LED verde 1 Hz ──────────────────────────────
    if (ahora - tiempoLed >= LED_INTERVALO_MS) {
      tiempoLed  = ahora;
      estadoLed2 = !estadoLed2;
      digitalWrite(LED2, estadoLed2 ? HIGH : LOW);
    }

    // ── Fase 0: Sistema en espera — aguardando objeto en IR1 ─
    //    Motor detenido hasta que IR1 detecte un objeto
    if (esperandoIR1 && !motorActivo) {
      if (digitalRead(IR1) == LOW) {         // LOW = objeto detectado
        esperandoIR1 = false;
        esperandoIR2 = true;
        motorActivo  = true;
        Serial.println(F("[IR1] Objeto detectado — Motor ON | Avanzando a IR2"));
      }
      // Motor quieto mientras no haya detección: no se llama pulsarMotor()
    }

    // ── Fase 1: Motor girando hasta que IR2 se active ────────
    if (motorActivo && esperandoIR2) {
      if (digitalRead(IR2) == LOW) {         // IR2 detectado → parar motor
        motorActivo = false;
        esperandoIR2 = false;
        Serial.println(F("[IR2] Detectado — Motor DETENIDO | Servo -> 0deg"));

        servo.write(SERVO_ACTIVO);
        servoEnActivo = true;
        tiempoServo   = ahora;
      } else {
        pulsarMotor();                        // Seguir girando hacia IR2
      }
    }

    // ── Fase 2: Servo activo, esperar 5 segundos ─────────────
    if (servoEnActivo && (ahora - tiempoServo >= SERVO_ESPERA_MS)) {
      servoEnActivo = false;
      Serial.println(F("[SERVO] 5s cumplidos — Servo -> 90deg | Motor ON -> IR3"));

      servo.write(SERVO_REPOSO);
      motorActivo  = true;
      esperandoIR3 = true;
    }

    // ── Fase 3: Motor girando hasta que IR3 se active ────────
    if (motorActivo && esperandoIR3) {
      if (digitalRead(IR3) == LOW) {         // IR3 detectado → ciclo completo
        motorActivo  = false;
        esperandoIR3 = false;
        Serial.println(F("[IR3] Detectado — Ciclo completo"));
        Serial.println(F("[INFO] Volviendo a fase 0 — Esperando IR1..."));

        // Reiniciar ciclo: volver a esperar IR1
        esperandoIR1 = true;
      } else {
        pulsarMotor();                        // Seguir girando hacia IR3
      }
    }

  } // fin if FUNCIONANDO

}