#ifndef H_GOAP_H_
#define H_GOAP_H_
#undef NDEBUG
#include <string>
#include <memory>
#include <set>
#include <flecs.h>
#include <Ogre.h>
namespace goap
{
template <typename State> struct BaseAction {
	std::string m_name;
    State prereq;
    State effects;
    int m_cost;

public:
	enum { OK = 0, BUSY, ABORT };

public:
    BaseAction(const std::string &name, const State &prereq, const State &effects, int cost)
        : m_name(name)
        , prereq(prereq)
        , effects(effects)
        , m_cost(cost)
	{
	}
    virtual const std::string &get_name() const
	{
		return m_name;
	}
    void plan_effects(State &state)
	{
		//		std::cout << m_name << " pre: " << &state << " " << state.flags
		//			  << std::endl;
		_plan_effects(state);
		//		std::cout << m_name << " post: " << &state << " " << state.flags
		//			  << std::endl;
	}
    virtual bool can_run(const State &state, bool debug = false)
    {
        return state.distance_to(prereq) == 0;
    }
    virtual void _plan_effects(State &state)
    {
        state.apply(effects);
    }
    // constant cost
    virtual int get_cost(const State &state) const
    {
        return m_cost;
    }
};
template <typename State, typename Act, int N = 100> class BasePlanner {
	struct VisitedState {
		int priority;
		int cost;
		State state;

		// Used to reconstruct the path
		VisitedState *parent;
		Act *action;

		// Only used for linked list management
		VisitedState *next;
	};
	VisitedState nodes[N];
	void visited_states_array_to_list(VisitedState *nodes, int len)
	{
		for (auto i = 0; i < len - 1; i++) {
			nodes[i].next = &nodes[i + 1];
		}
		nodes[len - 1].next = nullptr;
	}
	VisitedState *priority_list_pop(VisitedState *&list)
	{
		VisitedState *min_elem = nullptr;
		auto min_prio = std::numeric_limits<int>::max();

		// Find the element with the lowest priority
		for (auto p = list; p != nullptr; p = p->next) {
			if (p->priority < min_prio) {
				min_elem = p;
				min_prio = p->priority;
			}
		}

		// Remove it from the list
		if (min_elem == list) {
			list = list->next;
		} else {
			for (auto p = list; p != nullptr; p = p->next) {
				if (p->next == min_elem) {
					p->next = p->next->next;
					min_elem->next = nullptr;
				}
			}
		}

		return min_elem;
	}

	VisitedState *list_pop_head(VisitedState *&head)
	{
		auto ret = head;
		head = head->next;
		ret->next = nullptr;
		return ret;
	}
	void list_push_head(VisitedState *&head, VisitedState *elem)
	{
		elem->next = head;
		head = elem;
	}
	void update_queued_state(VisitedState *previous,
				 const VisitedState *current)
	{
		if (previous->cost > current->cost) {
			previous->priority = current->cost;
			previous->priority = current->priority;
			previous->parent = current->parent;
			previous->action = current->action;
		}
	}

public:
	struct BaseGoal {
		std::string m_name;
        State goal;

	public:
        BaseGoal(const std::string &name, const State &goal)
            : m_name(name), goal(goal)
		{
		}
		/** Checks if the goal is reached for the given state. */
		virtual bool is_reached(const State &state) const
		{
            return distance_to(state) == 0;
		}

		/** Computes the distance from state to goal. */
        virtual int distance_to(const State &state) const
        {
            return state.distance_to(goal);
        }

		virtual ~BaseGoal() = default;
		const std::string &get_name() const
		{
			return m_name;
		}
	};

	/** Finds a plan from state to goal and returns its length.
     *
     * If path is given, then the found path is stored there.
     */
    int plan(const State &state, const BaseGoal &goal, Act *actions[],
		 unsigned action_count, Act **path = nullptr, int path_len = 10)
	{
		visited_states_array_to_list(nodes, N);

		auto free_nodes = &nodes[0];

		VisitedState *open = list_pop_head(free_nodes);
		VisitedState *close = nullptr;

		open->state = state;
		open->cost = 0;
		open->priority = 0;
		open->parent = nullptr;
		open->action = nullptr;

		while (open) {
			VisitedState *current = priority_list_pop(open);
			list_push_head(close, current);

			if (goal.is_reached(current->state)) {
				auto len = 0;
				for (auto p = current->parent; p;
				     p = p->parent) {
					len++;
				}

				if (len > path_len) {
                    OgreAssert(len <= path_len, "Out of plan length");
                    return -3;
				}

				if (path) {
					auto i = len - 1;
					for (auto p = current; p->parent;
					     p = p->parent, i--) {
						path[i] = p->action;
					}
				}

                OgreAssert(len >= 0, "Bad plan length");
				return len;
			}

			for (auto i = 0u; i < action_count; i++) {
				auto action = actions[i];

				if (action->can_run(current->state)) {
					// Cannot allocate a new node, abort
					if (free_nodes == nullptr) {
						// Garbage collect the node that is most unlikely to be
						// visited (i.e. lowest priority)
						VisitedState *gc,
							*gc_prev = nullptr;
						for (gc = open; gc && gc->next;
						     gc = gc->next) {
							gc_prev = gc;
						}

						if (!gc) {
                            OgreAssert(gc, "Out of memory");
							return -2 /* OOM */;
						}

						if (gc_prev) {
							gc_prev->next = nullptr;
						}

						free_nodes = gc;
					}

					auto neighbor =
						list_pop_head(free_nodes);
					neighbor->state = current->state;
					action->plan_effects(neighbor->state);
					neighbor->cost =
						current->cost + 1 +
						action->get_cost(
							neighbor->state);
					neighbor->priority =
						current->priority + 1 +
						goal.distance_to(
							neighbor->state);
					neighbor->parent = current;
					neighbor->action = action;

					bool should_insert = true;

					// Check if the node is already in the list of nodes
					// scheduled to be visited
					for (auto p = open; p; p = p->next) {
						if (p->state ==
						    neighbor->state) {
							should_insert = false;
							update_queued_state(
								p, neighbor);
						}
					}

					// Check if the state is in the list of already visited
					// state
					for (auto p = close; p; p = p->next) {
						if (p->state ==
						    neighbor->state) {
							should_insert = false;
							update_queued_state(
								p, neighbor);
						}
					}

					if (should_insert) {
						list_push_head(open, neighbor);
					} else {
						list_push_head(free_nodes,
							       neighbor);
					}
				}
			}
		}

		// Reaching here means we did not find a path
		return -1 /* No path */;
	}
};
template <class RunnerType, class State>
struct DeclareAction : public BaseAction<State> {
	RunnerType runner;
	template <class... Args>
	DeclareAction(const std::string &name, Args... args)
		: runner(args...)
        , BaseAction<State>(name, &runner)
	{
	}
};
}

#endif