/**
* F29AI - Artificial Intelligence and Intelligent Agents
*
* Coursework - Part I - A* Search
*
* This is the main part of the program, which extends
* the starter code given to us as part of the
* coursework assignment.
*
* @author Ryan Shah
* @author Francisco Javier Chiyah Garcia
*/

#include "astar.cpp"

#include <iostream>
#include <stdio.h>
#include <string>

/**
* State definitions based on energy cost
*/
const double STATE_EMPTY = 1.0;
const double STATE_TRAP = 5.0;

struct Robot {
    int x;
    int y;

    Robot(int x, int y) {
        x = x;
        y = y;
    }
};

struct Graph {

    struct State {
        double energy_cost;
        bool accessible;
        int x;
        int y;

        State() {
        }

        State(int xPos, int yPos) {
            energy_cost = STATE_EMPTY;
            accessible = true;
            x = xPos;
            y = yPos;
        }

        State(double cost, bool access, int xPos, int yPos) {
            energy_cost = cost;
            accessible = access;
            x = xPos;
            y = yPos;
        }

        double getEnergyCost() {
            if (accessible) {
                return energy_cost;
            }
            return 100;
        }

        bool operator==(const State& s) const {
            return ((x == s.x) && (y == s.y));
        }

        bool operator!=(const State& s) const {
            return !((x == s.x) && (y == s.y));
        }

        bool operator<(const State& s) const {
            if(y < s.y)
                return true;
            else if(y > s.y)
                return false;
            else {
                if(x < s.x)
                    return true;
                return false;
            }
        }
    };

    int rows, cols;
    vector<vector<State>> gridMatrix;

    Graph(int r, int c) {
        rows = r;
        cols = c;

        for(int i = 0; i < rows; i++) {
            vector<State> temp;
            for(int j = 0; j < cols; j++) {
                temp.push_back(State(i, j));
            }
            gridMatrix.push_back(temp);
        }

        //traps
        gridMatrix[2][3].energy_cost = STATE_TRAP;
        //walls
        gridMatrix[1][3].accessible = false;
        gridMatrix[1][4].accessible = false;
        gridMatrix[2][0].accessible = false;
        gridMatrix[4][2].accessible = false;
        gridMatrix[3][3].accessible = false;
        gridMatrix[4][3].accessible = false;
        gridMatrix[3][5].accessible = false;
        gridMatrix[3][7].accessible = false;

    }

    /**
    * Returns a vector containing the
    * neighbouring States for a given
    * State in order: N - E - S - W
    *
    * @param s A State
    * @return A vector containing all neighbouring States for the given State
    */
    vector<State> neighbours(State s) {
        vector<State> neighbour_vec;

        // Get each neighbour by coordinates
        if (s.x - 1 >= 0) { // West
            neighbour_vec.push_back(gridMatrix[s.x - 1][s.y]);
        }
        if (s.y + 1 < rows) { // South
            neighbour_vec.push_back(gridMatrix[s.x][s.y + 1]);
        }
        if (s.x + 1 < cols) { // East
            neighbour_vec.push_back(gridMatrix[s.x + 1][s.y]);
        }
        if (s.y - 1 >= 0) { // North
            neighbour_vec.push_back(gridMatrix[s.x][s.y - 1]);
        }

        return(neighbour_vec);
    }

    /**
    * Returns the cost of moving from one
    * State to another.
    *
    * @param  a The initial State
    * @param  b The destination State
    * @return The cost of moving from the initial to the destination State
    */
    double cost(State a, State b) {
        // This will not work if one of the states is not in the matrix
        return b.energy_cost;
    }
};

typedef typename Graph::State State;

/**
* Returns the heuristic value of transitioning
* from State a to State b using the Best-First
* heuristic search function.
*
* @param  a The initial State
* @param  b The destination State
* @return The heuristic value of transitioning from the initial to the destination State
*/
double heuristic_best_first(State a, State b) {
    State curState;

    /*if (a == b)
    return(0);
    else
    curState = a;

    return(-1);*/

    return(-1);
}

/**
 * Returns the heuristic value of transitioning
 * from State a to State b using the Manhattan Distance
 * heuristic function.
 *
 * @param  a The initial State
 * @param  b The destination State
 * @return The heuristic value of transitioning from the initial to the destination State
 */
double heuristic_manhattan(State a, State b) {
    return abs((a.x - b.x) + (a.y - b.y));

    /*
        It should be obvious that if there are no obstacles, then moving |x - x_goal|
        steps right or left, then |y - y_goal| steps up or down (or y, then x) cannot be
        longer than the actual shortest path to the goal with obstacles, therefore the
        heuristic is admissible.
     */
}

void display_matrix(Graph g) {
    for (int y = 0; y < g.rows; y++) {
        for (int x = 0; x < g.cols; x++) {
            State temp = g.gridMatrix[y][x];
            if(temp.accessible)
                std::cout << "[" << temp.energy_cost << "]";
            else
                std::cout << "[" << "W" << "]";
        }
        std::cout << "\n";
    }
}

void display_vector(vector<State> states) {
    std::vector<State>::iterator it;

    for (it = states.begin(); it < states.end(); it++) {
        std::cout << "[" << (*it).x << ", " << (*it).y << "] ";
    }
}

/**
* Display the solution path.
*
* @param g The graph of the solution
* @param path The solution path vector
*/
void display_solution(Graph g, vector<State> path) {
    vector<vector<Graph::State> > matrix = g.gridMatrix;
    vector<vector<std::string> > str_matrix;

    for(int y = 0; y < g.rows; y++) {
        vector<std::string> row;

        for(int x = 0; x < g.cols; x++) {
            Graph::State temp = matrix[y][x];
            if(temp.accessible)
                row.push_back((temp.energy_cost == STATE_EMPTY ? "1" : "T"));
            else
                row.push_back("W");
        }
        str_matrix.push_back(row);
    }

    for(int i = 0; i < path.size(); i++) {
       Graph::State temp = path[i];
        if(i == 0)
            str_matrix[temp.y][temp.x] = "S";
        else if(i == path.size() - 1)
            str_matrix[temp.y][temp.x] = "G";
        else
            str_matrix[temp.y][temp.x] = "->";
    }

    for(int y = 0; y < g.rows; y++) {
        for(int x = 0; x < g.cols; x++) {
            std::cout << "[" << str_matrix[y][x] << "]";
        }
        std::cout << "\n";
    }
}

int main() {
    Graph g = Graph(5, 8);
    display_matrix(g);

    State initial_state = g.gridMatrix[0][0];
    State destination_state = g.gridMatrix[2][0];

    //display_vector(g.neighbours(State(1,1)));

    std::cout << "\nPath:\n";
    //Call search
    vector<State> path;
    a_star_search(g, initial_state, destination_state, heuristic_manhattan, path);

    display_vector(path);
    std::cout << "\n\nSolution:\n";
    //Display result
    display_solution(g, path);

}