// G4Zero vezérlő szoftver v1.0 (FSM alaprendszer)
// Készítette: ChatGPT & Sándor - 2025

#include <Servo.h>

// === Szervók ===
Servo szervoElso;     // Első szárnyak (jobb+bal)
Servo szervoHatso;    // Hátsó vezérsíkok
Servo szervoFelso;    // Felső vezérsík

const int szervoPin1 = 32;
const int szervoPin2 = 33;
const int szervoPin3 = 34;

// === Mikrokapcsolók (LOW = aktivált, HIGH = nincs megnyomva) ===
const int mikrokapcs[10] = {22, 23, 24, 25, 26, 27, 28, 29, 30, 31};

// === LED-ek ===
const int ledZold = 39;
const int ledPiros = 40;

// === Buzzerek ===
const int buzzer[4] = {41, 42, 43, 44};

// === RC bemenetek ===
const int ch5Pin = 45;  // 3 állású kapcsoló
const int ch6Pin = 46;  // 2 állású kapcsoló

// === Állapotok ===
enum Uzemmod { LEVEGO, FOLD, VIZ, VIZ_ALATT };
Uzemmod uzemmod = FOLD;

// === Inicializáció ===
void setup() {
  Serial.begin(9600);

  szervoElso.attach(szervoPin1);
  szervoHatso.attach(szervoPin2);
  szervoFelso.attach(szervoPin3);

  for (int i = 0; i < 10; i++) pinMode(mikrokapcs[i], INPUT_PULLUP);
  pinMode(ledZold, OUTPUT);
  pinMode(ledPiros, OUTPUT);
  for (int i = 0; i < 4; i++) pinMode(buzzer[i], OUTPUT);
}

// === Üzemmód olvasás RC kapcsolókból ===
Uzemmod readUzemmod() {
  unsigned long ch5 = pulseIn(ch5Pin, HIGH, 25000);
  unsigned long ch6 = pulseIn(ch6Pin, HIGH, 25000);

  if (ch5 < 1300) return LEVEGO;
  else if (ch5 < 1700) return FOLD;
  else return (ch6 < 1300) ? VIZ_ALATT : VIZ;
}

// === Szervómozgatás + mikrokapcsoló ellenőrzés ===
bool mozgasSzarny(Servo& szervo, int celSzog, int pinNyitva, int pinZarva, int buzzerPin) {
  szervo.write(celSzog);
  unsigned long startTime = millis();
  while (millis() - startTime < 2000) {
    if ((celSzog >= 89 && digitalRead(pinNyitva) == LOW) ||
        (celSzog == 0 && digitalRead(pinZarva) == LOW)) {
      return true; // minden oké
    }
  }
  digitalWrite(buzzerPin, HIGH);
  delay(500);
  digitalWrite(buzzerPin, LOW);
  return false; // hiba
}

// === Üzemmód beállítása ===
void allitasUzemmod(Uzemmod mod) {
  bool statusElso = false;
  bool statusHatso = false;
  bool statusFelso = false;

  switch(mod) {
    case LEVEGO:
      statusElso  = mozgasSzarny(szervoElso, 90, mikrokapcs[1], mikrokapcs[0], buzzer[0]);
      statusHatso = mozgasSzarny(szervoHatso, 90, mikrokapcs[5], mikrokapcs[4], buzzer[1]);
      statusFelso = mozgasSzarny(szervoFelso, 90, mikrokapcs[9], mikrokapcs[8], buzzer[2]);
      break;
    case FOLD:
      statusElso  = mozgasSzarny(szervoElso, 0, mikrokapcs[1], mikrokapcs[0], buzzer[0]);
      statusHatso = mozgasSzarny(szervoHatso, 0, mikrokapcs[5], mikrokapcs[4], buzzer[1]);
      statusFelso = mozgasSzarny(szervoFelso, 0, mikrokapcs[9], mikrokapcs[8], buzzer[2]);
      break;
    case VIZ:
    case VIZ_ALATT:
      statusElso  = mozgasSzarny(szervoElso, 0, mikrokapcs[1], mikrokapcs[0], buzzer[0]);
      statusHatso = mozgasSzarny(szervoHatso, 90, mikrokapcs[5], mikrokapcs[4], buzzer[1]);
      statusFelso = mozgasSzarny(szervoFelso, 90, mikrokapcs[9], mikrokapcs[8], buzzer[2]);
      break;
  }

  // LED visszajelzés
  if (statusElso && statusHatso && statusFelso) {
    digitalWrite(ledZold, HIGH);
    digitalWrite(ledPiros, LOW);
  } else {
    digitalWrite(ledZold, LOW);
    digitalWrite(ledPiros, HIGH);
  }
}

// === Fő ciklus ===
void loop() {
  Uzemmod ujMod = readUzemmod();
  if (ujMod != uzemmod) {
    uzemmod = ujMod;
    allitasUzemmod(uzemmod);
  }
  delay(500); // kis debounce késleltetés
}