#pragma once

#include <vector>
#include <memory>
#include <cmath>
#include <glm/glm.hpp>
#include "NavGraphNode.h"
#include "GameLogic/SceneGraph/StreetNode.h"
#include "GameLogic/SceneGraph/HouseObject.h"
#include "GameLogic/SceneGraph/SceneNode.h"

class NavGraph {
public:


    NavGraph(std::shared_ptr<StreetNode> streetNode) : 
        width(streetNode->getDimensions().x),
        height(streetNode->getDimensions().z),
        gridSize(streetNode->getGridSize()) {

    float halfWidth = width / 2.0f;
    float halfHeight = height / 2.0f;

    for (float x = -halfWidth; x <= halfWidth; x += gridSize) {
        for (float z = -halfHeight; z <= halfHeight; z += gridSize) {
            auto newNode = std::make_shared<NavGraphNode>(glm::vec2(x,z));
            nodes.push_back(newNode);
            
        }
    }

        connectNodes();
    }

    void addNode(std::shared_ptr<NavGraphNode> node) {
        nodes.push_back(node);
    }


    glm::vec3 convertToWorldGrid(const glm::vec3& worldPos, float gridSize) {
    //return glm::floor(worldPos / gridSize);
    return glm::floor((worldPos + glm::vec3(gridSize * 0.5f)) / gridSize);  // Centering grid conversion - grid 1:1 rn
}



void addObstacles(std::shared_ptr<SceneObjectWithPhysics> house) {

    
    glm::vec3 worldPos = house->getAttachedPhysicssObj()->getPosition();
    glm::vec3 gridPos = convertToWorldGrid(worldPos, gridSize);

    
    glm::vec3 size = glm::vec3(1,1,1); //fix
    glm::ivec3 gridMin = glm::ivec3(glm::floor((worldPos - size) / gridSize));
    glm::ivec3 gridMax = glm::ivec3(glm::floor((worldPos + size) / gridSize));


    for (int x = gridMin.x; x <= gridMax.x; x++) {
        for (int z = gridMin.z; z <= gridMax.z; z++) {
            // Mark the corresponding grid cells as unwalkable
            std::shared_ptr<NavGraphNode> node = getNodeAt(x, z);  // This function should retrieve the node based on grid coordinates
            if (node) {
                node->setWalkable(false);
                //VKL_LOG("Unwalkable set");
            }
        }
    }
}

    void setObjectNavNode (std::shared_ptr<SceneObject> object){
        //all the objects in the sceneGraph potentially but ok
        float x = object->getObjectNavOrigin()[0];
        float z = object->getObjectNavOrigin()[1];

        object->setAssignedNavNode(getNodeAt(x,z));
        
        std::ostringstream logStream;
        logStream << "ASSIGNED NODE " << glm::to_string(getNodeAt(x,z)->getPosition());
        VKL_LOG(logStream.str());

    }

    std::shared_ptr<NavGraphNode> getNodeAt(float x, float z){
        for (auto node:nodes){
            if (node->getPosition()[0]==x && node->getPosition()[1] == z){
                return node;
            }
        }
        return nullptr;
    }


    void connectNodes() {
        // Grid is 1:1 rn modify if neccesary
        for (auto& currentNode : nodes) {
            if (!currentNode->isWalkable()) continue;

            glm::vec2 currentPosition = currentNode->getPosition();

            // Define potential neighbor positions including diagonals
            std::vector<glm::vec2> potentialNeighborPositions = {
                {currentPosition.x - 1, currentPosition.y},     // Left
                {currentPosition.x + 1, currentPosition.y},     // Right
                {currentPosition.x, currentPosition.y - 1},     // Top
                {currentPosition.x, currentPosition.y + 1},     // Bottom
                {currentPosition.x - 1, currentPosition.y - 1}, // Top-left
                {currentPosition.x + 1, currentPosition.y - 1}, // Top-right
                {currentPosition.x - 1, currentPosition.y + 1}, // Bottom-left
                {currentPosition.x + 1, currentPosition.y + 1}  // Bottom-right
            };

            // Try connecting to each potential neighbor position
            for (const auto& neighborPos : potentialNeighborPositions) {
                auto neighbor = getNodeAt(neighborPos.x, neighborPos.y);
                if (neighbor && neighbor->isWalkable()) {
                    currentNode->addNeighbor(neighbor);
                }
            }
        }
    }



    const std::vector<std::shared_ptr<NavGraphNode>>& getNodes() const { return nodes; }

private:
    float width;
    float height;
    float gridSize;
    std::vector<std::shared_ptr<NavGraphNode>> nodes;
    //std::vector<std::shared_ptr<NavGraphEdge>> edges;
    int countUnWalkable = 0; //simple testing
};
