#include <AccelStepper.h>

// ── Pole robocze i Ruch (Zgodnie z plikiem config.h) ────────────────────
constexpr float STEPS_PER_MM_X      = 80.0f; //
constexpr float STEPS_PER_MM_Y      = 80.0f; //
constexpr float TEST_SPEED_MM_S     = 150.0f;   // Wolna prędkość testowa z config.h
constexpr float ACCEL_MM_S2         = 200.0f; //100.0f; // Łagodne przyspieszenie

// ── Piny GPIO (Zaktualizowane na podstawie pliku config.h) ─────────────────────
constexpr uint8_t PIN_X_STEP        = 26; //
constexpr uint8_t PIN_X_DIR         = 27; //
constexpr uint8_t PIN_Y_STEP        = 14; //
constexpr uint8_t PIN_Y_DIR         = 15; //
constexpr uint8_t PIN_STEPPER_EN    = 13; //

// Wyłączniki krańcowe (Zaktualizowane piny z config.h)
constexpr uint8_t PIN_LIMIT_X_MIN   = 36; // LEWA STRONA (DOM X)
constexpr uint8_t PIN_LIMIT_X_MAX   = 34; // PRAWA STRONA (KONIEC X)
constexpr uint8_t PIN_LIMIT_Y_MIN   = 39; //
constexpr uint8_t PIN_LIMIT_Y_MAX   = 35; //

// Serwo i Przyciski (Zgodnie z config.h)
constexpr uint8_t PIN_SERVO         = 25; //
constexpr int     SERVO_ANGLE_UP    = 90; //
constexpr int     SERVO_ANGLE_DOWN  = 50; //
constexpr uint8_t PIN_BTN_UP        = 32; //
constexpr uint8_t PIN_BTN_DOWN      = 33; //
constexpr uint8_t PIN_BTN_OK        = 4;  //

// ── Konfiguracja Timera Oszczędzania Energii ─────────────────────────────────
constexpr uint32_t STEPPER_TIMEOUT_MS = 2000; // 2 sekundy bezczynności przed odłączeniem prądu

// Inicjalizacja obiektów dla silników CoreXY / H-Bot
AccelStepper motorA(AccelStepper::DRIVER, PIN_X_STEP, PIN_X_DIR);
AccelStepper motorB(AccelStepper::DRIVER, PIN_Y_STEP, PIN_Y_DIR);

// Zmienne pomocnicze
bool isPenDown = false;
bool lastBtnOkState = HIGH;
uint32_t lastActivityTime = 0;
bool steppersEnabled = false;

// Funkcja włączająca prąd w silnikach
void enableSteppers() {
    if (!steppersEnabled) {
        digitalWrite(PIN_STEPPER_EN, LOW); // LOW = Włączone
        steppersEnabled = true;
        Serial.println("[ZASILANIE] Silniki włączone");
    }
    lastActivityTime = millis(); // Resetowanie licznika czasu aktywności
}

// Funkcja odcinająca prąd w silnikach (ochrona przed grzaniem)
void disableSteppers() {
    if (steppersEnabled) {
        motorA.stop();
        motorB.stop();
        digitalWrite(PIN_STEPPER_EN, HIGH); // HIGH = Odłączone
        steppersEnabled = false;
        Serial.println("[ZASILANIE] Bezczynność 4s -> Odcięcie prądu (Silniki chłodne)");
    }
}

// Sterowanie serwem (natywne PWM dla ESP32)
void setServoAngle(uint8_t pin, int angle) {
    int dutyCycle = map(angle, 0, 180, 13, 26);
    analogWrite(pin, dutyCycle);
}

// Sprawdzenie stanu krańcówki (LOW = wciśnięta)
bool isLimitTriggered(uint8_t pin) {
    return digitalRead(pin) == LOW;
}

void setup() {
    Serial.begin(115200);
    //while (!Serial) delay(10);
    delay(500);

    // Konfiguracja wejść
    pinMode(PIN_LIMIT_X_MIN, INPUT);
    pinMode(PIN_LIMIT_X_MAX, INPUT);
    pinMode(PIN_LIMIT_Y_MIN, INPUT);
    pinMode(PIN_LIMIT_Y_MAX, INPUT);
    
    pinMode(PIN_BTN_UP, INPUT_PULLUP);
    pinMode(PIN_BTN_DOWN, INPUT_PULLUP);
    pinMode(PIN_BTN_OK, INPUT_PULLUP);

    // Konfiguracja pinu Enable silników
    pinMode(PIN_STEPPER_EN, OUTPUT);
    steppersEnabled = false; 
    enableSteppers();

    // Konfiguracja PWM dla Serwa (50Hz)
    analogWriteFrequency(PIN_SERVO, 50); 
    setServoAngle(PIN_SERVO, SERVO_ANGLE_UP); // Pisak w górze na starcie

    // Maksymalne prędkości silników
    motorA.setMaxSpeed(TEST_SPEED_MM_S * STEPS_PER_MM_X);
    motorB.setMaxSpeed(TEST_SPEED_MM_S * STEPS_PER_MM_Y);
}

void loop() {
    uint32_t currentTime = millis();

    // ─── 1. OBSŁUGA SERWA (PRZYCISK OK) ──────────────────────────────────────
    // bool currentBtnOkState = digitalRead(PIN_BTN_OK);
    
    // if (currentBtnOkState == LOW && lastBtnOkState == HIGH) {
    //     delay(50); // Debounce
    //     if (digitalRead(PIN_BTN_OK) == LOW) { 
    //         enableSteppers(); // Aktywność użytkownika wybudza silniki
    //         isPenDown = !isPenDown; 
            
    //         if (isPenDown) {
    //             setServoAngle(PIN_SERVO, SERVO_ANGLE_DOWN); //
    //             Serial.println("[SERWO] Pisak opuszczony (DOWN)");
    //         } else {
    //             setServoAngle(PIN_SERVO, SERVO_ANGLE_UP); //
    //             Serial.println("[SERWO] Pisak podniesiony (UP)");
    //         }
    //     }
    // }
    // lastBtnOkState = currentBtnOkState;

    // ─── 2. OBSŁUGA RUCHU OSI X (PRZYCISKI UP / DOWN) ────────────────────────
    bool upPressed = (digitalRead(PIN_BTN_UP) == LOW);
    bool downPressed = (digitalRead(PIN_BTN_DOWN) == LOW);

    // if (isLimitTriggered(PIN_LIMIT_X_MAX)){
    //     Serial.println("Krańcówka X_MAX (PRAWA) aktywna!");
    // }

    // if (isLimitTriggered(PIN_LIMIT_X_MIN)){
    //     Serial.println("Krańcówka X_MIN (LEWA) aktywna!");
    // }

    // if (isLimitTriggered(PIN_LIMIT_Y_MIN)){
    //     Serial.println("Krańcówka Y_MIN (GORNA) aktywna!");
    // }

    // if (isLimitTriggered(PIN_LIMIT_Y_MAX)){
    //     Serial.println("Krańcówka X_MAX (DOLNA) aktywna!");
    // }


    if (upPressed && !downPressed) { // Wciśnięty UP
        enableSteppers(); 
        
        // --- RUCH W PRAWO ---
        // Sprawdzamy, czy nie została osiągnięta prawa krańcówka (X_MAX)
        if (isLimitTriggered(PIN_LIMIT_X_MAX)) {
            Serial.println("[STOP] Krańcówka X_MAX (PRAWA) aktywna! Ruch zablokowany.");
            motorA.stop();
            motorB.stop();
        } else {
            // Oba silniki kręcą się w tę samą stronę (np. wartości dodatnie)
            motorA.setSpeed(TEST_SPEED_MM_S * STEPS_PER_MM_X);
            motorB.setSpeed(TEST_SPEED_MM_S * STEPS_PER_MM_Y);
            motorA.runSpeed();
            motorB.runSpeed();
        }
    } 
    else if (downPressed && !upPressed) { // Przycisk DOWN
        enableSteppers();
        
        // --- RUCH W LEWO ---
        // Sprawdzamy, czy nie została osiągnięta lewa krańcówka (X_MIN)
        if (isLimitTriggered(PIN_LIMIT_X_MIN)) {
            Serial.println("[STOP] Krańcówka X_MIN (LEWA) aktywna! Ruch zablokowany.");
            motorA.stop();
            motorB.stop();
        } else {
            // Oba silniki kręcą się w tę samą stronę, ale w przeciwnym kierunku (ujemne prędkości)
            motorA.setSpeed(-(TEST_SPEED_MM_S * STEPS_PER_MM_X));
            motorB.setSpeed(-(TEST_SPEED_MM_S * STEPS_PER_MM_Y));
            motorA.runSpeed();
            motorB.runSpeed();
        }
    } 
    // else {
    //     // Gdy żaden przycisk nie jest wciśnięty -> wyhamuj silniki
    //     motorA.stop();
    //     motorB.stop();
    // }

    // ─── 3. AUTOMATYCZNY TIMER USYPIANIA SILNIKÓW ────────────────────────────
    if (steppersEnabled && (currentTime - lastActivityTime >= STEPPER_TIMEOUT_MS)) {
        disableSteppers();
    }
}