#pragma once

#include <vector>
#include <cmath>

#include "defaults/defaults.hpp"
#include "map/map.hpp"



#include <vector>
#include <queue>
#include <cmath>
#include <map>
#include <algorithm>
#include <limits>

// --- Helper Structs for A* ---

// Node represents a single tile/cell in the grid.
struct Node {
    int x;
    int y;
    float g_cost = std::numeric_limits<float>::infinity(); // Cost from start
    float h_cost = 0.0f;                                   // Estimated cost to goal (Heuristic)
    Node* parent = nullptr;

    float f_cost() const {
        return g_cost + h_cost;
    }
};

// Custom comparison for the priority queue (min-heap).
// A* prioritizes the node with the LOWEST F-cost.
struct CompareNode {
    bool operator()(const Node* a, const Node* b) const {
        return a->f_cost() > b->f_cost(); // '>' for min-heap
    }
};

// --- Heuristic Function ---

// Use Euclidean distance for a grid with diagonal movement (optimal)
float get_Heuristic(int x1, int y1, int x2, int y2) {
    float dx = static_cast<float>(x1 - x2);
    float dy = static_cast<float>(y1 - y2);
    return std::sqrt(dx * dx + dy * dy);
}

// Map key to uniquely identify a grid cell
long long get_Map_Key(int x, int y, int grid_w) {
    return static_cast<long long>(y) * grid_w + x;
}

bool is_Path_Blocked(
    const std::vector<bool>& grid,
    int grid_w, int grid_h,
    int current_x, int current_y,
    int obj_w_tiles, int obj_h_tiles)
{
    // The path is blocked if any tile within the object's bounding box
    // at the current (current_x, current_y) position collides.
    for (int dy = 0; dy < obj_h_tiles; ++dy) {
        for (int dx = 0; dx < obj_w_tiles; ++dx) {
            int check_x = current_x + dx;
            int check_y = current_y + dy;

            // Check if coordinates are within the grid bounds
            if (check_x < 0 || check_x >= grid_w || check_y < 0 || check_y >= grid_h) {
                // Technically outside the map is a collision/unwalkable boundary
                return true;
            }

            // Calculate 1D index: grid[y * grid_w + x]
            // Collision is true if the value is 'true'
            if (grid[static_cast<size_t>(check_y * grid_w + check_x)]) {
                return true;
            }
        }
    }
    return false;
}

std::vector<SDL_FPoint> find_Path(const SDL_FRect& object_rect, const SDL_FPoint& dest_px, const int max_dist, const int min_dist, std::vector<bool> grid) {
    int collider_size = Tile::tile_collider_size;
    int tx = static_cast<int>(object_rect.x / Tile::tile_collider_size);
    int ty = static_cast<int>(object_rect.y / Tile::tile_collider_size);

    if ( ! (tx < 0 || ty < 0 ||
            tx >= static_cast<int>(Map::size.x * Tile::tile_collider_size) ||
            ty >= static_cast<int>(Map::size.y * Tile::tile_collider_size)) ) {
        size_t id = static_cast<size_t>(ty * (Map::size.x * Tile::tile_collider_size) + tx);
        grid[id] = false;
    }

    int grid_w = static_cast<size_t>(Map::size.x * (Tile::tile_size / collider_size));
    int grid_h = static_cast<size_t>(Map::size.y * (Tile::tile_size / collider_size));

    int obj_w_tiles = static_cast<int>(std::ceil(object_rect.w / collider_size));
    int obj_h_tiles = static_cast<int>(std::ceil(object_rect.h / collider_size));

    // Start/Goal cells (top-left tile corner of the object's occupied space)
    int start_x = static_cast<int>(object_rect.x) / collider_size;
    int start_y = static_cast<int>(object_rect.y) / collider_size;

    // Original goal tile is still used for the heuristic calculation (H-cost)
    // as it guides the search *toward* the general destination area.
    int goal_x  = dest_px.x / collider_size;
    int goal_y  = dest_px.y / collider_size;

    // Check if goal tile is reachable/valid (initial check remains useful)
    /*
    if (is_Path_Blocked(grid, grid_w, grid_h, goal_x, goal_y, obj_w_tiles, obj_h_tiles)) {
        // Only return if the target TILE is blocked, but even then the range might be reachable.
        // For a ranged search, it's safer to only return empty if start is blocked.
        // For now, let's keep the existing logic.
        // return {};
    }
    */

    // --- A* Initialization ---
    std::priority_queue<Node*, std::vector<Node*>, CompareNode> open_set;
    std::map<long long, Node> all_nodes;

    // Start Node Setup
    long long start_key = get_Map_Key(start_x, start_y, grid_w);
    Node& start_node = all_nodes[start_key];
    start_node.x = start_x;
    start_node.y = start_y;
    start_node.g_cost = 0.0f;
    start_node.h_cost = get_Heuristic(start_x, start_y, goal_x, goal_y);

    open_set.push(&start_node);

    // Variables for Ranged Goal Check (in pixels)
    float min_dist_sq = static_cast<float>(min_dist) * min_dist;
    float max_dist_sq = static_cast<float>(max_dist) * max_dist;


    int iii = 0;

    // --- A* Main Loop ---
    while (!open_set.empty()) {
        iii++;
        Node* current_node = open_set.top();
        open_set.pop();

        // 1. Check for Ranged Goal Condition
        // Calculate the center pixel coordinates of the current tile
        float current_center_x = (current_node->x + 0.5f) * collider_size;
        float current_center_y = (current_node->y + 0.5f) * collider_size;

        // Calculate squared Euclidean distance to the *exact* destination pixel
        float dx = current_center_x - dest_px.x;
        float dy = current_center_y - dest_px.y;
        float dist_sq = dx * dx + dy * dy;

        // Check if distance is within [min_dist, max_dist] (using squared distance for performance)
        if (dist_sq <= max_dist_sq && dist_sq >= min_dist_sq) {
            // Path found! This node is a valid stopping point within the range.
            std::vector<SDL_FPoint> path;
            path.push_back({dest_px.x, dest_px.y});
            Node* runner = current_node;

            // Reconstruct path from the current valid node back to start
            while (runner) {
                iii++;
                // Convert tile coordinates back to the center of the tile in pixel coordinates
                SDL_FPoint point;
                point.x = (runner->x + 0.5f) * collider_size;
                point.y = (runner->y + 0.5f) * collider_size;
                path.push_back(point);
                runner = runner->parent;
                if (iii > 3500) {
                    path.clear();
                    break;
                }
            }

            // Path is currently: [Goal Tile Center, ..., Start Tile Center]
            std::reverse(path.begin(), path.end());

            // As requested: Remove the starting point, as the path should contain the next move(s)
            if (path.size() > 1) {
                path.erase(path.begin());
            }

            // As requested: The returned vector should be [goal, ..., first move]
            std::reverse(path.begin(), path.end());

            // The last point in the current path (the final node/tile center) is the stopping point.
            // You may want to replace this with the exact target *pixel* if the goal is to stop
            // at the closest point on the boundary of the allowed range, but since this
            // node is just a *tile* within the range, we'll leave it as the tile center
            // to follow the existing logic structure.

            return path;
        }

        // The rest of the A* logic (neighbor exploration) remains the same
        // -------------------------------------------------------------------
        // 2. Explore Neighbors (8 directions for diagonal movement)
        for (int dx = -1; dx <= 1; ++dx) {
            for (int dy = -1; dy <= 1; ++dy) {
        iii++;
                if (dx == 0 && dy == 0) continue;

                int neighbor_x = current_node->x + dx;
                int neighbor_y = current_node->y + dy;

                // Check bounds and collision
                if (neighbor_x < 0 || neighbor_x >= grid_w || neighbor_y < 0 || neighbor_y >= grid_h) {
                    continue;
                }
                if (is_Path_Blocked(grid, grid_w, grid_h, neighbor_x, neighbor_y, obj_w_tiles, obj_h_tiles)) {
                    continue;
                }

                float move_cost = (dx != 0 && dy != 0) ? 1.41421356f : 1.0f;
                float tentative_g_cost = current_node->g_cost + move_cost;

                long long neighbor_key = get_Map_Key(neighbor_x, neighbor_y, grid_w);
                Node& neighbor_node = all_nodes[neighbor_key];

                // Initialize new node if it's the first visit
                if (neighbor_node.g_cost == std::numeric_limits<float>::infinity()) {
                    neighbor_node.x = neighbor_x;
                    neighbor_node.y = neighbor_y;
                    // H-cost still points to the original goal tile to guide the search
                    neighbor_node.h_cost = get_Heuristic(neighbor_x, neighbor_y, goal_x, goal_y);
                }

                // 3. Update Path/Add to Open Set
                if (tentative_g_cost < neighbor_node.g_cost) {
                    neighbor_node.parent = current_node;
                    neighbor_node.g_cost = tentative_g_cost;

                    open_set.push(&neighbor_node);
                }
            }
        }
    }


    // Path not found
    return {};
}