#include "AStar.h"

bool AStar::isValid(int row, int col) {
    return (row >= 0) && (row < ROW) &&
        (col >= 0) && (col < COL);
}

bool AStar::isUnBlocked(char** grid, int row, int col) {
    if (grid[row][col] == MazeGenerator::Maze_Char_Space)
        return (true);
}

bool AStar::isDestination(int row, int col, Pair dest) {
    if (row == dest.first && col == dest.second)
        return (true);
    else
        return (false);
}
 
double AStar::calculateHValue(int row, int col, Pair dest) {
    return ((double)sqrt((row - dest.first) * (row - dest.first)
        + (col - dest.second) * (col - dest.second)));
}

void AStar::tracePath(solution solutionDetails[][COL], Pair dest, int* &aStarTrace, int& aStarTraceCount) {
    printf("\nThe Path is ");
    std::cout << std::endl;
    int row = dest.first;
    int col = dest.second;
    int count = 0;
    std::stack<Pair> Path;

  
    aStarTrace = new int[100];
    while (!(solutionDetails[row][col].parent_i == row
        && solutionDetails[row][col].parent_j == col))     {
        Path.push(std::make_pair(row, col));
        int temp_row = solutionDetails[row][col].parent_i;
        int temp_col = solutionDetails[row][col].parent_j;
        row = temp_row;
        col = temp_col;

        aStarTrace[count] = row;
        count++;
        aStarTrace[count] = col;
        count++;
        
        aStarTraceCount++;
    }
 
    Path.push(std::make_pair(row, col));
    while (!Path.empty())  {
        std::pair<int, int> p = Path.top();
        Path.pop();
    
        printf("-> (%d,%d) ", p.first, p.second);

    
    }
    return;
}

void AStar::aStarSearch(char** grid, Pair src, Pair dest, int* &aStarTrace, int& aStarTraceCount) { 
    if (isValid(src.first, src.second) == false) {
        printf("Source is invalid\n");
        return;
    }
 
    if (isValid(dest.first, dest.second) == false) {
        printf("Destination is invalid\n");
        return;
    }

    if (isUnBlocked(grid, src.first, src.second) == false ||
        isUnBlocked(grid, dest.first, dest.second) == false) {
        printf("Source or the destination is blocked\n");
        return;
    }

    if (isDestination(src.first, src.second, dest) == true) {
        printf("We are already at the destination\n");
        return;
    }

    bool closedList[ROW][COL];
    memset(closedList, false, sizeof(closedList));

    solution solutionDetails[ROW][COL];

    int i, j;

    for (i = 0; i < ROW; i++) {
        for (j = 0; j < COL; j++) {
            solutionDetails[i][j].f = FLT_MAX;
            solutionDetails[i][j].g = FLT_MAX;
            solutionDetails[i][j].h = FLT_MAX;
            solutionDetails[i][j].parent_i = -1;
            solutionDetails[i][j].parent_j = -1;
        }
    }

    i = src.first, j = src.second;
    solutionDetails[i][j].f = 0.0;
    solutionDetails[i][j].g = 0.0;
    solutionDetails[i][j].h = 0.0;
    solutionDetails[i][j].parent_i = i;
    solutionDetails[i][j].parent_j = j;

    std::set<pPair> openList;

    openList.insert(std::make_pair(0.0, std::make_pair(i, j)));

    bool foundDest = false;

    while (!openList.empty()) {
        pPair p = *openList.begin();

        openList.erase(openList.begin());

        i = p.second.first;
        j = p.second.second;
        closedList[i][j] = true;

        double gNew, hNew, fNew;

        if (isValid(i - 1, j) == true) {
            if (isDestination(i - 1, j, dest) == true) {
                solutionDetails[i - 1][j].parent_i = i;
                solutionDetails[i - 1][j].parent_j = j;
                printf("The destination is found\n");
                tracePath(solutionDetails, dest, aStarTrace, aStarTraceCount);
                foundDest = true;
                return;
            }
            else if (closedList[i - 1][j] == false &&
                isUnBlocked(grid, i - 1, j) == true)  {
                gNew = solutionDetails[i][j].g + 1.0;
                hNew = calculateHValue(i - 1, j, dest);
                fNew = gNew + hNew;
 
                if (solutionDetails[i - 1][j].f == FLT_MAX ||
                    solutionDetails[i - 1][j].f > fNew) {
                    openList.insert(std::make_pair(fNew,
                        std::make_pair(i - 1, j)));
                    solutionDetails[i - 1][j].f = fNew;
                    solutionDetails[i - 1][j].g = gNew;
                    solutionDetails[i - 1][j].h = hNew;
                    solutionDetails[i - 1][j].parent_i = i;
                    solutionDetails[i - 1][j].parent_j = j;
                }
            }
        }
        if (isValid(i + 1, j) == true) {
            if (isDestination(i + 1, j, dest) == true) { 
                solutionDetails[i + 1][j].parent_i = i;
                solutionDetails[i + 1][j].parent_j = j;
                printf("The destination is found\n");
                tracePath(solutionDetails, dest, aStarTrace, aStarTraceCount);
                foundDest = true;
                return;
            } 
            else if (closedList[i + 1][j] == false &&
                isUnBlocked(grid, i + 1, j) == true) {
                gNew = solutionDetails[i][j].g + 1.0;
                hNew = calculateHValue(i + 1, j, dest);
                fNew = gNew + hNew;

                if (solutionDetails[i + 1][j].f == FLT_MAX ||
                    solutionDetails[i + 1][j].f > fNew) {
                    openList.insert(std::make_pair(fNew, std::make_pair(i + 1, j)));
                    solutionDetails[i + 1][j].f = fNew;
                    solutionDetails[i + 1][j].g = gNew;
                    solutionDetails[i + 1][j].h = hNew;
                    solutionDetails[i + 1][j].parent_i = i;
                    solutionDetails[i + 1][j].parent_j = j;
                }
            }
        }

        if (isValid(i, j + 1) == true) {
            if (isDestination(i, j + 1, dest) == true)  {
                solutionDetails[i][j + 1].parent_i = i;
                solutionDetails[i][j + 1].parent_j = j;
                printf("The destination is found\n");
                tracePath(solutionDetails, dest, aStarTrace, aStarTraceCount);
                foundDest = true;
                return;
            }

            else if (closedList[i][j + 1] == false &&
                isUnBlocked(grid, i, j + 1) == true)  {
                gNew = solutionDetails[i][j].g + 1.0;
                hNew = calculateHValue(i, j + 1, dest);
                fNew = gNew + hNew;

                if (solutionDetails[i][j + 1].f == FLT_MAX ||
                    solutionDetails[i][j + 1].f > fNew) {
                    openList.insert(std::make_pair(fNew,
                        std::make_pair(i, j + 1)));

                    solutionDetails[i][j + 1].f = fNew;
                    solutionDetails[i][j + 1].g = gNew;
                    solutionDetails[i][j + 1].h = hNew;
                    solutionDetails[i][j + 1].parent_i = i;
                    solutionDetails[i][j + 1].parent_j = j;
                }
            }
        }
 
        if (isValid(i, j - 1) == true) {
            if (isDestination(i, j - 1, dest) == true) { 
                solutionDetails[i][j - 1].parent_i = i;
                solutionDetails[i][j - 1].parent_j = j;
                printf("The destination is found\n");
                tracePath(solutionDetails, dest, aStarTrace, aStarTraceCount);
                foundDest = true;
                return;
            }
            else if (closedList[i][j - 1] == false &&
                isUnBlocked(grid, i, j - 1) == true)  {
                gNew = solutionDetails[i][j].g + 1.0;
                hNew = calculateHValue(i, j - 1, dest);
                fNew = gNew + hNew;
 
                if (solutionDetails[i][j - 1].f == FLT_MAX ||
                    solutionDetails[i][j - 1].f > fNew) {
                    openList.insert(std::make_pair(fNew,
                        std::make_pair(i, j - 1)));

                    solutionDetails[i][j - 1].f = fNew;
                    solutionDetails[i][j - 1].g = gNew;
                    solutionDetails[i][j - 1].h = hNew;
                    solutionDetails[i][j - 1].parent_i = i;
                    solutionDetails[i][j - 1].parent_j = j;
                }
            }
        }

        if (isValid(i - 1, j + 1) == true) {
            if (isDestination(i - 1, j + 1, dest) == true) {
                solutionDetails[i - 1][j + 1].parent_i = i;
                solutionDetails[i - 1][j + 1].parent_j = j;
                printf("The destination is found\n");
                tracePath(solutionDetails, dest, aStarTrace, aStarTraceCount);
                foundDest = true;
                return;
            }
            else if (closedList[i - 1][j + 1] == false &&
                isUnBlocked(grid, i - 1, j + 1) == true) {
                gNew = solutionDetails[i][j].g + 1.414;
                hNew = calculateHValue(i - 1, j + 1, dest);
                fNew = gNew + hNew;

                if (solutionDetails[i - 1][j + 1].f == FLT_MAX ||
                    solutionDetails[i - 1][j + 1].f > fNew) {
                    openList.insert(std::make_pair(fNew,
                        std::make_pair(i - 1, j + 1)));
 
                    solutionDetails[i - 1][j + 1].f = fNew;
                    solutionDetails[i - 1][j + 1].g = gNew;
                    solutionDetails[i - 1][j + 1].h = hNew;
                    solutionDetails[i - 1][j + 1].parent_i = i;
                    solutionDetails[i - 1][j + 1].parent_j = j;
                }
            }
        }

        if (isValid(i - 1, j - 1) == true) {
            if (isDestination(i - 1, j - 1, dest) == true) {
                solutionDetails[i - 1][j - 1].parent_i = i;
                solutionDetails[i - 1][j - 1].parent_j = j;
                printf("The destination is found\n");
                tracePath(solutionDetails, dest, aStarTrace, aStarTraceCount);
                foundDest = true;
                return;
            }
            else if (closedList[i - 1][j - 1] == false &&
                isUnBlocked(grid, i - 1, j - 1) == true) {
                gNew = solutionDetails[i][j].g + 1.414;
                hNew = calculateHValue(i - 1, j - 1, dest);
                fNew = gNew + hNew;
                if (solutionDetails[i - 1][j - 1].f == FLT_MAX ||
                    solutionDetails[i - 1][j - 1].f > fNew) {
                    openList.insert(std::make_pair(fNew, std::make_pair(i - 1, j - 1)));
                    solutionDetails[i - 1][j - 1].f = fNew;
                    solutionDetails[i - 1][j - 1].g = gNew;
                    solutionDetails[i - 1][j - 1].h = hNew;
                    solutionDetails[i - 1][j - 1].parent_i = i;
                    solutionDetails[i - 1][j - 1].parent_j = j;
                }
            }
        }
 
        if (isValid(i + 1, j + 1) == true) {
            if (isDestination(i + 1, j + 1, dest) == true) {
                solutionDetails[i + 1][j + 1].parent_i = i;
                solutionDetails[i + 1][j + 1].parent_j = j;
                printf("The destination is found\n");
                tracePath(solutionDetails, dest, aStarTrace, aStarTraceCount);
                foundDest = true;
                return;
            }
            else if (closedList[i + 1][j + 1] == false &&
                isUnBlocked(grid, i + 1, j + 1) == true) {
                gNew = solutionDetails[i][j].g + 1.414;
                hNew = calculateHValue(i + 1, j + 1, dest);
                fNew = gNew + hNew;
                if (solutionDetails[i + 1][j + 1].f == FLT_MAX ||
                    solutionDetails[i + 1][j + 1].f > fNew) {
                    openList.insert(std::make_pair(fNew,
                        std::make_pair(i + 1, j + 1)));

                    solutionDetails[i + 1][j + 1].f = fNew;
                    solutionDetails[i + 1][j + 1].g = gNew;
                    solutionDetails[i + 1][j + 1].h = hNew;
                    solutionDetails[i + 1][j + 1].parent_i = i;
                    solutionDetails[i + 1][j + 1].parent_j = j;
                }
            }
        }

        if (isValid(i + 1, j - 1) == true) { 
            if (isDestination(i + 1, j - 1, dest) == true) {
                solutionDetails[i + 1][j - 1].parent_i = i;
                solutionDetails[i + 1][j - 1].parent_j = j;
                printf("The destination is found\n");
                tracePath(solutionDetails, dest, aStarTrace, aStarTraceCount);
                foundDest = true;
                return;
            }

            else if (closedList[i + 1][j - 1] == false &&
                isUnBlocked(grid, i + 1, j - 1) == true) {
                gNew = solutionDetails[i][j].g + 1.414;
                hNew = calculateHValue(i + 1, j - 1, dest);
                fNew = gNew + hNew;
                if (solutionDetails[i + 1][j - 1].f == FLT_MAX ||
                    solutionDetails[i + 1][j - 1].f > fNew) {
                    openList.insert(std::make_pair(fNew,
                        std::make_pair(i + 1, j - 1)));

                    solutionDetails[i + 1][j - 1].f = fNew;
                    solutionDetails[i + 1][j - 1].g = gNew;
                    solutionDetails[i + 1][j - 1].h = hNew;
                    solutionDetails[i + 1][j - 1].parent_i = i;
                    solutionDetails[i + 1][j - 1].parent_j = j;
                }
            }
        }
    }
    if (foundDest == false)
        printf("Failed to find the Destination\n");

    return;
}