#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <EEPROM.h>

/////////////////////////////////
//       05/08/2026
//       Version 8.5 OLED
//       Por: LU4EG
/////////////////////////////////

// ==============================
// 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);


// ==============================
// CONTROL
// ==============================
bool autoMode = false;
bool lastConfigBtn = HIGH;
unsigned long configPressTime = 0;


// ==============================
// RELAYS
// ==============================
#define RELAY_AZ_CW   12
#define RELAY_AZ_CCW  13
#define RELAY_EL_UP   A2
#define RELAY_EL_DOWN A3


// ==============================
// BUTTONS
// ==============================
// A4 ya NO puede utilizarse:
// A4 = SDA del OLED
//
// BTN_EL_UP pasa a D2
// ==============================
#define BTN_AZ_CW    9
#define BTN_AZ_CCW   8
#define BTN_EL_DOWN  11
#define BTN_EL_UP    2
#define BTN_CONFIG   10


// ==============================
// POTS
// ==============================
#define POT_AZ A0
#define POT_EL A1


// ==============================
// CALIB DATA
// ==============================
struct CalibData {
  int azMin;
  int azMax;
  int elMin;
  int elMax;
};

CalibData calib;


// ==============================
// TARGET GS-232
// ==============================
int targetAz = 0;
int targetEl = 0;

char serialBuf[20];
byte serialPos = 0;


// ==============================
// CONTROL
// ==============================
#define AZ_DEADBAND 2
#define EL_DEADBAND 1

enum Dir {
  STOP,
  CW,
  CCW
};

Dir azDir = STOP;
Dir elDir = STOP;


// ==============================
// DISPLAY CONTROL
// ==============================
unsigned long lastDisplay = 0;
const int displayInterval = 200;


// ==============================
// CONFIG
// ==============================
byte cfgStep = 0;
bool inConfig = false;


// =========================================================
// OLED HELPERS
// =========================================================

void clearDisplay() {

  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
}


// =========================================================
// STOP
// =========================================================

void stopAll() {

  digitalWrite(RELAY_AZ_CW, LOW);
  digitalWrite(RELAY_AZ_CCW, LOW);
  digitalWrite(RELAY_EL_UP, LOW);
  digitalWrite(RELAY_EL_DOWN, LOW);
}


// =========================================================
// ADC SMOOTHING
// =========================================================

int smoothRead(int pin) {

  long sum = 0;

  for (int i = 0; i < 5; i++)
    sum += analogRead(pin);

  return sum / 5;
}


// =========================================================
// AZ
// =========================================================

int readAz() {

  int adc = smoothRead(POT_AZ);

  // ==========================================
  // EXTREMO INFERIOR DE CALIBRACIÓN
  // ==========================================

  if (adc <= calib.azMin)
    return 180;


  // ==========================================
  // EXTREMO SUPERIOR DE CALIBRACIÓN
  // ==========================================

  if (adc >= calib.azMax)
    return 181;


  // ==========================================
  // INTERPOLACIÓN
  // ==========================================

  long recorrido = map(adc,
                        calib.azMin,
                        calib.azMax,
                        0,
                        359);

  int az = 180 - recorrido;


  // ==========================================
  // PASO CIRCULAR
  // ==========================================

  if (az < 0)
    az += 360;

  return az;
}


// =========================================================
// ELEVACIÓN
// =========================================================

int readEl() {

  return constrain(
    map(smoothRead(POT_EL),
        calib.elMin,
        calib.elMax,
        0,
        90),
    0,
    90
  );
}


// =========================================================
// SERIAL GS-232
// =========================================================

void handleSerial() {

  while (Serial.available()) {

    char c = Serial.read();

    if (c == '\n' || c == '\r') {

      serialBuf[serialPos] = 0;

      if (serialBuf[0] == 'W') {

        int az, el;

        if (sscanf(serialBuf, "W%d %d", &az, &el) == 2) {

          targetAz = constrain(az, 0, 359);
          targetEl = constrain(el, 0, 90);
        }
      }

      serialPos = 0;
    }
    else {

      if (serialPos < sizeof(serialBuf) - 1)
        serialBuf[serialPos++] = c;
    }
  }
}


// =========================================================
// AUTO AZ
// =========================================================

Dir autoAzDir() {

  int actual = readAz();

  int d = abs(actual - targetAz);

  // Ya llegó
  if (d <= AZ_DEADBAND || d >= (360 - AZ_DEADBAND))
    return STOP;


  // ==========================================
  // CRUCE POR EL NORTE
  // ==========================================

  if (actual <= 180 && targetAz > 180)
    return CCW;

  if (actual > 180 && targetAz <= 180)
    return CW;


  // ==========================================
  // AMBOS ENTRE 0..180
  // ==========================================

  if (actual <= 180) {

    if (targetAz > actual)
      return CW;
    else
      return CCW;
  }


  // ==========================================
  // AMBOS ENTRE 181..359
  // ==========================================

  if (targetAz > actual)
    return CW;
  else
    return CCW;
}


// =========================================================
// AUTO ELEVATION
// =========================================================

Dir autoElDir() {

  int err = targetEl - readEl();

  if (abs(err) <= EL_DEADBAND)
    return STOP;

  return (err > 0) ? CW : CCW;
}


// =========================================================
// INPUTS
// =========================================================

void readInputs() {

  bool azCW  = digitalRead(BTN_AZ_CW) == LOW;
  bool azCCW = digitalRead(BTN_AZ_CCW) == LOW;

  bool elUP  = digitalRead(BTN_EL_UP) == LOW;
  bool elDN  = digitalRead(BTN_EL_DOWN) == LOW;

  int az = readAz();
  int el = readEl();


  // ==========================
  // AUTO MODE
  // ==========================

  if (autoMode) {

    azDir = autoAzDir();
    elDir = autoElDir();

    return;
  }


  // ==========================
  // MANUAL MODE
  // ==========================


  // --------------------------
  // AZIMUT
  // LÍMITES 180 / 181
  // --------------------------

  if (azCW && !azCCW) {

    if (az == 180) {

      azDir = STOP;

    }
    else {

      azDir = CW;
    }
  }

  else if (azCCW && !azCW) {

    if (az == 181) {

      azDir = STOP;

    }
    else {

      azDir = CCW;
    }
  }

  else {

    azDir = STOP;
  }


  // --------------------------
  // ELEVACIÓN
  // --------------------------

  if (elUP && !elDN) {

    if (el >= 90) {

      elDir = STOP;

    }
    else {

      elDir = CW;
    }
  }

  else if (elDN && !elUP) {

    if (el <= 0) {

      elDir = STOP;

    }
    else {

      elDir = CCW;
    }
  }

  else {

    elDir = STOP;
  }
}


// =========================================================
// DISPLAY NORMAL
// =========================================================
void drawArrowUp(int x, int y) {

  display.drawLine(x + 3, y,     x + 3, y + 7, SSD1306_WHITE);
  display.drawLine(x + 3, y,     x,     y + 3, SSD1306_WHITE);
  display.drawLine(x + 3, y,     x + 6, y + 3, SSD1306_WHITE);
}


void drawArrowDown(int x, int y) {

  display.drawLine(x + 3, y,     x + 3, y + 7, SSD1306_WHITE);
  display.drawLine(x + 3, y + 7, x,     y + 4, SSD1306_WHITE);
  display.drawLine(x + 3, y + 7, x + 6, y + 4, SSD1306_WHITE);
}
void handleNormal() {

  int az = readAz();
  int el = readEl();


  // ==========================
  // RELAYS
  // ==========================

  digitalWrite(RELAY_AZ_CW,  azDir == CW);
  digitalWrite(RELAY_AZ_CCW, azDir == CCW);

  digitalWrite(RELAY_EL_UP,  elDir == CW);
  digitalWrite(RELAY_EL_DOWN, elDir == CCW);


  // ==========================
  // OLED
  // ==========================

  display.clearDisplay();


  // ==========================================
  // LINEA SUPERIOR
  // TARGET / MODO
  // ==========================================

  display.setTextSize(1);
  display.setCursor(0, 0);

  if (autoMode) {

    display.print("TA:");
    display.print(targetAz);

    display.print("  TE:");
    display.print(targetEl);

    display.setCursor(100, 0);
    display.print("AUT");

  }
  else {

    display.print("TA:---  TE:--");

    display.setCursor(100, 0);
    display.print("MAN");
  }


  // ==========================================
  // ELEVACION - IZQUIERDA
  // ==========================================

  display.setTextSize(2);
  display.setCursor(0, 22);

  display.print("E:");

  if (el < 10)
    display.print("0");

  display.print(el);


  // Indicador movimiento ELEVACION

  display.setTextSize(1);
  display.setCursor(0, 48);

  if (elDir == CW)
     drawArrowUp(0, 48);

  else if (elDir == CCW)
    drawArrowDown(0, 48);

  else
      display.setTextSize(1);
      display.setCursor(1, 48);
      display.print("*");


  // ==========================================
  // AZIMUT - DERECHA
  // ==========================================

  display.setTextSize(2);
  display.setCursor(68, 22);

  display.print("A:");

  if (az < 100)
    display.print("0");

  if (az < 10)
    display.print("0");

  display.print(az);


  // Indicador movimiento AZIMUT

  display.setTextSize(1);
  display.setCursor(68, 48);

  if (azDir == CW)
    display.print(">");

  else if (azDir == CCW)
    display.print("<");

  else
    display.print("*");


  display.display();
}


// =========================================================
// CALIBRATION
// =========================================================

void handleCalibration() {

  static bool released = false;


  // ------------------------------------
  // Movimiento manual durante calibración
  // ------------------------------------

  bool azCW  = digitalRead(BTN_AZ_CW) == LOW;
  bool azCCW = digitalRead(BTN_AZ_CCW) == LOW;

  bool elUP  = digitalRead(BTN_EL_UP) == LOW;
  bool elDN  = digitalRead(BTN_EL_DOWN) == LOW;


  digitalWrite(RELAY_AZ_CW,
               azCW && !azCCW);

  digitalWrite(RELAY_AZ_CCW,
               azCCW && !azCW);

  digitalWrite(RELAY_EL_UP,
               elUP && !elDN);

  digitalWrite(RELAY_EL_DOWN,
               elDN && !elUP);


  // ------------------------------------
  // Esperar liberación del botón CONFIG
  // ------------------------------------

  if (!released) {

    if (digitalRead(BTN_CONFIG) == HIGH)
      released = true;

    return;
  }


  // =====================================================
  // CALIBRATION STEPS
  // =====================================================

  switch (cfgStep) {


    // =================================================
    // CAL 1
    // =================================================

    case 0:

      display.clearDisplay();

      display.setTextSize(2);
      display.setCursor(0, 0);
      display.print("CAL1");

      display.setTextSize(1);
      display.setCursor(0, 24);
      display.print("AZ = 180");

      display.setCursor(0, 40);
      display.print("ADC: ");
      display.print(smoothRead(POT_AZ));

      display.display();


      if (digitalRead(BTN_CONFIG) == LOW) {

        calib.azMin = smoothRead(POT_AZ);

        while (digitalRead(BTN_CONFIG) == LOW);

        delay(200);

        cfgStep++;
      }

      break;


    // =================================================
    // CAL 2
    // =================================================

    case 1:

      display.clearDisplay();

      display.setTextSize(2);
      display.setCursor(0, 0);
      display.print("CAL2");

      display.setTextSize(1);
      display.setCursor(0, 24);
      display.print("AZ = 181");

      display.setCursor(0, 40);
      display.print("ADC: ");
      display.print(smoothRead(POT_AZ));

      display.display();


      if (digitalRead(BTN_CONFIG) == LOW) {

        calib.azMax = smoothRead(POT_AZ);

        while (digitalRead(BTN_CONFIG) == LOW);

        delay(200);

        cfgStep++;
      }

      break;


    // =================================================
    // CAL 3
    // =================================================

    case 2:

      display.clearDisplay();

      display.setTextSize(2);
      display.setCursor(0, 0);
      display.print("CAL3");

      display.setTextSize(1);
      display.setCursor(0, 24);
      display.print("EL = 000");

      display.setCursor(0, 40);
      display.print("ADC: ");
      display.print(smoothRead(POT_EL));

      display.display();


      if (digitalRead(BTN_CONFIG) == LOW) {

        calib.elMin = smoothRead(POT_EL);

        while (digitalRead(BTN_CONFIG) == LOW);

        delay(200);

        cfgStep++;
      }

      break;


    // =================================================
    // CAL 4
    // =================================================

    case 3:

      display.clearDisplay();

      display.setTextSize(2);
      display.setCursor(0, 0);
      display.print("CAL4");

      display.setTextSize(1);
      display.setCursor(0, 24);
      display.print("EL = 090");

      display.setCursor(0, 40);
      display.print("ADC: ");
      display.print(smoothRead(POT_EL));

      display.display();


      if (digitalRead(BTN_CONFIG) == LOW) {

        calib.elMax = smoothRead(POT_EL);

        EEPROM.put(0, calib);


        // ==========================================
        // CAL OK
        // ==========================================

        display.clearDisplay();

        display.setTextSize(2);
        display.setCursor(20, 10);
        display.print("CAL OK");

        display.setTextSize(1);
        display.setCursor(15, 40);
        display.print("EEPROM SAVED");

        display.display();

        delay(2000);


        released = false;
        inConfig = false;
        cfgStep = 0;
      }

      break;
  }
}


// =========================================================
// SETUP
// =========================================================

void setup() {

  Serial.begin(9600);


  // ==============================
  // RELAYS
  // ==============================

  pinMode(RELAY_AZ_CW, OUTPUT);
  pinMode(RELAY_AZ_CCW, OUTPUT);

  pinMode(RELAY_EL_UP, OUTPUT);
  pinMode(RELAY_EL_DOWN, OUTPUT);


  // ==============================
  // BUTTONS
  // ==============================

  pinMode(BTN_AZ_CW, INPUT_PULLUP);
  pinMode(BTN_AZ_CCW, INPUT_PULLUP);

  pinMode(BTN_EL_DOWN, INPUT_PULLUP);

  // IMPORTANTE:
  // BTN_EL_UP ahora está en D2
  pinMode(BTN_EL_UP, INPUT_PULLUP);

  pinMode(BTN_CONFIG, INPUT_PULLUP);


  // ==============================
  // OLED
  // ==============================

  if (!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDRESS)) {

    // Si el OLED no responde,
    // detenemos todo.

    while (true) {
      delay(100);
    }
  }


  display.clearDisplay();

  display.setTextColor(SSD1306_WHITE);


  // ==============================
  // INICIO
  // ==============================

  display.setTextSize(2);
  display.setCursor(12, 10);
  display.print("LU4EG");

  display.setTextSize(1);
  display.setCursor(20, 36);
  display.print("GS-232 OLED");

  display.display();

  delay(1000);


  display.clearDisplay();

  display.setTextSize(2);
  display.setCursor(10, 10);
  display.print("Iniciando.");

  display.display();

  delay(500);


  display.clearDisplay();

  display.setCursor(10, 10);
  display.print("Iniciando..");

  display.display();

  delay(500);


  display.clearDisplay();

  display.setCursor(10, 10);
  display.print("Iniciando...");

  display.display();

  delay(500);


  // ==============================
  // STOP
  // ==============================

  stopAll();


  // ==============================
  // EEPROM
  // ==============================

  EEPROM.get(0, calib);


  // ==============================
  // CONFIG AL ARRANQUE
  // ==============================

  if (digitalRead(BTN_CONFIG) == LOW) {

    inConfig = true;
  }


  // ==============================
  // POSICION ACTUAL
  // ==============================

  targetAz = readAz();
  targetEl = readEl();
}


// =========================================================
// LOOP
// =========================================================

void loop() {


  // =====================================================
  // CONFIGURACION
  // =====================================================

  if (inConfig) {

    handleCalibration();

    return;
  }


  // =====================================================
  // BOTON CONFIG
  // =====================================================

  bool cfg = digitalRead(BTN_CONFIG);


  if (lastConfigBtn == HIGH && cfg == LOW)
    configPressTime = millis();


  if (lastConfigBtn == LOW && cfg == HIGH) {

    if (millis() - configPressTime < 600) {

      autoMode = !autoMode;

      stopAll();
    }
  }


  lastConfigBtn = cfg;


  // =====================================================
  // SERIAL
  // =====================================================

  handleSerial();


  // =====================================================
  // INPUTS
  // =====================================================

  readInputs();


  // =====================================================
  // DISPLAY
  // =====================================================

  if (millis() - lastDisplay > displayInterval) {

    lastDisplay = millis();
// =========================================================
// FLECHAS OLED
// =========================================================


    handleNormal();
  }
}