#ifndef WEG_ALGO
#define WEG_ALGO

#define WINDOWS 1

#ifdef WINDOWS
#include "stdio.h"
#include "inttypes.h"
#endif // WINDOWS

#define HOEHE 14
#define BREITE 14
#define MAX_WEG_K 16 //Maximale Anzahl an Wegknoten
#define K_COUNT 60
#define INVALID -1
#define INVALID_SHORTTEST 127

int8_t g_kCount = 1;

// k[0] = Start
struct Knoten {
    int8_t distance; // Code Optimiert für max 127 distance
    int8_t vorgaenger;
    int8_t besucht;
    int8_t x;
    int8_t y;
    int8_t vip[4]; // 0 = Start // 1 = Tisch1 ...
    int8_t KnotenNachbarDistanceSumme[K_COUNT];
    int8_t KnotenNachbarDistanceX[K_COUNT];
    int8_t KnotenNachbarDistanceY[K_COUNT];
} K[K_COUNT];

struct weg {
    int8_t knoten;
    int8_t xPos;
    int8_t yPos;
};

enum punkte {
    a, b, c, d, e, f, g, h, i, j, k, l, m, n, o, p, q, r, s, t, u, v, w, x, y, z
};

enum printMethode {
    JUNK, ALPHABET
};

char array[HOEHE][BREITE] = {
        "##############",
        "#     S      #",
        "##   #####   #",
        "#    11      #",
        "#            #",
        "#   ####   2 #",
        "#      #   2 #",
        "#      #     #",
        "#   ##########",
        "#   #        #",
        "#   #   33   #",
        "#   ###      #",
        "#            #",
        "##############"
};

// Initalisiere Knoten und Startpunkt
void initStruct() {
    // Init array
    for (int8_t i = 0; i < K_COUNT; i++) {
        K[i].distance = -1;
        K[i].vorgaenger = -1;
        K[i].besucht = 0;
        K[i].x = -1;
        K[i].y = -1;
        K[i].vip[0] = 0;
        K[i].vip[1] = 0;
        K[i].vip[2] = 0;
        K[i].vip[3] = 0;

        for (int8_t j = 0; j < K_COUNT; j++) {
            K[i].KnotenNachbarDistanceSumme[j] = 0;
            K[i].KnotenNachbarDistanceX[j] = 0;
            K[i].KnotenNachbarDistanceY[j] = 0;

        }
    }
}

void algJunkSpecialSet(char Punktbuchstabe, char Tischnummer) {

    for (int8_t i = 1; i < HOEHE - 1; i++) {
        for (int8_t j = 1; j < BREITE - 1; j++) {
            if (array[i][j] == Tischnummer) {
                //Check Oben
                if (array[i - 1][j - 1] == ' ') array[i - 1][j - 1] = Punktbuchstabe;
                if (array[i - 1][j] == ' ') array[i - 1][j] = Punktbuchstabe;
                if (array[i - 1][j + 1] == ' ') array[i - 1][j + 1] = Punktbuchstabe;

                //Check links rechts
                if (array[i][j - 1] == ' ') array[i][j - 1] = Punktbuchstabe;
                if (array[i][j + 1] == ' ') array[i][j + 1] = Punktbuchstabe;

                //Check Unten
                if (array[i + 1][j - 1] == ' ') array[i + 1][j - 1] = Punktbuchstabe;
                if (array[i + 1][j] == ' ') array[i + 1][j] = Punktbuchstabe;
                if (array[i + 1][j + 1] == ' ') array[i + 1][j + 1] = Punktbuchstabe;
            }
        }

    }
}

// Setzte Eck und Tischpunkte
void algoJunk() {

    int8_t links = 0, rechts = 0, oben = 0, unten = 0;

    for (int8_t i = 1; i < HOEHE - 1; i++) {
        for (int8_t j = 1; j < BREITE - 1; j++) {

            //   # = J = Junk         1 = X = Tisch1        2 = Y = Tisch2        3 = Z = Tisch3
            if (array[i][j] == '#' || array[i][j] == '1' || array[i][j] == '2' || array[i][j] == '3') {

                if ((array[i - 1][j] == ' ') || (array[i - 1][j] == 'J'))
                    oben = 1;

                if ((array[i + 1][j] == ' ') || (array[i + 1][j] == 'J'))
                    unten = 1;

                if ((array[i][j - 1] == ' ') || (array[i][j - 1] == 'J'))
                    links = 1;

                if ((array[i][j + 1] == ' ') || (array[i][j + 1] == 'J'))
                    rechts = 1;

                if ((array[i - 1][j + 1] == ' ') && oben && rechts)
                    array[i - 1][j + 1] = 'J';

                if ((array[i - 1][j - 1] == ' ') && oben && links)
                    array[i - 1][j - 1] = 'J';

                if ((array[i + 1][j + 1] == ' ') && unten && rechts)
                    array[i + 1][j + 1] = 'J';

                if ((array[i + 1][j - 1] == ' ') && unten && links)
                    array[i + 1][j - 1] = 'J';

                links = 0; rechts = 0; oben = 0; unten = 0;
            }
        }
    }
}

int8_t algoDijkstraNextKnoten(int8_t kNum) {

    int8_t shortway = INVALID_SHORTTEST; // 127
    int8_t shortnum = INVALID_SHORTTEST;

    for (int8_t i = 0; i < g_kCount; i++) {
        if (K[kNum].KnotenNachbarDistanceSumme[i] > 0 && K[i].besucht == 0) { // Valider Punkt

            if (K[kNum].KnotenNachbarDistanceSumme[i] < shortway) { // Ist dieser Punkt kürzer als der vorherige
                shortway = K[kNum].KnotenNachbarDistanceSumme[i];
                shortnum = i;
            }

        }
    }

    if (shortnum == INVALID_SHORTTEST)
        return -1;

    return shortnum;
}

//Möglicherweise nicht notwendig -> im Notfall für sicheren Weg
void algoJunk2() {

    for (int8_t i = 1; i < HOEHE - 1; i++) {
        for (int8_t j = 1; j < BREITE - 1; j++) {

            //   # = J = Junk         1 = X = Tisch1        2 = Y = Tisch2        3 = Z = Tisch3
            if (array[i][j] == 'J') {

                // Links && Oben
                if ((array[i - 1][j] == ' ') && (array[i][j - 1] == ' ') && (array[i - 1][j - 1] == ' '))
                    array[i - 1][j - 1] = 'J';

                // Links && Unten
                if ((array[i][j - 1] == ' ') && (array[i + 1][j] == ' ') && (array[i + 1][j - 1] == ' '))
                    array[i + 1][j - 1] = 'J';

                // Rechts && Oben
                if ((array[i - 1][j] == ' ') && (array[i][j + 1] == ' ') && (array[i - 1][j + 1] == ' '))
                    array[i - 1][j + 1] = 'J';

                // Rechts && Unten
                if ((array[i + 1][j] == ' ') && (array[i][j + 1] == ' ') && (array[i + 1][j + 1] == ' '))
                    array[i + 1][j + 1] = 'J';
            }

        }
    }
}

//Suche Konoten mit selben x / y postition
int8_t getKnoten(int8_t yPos, int8_t xPos) {

    for (int8_t i = 0; i < g_kCount; i++) {
        if (K[i].x == xPos && K[i].y == yPos)
            return i;
    }
    return 0; // nicht notwendig
}


void printFeld(enum printMethode m) {
#ifdef WINDOWS
    int c;
    for (int8_t i = 0; i < HOEHE; i++) {
        for (int8_t j = 0; j < BREITE; j++) {

            if ((m == ALPHABET) && (array[i][j] == 'J'))
            {
                c = (char)getKnoten(i, j) + 'a'; //Offset für ASCII
                printf("%c", c);
            }
            else { // JUNK
                printf("%c", array[i][j]);
            }
        }
        printf("\n");
    }
#endif
}

int8_t isVIP(int8_t yPos, int8_t xPos, char tNum) {
    //Check Oben
    if (array[yPos - 1][xPos - 1] == tNum) return 1;
    if (array[yPos - 1][xPos] == tNum) return 1;
    if (array[yPos - 1][xPos + 1] == tNum) return 1;

    //Check links rechts
    if (array[yPos][xPos - 1] == tNum) return 1;
    if (array[yPos][xPos + 1] == tNum) return 1;

    //Check Unten
    if (array[yPos + 1][xPos - 1] == tNum) return 1;
    if (array[yPos + 1][xPos] == tNum) return 1;
    if (array[yPos + 1][xPos + 1] == tNum) return 1;

    return 0;
}



void initStructUpdate() {

    for (int8_t i = 1; i < HOEHE; i++) {
        for (int8_t j = 1; j < BREITE; j++) {

            // Start init (sonder Bedingung)
            if ('S' == array[i][j]) {
                K[0].distance = 0;
                K[0].vorgaenger = 0;
                K[0].vip[0] = 1;
                K[0].x = j;
                K[0].y = i;
                K[0].besucht = 1;
            }

            // Update Table and Junk
            if (array[i][j] == 'J' && 0 == getKnoten(i, j)) {
                K[g_kCount].x = j;
                K[g_kCount].y = i;
                K[g_kCount].vip[1] = isVIP(i, j, '1');
                K[g_kCount].vip[2] = isVIP(i, j, '2');
                K[g_kCount].vip[3] = isVIP(i, j, '3');

                g_kCount++;
            }
        }
    }

}

int8_t algJunkSpecial() {

    int8_t vip1 = 0;
    int8_t vip2 = 0;
    int8_t vip3 = 0;

    for (int8_t i = 0; i < g_kCount; i++) {
        if (K[i].vip[1]) vip1++;
        if (K[i].vip[2]) vip2++;
        if (K[i].vip[3]) vip3++;
    }

    if (vip1 == 0) algJunkSpecialSet('J', '1');
    if (vip2 == 0) algJunkSpecialSet('J', '2');
    if (vip3 == 0) algJunkSpecialSet('J', '3');

    if (vip1 == 0 || vip2 == 0 || vip3 == 0)
        return 1; // Struct updaten

    return 0; // Struct nicht updaten
}


// hier optimieren für kreuz suche oder ähnlich
void algoDistanceLRUD(int8_t kNum) {

    // Such Rechts
    for (int8_t i = K[kNum].x + 1; i < BREITE; i++) {
        if (array[K[kNum].y][i] == '#') break;
        if (array[K[kNum].y][i] == 'J') {
            int kFound = getKnoten(K[kNum].y, i);
            K[kNum].KnotenNachbarDistanceSumme[kFound] = i - K[kNum].x;
            K[kNum].KnotenNachbarDistanceX[kFound] = i - K[kNum].x;
            K[kNum].KnotenNachbarDistanceY[kFound] = 0;
            break;
        }
    }
    // Suche Links
    for (int8_t i = K[kNum].x - 1; i > 0; i--) {
        if (array[K[kNum].y][i] == '#') break;
        if (array[K[kNum].y][i] == 'J') {
            int kFound = getKnoten(K[kNum].y, i);
            K[kNum].KnotenNachbarDistanceSumme[kFound] = ((i - K[kNum].x) * (-1));
            K[kNum].KnotenNachbarDistanceX[kFound] = i - K[kNum].x;
            K[kNum].KnotenNachbarDistanceY[kFound] = 0;
            break;
        }
    }
    // Suche unten
    for (int8_t i = K[kNum].y + 1; i < HOEHE; i++) {
        if (array[i][K[kNum].x] == '#') break;
        if (array[i][K[kNum].x] == 'J') {
            int kFound = getKnoten(i, K[kNum].x);
            K[kNum].KnotenNachbarDistanceSumme[kFound] = i - K[kNum].y;
            K[kNum].KnotenNachbarDistanceX[kFound] = 0;
            K[kNum].KnotenNachbarDistanceY[kFound] = i - K[kNum].y;
            break;
        }
    }
    // Suche oben
    for (int8_t i = K[kNum].y - 1; i > 0; i--) {
        if (array[i][K[kNum].x] == '#') break;
        if (array[i][K[kNum].x] == 'J') {
            int kFound = getKnoten(i, K[kNum].x);
            K[kNum].KnotenNachbarDistanceSumme[kFound] = ((i - K[kNum].y) * (-1));
            K[kNum].KnotenNachbarDistanceX[kFound] = 0;
            K[kNum].KnotenNachbarDistanceY[kFound] = i - K[kNum].y;
            break;
        }
    }
}

//Prototyp Abfrage für Querfeld
void algoDistanceQUER(int8_t kNum) {

    //// RechtsOben
    //if (array[K[kNum].y - 1][K[kNum].x] == ' ' && array[K[kNum].y][K[kNum].x + 1] == ' ' && array[K[kNum].y - 1][K[kNum].x + 1] === 'J')
    //  //setzen


    //  // RechtsUnten
    //  if (array[K[kNum].y - 1][K[kNum].x] == ' ' && array[K[kNum].y][K[kNum].x + 1] == ' ' && array[K[kNum].y + 1][K[kNum].x + 1] > 'a')
    //      //setzen


    //      // LinksOben
    //      if (array[K[kNum].y - 1][K[kNum].x] == ' ' && array[K[kNum].y][K[kNum].x - 1] == ' ' && array[K[kNum].y - 1][K[kNum].x - 1] > 'a')
    //          //setzen


    //          // LinksUnten
    //          if (array[K[kNum].y + 1][K[kNum].x] == ' ' && array[K[kNum].y][K[kNum].x - 1] == ' ' && array[K[kNum].y + 1][K[kNum].x - 1] > 'a')
    //              //setzen
}

void algoDistance() {

    for (int8_t i = 0; i < g_kCount; i++)
        algoDistanceLRUD(i);

    for (int8_t i = 0; i < g_kCount; i++)
        algoDistanceQUER(i);

}


void algoDijkstra() {

    int start = 0;
    int randKnoten = -1;
    int nextKnoten = -1;

    int randDistance = 0;
    int nextDistance = 0;
    int startNextDistance = 0;

    // Vorgaenger von Start setzen
    for (int8_t i = 0; i < g_kCount; i++) {
        if (K[start].KnotenNachbarDistanceSumme[i] > 0)
            K[i].vorgaenger = start;
    }

    while (1) {
        // Suche kürzesten Knoten von Start
        randKnoten = algoDijkstraNextKnoten(start);

        // Abbruch kein Knoten gefunden
        if (randKnoten == -1)
            break;

        // Hole die Distanz zum Randknoten
        randDistance = K[start].KnotenNachbarDistanceSumme[randKnoten];

        // Durchsuche alle NextKnoten
        for (int8_t i = 0; i < g_kCount; i++) {
            nextDistance = K[randKnoten].KnotenNachbarDistanceSumme[i];

            // Wenn keine Verbindung -> Abbruch
            if (nextDistance == 0)
                continue;

            // Hole die Distanz von Start zum NextKnoten
            startNextDistance = K[start].KnotenNachbarDistanceSumme[i];

            // Wenn kürzer oder nicht existiert dann setzte die Verbindung
            if (startNextDistance == 0 || startNextDistance > randDistance + nextDistance) {
                K[start].KnotenNachbarDistanceSumme[i] = randDistance + nextDistance;
                K[i].vorgaenger = randKnoten;
            }

        }
        K[randKnoten].besucht = 1;
    }
}

void printStart() {
#ifdef WINDOWS

    for (int8_t i = 0; i < g_kCount; i++) {
        printf("%c = %d  ", (char)i + 'a', K[0].KnotenNachbarDistanceSumme[i]);
    }
    printf("\nVorgaenger: \n");
    for (int8_t i = 0; i < g_kCount; i++) {
        printf("%c = %c  ", (char)i + 'a', ((char)K[i].vorgaenger) + 'a');
    }
    printf("\n");
#endif
}


int8_t checkAufVorgaengerFehlt() {

    for (int8_t i = 0; i < g_kCount; i++)
        if (K[i].vorgaenger == -1)
            return -1;

    return 0;
}


/////////////////Roboter fahren ANFANG


#ifdef WINDOWS

void rotateR() {
    printf("rechts drehung\n");
}
void rotateL() {
    printf("links drehung\n");
}
void drive1() {
    printf("fahren x positiv\n");
}
void drive2() {
    printf("fahren x negativ\n");
}
void drive3() {
    printf("fahren y positiv\n");
}
void drive4() {
    printf("fahren y negativ\n");
}

#endif

////////////////////////////////////////////////////////////einstellen

void drive() {

    Motor_Drive(Port_A, motorDir, 35);
    Motor_Drive(Port_B, motorDir, 35);
    Delay(1000);
}
void rotateR() {
    Motor_Drive(Port_A, Motor_dir_forward, 50); //Drehen 90 grad anpassen !!!
    Motor_Drive(Port_B, Motor_dir_forward, 10);
    Delay(1000);
}
void rotateL() {
    Motor_Drive(Port_A, Motor_dir_forward, 10); //Drehen 90 grad anpassen !!!
    Motor_Drive(Port_B, Motor_dir_forward, 50);
    Delay(1000);

}

//////////////////////////////////////////////////////////einstellen


void suche_start() {

    for (int i = 0; i < 14; i++) {
        for (int j = 0; j < 14; j++) {

            if (array[i][j] == 'S') {
                position[0] = i;
                position[1] = j;

            }

        }
    }
}

#define NORDEN 1
#define OSTEN  2
#define SUEDEN 3
#define WESTEN 4
int ausrichtung = NORDEN;       //Startrichtung
int position[2];

void RobiFahr(int y, int x) {

    int x_now = position[0];
    int y_now = position[1];

    printf("%d %d\n", y_now, x_now);


    if (x > 0) {
        switch (ausrichtung) {
        case NORDEN:
            rotateR();
            ausrichtung = OSTEN;
            printf("Osten\n");
            break;
        case SUEDEN:
            rotateL();
            ausrichtung = OSTEN;
            printf("Osten\n");
            break;
        case WESTEN:
            rotateL();
            rotateL();
            ausrichtung = OSTEN;
            printf("Osten\n");
            break;
        }
        for (int i = 0; i < x; i++)
            drive();
        position[1] ++;
    }

    if (x < 0) {
        switch (ausrichtung) {
        case NORDEN:
            rotateL();
            ausrichtung = WESTEN;
            printf("Westen\n");
            break;
        case OSTEN:
            rotateL();
            rotateL();
            ausrichtung = WESTEN;
            printf("Westen\n");
            break;
        case SUEDEN:
            rotateR();
            ausrichtung = WESTEN;
            printf("Westen\n");
            break;
        }
        x = x * (-1);
        for (int i = 0; i < x; i++) {
            drive();
            position[1]--;
        }
    }

    if (y > 0) {
        switch (ausrichtung) {
        case NORDEN:
            rotateR();
            rotateR();
            ausrichtung = SUEDEN;
            printf("Sueden\n");
            break;
        case OSTEN:
            rotateR();
            ausrichtung = SUEDEN;
            printf("Sueden\n");
            break;
        case WESTEN:
            rotateL();
            ausrichtung = SUEDEN;
            printf("Sueden\n");
            break;


        }
        for (int i = 0; i < y; i++) {
            drive();
            position[0]++;
        }

    }

    if (y < 0) {
        switch (ausrichtung) {
        case WESTEN:
            rotateR();
            ausrichtung = NORDEN;
            printf("Norden\n");
            break;
        case OSTEN:
            rotateL();
            ausrichtung = NORDEN;
            printf("Norden\n");
            break;
        case SUEDEN:
            rotateR();
            rotateR();
            ausrichtung = NORDEN;
            printf("Norden\n");
            break;


        }
        y = y * (-1);
        for (int i = 0; i < y; i++) {
            drive();
            position[0]--;
        }
    }




    ////////////////////////////////////////einstellen

    if (x == 0 && y == 0) {

        if (array[x_now + 1][y_now+1] == '1' || array[x_now + 1][y_now+1] == '2' || array[x_now + 1][y_now+1] == '3') {
            printf("rechts unten\n");
            Motor_Drive(Port_A, Motor_dir_forward, 10);             //Drehung einstellen
            Motor_Drive(Port_B, Motor_dir_forward, 50);
            Delay(1000);
            Motor_Drive(Port_A, Motor_dir_forward, 10);
            Motor_Drive(Port_B, Motor_dir_forward, 50);
            Delay(1000);
        }

        if (array[x_now - 1][y_now-1] == '1' || array[x_now - 1][y_now-1] == '2' || array[x_now - 1][y_now-1] == '3') {
            printf("links oben\n");
            Motor_Drive(Port_A, Motor_dir_forward, 10);
            Motor_Drive(Port_B, Motor_dir_forward, 50);
            Delay(1000);
            Motor_Drive(Port_A, Motor_dir_forward, 10);
            Motor_Drive(Port_B, Motor_dir_forward, 50);
            Delay(1000);
        }

        if (array[x_now-1][y_now + 1] == '1' || array[x_now-1][y_now + 1] == '2' || array[x_now-1][y_now + 1] == '3') {
            printf("links unten\n");
            Motor_Drive(Port_A, Motor_dir_forward, 10);
            Motor_Drive(Port_B, Motor_dir_forward, 50);
            Delay(1000);
            Motor_Drive(Port_A, Motor_dir_forward, 10);
            Motor_Drive(Port_B, Motor_dir_forward, 50);
            Delay(1000);
        }

        if (array[x_now][y_now - 1] == '1' || array[x_now][y_now - 1] == '2' || array[x_now][y_now - 1] == '3') {
            printf("rechts oben\n");
            Motor_Drive(Port_A, Motor_dir_forward, 10);
            Motor_Drive(Port_B, Motor_dir_forward, 50);
            Delay(1000);
            Motor_Drive(Port_A, Motor_dir_forward, 10);
            Motor_Drive(Port_B, Motor_dir_forward, 50);
            Delay(1000);
        }

    }

    //////////////////////////////////////////////einstellen
}


////////////////////////////Roboter fahren ENDE


void calcWeg(struct weg* t1, int8_t * arrLength, int8_t table) {

    struct weg trash[MAX_WEG_K];
    (*arrLength) = 0;

    //Initialisieren der struct arrays
    for (int8_t i = 0; i < MAX_WEG_K; i++) {
        t1[i].knoten = -1;
        trash[i].knoten = -1;

        t1[i].xPos = 0;
        t1[i].yPos = 0;
        trash[i].yPos = 0;
        trash[i].yPos = 0;
    }

    // Finde kuerzersten VIP[]
    int8_t zieldis1 = INVALID_SHORTTEST;
    int8_t zielpos1 = 0;
    for (int8_t i = 0; i < g_kCount; i++) {
        if (K[i].vip[table] == 1) {
            if (K[0].KnotenNachbarDistanceSumme[i] < zieldis1) {
                zieldis1 = K[0].KnotenNachbarDistanceSumme[i];
                zielpos1 = i;
            }
        }
    }

    // Vorgaenger Knoten finden
    int8_t laufKnoten = zielpos1;
    trash[0].knoten = zielpos1;
    for (int8_t j = 1; laufKnoten != 0; j++) { // 0 = der Start Knoten
        trash[j].knoten = K[laufKnoten].vorgaenger;
        laufKnoten = trash[j].knoten;
    }

    // Array Laenge finden
    while (1) {
        if (trash[(*arrLength)].knoten == -1)
            break;
        (*arrLength)++;
    }

    // Array umsortieren und ins Fahrarray schreiben
    for (int8_t i = 0, j = (*arrLength) - 1; i < (*arrLength); i++, j--) {
        t1[i].knoten = trash[j].knoten;
    }

    // Array mit xPos und yPos fuellen
    for (int8_t i = 0; i < (*arrLength) - 1; i++) {
        t1[i].xPos = K[t1[i].knoten].KnotenNachbarDistanceX[t1[i + 1].knoten];
        t1[i].yPos = K[t1[i].knoten].KnotenNachbarDistanceY[t1[i + 1].knoten];
    }

}

void sendeWeg() {

    struct weg t1[MAX_WEG_K];
    int8_t aLength = 0;

    for (int8_t j = 1; j <= 3; j++) {

        calcWeg(&t1[0], &aLength, j);

        //Hinweg übergeben an Roboter
        for (int8_t i = 0; i < aLength; i++) {
            RobiFahr(t1[i].yPos, t1[i].xPos);
        }

        //Rückweg übergeben an Roboter
        for (int8_t i = aLength - 1; i >= 0; i--) {
            RobiFahr(t1[i].yPos * (-1), t1[i].xPos * (-1));
        }

    }
}



int main() {
    suche_start();
    initStruct();
    printFeld(JUNK);

    algoJunk();
    printFeld(JUNK);

    //algoJunk2();
    //printFeld(JUNK);

    initStructUpdate();
    printFeld(ALPHABET);

    if (algJunkSpecial());
    initStructUpdate();
    printFeld(ALPHABET);

    algoDistance();
    algoDijkstra();
    printStart();

    if (checkAufVorgaengerFehlt())
        return 1; //Ausgabe am Bildschirm Error 404 not found

    sendeWeg();



    return 0;
}


#endif // !WEG_ALGO