#ifndef MOVEMENT_MANAGER_H
#define MOVEMENT_MANAGER_H

#include <vector>
#include "Gamelogic/NavGraph/NavGraph.h"


#include <set>
#include <queue>
#include <unordered_map>
#include <unordered_set>
#include <limits>
#include <algorithm>
#include <glm/glm.hpp>


    class MovementManager {
    public:
        MovementManager(std::shared_ptr<NavGraph> navGraph) : navGraph(navGraph) {}

        // Function to move an object to a target node using a shortest path algorithm
        void moveObjectToNode(std::shared_ptr<SceneObject> object, float x, float y) {
            std::shared_ptr<NavGraphNode> currentNode = object->assignedNavNode;
            std::shared_ptr<NavGraphNode> targetNode = navGraph->getNodeAt(x, y);
            if (!currentNode || !targetNode) {
                std::cout << "Invalid start or target node." << std::endl;
                return;
            }
            std::vector<std::shared_ptr<NavGraphNode>> path = findShortestPath(currentNode, targetNode);
            if (path.empty()) {
                std::cout << "No path available from (" << currentNode->getPosition()[0] << ", " << currentNode->getPosition()[1] << ") to (" << targetNode->getPosition()[0] << ", " << targetNode->getPosition()[1] << ")." << std::endl;
            } else {
                moveObjectAlongPath(object, path);
                std::cout << "Path found with " << path.size() << " steps." << std::endl;
            }
        }

        void moveObjectAlongPath(std::shared_ptr<SceneObject> object, const std::vector<std::shared_ptr<NavGraphNode>>& path) {
            if (path.empty()) {
            std::cout << "No path available." << std::endl;
            return;
            }
            object->startPath(path);
        };

    void updateObjectPosition(std::shared_ptr<SceneObject> object, float newX, float newY) {
        std::shared_ptr<NavGraphNode> currentNode = object->getCurrentNavNode();
        std::shared_ptr<NavGraphNode> newNode = navGraph->getNodeAt(std::floor(newX), std::floor(newY));;

        std::cout << "Current Node: (" << currentNode->getPosition()[0] << ", " << currentNode->getPosition()[1] << ")" << std::endl;
        std::cout << "New Position: (" << newX << ", " << newY << ")" << std::endl;

        if (!newNode) {
            std::cout << "New node is invalid." << std::endl;
            return;
        }

        if (newNode == currentNode) {
            std::cout << "New node is the same as the current node." << std::endl;
            return;
        }

        std::shared_ptr<NavGraphNode> targetNode = navGraph->getNodeAt(3, 3);
        if (!targetNode) {
            std::cout << "Object has no target node." << std::endl;
            return;
        }

        std::cout << "Target Node: (" << targetNode->getPosition()[0] << ", " << targetNode->getPosition()[1] << ")" << std::endl;

        std::vector<std::shared_ptr<NavGraphNode>> newPath = findShortestPath(newNode, targetNode);
        if (!newPath.empty()) {
            moveObjectAlongPath(object, newPath);
            std::cout << "Path recalculated. New path length: " << newPath.size() << std::endl;
        } else {
            std::cout << "No new path available from (" << newNode->getPosition()[0] << ", " << newNode->getPosition()[1] << ") to (" << targetNode->getPosition()[0] << ", " << targetNode->getPosition()[1] << ")." << std::endl;
        }
    }


    private:
        std::shared_ptr<NavGraph> navGraph;

        // A*
        std::vector<std::shared_ptr<NavGraphNode>> findShortestPath(
            std::shared_ptr<NavGraphNode> startNode, std::shared_ptr<NavGraphNode> targetNode) {

                VKL_LOG("Starting A* with Start Node at: " + glm::to_string(startNode->getPosition()) +
                    " and Target Node at: " + glm::to_string(targetNode->getPosition()));

            if (!startNode->isWalkable() || !targetNode->isWalkable()) {
                VKL_LOG("Start or target node is not walkable.");
                return {};  // Ensuring both nodes are walkable
            }

            std::priority_queue<std::pair<float, std::shared_ptr<NavGraphNode>>,
                                std::vector<std::pair<float, std::shared_ptr<NavGraphNode>>>,
                                std::greater<std::pair<float, std::shared_ptr<NavGraphNode>>>> openSet;

            std::unordered_map<std::shared_ptr<NavGraphNode>, std::shared_ptr<NavGraphNode>> cameFrom;
            std::unordered_map<std::shared_ptr<NavGraphNode>, float> gScore, fScore;
            std::unordered_set<std::shared_ptr<NavGraphNode>> openSetItems;


            VKL_LOG("Inside A*");
            for (const auto& node : navGraph->getNodes()) {
                gScore[node] = std::numeric_limits<float>::infinity();
                fScore[node] = std::numeric_limits<float>::infinity();
            }
            gScore[startNode] = 0.0f;
            fScore[startNode] = heuristic(startNode, targetNode);

            openSet.push({fScore[startNode], startNode});
            openSetItems.insert(startNode);

            while (!openSet.empty()) {
                auto current = openSet.top().second;
                openSet.pop();
                openSetItems.erase(current);

                if (current->getPosition()[0] == targetNode->getPosition()[0] && current->getPosition()[1]==targetNode->getPosition()[1]) {
                    VKL_LOG("END PATH");
                    return reconstructPath(cameFrom, current);
                }

                //std::cout << "Processing node at (" << current->getPosition()[0] << ", " << current->getPosition()[1] << ")" << std::endl;
                for (auto& neighbor : current->getNeighbors()) {
                    float tentative_gScore = gScore[current] + glm::distance(glm::vec2(current->getPosition()[0], current->getPosition()[1]),
                                                         glm::vec2(neighbor->getPosition()[0], neighbor->getPosition()[1]));

                    //std::cout << "Checking neighbor at (" << neighbor->getPosition()[0] << ", " << neighbor->getPosition()[1] << ") with tentative gScore: " << tentative_gScore << std::endl;



                    if (tentative_gScore < gScore[neighbor]) {
                        cameFrom[neighbor] = current;
                        gScore[neighbor] = tentative_gScore;
                        fScore[neighbor] = tentative_gScore + heuristic(neighbor, targetNode);

                        if (!openSetItems.count(neighbor)) {
                            openSet.push({fScore[neighbor], neighbor});
                            openSetItems.insert(neighbor);
                            //VKL_LOG("Adding neighbor to open set: " + glm::to_string(neighbor->getPosition()));
                        }
                    }
                }
            }

            // Empty path if no path is found
            return {};
        }

        // Helper function to reconstruct path from start to target node
        std::vector<std::shared_ptr<NavGraphNode>> reconstructPath(
            const std::unordered_map<std::shared_ptr<NavGraphNode>, std::shared_ptr<NavGraphNode>>& cameFrom,
            std::shared_ptr<NavGraphNode> current) {

            std::vector<std::shared_ptr<NavGraphNode>> path;
            while (cameFrom.find(current) != cameFrom.end()) { // Ensures the key exists
                path.push_back(current);
                current = cameFrom.at(current); // Safe access using at(), which will throw std::out_of_range if key does not exist
            }
            std::reverse(path.begin(), path.end());
            return path;
        }

        // Heuristic (distance) for A*
        float heuristic(std::shared_ptr<NavGraphNode> a, std::shared_ptr<NavGraphNode> b) {
            // std::cout << "Heuristic distance: " << glm::distance(glm::vec2(a->getPosition().x, a->getPosition().y),
            //              glm::vec2(b->getPosition().x, b->getPosition().y)) << std::endl;
            return glm::distance(glm::vec2(a->getPosition()[0], a->getPosition()[1]),
                         glm::vec2(b->getPosition()[0], b->getPosition()[1]));
            
        }

        
    };

    




#endif // MOVEMENT_MANAGER_H
