/*
+5V
 |
[10k]
 |
A5
 |
[10k]---- BTN1 ---- GND   (EL_UP)
 |
[10k]---- BTN2 ---- GND   (CONFIG)
*/

#include <LiquidCrystal.h>
#include <EEPROM.h>

// ==============================
// LCD
// ==============================
LiquidCrystal lcd(2, 3, 4, 5, 6, 7);

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

// ==============================
// BUTTONS
// ==============================
#define BTN_AZ_CW   9
#define BTN_AZ_CCW  11
#define BTN_EL_DOWN 8

#define SW_MODE     10

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

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

CalibData calib;

// ==============================
// STATE
// ==============================
int targetAz = 0;
int targetEl = 0;

int currentAz = 0;
int currentEl = 0;

bool systemReady = false;
bool manualMode = false;
bool configMode = false;

// ==============================
// CONFIG
// ==============================
int cfgStep = 0;
bool inConfig = false;
bool configLatch = false;

// ==============================
// SERIAL
// ==============================
char line[32];
byte idx = 0;

// ==============================
// STOP ALL
// ==============================
void stopAll() {
  digitalWrite(RELAY_AZ_CW, LOW);
  digitalWrite(RELAY_AZ_CCW, LOW);
  digitalWrite(RELAY_EL_UP, LOW);
  digitalWrite(RELAY_EL_DOWN, LOW);
}

// ==============================
// LCD PRINT
// ==============================
void printLine(byte row, const char *msg) {
  lcd.setCursor(0, row);
  for (int i = 0; i < 16; i++)
    lcd.print(msg[i] ? msg[i] : ' ');
}

// ==============================
// READ POS
// ==============================
int readAz() {
  return constrain(map(analogRead(POT_AZ), calib.azMin, calib.azMax, 0, 360), 0, 360);
}

int readEl() {
  return constrain(map(analogRead(POT_EL), calib.elMin, calib.elMax, 0, 180), 0, 180);
}

// ==============================
// A5 FILTER (ANTI RUIDO)
// ==============================
int readA5Filtered() {
  long sum = 0;
  for (int i = 0; i < 5; i++) {
    sum += analogRead(A5);
  }
  return sum / 5;
}

// ==============================
// A5 STATE
// ==============================
int leerA5() {

  int v = readA5Filtered();

  if (v < 500) return 1;   // EL_UP
  if (v < 800) return 2;   // CONFIG
  return 0;
}

// ==============================
// SERIAL RX (TRAMAS Wxxx yyy)
// ==============================
void leerSerial() {

  while (Serial.available()) {

    char c = Serial.read();

    if (c < 32 && c != '\r' && c != '\n') continue;

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

      if (idx > 0) {

        line[idx] = '\0';
        idx = 0;

        int az, el;

        if (line[0] == 'W' && sscanf(line, "W%d %d", &az, &el) == 2) {
          targetAz = constrain(az, 0, 360);
          targetEl = constrain(el, 0, 180);
        }
      }

    } else {
      if (idx < sizeof(line) - 1) line[idx++] = c;
      else idx = 0;
    }
  }
}

// ==============================
// LATCH CONFIG (ANTI FALSE ENTRY)
// ==============================
void updateConfigLatch() {

  int st = leerA5();

  if (!configLatch && st == 2) {
    configLatch = true;
    inConfig = true;
    lcd.clear();
  }

  if (configLatch && st == 0) {
    configLatch = false;
  }
}

// ==============================
// SW_MODE (ACEPTAR)
// ==============================
bool acceptPressed() {
  static bool last = HIGH;
  bool now = digitalRead(SW_MODE);

  bool pressed = (last == HIGH && now == LOW);
  last = now;

  delay(25); // debounce simple
  return pressed;
}

// ==============================
// DIR
// ==============================
char azDir() {
  int d = targetAz - currentAz;
  if (abs(d) <= 2) return '*';
  return (d > 0) ? '>' : '<';
}

char elDir() {
  int d = targetEl - currentEl;
  if (abs(d) <= 2) return '*';
  return (d > 0) ? '^' : 'v';
}

// ==============================
// CONFIG MENU
// ==============================
void handleConfig() {

  if (!inConfig) {
    inConfig = true;
    cfgStep = 0;
    lcd.clear();
  }

  char b1[17];
  char b2[17];

  switch (cfgStep) {

    case 0:
      snprintf(b1, 17, "AZ MIN 0");
      snprintf(b2, 17, "ACEPTAR");
      if (acceptPressed()) {
        calib.azMin = analogRead(POT_AZ);
        cfgStep++;
      }
      break;

    case 1:
      snprintf(b1, 17, "AZ MAX 360");
      snprintf(b2, 17, "ACEPTAR");
      if (acceptPressed()) {
        calib.azMax = analogRead(POT_AZ);
        cfgStep++;
      }
      break;

    case 2:
      snprintf(b1, 17, "EL MIN 0");
      snprintf(b2, 17, "ACEPTAR");
      if (acceptPressed()) {
        calib.elMin = analogRead(POT_EL);
        cfgStep++;
      }
      break;

    case 3:
      snprintf(b1, 17, "EL MAX 90");
      snprintf(b2, 17, "ACEPTAR");
      if (acceptPressed()) {
        calib.elMax = analogRead(POT_EL);
        cfgStep++;
      }
      break;

    case 4:
      snprintf(b1, 17, "GUARDAR EEPROM");
      snprintf(b2, 17, "SALIR");

      if (acceptPressed()) {
        EEPROM.put(0, calib);
        inConfig = false;
        cfgStep = 0;
        lcd.clear();
      }
      break;
  }

  printLine(0, b1);
  printLine(1, b2);
}

// ==============================
// CONTROL MANUAL
// ==============================
void controlManual() {

  stopAll();

  if (digitalRead(BTN_AZ_CW) == LOW) digitalWrite(RELAY_AZ_CW, HIGH);
  if (digitalRead(BTN_AZ_CCW) == LOW) digitalWrite(RELAY_AZ_CCW, HIGH);
  if (digitalRead(BTN_EL_DOWN) == LOW) digitalWrite(RELAY_EL_DOWN, HIGH);

  if (leerA5() == 1)
    digitalWrite(RELAY_EL_UP, HIGH);
}

// ==============================
// CONTROL AUTO
// ==============================
void controlAuto() {

  if (abs(currentAz - targetAz) > 2) {
    digitalWrite(RELAY_AZ_CW, currentAz < targetAz);
    digitalWrite(RELAY_AZ_CCW, currentAz > targetAz);
  } else stopAll();

  if (abs(currentEl - targetEl) > 2) {
    digitalWrite(RELAY_EL_UP, currentEl < targetEl);
    digitalWrite(RELAY_EL_DOWN, currentEl > targetEl);
  }
}

// ==============================
// DISPLAY
// ==============================
void actualizarDisplay() {

  char l1[17];
  char l2[17];

  snprintf(l1, sizeof(l1),
           "4EG TA:%03d TE:%02d",
           targetAz,
           targetEl);

  snprintf(l2, sizeof(l2),
           "%-3s %cA:%03d %cE:%03d",
           manualMode ? "MAN" : "AUT",
           azDir(),
           currentAz,
           elDir(),
           currentEl);

  printLine(0, l1);
  printLine(1, l2);
}

// ==============================
// SETUP
// ==============================
void setup() {

  Serial.begin(9600);

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

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

  pinMode(SW_MODE, INPUT_PULLUP);

  stopAll();

  lcd.begin(16, 2);

  EEPROM.get(0, calib);

  if (calib.azMin == 0) calib.azMin = 100;
  if (calib.azMax == 0) calib.azMax = 900;
  if (calib.elMin == 0) calib.elMin = 100;
  if (calib.elMax == 0) calib.elMax = 900;

  lcd.print("EGRotor INIT");
  delay(1000);
  lcd.clear();

  systemReady = true;
}

// ==============================
// LOOP
// ==============================
void loop() {
leerSerial();
  currentAz = readAz();
  currentEl = readEl();

  updateConfigLatch();

  configMode = inConfig;
  manualMode = (digitalRead(SW_MODE) == LOW);

  if (configMode) {
    handleConfig();
    return;
  }

  if (manualMode) controlManual();
  else controlAuto();

  if (systemReady) {
    actualizarDisplay();
  }
}