#include <AccelStepper.h>
#include <ButtonDebounce.h>
#include <MD_MAX72xx.h>
#include <LedControl.h>

#define STEPSIZE 52
#define MAX_MOVES 256
#define UP 1
#define LEFT 2
#define DOWN 3
#define RIGHT 4

// Achtung: Stellen Sie sicher, dass diese Pins beim ESP32 S3 verfügbar und nicht reserviert sind.
#define CLK_PIN 18 // Verändert für ESP32 S3
#define DATA_PIN 23 // Verändert für ESP32 S3
#define CS_PIN 5 // Verändert für ESP32 S3
#define NUM_SEGMENTS 4

AccelStepper downRight (1, 16, 17); // Verändert für ESP32 S3
AccelStepper downLeft (1, 4, 5); // Verändert für ESP32 S3
AccelStepper upLeft (1, 27, 26); // Verändert für ESP32 S3
AccelStepper upRight (1, 32, 33); // Verändert für ESP32 S3

ButtonDebounce up(1, 100); // Verändert für ESP32 S3
ButtonDebounce left(2, 100); // Verändert für ESP32 S3
ButtonDebounce down(42, 100); // Verändert für ESP32 S3
ButtonDebounce right(41, 100); // Verändert für ESP32 S3
ButtonDebounce enter(40, 100); // Verändert für ESP32 S3

LedControl lc = LedControl(DATA_PIN, CLK_PIN, CS_PIN, NUM_SEGMENTS);

int currentRow = 7;
int currentCol = 0;
int currentModule = 3;
int moves[MAX_MOVES];
int count = 0;
int upCounter = 1;
int downCounter = 1;
int speed = 1500;
int stepSize = 100;
int start = 0;
int first = 1;
int prevDownState = 1;

void upChanged(const int state) {
    byte block = 0;
    if (String(state) == "0") {
        if (currentRow <= 7 && currentRow > 0 && currentModule <= 1) {
            currentRow--;
        } else if (currentRow < 7 && currentModule > 1) {
            currentRow++;
        } else if (currentModule >= 3) {
            currentModule = currentModule - 3;
            currentCol = 7 - currentCol;
            currentRow = 7;
        } else if (currentModule == 2 && currentRow >= 7) {
            currentModule = currentModule - 1;
            currentCol = 7 - currentCol;
            currentRow = 7;
        } else {
            block = 1;
        }
        Serial.print(currentModule);
        Serial.print(currentRow);
        Serial.println(currentCol);
        if (!block) {
            moves[count] = UP;
            count++;
        }
    }
}

void leftChanged(const int state) {
    byte block = 0;
    if (String(state) == "0") {
        if (currentCol > 0 && currentModule > 1) {
            currentCol--;
        } else if (currentModule == 2 && currentCol > 0 || currentModule == 3) {
            currentModule--;
            currentCol = 7;
        } else if (currentCol < 7 && currentModule <= 1) {
            currentCol++;
        } else if (currentModule == 0 && currentCol >= 7) {
            currentModule++;
            currentCol = 0;
        }
        Serial.print(currentModule);
        Serial.print(currentRow);
        Serial.println(currentCol);
        if (!block) {
            moves[count] = LEFT;
            count++;
        }
    }
}

void downChanged(const int state) {
    byte block = 0;
    if (String(state) == "0") {
        if (currentRow < 7 && currentModule <= 1) {
            currentRow++;
        } else if (currentRow > 0 && currentModule > 1) {
            currentRow--;
        } else if (currentModule == 0) {
            currentModule = currentModule + 3;
            currentCol = 7 - currentCol;
            currentRow = 7;
        } else if (currentModule == 1) {
            currentModule++;
            currentCol = 7 - currentCol;
            currentRow = 7;
        }
        Serial.print(currentModule);
        Serial.print(currentRow);
        Serial.println(currentCol);
        if (!block) {
            moves[count] = DOWN;
            count++;
        }
    }
}

void rightChanged(const int state) {
    byte block = 0;
    if (String(state) == "0") {
        if (currentCol < 7 && currentModule > 1) {
            currentCol++;
        } else if (currentModule == 2 && currentCol >= 7) {
            currentModule++;
            currentCol = 0;
        } else if (currentCol > 0 && currentModule <= 1) {
            currentCol--;
        } else if (currentModule == 1 && currentCol <= 0) {
            currentModule--;
            currentCol = 7;
        }
        Serial.print(currentModule);
        Serial.print(currentRow);
        Serial.println(currentCol);
        if (!block) {
            moves[count] = RIGHT;
            count++;
        }
    }
}

void enterChanged(const int state) {
    if (String(state) == "0") {
        for (int i = 0; i < MAX_MOVES; i++) {
            if (moves[i] == UP) {
                moveUp(stepSize);
            } else if (moves[i] == LEFT) {
                moveLeft(stepSize);
            } else if (moves[i] == DOWN) {
                moveDown(stepSize);
            } else if (moves[i] == RIGHT) {
                moveRight(stepSize);
            }
        }
        // Reset stepper positions and counters after playback
        downRight.setCurrentPosition(0);
        downLeft.setCurrentPosition(0);
        upRight.setCurrentPosition(0);
        upLeft.setCurrentPosition(0);
        count = 0;  // Reset move counter
    }
}

void setup() {
    Serial.begin(115200); // Ändern Sie die Baudrate auf 115200 für ESP32

    up.setCallback(upChanged);
    left.setCallback(leftChanged);
    down.setCallback(downChanged);
    right.setCallback(rightChanged);
    enter.setCallback(enterChanged);

    for (int i = 0; i < NUM_SEGMENTS; i++) {
        lc.shutdown(i, false);
        lc.setIntensity(i, 8);
        lc.clearDisplay(i);
    }

    lc.setLed(currentModule, currentRow, currentCol, true);

    downRight.setMaxSpeed(300);
    downLeft.setMaxSpeed(300);
    upRight.setMaxSpeed(300);
    upLeft.setMaxSpeed(300);
    upLeft.setAcceleration(2000);
    upRight.setAcceleration(2000);
    downLeft.setAcceleration(2000);
    downRight.setAcceleration(2000);

    pinMode(STEPSIZE, INPUT_PULLUP);
}

void loop() {
    up.update();
    left.update();
    down.update();
    right.update();
    enter.update();

    lc.setLed(currentModule, currentRow, currentCol, true);

    if (digitalRead(STEPSIZE)) {
        stepSize = 100;
    } else {
        stepSize = 1000;
    }

    if (count > MAX_MOVES) {
        count = 0;
    }
}

void moveUp(int steps) {
    upLeft.move(-steps);
    upRight.move(steps);
    downLeft.move(-steps);
    downRight.move(steps);

    do {
        upLeft.run();
        upRight.run();
        downLeft.run();
        downRight.run();
    } while (upLeft.distanceToGo() != 0 && upRight.distanceToGo() != 0);
}

void moveDown(int steps) {
    upLeft.move(steps);
    upRight.move(-steps);
    downLeft.move(steps);
    downRight.move(-steps);

    do {
        upLeft.run();
        upRight.run();
        downLeft.run();
        downRight.run();
    } while (upLeft.distanceToGo() != 0 && upRight.distanceToGo() != 0);
}

void moveLeft(int steps) {
    upLeft.move(-steps);
    upRight.move(-steps);
    downLeft.move(-steps);
    downRight.move(-steps);

    do {
        upLeft.run();
        upRight.run();
        downLeft.run();
        downRight.run();
    } while (upLeft.distanceToGo() != 0 && upRight.distanceToGo() != 0);
}

void moveRight(int steps) {
    upLeft.move(steps);
    upRight.move(steps);
    downLeft.move(steps);
    downRight.move(steps);

    do {
        upLeft.run();
        upRight.run();
        downLeft.run();
        downRight.run();
    } while (upLeft.distanceToGo() != 0 && upRight.distanceToGo() != 0);
}