/*
* File:   main.cpp
* Author: tdahl01
*
* Created on 15 October 2009, 21:43
*/

#include <stdlib.h>
#include <iostream>

using namespace std;

const int ACTIONS_NUMOF = 6;
const int MAX_SUCC_STATES = ACTIONS_NUMOF;

// Actions
const int NONE = -1;
const int GO_NORTH = 0;
const int GO_EAST = 1;
const int GO_SOUTH = 2;
const int GO_WEST = 3;
const int GET_SWORD = 4;
const int SLAY_DRAGON = 5;

// Locations
const int LOC_SOUTHWEST = 0;
const int LOC_SOUTH = 1;
const int LOC_SOUTHEAST = 2;
const int LOC_WEST = 3;
const int LOC_CENTER = 4;
const int LOC_EAST = 5;
const int LOC_NORTHWEST = 6;
const int LOC_NORTH = 7;
const int LOC_NORTHEAST = 8;

const bool HAS_SWORD = true;
const bool NO_SWORD = false;

const bool DRAGON_ALIVE = false;
const bool DRAGON_DEAD = true;

const int INIT_LOCATION = LOC_SOUTHWEST;
const bool INIT_SWORD_STATE = NO_SWORD;
const bool INIT_DRAGON_STATE = DRAGON_ALIVE;

const int LOC_SWORD = LOC_NORTHWEST;
const int SWORD_ROW = LOC_SWORD / 3;
const int SWORD_COLUMN = LOC_SWORD % 3;

const int LOC_DRAGON = LOC_SOUTHEAST;
const int DRAGON_ROW = LOC_DRAGON / 3;
const int DRAGON_COLUMN = LOC_DRAGON % 3;
const int MANHATTAN_DISTANCE_SWORD_DRAGON = abs(DRAGON_ROW-SWORD_ROW)+abs(DRAGON_COLUMN-SWORD_COLUMN);
const bool GOAL_DRAGON_STATE = DRAGON_DEAD;

// This class represents the complex state of the Vacuum World.
class DDState{
public:
    DDState(int location){
        this->location = location;
        swordstate = NO_SWORD;
        dragonstate = DRAGON_ALIVE;
    };

    int getLocation(){return location;};
    void setSwordState(bool swordstate){this->swordstate = swordstate;};
    bool getSwordState(){return swordstate;};
    void setDragonState(bool dragonstate){this->dragonstate = dragonstate;};
    bool getDragonState(){return dragonstate;};

    void print(){
        cout << "  location - ";
        switch (location){
            case LOC_NORTHWEST: cout << "Northwest" << endl; break;
            case LOC_NORTH: cout << "North" << endl; break;
            case LOC_NORTHEAST: cout << "Northeast" << endl; break;
            case LOC_WEST: cout << "West" << endl; break;
            case LOC_CENTER: cout << "Center" << endl; break;
            case LOC_EAST: cout << "East" << endl; break;
            case LOC_SOUTHWEST: cout << "Southwest" << endl; break;
            case LOC_SOUTH: cout << "South" << endl; break;
            case LOC_SOUTHEAST: cout << "Southeast" << endl; break;
            default: cout << "Nowhere" << endl; break;
        }
       cout << "  sword - ";
        if(swordstate == NO_SWORD)
            cout << "No sword" << endl;
        else
            cout << "Has sword" << endl;

        cout << "  dragon - ";
        if(dragonstate == DRAGON_ALIVE)
            cout << "Dragon alive" << endl;
        else
            cout << "Dragon dead" << endl;
    };

private:
    int location;
    bool swordstate;
    bool dragonstate;
};

// This class collects three successor states and allows them to be hanled
// as a single object.
class DDSuccStates {
public:
    void setSuccStatesNumof(int s){succstatesnumof = s;};
    int getSuccStatesNumof(){return succstatesnumof;};
    void setAction(int ssidx, int action){actions[ssidx] = action;};
    int getAction(int ssidx){return actions[ssidx];};
    void setSuccState(int ssidx, DDState* ss){succstates[ssidx]=ss;};
    DDState* getSuccState(int ssidx){return succstates[ssidx];};

private:
    int succstatesnumof;
    int actions[MAX_SUCC_STATES];
    DDState* succstates[MAX_SUCC_STATES];
};

// This class implements the details of the TravelWales problem
class DungeonsOfDoom {
public:

    // This method calculates the different successor states to the
    // given original state produced by following the available roads.
    static DDSuccStates* successor_states(DDState* state){

        // You have to completely change this function to implement the
        // Dungeons of Doom environment.

        // Calculate the successor states
        int sscount = 0;
        DDSuccStates* succstates = new DDSuccStates();
        int loc = state->getLocation();
        bool sword = state->getSwordState();

        if(loc == LOC_SOUTHWEST && sword == NO_SWORD){
            DDState* ss = new DDState(LOC_WEST);
            ss->setSwordState(NO_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_NORTH);
            sscount++;
        }

        if (loc == LOC_WEST && sword == NO_SWORD){
            DDState* ss = new DDState(LOC_NORTHWEST);
            ss->setSwordState(NO_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_NORTH);
            sscount++;

            ss = new DDState(LOC_SOUTHWEST);
            ss->setSwordState(NO_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_SOUTH);
            sscount++;

            ss = new DDState(LOC_CENTER);
            ss->setSwordState(NO_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_EAST);
            sscount++;
        }

        else if (loc == LOC_WEST && sword == HAS_SWORD){
            DDState* ss = new DDState(LOC_NORTHWEST);
            ss->setSwordState(HAS_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_NORTH);
            sscount++;

            ss = new DDState(LOC_SOUTHWEST);
            ss->setSwordState(HAS_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_SOUTH);
            sscount++;

            ss = new DDState(LOC_CENTER);
            ss->setSwordState(HAS_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_EAST);
            sscount++;
        }

       if (loc == LOC_NORTHWEST && sword == NO_SWORD){
            DDState* ss = new DDState(LOC_WEST);
            ss->setSwordState(HAS_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_SOUTH);
            sscount++;
            }

       else if (loc == LOC_NORTHWEST && sword == HAS_SWORD){
            DDState* ss = new DDState(LOC_WEST);
            ss->setSwordState(HAS_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_SOUTH);
            sscount++;
            }

       if (loc == LOC_CENTER && sword == NO_SWORD){
            DDState* ss = new DDState(LOC_WEST);
            ss->setSwordState(NO_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_WEST);
            sscount++;

            ss = new DDState(LOC_NORTH);
            ss->setSwordState(NO_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_NORTH);
            sscount++;

            ss = new DDState(LOC_SOUTH);
            ss->setSwordState(NO_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_SOUTH);
            sscount++;

            ss = new DDState(LOC_EAST);
            ss->setSwordState(NO_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_EAST);
            sscount++;
            }

       else if (loc == LOC_CENTER && sword == HAS_SWORD){
            DDState* ss = new DDState(LOC_WEST);
            ss->setSwordState(HAS_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_WEST);
            sscount++;

            ss = new DDState(LOC_NORTH);
            ss->setSwordState(HAS_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_NORTH);
            sscount++;

            ss = new DDState(LOC_SOUTH);
            ss->setSwordState(HAS_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_SOUTH);
            sscount++;

            ss = new DDState(LOC_EAST);
            ss->setSwordState(HAS_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_EAST);
            sscount++;
            }


        if (loc == LOC_NORTH && sword == NO_SWORD){
            DDState* ss = new DDState(LOC_CENTER);
            ss->setSwordState(NO_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_SOUTH);
            sscount++;

            ss = new DDState(LOC_NORTHEAST);
            ss->setSwordState(NO_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_EAST);
            sscount++;
        }

       else if (loc == LOC_NORTH && sword == HAS_SWORD){
            DDState* ss = new DDState(LOC_CENTER);
            ss->setSwordState(HAS_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_SOUTH);
            sscount++;

            ss = new DDState(LOC_NORTHEAST);
            ss->setSwordState(HAS_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_EAST);
            sscount++;
        }

       if (loc == LOC_NORTHEAST && sword == NO_SWORD){
            DDState* ss = new DDState(LOC_NORTH);
            ss->setSwordState(NO_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_WEST);
            sscount++;

            ss = new DDState(LOC_EAST);
            ss->setSwordState(NO_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_SOUTH);
            sscount++;
        }

       else if (loc == LOC_NORTHEAST && sword == HAS_SWORD){
            DDState* ss = new DDState(LOC_NORTH);
            ss->setSwordState(HAS_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_WEST);
            sscount++;

            ss = new DDState(LOC_EAST);
            ss->setSwordState(HAS_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_SOUTH);
            sscount++;
        }

        if (loc == LOC_EAST && sword == NO_SWORD){
            DDState* ss = new DDState(LOC_CENTER);
            ss->setSwordState(NO_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_WEST);
            sscount++;

            ss = new DDState(LOC_NORTHEAST);
            ss->setSwordState(NO_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_NORTH);
            sscount++;
        }

       else if (loc == LOC_EAST && sword == HAS_SWORD){
            DDState* ss = new DDState(LOC_CENTER);
            ss->setSwordState(HAS_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_WEST);
            sscount++;

            ss = new DDState(LOC_NORTHEAST);
            ss->setSwordState(HAS_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_NORTH);
            sscount++;
        }

       if (loc == LOC_SOUTH && sword == NO_SWORD){
            DDState* ss = new DDState(LOC_CENTER);
            ss->setSwordState(NO_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_NORTH);
            sscount++;

            ss = new DDState(LOC_SOUTHEAST);
            ss->setSwordState(NO_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_EAST);
            sscount++;
        }

       else if (loc == LOC_SOUTH && sword == HAS_SWORD){
            DDState* ss = new DDState(LOC_CENTER);
            ss->setSwordState(HAS_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_NORTH);
            sscount++;

            ss = new DDState(LOC_SOUTHEAST);
            ss->setSwordState(HAS_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_EAST);
            sscount++;
        }

        if (loc == LOC_SOUTHEAST && sword == NO_SWORD){
            DDState* ss = new DDState(LOC_SOUTH);
            ss->setSwordState(NO_SWORD);
            succstates->setSuccState(sscount,ss);
            succstates->setAction(sscount,GO_WEST);
            sscount++;
        }

        else if (loc == LOC_SOUTHEAST && sword == HAS_SWORD){
            DDState* ss = new DDState(LOC_SOUTHEAST);
            ss->setSwordState(HAS_SWORD);
            succstates->setSuccState(sscount,ss);
            ss->setDragonState(DRAGON_DEAD);
            succstates->setAction(sscount, SLAY_DRAGON);
            sscount++;
        }

        succstates->setSuccStatesNumof(sscount);
        return succstates;

    };

    // This method evaluates the given state and returns 'true' if it is a
    // goal state.
    static bool goal_test(DDState* state){
        if(state->getDragonState() == DRAGON_DEAD){
            cout << "Goal test succeeded!" << endl;
            return true;
        } else
            return false;
    };

    static int manhattan_distance(int r1, int c1, int r2, int c2){
      int dist = abs(r1-r2);
      dist += abs(c1-c2);
      return dist;
    };

    // This function calculates an optimistic estimate of the cost of
    // reaching a goal from the given state.
    static int heuristic(DDState* state){
        int heur = 0;

        // You have to implement a sensible heuristic function here.
        return heur;
    };

    static void print_action(int action){
        switch(action){
            case GO_NORTH: cout << "Go north" << endl; break;
            case GO_EAST: cout << "Go east" << endl; break;
            case GO_SOUTH: cout << "Go south" << endl; break;
            case GO_WEST: cout << "Go west" << endl; break;
            case GET_SWORD: cout << "Get sword" << endl; break;
            default: cout << "Slay dragon" << endl;
        }
    };

};

class SearchNode {
public:
    SearchNode(SearchNode* snparent=NULL){
        this->snparent = snparent;
        action = NONE;
        depth=0;
    };

    SearchNode* getParent(){return snparent;};
    void setAction(int action){this->action = action;};
    int getAction(){return action;};
    void setDepth(int depth){this->depth = depth;};
    int getDepth(){return depth;};
    void setHeuristic(int heuristic){this->heuristic = heuristic;};
    int getHeuristic(){return heuristic;};
    void setState(DDState* state){this->state = state;};
    DDState* getState(){return state;};
    void setSuccessor(int succidx, SearchNode* succ){successors[succidx] = succ;};
    SearchNode* getSuccessor(int ssidx){return successors[ssidx];};

    void print(){
        cout << "Node: address - " << this << endl;
        cout << "  parent - " << snparent << endl;
        cout << "  action - ";
        DungeonsOfDoom::print_action(action);
        state->print();
        cout << "  depth - " << depth << endl;
        cout << "  heuristic - " << heuristic << endl;
    }

private:
    SearchNode* snparent;
    int action;
    DDState* state;
    int depth;
    int heuristic;
    SearchNode* successors[MAX_SUCC_STATES];
};

// This class implements a dynamically linked list of SearchNodes to be used
// for the fringe.  The list can be used both as a FIFO and a FILO queue.
class QueueNode {
public:
    QueueNode(SearchNode* sn){
        this->sn = sn;
        next = NULL;
        previous = NULL;
    }

    void setPrevious(QueueNode* qn){previous = qn;};
    QueueNode* getPrevious(){return previous;};
    void setNext(QueueNode* qn){next = qn;};
    QueueNode* getNext(){return next;};

    // This function adds a new SearchNode to the end of the list to make it
    // behave as a first-in-first-out (FIFO) queue.
    QueueNode* addLast(SearchNode* sn){
        if(next == NULL){
            QueueNode* qn = new QueueNode(sn);
            qn->setPrevious(this);
            next = qn;
        } else
            next = next->addLast(sn);
        return this;
    }

    // This function adds a new SearchNode to the front of the list to make
    // it behave as a firt-in-last-out (FILO) queue.
    QueueNode* addFirst(SearchNode* sn){
        QueueNode* qn = new QueueNode(sn);
        qn->next = this;
        previous = qn;
        return qn;
    }

    QueueNode* addSorted(SearchNode* sn){
        if(sn->getHeuristic() < this->sn->getHeuristic()){
            QueueNode* qn = new QueueNode(sn);
            qn->setNext(this);
            qn->setPrevious(previous);
            previous = qn;
            return qn;
        } else if(next == NULL){
            QueueNode* qn = new QueueNode(sn);
            qn->setPrevious(this);
            next = qn;
        } else {
            next = next->addSorted(sn);
        }
        return this;
    }

    // This function returns the first SearchNode in the list without
    // removing it.
    SearchNode* readFirst(){return sn;}

    // This function returns the list with the first SearchNode removed.
    QueueNode* popFirst(){
        QueueNode* local_next = next;
        free(this);
        return local_next;
    }

    int queue_size(){
        if(next == NULL) return 1;
        else return next->queue_size()+1;
    }

    void print(){
        sn->print();
        if(next != NULL)
            next->print();
    }

private:
    SearchNode* sn;
    QueueNode* next;
    QueueNode* previous;
};

// This program implements A* search for the eight puxxle problem
int main(int argc, char** argv) {

    // Some initialisation
    DDSuccStates* succstates = new DDSuccStates();

    // Set the initial state
    DDState* initstate = new DDState(INIT_LOCATION);
    initstate->setSwordState(INIT_SWORD_STATE);
    initstate->setDragonState(INIT_DRAGON_STATE);

    // Create the root node for the search tree
    SearchNode* searchtree = new SearchNode();
    SearchNode* sn = searchtree;
    sn->setState(initstate);
    sn->setHeuristic(DungeonsOfDoom::heuristic(initstate));
    cout << "Initial node:" << endl;
    sn->print();

    // Create the first node in the fringe
    cout << "Creating queue" << endl;
    QueueNode* fringe = new QueueNode(sn);
    sn = fringe->readFirst();

    // Expand fringe nodes until the goal is reached
    // or the fringe becomes empty
    while(!DungeonsOfDoom::goal_test(sn->getState()) && fringe != NULL){

        // Remove the first node of the fringe
        fringe = fringe->popFirst();

        // Get successor states
        succstates = DungeonsOfDoom::successor_states(sn->getState());
        int succstatesnumof = succstates->getSuccStatesNumof();
        // Create search nodes and add to search tree
        cout << "Succ states numof " << succstatesnumof << endl;
        for(int snidx=0;snidx<succstatesnumof;snidx++){
            SearchNode* nextsn = new SearchNode(sn);

            DDState* state = succstates->getSuccState(snidx);
            nextsn->setAction(succstates->getAction(snidx));
            nextsn->setState(state);
            nextsn->setDepth(sn->getDepth()+1);
            //cout << "Creating successor node for action ";
            //DungeonsOfDoom::print_action(succstates->getAction(snidx));
            //nextsn->print();
            sn->setSuccessor(snidx,nextsn);
            // Add the SearchNode to the fringe.
            if(fringe == NULL)
                fringe = new QueueNode(nextsn);
            else
                // Do breadth-first search
                fringe = fringe->addLast(nextsn); // Breadth first search
        }

        // Find the next SearchNode in the fringe
        cout << "Fringe size " << fringe->queue_size() << endl;
        //cout << "New fringe:" << endl;
        //fringe->print();
        sn = fringe->readFirst();
        cout << "Evaluating node" << endl;
        sn->print();

        //char a;
        //cin >> a;
    }

    // Print out the complete solution
    cout << "Solution:" << endl;
    SearchNode* tmpsn = sn;
    cout << "Goal ";
    do{
        if (tmpsn->getParent() == NULL)
            cout << "Start ";
        tmpsn->print();
        tmpsn = tmpsn->getParent();
    }while(tmpsn != NULL);

    return (EXIT_SUCCESS);
}