#include "planner.hpp"
#include "action_db.hpp"
#include "godot_cpp/classes/engine.hpp"
#include "godot_cpp/classes/global_constants.hpp"
#include "godot_cpp/templates/hashfuncs.hpp"
#include "utils/godot_macros.hpp"
#include <cstdint>
#include <godot_cpp/variant/utility_functions.hpp>

namespace goap {
WorldStateNode::WorldStateNode(WorldState const &goal, ActorWorldState *context)
: state{}, open_requirements{goal}, context{context} {}

WorldStateNode::WorldStateNode(WorldStateNode const &last_state, Action const *last_action)
: state{last_state.state}
, open_requirements{last_state.open_requirements}
, last_action{last_action}
, context{last_state.context} {
    for(WorldProperty const &req : last_action->get_required())
        this->open_requirements[req.key] = req.value;
    for(WorldProperty const &effect : last_action->get_effects())
        if(this->open_requirements.has(effect.key))
            this->state[effect.key] = effect.value;
}

int WorldStateNode::requirements_unmet() const {
    int requirements = this->open_requirements.size();
    for(WorldProperty const &req : this->open_requirements) {
        if(this->state.has(req.key) && this->state[req.key] == req.value) {
            --requirements;
        } else if(this->context->check_property_match(req)) {
            --requirements;
        }
    }
    return requirements;
}

uint32_t WorldStateNodeHasher::hash(goap::WorldStateNode const &state) {
    uint32_t hash{1};
    for(goap::WorldProperty const &prop : state.state) {
        hash = gd::hash_murmur3_one_32(prop.key.hash(), hash);
        hash = gd::hash_murmur3_one_32(prop.value.hash(), hash);
    }
    return gd::hash_fmix32(hash);
}

bool operator==(goap::WorldStateNode const &lhs, goap::WorldStateNode const &rhs) {
    if(lhs.state.size() != rhs.state.size())
        return false;
    for(goap::WorldProperty const &prop_lhs : lhs.state) {
        if((!rhs.state.has(prop_lhs.key)) || rhs.state[prop_lhs.key] != prop_lhs.value)
            return false;
    }
    for(goap::WorldProperty const &prop_rhs : rhs.state) {
        if((!lhs.state.has(prop_rhs.key)) || lhs.state[prop_rhs.key] != prop_rhs.value)
            return false;
    }
    return true;
}

bool operator!=(goap::WorldStateNode const &lhs, goap::WorldStateNode const &rhs) {
    return !(lhs == rhs);
}

bool operator<(goap::WorldStateNode const &lhs, goap::WorldStateNode const &rhs) {
    return lhs.state.size() < rhs.state.size();
}

bool operator<=(goap::WorldStateNode const &lhs, goap::WorldStateNode const &rhs) {
    return lhs.state.size() <= rhs.state.size();
}

bool operator>(goap::WorldStateNode const &lhs, goap::WorldStateNode const &rhs) {
    return lhs.state.size() > rhs.state.size();
}

bool operator>=(goap::WorldStateNode const &lhs, goap::WorldStateNode const &rhs) {
    return lhs.state.size() >= rhs.state.size();
}

void Planner::_bind_methods() {
#define CLASSNAME Planner
    GDPROPERTY_HINTED(actions_inspector, gd::Variant::ARRAY, gd::PROPERTY_HINT_ARRAY_TYPE, ActionDB::get_array_hint());
}

void Planner::_enter_tree() { GDGAMEONLY();
    this->world_state = this->get_node<ActorWorldState>("../ActorWorldState");
}

Plan Planner::plan_for_goal(gd::Ref<Goal> goal) {
    // exit if goal reference is invalid or goal is already completed
    if(!goal.is_valid()) {
        gd::UtilityFunctions::push_warning(this->get_path(), " attempt to plan for invalid goal");
        return {};
    }
    // The node representing the start of the search and the goal of the plan.
    WorldStateNode goal_node{goal->desired_state, this->world_state};
    if(goal_node.requirements_unmet() == 0) {
        return {};
    }
    gd::Vector<WorldStateNode> open{goal_node};
    NodeMap from{}; // mapping nodes to the next node in the plan
    NodeScoreMap best_path_cost{}; // map nodes to the cost of the best path found so far
    NodeScoreMap heuristic_cost{}; // map nodes to their approximate cost to get to their goal

    // insert with the goal node into the cost maps,
    // path scored 0 as it's the goal
    best_path_cost.insert(goal_node, 0.f);
    heuristic_cost.insert(goal_node, goal_node.requirements_unmet());

    WorldStateNode current{goal_node};
    // repeat until current has no traversible edges
    while(!open.is_empty()) {
        current = open.get(0);
        // return found path when all requirements are met
        if(current.requirements_unmet() == 0)
            return this->unroll_plan(current, from);
        open.remove_at(0); // pop current off the top of the open set
        // get all actions that contribute to the open requirements of `current`
        gd::Vector<Action const *> edges{this->get_neighbours(current)};
        for(Action const * action : edges) {
            // construct the node produced by traversing edge
            WorldStateNode node_through{current, action};
            // calculate the cost of getting here from current through action.
            float const path_cost{best_path_cost.get(current) + 1.f};
            // store only if path through `current` represents a new fastest way to get to the new node
            if(!best_path_cost.has(node_through) || best_path_cost.get(node_through) >= path_cost) {
                // update cheapest route found
                best_path_cost[node_through] = path_cost;
                // approximate cost to goal
                heuristic_cost[node_through] = path_cost + node_through.requirements_unmet();
                // store path to get to this point
                from[node_through] = current;
                // (re-) insert node according to priority based on unmet requirements
                if(open.has(node_through))
                    open.erase(node_through);
                open.ordered_insert(node_through);
            }
        }
    }

    gd::UtilityFunctions::push_warning("failed to find plan for goal ", goal->get_path());
    return {};
}

void Planner::set_actions_inspector(gd::Array array) {
    this->actions.clear();
    for(size_t i = 0; i < array.size(); ++i) {
        Action const *action = int(array[i]) < 0 ? nullptr : ActionDB::get_action(array[i]);
        if(action != nullptr && !this->actions.has(action)) {
            this->actions.push_back(action);
        } else if(gd::Engine::get_singleton()->is_editor_hint() && !this->actions.has(nullptr)) {
            this->actions.push_back(nullptr);
        }
    }
}

gd::Array Planner::get_actions_inspector() const {
    gd::Array array{};
    for(Action const *action : this->actions)
        array.push_back(action == nullptr ? -1 : action->get_id());
    return array;
}

gd::Vector<Action const *> &Planner::get_actions() {
    return this->actions;
}

gd::Vector<Action const *> Planner::get_neighbours(WorldStateNode const &from) const {
    gd::Vector<Action const *> retval{};
    for(Action const *action : this->actions) {
        if(!action->is_possible(from.context) || !this->does_action_contribute(action, from))
            continue;
        retval.push_back(action);
    }
    return retval;
}

bool Planner::does_action_contribute(Action const *action, WorldStateNode const &node) const {
    WorldState const &effects = action->get_effects();
    for(WorldProperty const &goal : node.open_requirements) {
        if(node.state.has(goal.key) && goal.value == node.state.get(goal.key))
            continue; // requirement already met, don't count as requiring contribution
        if(effects.has(goal.key) && effects[goal.key] == goal.value)
            return true;
    }
    return false;
}

Plan Planner::unroll_plan(WorldStateNode const &start_node, NodeMap const &from) {
    Plan plan{};
    WorldStateNode node{start_node};
#if DEBUG_ENABLED
    gd::UtilityFunctions::print("plan (", this->get_path(), "):");
#endif
    while(node.last_action != nullptr) {
        plan.push_back(node.last_action);
#if DEBUG_ENABLED
        gd::UtilityFunctions::print(" (", plan.size(), ") ", node.last_action->get_class());
#endif
        node = from.get(node);
    }
    return plan;
}
}