// Shows how to run AccelStepper in the simplest,
// fixed speed mode with no accelerations
#include "AccelStepper.h"

AccelStepper stepper; // Defaults to AccelStepper::FULL4WIRE (4 pins) on 2, 3, 4, 5

const int SPR = 500; //O koľko sa otočí každé poschodie

const int pocetPoschodi = 4;

bool smerHore;
int aktualnePoschodie;
int dalsiePoschodie;
bool stlacenePoschodia[pocetPoschodi];

void setup() {
    Serial.begin(9600);
    stepper.setMaxSpeed(1000);//Set the maximum motor speed in steps per second
    stepper.setAcceleration(200);//Set the maximum acceleration in steps per second^2

    //Nastavenie pinov 6 - 9 na INPUT_PULLUP (Tlačidlá)
    for (int i = 6; i <= 9; ++i) {
        pinMode(i, INPUT_PULLUP);
    }

    smerHore = true; //Či ide výťah smerom hore alebo dole
    aktualnePoschodie = 0;
    stepper.setCurrentPosition(0); //Nastavenie aktuálnej pozície motora na nulová
    dalsiePoschodie = -1; //Reprezentácia, že neexistuje žiadne dalšie poschodie

    // Vynulovanie stlačených tlačidiel
    for (int i = 0; i < pocetPoschodi; i++) {
        stlacenePoschodia[i] = false;
    }
}
 
void loop() {
    //Posúvanie výťahu na dalšie poschodie, ak nie je dalšie poschodie, ostáva kde je
    if (dalsiePoschodie != -1) {
        chodNaPoschodie(dalsiePoschodie);
    }
    stepper.run(); //Stará sa o samotné posúvanie motora, potrebné volať v loope

    //Kontrola stlačených kláves (Piny 6 - 9)
    for (int i = 6; i <= 9; i++) {
        if (digitalRead(i) == LOW) {
            stlacenePoschodia[i - 6] = true;
        }
    }

    //Ak výťah dorazil na dalšie Poschodie, nastaviť aktuálne poschodie a vyprázdniť dalšie poschodie
    if (dalsiePoschodie != -1 && jeNaPoschodi(dalsiePoschodie)) {
        stlacenePoschodia[dalsiePoschodie] = false;
        aktualnePoschodie = dalsiePoschodie; //Nastaví aktuálne poschodie na to, na ktoré dorazilo
        dalsiePoschodie = -1; //Vynulovanie dalšieho poschodia
        delay(1000); //Čakanie 1sek
    }

    bool jeEsteNejake = false; //Či je zvolené ešte nejaké poschodie v svojom smere
    if (dalsiePoschodie == -1) {
        if (!smerHore) {
            for (int i = aktualnePoschodie - 1; i >= 0; --i) { //Kontroluje iba poschodia smerom dole
                if (spracujPoschodie(i)) {
                    jeEsteNejake = true;
                }
            }
        } else {
            for (int i = aktualnePoschodie; i < pocetPoschodi; i++) { //Kontroluje iba poschodia smerom hore
                if (spracujPoschodie(i)) {
                    jeEsteNejake = true;
                }
            }
        }
    }
    //Ak neexistuje žiadne zvolené poschodie v svojom smere, smer otočí
    if (!jeEsteNejake) {
        smerHore = !smerHore;
    }
}


void chodNaPoschodie(int poschodie) {
    stepper.moveTo(SPR * poschodie);
}

bool jeNaPoschodi(int poschodie) {
    return stepper.currentPosition() == SPR * poschodie;
}

bool spracujPoschodie(int poschodie) {
    if (dalsiePoschodie == -1 && stlacenePoschodia[poschodie]) {
        dalsiePoschodie = poschodie;
    }
    return dalsiePoschodie != -1;
}