Tyler Howard - NPCs using A* navigation with GOAP strategy

#pragma once

#include <vector>
#include <set>
#include <fstream>

using namespace std;

typedef bool (*wprecond_fptr)(vector<int>&, vector<int>&);
typedef void (*weffect_fptr)(vector<int>&, vector<int>&);
typedef bool (*wendcond_fptr)(vector<int>&, vector<int>&);
typedef int (*wcost_fptr)(vector<int>&, vector<int>&, vector<int>&, vector<int>&);

enum ObjectiveType
{
    WORLD_COIN = 0,
    WORLD_WOLF,
    WORLD_OVEN,
    WORLD_TABLE,
    WORLD_RESTAURANT,
    WORLD_GENERAL_STORE,
    WORLD_FUR_TRADER,
    WORLD_NPC,
    WORLD_WHEAT,
    WORLD_MILL,
    WORLD_DOCTOR,
    WORLD_NONE
};

struct WorldAction
{
    string name;
    wprecond_fptr precondition;
    weffect_fptr effect;
    ObjectiveType objective;
};

enum NPCHungerProblemEnums
{
    NPC_STATE_HUNGRY = 0,
    NPC_STATE_COINS,
    NPC_STATE_FLOUR,
    NPC_STATE_BREAD,
    NPC_STATE_PIZZA,
    NPC_STATE_SKINS,
    NPC_STATE_WHEAT,
    NPC_STATE_SICK,
    NPC_STATE_MEDICINE
};

enum WorldHungerProblemEnums
{
    WORLD_STATE_COINS = 0,
    WORLD_STATE_WOLVES,
    WORLD_STATE_WHEAT
};

static bool w_canHarvestWheat(vector<int>& NPCState, vector<int>& worldState)
{
    return worldState[WORLD_STATE_WHEAT] > 0;
}

static void w_harvestWheat(vector<int>& NPCState, vector<int>& worldState)
{
    --worldState[WORLD_STATE_WHEAT];
    ++NPCState[NPC_STATE_WHEAT];
}

static bool w_canProcessWheat(vector<int>& NPCState, vector<int>& worldState)
{
    return NPCState[NPC_STATE_WHEAT] > 0;
}

static void w_processWheat(vector<int>& NPCState, vector<int>& worldState)
{
    --NPCState[NPC_STATE_WHEAT];
    ++NPCState[NPC_STATE_FLOUR];
}

static bool w_canPickupMoney(vector<int>& NPCState, vector<int>& worldState)
{
    return worldState[WORLD_STATE_COINS] > 0;
}

static void w_pickupMoney(vector<int>& NPCState, vector<int>& worldState)
{
    --worldState[WORLD_STATE_COINS];
    ++NPCState[NPC_STATE_COINS];
}

static bool w_canEatBread(vector<int>& NPCState, vector<int>& worldState)
{
    return NPCState[NPC_STATE_HUNGRY] > 0 && NPCState[NPC_STATE_BREAD] > 0;
}

static void w_eatBread(vector<int>& NPCState, vector<int>& worldState)
{
    --NPCState[NPC_STATE_BREAD];
    --NPCState[NPC_STATE_HUNGRY];
}

static bool w_canEatPizza(vector<int>& NPCState, vector<int>& worldState)
{
    return NPCState[NPC_STATE_HUNGRY] > 0 && NPCState[NPC_STATE_PIZZA] > 0;
}

static void w_eatPizza(vector<int>& NPCState, vector<int>& worldState)
{
    --NPCState[NPC_STATE_PIZZA];
    --NPCState[NPC_STATE_HUNGRY];
}

static bool w_canBuyFlour(vector<int>& NPCState, vector<int>& worldState)
{
    return NPCState[NPC_STATE_HUNGRY] > 0 && NPCState[NPC_STATE_COINS] >= 1;
}

static void w_buyFlour(vector<int>& NPCState, vector<int>& worldState)
{
    --NPCState[NPC_STATE_COINS];
    ++NPCState[NPC_STATE_FLOUR];
}

static bool w_canBuyPizza(vector<int>& NPCState, vector<int>& worldState)
{
    return NPCState[NPC_STATE_HUNGRY] > 0 && NPCState[NPC_STATE_COINS] >= 2;
}

static void w_buyPizza(vector<int>& NPCState, vector<int>& worldState)
{
    NPCState[NPC_STATE_COINS] -= 2;
    ++NPCState[NPC_STATE_PIZZA];
}

static bool w_canBakeBread(vector<int>& NPCState, vector<int>& worldState)
{
    return NPCState[NPC_STATE_FLOUR] >= 1;
}

static void w_bakeBread(vector<int>& NPCState, vector<int>& worldState)
{
    --NPCState[NPC_STATE_FLOUR];
    ++NPCState[NPC_STATE_BREAD];
}

static bool w_checkForHungerProblemEndCondition(vector<int>& NPCState, vector<int>& worldState)
{
    return NPCState[NPC_STATE_HUNGRY] == 0 && NPCState[NPC_STATE_SICK] == 0;
}

static int w_calculateHungerProblemH(vector<int>& currentNPCState, vector<int>& currentWorldState, vector<int>& desiredNPCState, vector<int>& desiredWorldState)
{
    //all other variables don't matter - only thing that really matters for this problem is if hunger is 0
    return (currentNPCState[NPC_STATE_HUNGRY] - desiredNPCState[NPC_STATE_HUNGRY]) + (currentNPCState[NPC_STATE_SICK] - desiredNPCState[NPC_STATE_SICK]);
}

static bool w_canKillWolf(vector<int>& NPCState, vector<int>& worldState)
{
    return worldState[WORLD_STATE_WOLVES] > 0;
}

static void w_killWolf(vector<int>& NPCState, vector<int>& worldState)
{
    --worldState[WORLD_STATE_WOLVES];
    ++NPCState[NPC_STATE_SKINS];
}

static bool w_canSellSkin(vector<int>& NPCState, vector<int>& worldState)
{
    return NPCState[NPC_STATE_SKINS] > 0;
}

static void w_sellSkin(vector<int>& NPCState, vector<int>& worldState)
{
    --NPCState[NPC_STATE_SKINS];
    ++NPCState[NPC_STATE_COINS];
}

static bool w_canSellWheat(vector<int>& NPCState, vector<int>& worldState)
{
    return NPCState[NPC_STATE_WHEAT] > 0;
}

static void w_sellWheat(vector<int>& NPCState, vector<int>& worldState)
{
    --NPCState[NPC_STATE_WHEAT];
    ++NPCState[NPC_STATE_COINS];
}

static bool w_canBuyMedicine(vector<int>& NPCState, vector<int>& worldState)
{
    return NPCState[NPC_STATE_COINS] > 0 && NPCState[NPC_STATE_SICK] > 0;
}

static void w_buyMedicine(vector<int>& NPCState, vector<int>& worldState)
{
    --NPCState[NPC_STATE_COINS];
    ++NPCState[NPC_STATE_MEDICINE];
}

static bool w_canDrinkMedicine(vector<int>& NPCState, vector<int>& worldState)
{
    return NPCState[NPC_STATE_MEDICINE] > 0 && NPCState[NPC_STATE_SICK] > 0;
}

static void w_drinkMedicine(vector<int>& NPCState, vector<int>& worldState)
{
    --NPCState[NPC_STATE_MEDICINE];
    --NPCState[NPC_STATE_SICK];
}

static bool w_checkForSicknessProblemEndCondition(vector<int>& NPCState, vector<int>& worldState)
{
    return NPCState[NPC_STATE_SICK] == 0;
}

static int w_calculateSicknessProblemH(vector<int>& currentNPCState, vector<int>& currentWorldState, vector<int>& desiredNPCState, vector<int>& desiredWorldState)
{
    return currentNPCState[NPC_STATE_SICK] - desiredNPCState[NPC_STATE_SICK];
}

static bool w_checkForGreedProblemEndCondition(vector<int>& NPCState, vector<int>& worldState)
{
    return false;       //Greed is insatiable!
}

static int w_calculateGreedProblemH(vector<int>& currentNPCState, vector<int>& currentWorldState, vector<int>& desiredNPCState, vector<int>& desiredWorldState)
{
    return desiredNPCState[NPC_STATE_COINS] - currentNPCState[NPC_STATE_COINS];
}

enum WorldPathType
{
    WORLD_PATH_EMPTY = 0,
    WORLD_PATH_START,               //functionally equivalent to empty
    WORLD_PATH_END,             //functionally equivalent to empty
    WORLD_PATH_WATER,               //double cost (H)
    WORLD_PATH_BLOCKED,         //closed from the get-go

    //visited nodes turn into these types:
    WORLD_PATH_OPEN,
    WORLD_PATH_CLOSED,
    WORLD_PATH_WORLD_PATH
};

struct WorldAStarNode
{

public:

    WorldPathType type;
    WorldAStarNode* parent;

    int G;
    int H;

    int row;
    int col;

    bool open;
    bool travelled;

    ObjectiveType inhabitor;
    int npc;

    WorldAStarNode()
    {
        type = WORLD_PATH_EMPTY;
        parent = 0;
    }

    WorldAStarNode(WorldPathType pt, ObjectiveType inh, WorldAStarNode* prnt = 0)
    {
        type = pt;
        inhabitor = inh;
        parent = prnt;
        travelled = false;
        npc = -1;
    }

    WorldAStarNode(char c, int r, int co)
    {
        switch(c)
        {
        case 'D':
            type = WORLD_PATH_EMPTY;
            inhabitor = WORLD_DOCTOR;
            break;

        case 'N':
            type = WORLD_PATH_EMPTY;
            inhabitor = WORLD_NPC;
            break;

        default:
        case '.':
            type = WORLD_PATH_EMPTY;
            inhabitor = WORLD_NONE;
            break;

        case '/':
            type = WORLD_PATH_WATER;
            inhabitor = WORLD_NONE;
            break;

        case '#':
            type = WORLD_PATH_BLOCKED;
            inhabitor = WORLD_NONE;
            break;

        case 'O':
            //oven - bake bread
            type = WORLD_PATH_EMPTY;
            inhabitor = WORLD_OVEN;
            break;

        case 'W':
            //wolf - kill wolf
            type = WORLD_PATH_EMPTY;
            inhabitor = WORLD_WOLF;
            break;

        case 'C':
            //coin - pickup coin
            type = WORLD_PATH_EMPTY;
            inhabitor = WORLD_COIN;
            break;

        case 'F':
            //fur trader - sell skin
            type = WORLD_PATH_EMPTY;
            inhabitor = WORLD_FUR_TRADER;
            break;

        case 'G':
            //general store - buy flour
            type = WORLD_PATH_EMPTY;
            inhabitor = WORLD_GENERAL_STORE;
            break;

        case 'P':
            //pizza hut - buy pizza
            type = WORLD_PATH_EMPTY;
            inhabitor = WORLD_RESTAURANT;
            break;

        case 'T':
            //table - eat bread, eat pizza
            type = WORLD_PATH_EMPTY;
            inhabitor = WORLD_TABLE;
            break;

        case '*':
            type = WORLD_PATH_EMPTY;
            inhabitor = WORLD_WHEAT;
            break;

        case 'M':
            type = WORLD_PATH_EMPTY;
            inhabitor = WORLD_MILL;
            break;
        }

        parent = 0;
        row = r;
        col = co;
        H = G = -1;
        open = false;
    }

    bool isEmpty()
    {
        return (type == WORLD_PATH_EMPTY || type == WORLD_PATH_WATER) && (inhabitor == WORLD_NONE);
        travelled = false;
    }

    int getCost() const
    {
        return H + G;
    }
};

struct WorldAStarNodeComp
{
    bool operator() (const WorldAStarNode* lhs, const WorldAStarNode* rhs) const
    {
        if(lhs->getCost() == rhs->getCost())
        {
            if(lhs->H == rhs->H)
            {
                return lhs < rhs;
            }
            else
            {
                return lhs->H < rhs->H;
            }
        }
        else
        {
            return lhs->getCost() < rhs->getCost();
        }
    }
};

struct NPC
{
    int row;
    int col;

    WorldAStarNode* objectiveLoc;
    WorldAction* action;
    string nameOfCurrentAction;

    vector<int> state;
    vector<string> currentplan;
    vector<string> takenactions;

    bool dead;
};

class WorldGOAPNode
{

public:

    WorldGOAPNode(vector<int>& vars)
    {
        GOAPvars.resize(vars.size());
        for(unsigned int i = 0; i < vars.size(); ++i)
            GOAPvars[i] = vars[i];
        parent = 0;
        lastAction = -1;
    }

    WorldGOAPNode(WorldGOAPNode* p, int a)
    {
        parent = p;

        const vector<int>& v = p->GOAPvars;
        GOAPvars.resize(v.size());

        for(unsigned int i = 0; i < v.size(); ++i)
            GOAPvars[i] = v[i];

        lastAction = a;
    }

    ~WorldGOAPNode() {}

    const int getCost() const { return G + H; }

    int G;
    int H;

    WorldGOAPNode* parent;

    vector<int> GOAPvars;
    int lastAction;

};

struct WorldGOAPNodeComp
{
    bool operator() (const WorldGOAPNode* lhs, const WorldGOAPNode* rhs) const
    {
        if(lhs->getCost() == rhs->getCost())
        {
            if(lhs->H == rhs->H)
            {
                return lhs < rhs;
            }
            else
            {
                return lhs->H < rhs->H;
            }
        }
        else
        {
            return lhs->getCost() < rhs->getCost();
        }
    }
};

struct WorldGOAPNodeStateComp
{
    bool operator() (const WorldGOAPNode* lhs, const WorldGOAPNode* rhs) const
    {
        int cmpidx = -1;
        for(unsigned int i = 0; i < lhs->GOAPvars.size(); ++i)
        {
            if(lhs->GOAPvars[i] != rhs->GOAPvars[i])
            {
                cmpidx = i;
                break;
            }
        }

        if(cmpidx == -1) ++cmpidx;

        return lhs->GOAPvars[cmpidx] < rhs->GOAPvars[cmpidx];
    }
};

class World
{

public:

    World(const char* szFilePath)
    {
        //populate world map
        ifstream mapFile(szFilePath, ios::in | ios::binary);
        unsigned int row = 0, col = 0;
        //if character == 0D or 0A, charpointer += 2, reset column, and move to next row
        vector<char> d;

        mapFile.seekg(0, ios::end);
        d.resize(static_cast<int>(mapFile.tellg()));
        mapFile.seekg(0, ios::beg);

        mapFile.read(d.data(), d.size());

        mapFile.close();

        worldMap.resize(1);

        bool sickToggle = false;

        for(unsigned int i = 0; i < d.size(); ++i)
        {
            if(d[i] != 0x0D && d[i] != 0x0A)
            {
                WorldAStarNode p(d[i], row, col);
                worldMap[row].push_back(p);

                if(d[i] == 'N')
                {
                    NPC n;
                    n.row = row;
                    n.col = col;
                    n.dead = false;

                    n.state.resize(9);

                    n.state[NPC_STATE_HUNGRY] = 1;
                    n.state[NPC_STATE_SKINS] = 0;
                    n.state[NPC_STATE_PIZZA] = 0;
                    n.state[NPC_STATE_FLOUR] = 0;
                    n.state[NPC_STATE_BREAD] = 0;
                    n.state[NPC_STATE_COINS] = 0;
                    n.state[NPC_STATE_WHEAT] = 0;
                    n.state[NPC_STATE_SICK] = (sickToggle ? 1 : 0 );
                    n.state[NPC_STATE_MEDICINE] = 0;

                    sickToggle = !sickToggle;

                    npcs.push_back(n);
                }

                ++col;
            }
            else
            {
                worldMap.resize(++row + 1);
                col = 0;
                ++i;
            }
        }

        state_vars.resize(3);   //wolves, coins
        state_vars[WORLD_STATE_COINS] = countObjectives(WORLD_COIN);
        state_vars[WORLD_STATE_WOLVES] = countObjectives(WORLD_WOLF);
        state_vars[WORLD_STATE_WHEAT] = countObjectives(WORLD_WHEAT);

        registerAction("pickup coin", w_canPickupMoney, w_pickupMoney, WORLD_COIN);
        registerAction("buy flour", w_canBuyFlour, w_buyFlour, WORLD_GENERAL_STORE);
        registerAction("bake bread", w_canBakeBread, w_bakeBread, WORLD_OVEN);
        registerAction("buy pizza", w_canBuyPizza, w_buyPizza, WORLD_RESTAURANT);
        registerAction("eat bread", w_canEatBread, w_eatBread, WORLD_TABLE);
        registerAction("eat pizza", w_canEatPizza, w_eatPizza, WORLD_TABLE);
        registerAction("sell skin", w_canSellSkin, w_sellSkin, WORLD_FUR_TRADER);
        registerAction("kill wolf", w_canKillWolf, w_killWolf, WORLD_WOLF);
        registerAction("harvest wheat", w_canHarvestWheat, w_harvestWheat, WORLD_WHEAT);
        registerAction("process wheat", w_canProcessWheat, w_processWheat, WORLD_MILL);

        registerAction("sell wheat", w_canSellWheat, w_sellWheat, WORLD_GENERAL_STORE);

        registerAction("drink medicine", w_canDrinkMedicine, w_drinkMedicine, WORLD_TABLE);

        registerAction("buy medicine", w_canBuyMedicine, w_buyMedicine, WORLD_DOCTOR);

        registerEndCondition(w_checkForHungerProblemEndCondition);
        registerHCost(w_calculateHungerProblemH);

        currframe = 0;

        desiredNPCState.resize(9);
        desiredNPCState[NPC_STATE_HUNGRY] = 0;
        desiredNPCState[NPC_STATE_MEDICINE] = 0;
        desiredNPCState[NPC_STATE_COINS] = 10000;
        //don't care about rest

        desiredWorldState.resize(3);
        //don't care about rest

        for(unsigned int i = 0; i < npcs.size(); ++i)
            setNPCObjective(npcs[i]);

        resetPathDisplay();
    }

    void randomlyMoveNPC(NPC& n)
    {
        if(n.dead) return;

        WorldAStarNode* npcLoc = getNode(n.row, n.col);
        WorldAStarNode* npcChild;

        int i = rand() % 5;
        int x = 0;
        int y = 0;
        switch(i)
        {
        case 0:     //left
            x = -1;
            break;

        case 1:     //right
            x = 1;
            break;

        case 2:     //up
            y = -1;
            break;

        case 3:     //down
            y = 1;
            break;
        case 4: break;
        }

        npcChild = getNode( n.row + x, n.col + y );

        if(npcChild && npcChild->isEmpty())
        {
            npcLoc->inhabitor = WORLD_NONE;
            npcLoc->travelled = true;
            npcChild->inhabitor = WORLD_NPC;
            n.row = npcChild->row;
            n.col = npcChild->col;
        }
    }

    void setNPCObjective(NPC& n)
    {
        //run GOAP with simulated variables here
        vector<int> simulatedNPCState = n.state;
        vector<int> simulatedWorldState = state_vars;

        bool foundEnd = false;

        WorldGOAPNode* curr = new WorldGOAPNode(simulatedNPCState);
        curr->G = 0;
        curr->H = cost(curr->GOAPvars, simulatedWorldState, desiredNPCState, desiredWorldState);

        set<WorldGOAPNode*, WorldGOAPNodeComp> oNodes;
        set<WorldGOAPNode*, WorldGOAPNodeStateComp> cNodes;
        vector<WorldGOAPNode*> allNodes;

        oNodes.insert(curr);
        allNodes.push_back(curr);

        n.currentplan.clear();

        while(!foundEnd)
        {
            curr = *oNodes.begin();
            oNodes.erase(curr);
            cNodes.insert(curr);

            if(endcondition(curr->GOAPvars, simulatedWorldState))
            {
                foundEnd = true;

                //set NPC objective and action here

                //get one-step-below-root node
                WorldGOAPNode* firstActionNode = curr;
                while(firstActionNode->parent->parent != 0)
                {
                    firstActionNode = firstActionNode->parent;
                }

                n.action = &action_defs[firstActionNode->lastAction];
                n.objectiveLoc = findNearestNode(getNode(n.row, n.col), n.action->objective);

                //generate log
                WorldGOAPNode* c = curr;
                while(c->lastAction != -1)
                {
                    n.currentplan.insert(n.currentplan.begin(), action_defs[c->lastAction].name);
                    c = c->parent;
                }
                n.currentplan.push_back("success");

                n.nameOfCurrentAction = n.action->name;
            }
            else
            {
                for(unsigned int i = 0; i < action_defs.size(); ++i)
                {
                    if(action_defs[i].precondition(curr->GOAPvars, simulatedWorldState))
                    {
                        WorldGOAPNode* newNode = new WorldGOAPNode(curr, i);

                        action_defs[i].effect(newNode->GOAPvars, simulatedWorldState);
                        newNode->G = curr->G + 1;
                        newNode->H = cost(newNode->GOAPvars, simulatedWorldState, desiredNPCState, desiredWorldState);

                        if(cNodes.find(newNode) == cNodes.end())
                        {
                            oNodes.insert(newNode);
                            allNodes.push_back(newNode);
                        }
                        else
                            delete newNode;
                    }
                }

                if(oNodes.empty())
                {
                    foundEnd = true;

                    WorldGOAPNode* c = curr;
                    while(c->lastAction != -1)
                    {
                        n.currentplan.insert(n.currentplan.begin(), action_defs[c->lastAction].name);
                        c = c->parent;
                    }

                    n.currentplan.push_back("impossible");
                    n.dead = true;
                }
            }
        }

        for(unsigned int i = 0; i < allNodes.size(); ++i)
        {
            delete allNodes[i];
        }
    }

    int countObjectives(ObjectiveType t)
    {
        int count = 0;
        for(unsigned int i = 0; i < worldMap.size(); ++i)
        {
            for(unsigned int j = 0; j < worldMap[0].size(); ++j)
            {
                if(worldMap[i][j].inhabitor == t)
                    ++count;
            }
        }
        return count;
    }

    int getDistance(WorldAStarNode* curr, WorldAStarNode* end)
    {
        return abs(curr->col - end->col) + abs(curr->row - end->row);
    }

    WorldAStarNode* findNearestNode(WorldAStarNode* start, ObjectiveType t)
    {
        vector<WorldAStarNode*> nodes;

        for(unsigned int i = 0; i < worldMap.size(); ++i)
        {
            for(unsigned int j = 0; j < worldMap[i].size(); ++j)
            {
                if(worldMap[i][j].inhabitor == t)
                    nodes.push_back(&(worldMap[i][j]));
            }
        }

        if(nodes.size() == 0)
            return 0;
        else
        {
            WorldAStarNode* nodeToReturn = nodes[0];
            int smallestDistance = getDistance(start, nodes[0]);
            //int smallestDistance = getAStarDistance(start, nodes[0]);

            for(unsigned int i = 1; i < nodes.size(); ++i)
            {
                if(getDistance(start, nodes[i]) < smallestDistance)                 //TODO: change from manhattan distance to reachable via A*
                //int dist = getAStarDistance(start, nodes[i]);
                //if(dist != -1 && dist < smallestDistance)
                {
                    nodeToReturn = nodes[i];
                    smallestDistance = getDistance(start, nodes[i]);
                    //smallestDistance = dist;
                }
            }

            if(smallestDistance != -1)
                return nodeToReturn;
            else
                return 0;
        }
    }

    //never actually called, I believe
    int getAStarDistance(WorldAStarNode* start, WorldAStarNode* end)
    {
        bool foundEnd = false;
        set<WorldAStarNode*, WorldAStarNodeComp> oNodes;
        set<WorldAStarNode*, WorldAStarNodeComp> cNodes;

        oNodes.insert(start);

        while(!foundEnd)
        {
            if(oNodes.size() > 0)
            {
                WorldAStarNode* curr = *oNodes.begin();
                oNodes.erase(oNodes.begin());

                curr->open = false;

                if(curr == end)
                {
                    foundEnd = true;
                    int count = 0;

                    while(curr != 0)
                    {
                        ++count;
                        curr = curr->parent;
                    }

                    return count;
                }
                else
                    cNodes.insert(curr);

                for(int x = -1; x <= 1; ++x)
                {
                    for(int y = -1; y <= 1; ++y)
                    {
                        if((abs(x) == 1 && y == 0 || abs(y) == 1 && x == 0) && !foundEnd)
                        {
                            WorldAStarNode* surr = getNode(curr->row + x, curr->col + y);
                            if(surr != 0)
                            {
                                //reparenting
                                if( (surr->isEmpty() || surr == end) && cNodes.find(surr) == cNodes.end() )
                                {
                                    if(surr->open)
                                    {
                                        //if surr's G would lower if surr were connected to parent than if currently
                                        int newG = curr->G + (surr->type != WORLD_PATH_WATER ? 1 : 5);
                                        if(newG < surr->G)              //no improvement, choose old parent
                                        {
                                            oNodes.erase(surr);
                                            surr->parent = curr;
                                            surr->G = newG;
                                            oNodes.insert(surr);
                                        }
                                    }

                                    else
                                    {
                                        surr->H = getDistance(surr, end);
                                        surr->G = (surr->type != WORLD_PATH_WATER ? 1 : 5) + curr->G;
                                        surr->open = true;
                                        surr->parent = curr;
                                        oNodes.insert(surr);
                                    }
                                }
                            }
                        }
                    }
                }
            }
            else
            {
                foundEnd = true;
                return -1;
            }
        }
    }

    WorldAStarNode* getNode(unsigned int x, unsigned int y)
    {
        if( x < 0 || x >= worldMap.size() || y < 0 || y >= worldMap[x].size() ) return 0;

        return &(worldMap[x][y]);
    }

    void resetPathDisplay()
    {
        for(unsigned int i = 0; i < worldMap.size(); ++i)
        {
            for(unsigned int j = 0; j < worldMap[0].size(); ++j)
            {
                worldMap[i][j].npc = - 1;
            }
        }
    }

    void update()
    {
        resetPathDisplay();

        for(unsigned int i = 0; i < npcs.size(); ++i)
        {
            //hunger check over time
            if(currframe % 10 == 0 && npcs[i].state[NPC_STATE_HUNGRY] == 0)
                npcs[i].state[NPC_STATE_HUNGRY] = (rand() % 2 == 0) ? 0 : 1;

            //sickness check over time
            if(currframe % 20 == 0 && npcs[i].state[NPC_STATE_SICK] == 0)
                npcs[i].state[NPC_STATE_SICK] = (rand() % 2 == 0) ? 0 : 1;

            //if NPC hasn't satisfied their needs yet
            if(!endcondition(npcs[i].state, state_vars) && !npcs[i].dead)
            {

                WorldAStarNode* npcLoc = getNode(npcs[i].row, npcs[i].col);
                if(npcs[i].objectiveLoc == 0 || npcs[i].action == 0)
                {
                    setNPCObjective(npcs[i]);
                }
                else
                {
                    if(getDistance( npcLoc, npcs[i].objectiveLoc ) <= 1)
                    {
                        if(isActionValid(npcs[i]) && npcs[i].action->precondition(npcs[i].state, state_vars))
                        {
                            //change NPC state
                            //already ran precondition when determining objective
                            npcs[i].action->effect(npcs[i].state, state_vars);
                            npcs[i].takenactions.push_back(npcs[i].action->name);

                            //change world state
                            if(isObjectiveConsumable(npcs[i].objectiveLoc->inhabitor))
                                npcs[i].objectiveLoc->inhabitor = WORLD_NONE;

                            npcs[i].action = 0;
                        }
                        else
                        {
                            npcs[i].takenactions.push_back("plan invalid");
                        }

                        //      reformulate plan
                        //          observe world state
                        //          get new action by simulating NPC and world states, not by actually altering existing states
                        if(!endcondition(npcs[i].state, state_vars))
                            setNPCObjective(npcs[i]);
                    }


                    moveNPCTowardObjective(npcLoc, npcs[i], i);
                    resetPaths();
                }
            }
            else
            {
                randomlyMoveNPC(npcs[i]);
            }
        }

        for(unsigned int i = 0; i < npcs.size(); ++i)
        {
            if(!npcs[i].dead)
            {
                if(getNumSurroundSickNPCs(getNode(npcs[i].row, npcs[i].col)) > 0)
                {
                    npcs[i].state[NPC_STATE_SICK] = 1;
                }
            }
        }

        if(currframe % 5 == 0)
        {

            //wheat repopulates and dies according to Conway's Game of Life

            //first pass: mark tiles with their new result (i.e. die, live on, reproduce)
            //vector<WorldAStarNode*> wheatLives;       //encountered wheat that lives on to next generation - needs nothing done to it
            vector<WorldAStarNode*> wheatDies;      //encountered wheat that dies
            vector<WorldAStarNode*> wheatGrows;     //empty cells that touch exactly three cells

            for(unsigned int i = 0; i < worldMap.size(); ++i)
            {
                for(unsigned int j = 0; j < worldMap[0].size(); ++j)
                {
                    if(worldMap[i][j].inhabitor == WORLD_WHEAT)
                    {
                        //chance for wheat to die
                        int numwheat = getNumSurroundWheat(worldMap[i][j]);
                        if(numwheat < 2 || numwheat > 3)
                            wheatDies.push_back(&worldMap[i][j]);
                    }

                    if(worldMap[i][j].inhabitor == WORLD_NONE)
                    {
                        //chance to grow wheat in this cell
                        if(getNumSurroundWheat(worldMap[i][j]) == 3)
                            wheatGrows.push_back(&worldMap[i][j]);
                    }
                }
            }

            //second pass:
            for(unsigned int i = 0; i < wheatDies.size(); ++i)
            {
                wheatDies[i]->inhabitor = WORLD_NONE;
            }

            for(unsigned int i = 0; i < wheatGrows.size(); ++i)
            {
                wheatGrows[i]->inhabitor = WORLD_WHEAT;
            }

            state_vars[WORLD_STATE_WHEAT] = countObjectives(WORLD_WHEAT);
        }

        ++currframe;

        //"kill" dead NPCs
        for(unsigned int i = 0; i < worldMap.size(); ++i)
        {
            for(unsigned int j = 0; j < worldMap[0].size(); ++j)
            {
                NPC* n = getNPCAtLoc(i, j);
                if(n != 0 && n->dead == true)
                {
                    getNode(i, j)->inhabitor = WORLD_NONE;
                }
            }
        }

    }

    int getNumSurroundWheat(WorldAStarNode& node)
    {
        int numWheat = 0;
        for(int x = -1; x <= 1; ++x)
        {
            for(int y = -1; y <= 1; ++y)
            {
                if( !(x == 0 && y == 0) )
                {
                    WorldAStarNode* testNode = getNode(node.row + x, node.col + y);
                    if(testNode != 0 && testNode->inhabitor == WORLD_WHEAT)
                        ++numWheat;
                }
            }
        }
        return numWheat;
    }

    NPC* getNPCAtLoc(int x, int y)
    {
        WorldAStarNode* n = getNode(x, y);
        if(n == 0 || n->inhabitor != WORLD_NPC) return 0;

        for(unsigned int i = 0; i < worldMap.size(); ++i)
        {
            for(unsigned int j = 0; j < worldMap[0].size(); ++j)
            {
                for(unsigned int k = 0; k < npcs.size(); ++k)
                {
                    if(npcs[k].row == x && npcs[k].col == y)
                        return &npcs[i];
                }
            }
        }

        return 0;
    }

    int getNumSurroundSickNPCs(WorldAStarNode* node)
    {
        int numSick = 0;
        for(int x = -1; x <= 1; ++x)
        {
            for(int y = -1; y <= 1; ++y)
            {
                if( !(x == 0 && y == 0) )
                {
                    NPC* n = getNPCAtLoc(node->row + x, node->col + y);
                    if(n != 0 && !n->dead && n->state[NPC_STATE_SICK] > 0)
                        ++numSick;
                }
            }
        }
        return numSick;
    }

    bool isObjectiveConsumable(ObjectiveType t)
    {
        return t == WORLD_COIN || t == WORLD_WOLF || t == WORLD_WHEAT;
    }

    bool isActionValid(NPC& n)
    {
        //returns true if objective is still present at that location
        return n.objectiveLoc->inhabitor == n.action->objective;
    }

    void moveNPCTowardObjective(WorldAStarNode* npcLoc, NPC& n, int nindex)
    {
        bool foundEnd = false;
        set<WorldAStarNode*, WorldAStarNodeComp> oNodes;
        set<WorldAStarNode*, WorldAStarNodeComp> cNodes;

        oNodes.insert(npcLoc);

        WorldAStarNode* npcChild = 0;       //next location to step to
        WorldAStarNode* objective = n.objectiveLoc;

        while(!foundEnd)
        {
            if(oNodes.size() > 0)
            {
                WorldAStarNode* curr = *oNodes.begin();
                oNodes.erase(oNodes.begin());

                curr->open = false;

                if(curr == objective)
                {
                    foundEnd = true;

                    if(getDistance(npcLoc, objective) > 1)
                    {
                        while(curr->parent->parent != 0)
                        {
                            curr->npc = nindex;
                            curr = curr->parent;
                        }

                        npcChild = curr;
                    }
                    else
                        npcChild = npcLoc;
                }
                else
                    cNodes.insert(curr);

                for(int x = -1; x <= 1; ++x)
                {
                    for(int y = -1; y <= 1; ++y)
                    {
                        if((abs(x) == 1 && y == 0 || abs(y) == 1 && x == 0) && !foundEnd)
                        {
                            WorldAStarNode* surr = getNode(curr->row + x, curr->col + y);
                            if(surr != 0)
                            {
                                //reparenting
                                if( (surr->isEmpty() || surr == objective) && cNodes.find(surr) == cNodes.end() )
                                {
                                    if(surr->open)
                                    {
                                        //if surr's G would lower if surr were connected to parent than if currently
                                        int newG = curr->G + (surr->type != WORLD_PATH_WATER ? 1 : 5);
                                        if(newG < surr->G)              //no improvement, choose old parent
                                        {
                                            oNodes.erase(surr);
                                            surr->parent = curr;
                                            surr->G = newG;
                                            oNodes.insert(surr);
                                        }
                                    }

                                    else
                                    {
                                        surr->H = getDistance(surr, objective);
                                        surr->G = (surr->type != WORLD_PATH_WATER ? 1 : 5) + curr->G;
                                        surr->open = true;
                                        surr->parent = curr;
                                        oNodes.insert(surr);
                                    }
                                }
                            }
                        }
                    }
                }
            }
            else
            {
                foundEnd = true;
                randomlyMoveNPC(n);
                return;
            }
        }

        if(getDistance(npcLoc, n.objectiveLoc) > 1 && npcChild->isEmpty()) //isEmpty for situations when another NPC is blocking path
        {
            npcLoc->inhabitor = WORLD_NONE;
            npcLoc->travelled = true;
            npcChild->inhabitor = WORLD_NPC;
            n.row = npcChild->row;
            n.col = npcChild->col;
        }

    }

    void resetPaths()
    {
        for(unsigned int i = 0; i < worldMap.size(); ++i)
        {
            for(unsigned int j = 0; j < worldMap[0].size(); ++j)
            {
                worldMap[i][j].parent = 0;
                worldMap[i][j].open = false;
                worldMap[i][j].G = worldMap[i][j].H = -1;
            }
        }
    }

    void registerAction(const string& n, wprecond_fptr p, weffect_fptr e, ObjectiveType t)
    {
        WorldAction a;
        a.name = n;
        a.precondition = p;
        a.effect = e;
        a.objective = t;
        action_defs.push_back(a);
    }

    void registerEndCondition(wendcond_fptr e)
    {
        endcondition = e;
    }

    void registerHCost(wcost_fptr c)
    {
        cost = c;
    }

    vector<vector<WorldAStarNode>> worldMap;
    vector<NPC> npcs;
    vector<WorldAction> action_defs;

    vector<int> state_vars;
    vector<int> desiredNPCState;
    vector<int> desiredWorldState;

    wendcond_fptr endcondition;
    wcost_fptr cost;

    unsigned int currframe;
};