#pragma once

#include "render/Renderer.h"
#include <Eigen/Core>
#include <string>
#include <vector>

namespace FRG {

class PathFinder {
public:

    struct ObstaclePolygon {
        std::string name;
        std::vector<Eigen::Vector2f> polygon;
    };

	struct DynamicObstacle {
		Eigen::Vector3f position = Eigen::Vector3f::Zero();
		float radius = 0.0f;
	};

    struct Cell {
        int x = 0;
        int z = 0;
    };

    struct NavigationArea {
        std::string name;
        bool available = true;
        std::vector<Eigen::Vector2f> polygon;
    };

    void build(const std::string& configPath,
               const std::string& zipPath = "");

    std::vector<Eigen::Vector3f> findPath(const Eigen::Vector3f& start,
                                          const Eigen::Vector3f& end) const;

	std::vector<Eigen::Vector3f> findPath(const Eigen::Vector3f& start,
	                                      const Eigen::Vector3f& end,
	                                      const std::vector<DynamicObstacle>& dynamicObstacles) const;

	// Like findPath, but when the destination is unreachable the character
	// walks as close as possible instead of not moving at all.
	std::vector<Eigen::Vector3f> findPathToNearest(const Eigen::Vector3f& start,
	                                               const Eigen::Vector3f& end) const;
	std::vector<Eigen::Vector3f> findPathToNearest(const Eigen::Vector3f& start,
	                                               const Eigen::Vector3f& end,
	                                               const std::vector<DynamicObstacle>& dynamicObstacles) const;

    bool setAreaAvailable(const std::string& areaName, bool available);
    bool isReady() const { return ready; }
    bool isWalkable(const Eigen::Vector3f& point) const;
    const std::vector<NavigationArea>& getAreas() const { return areas; }
    const std::vector<ObstaclePolygon>& getObstaclePolygons() const { return obstaclePolygons; }
    float getFloorY() const { return floorY; }

    void addObstaclePolygon(const ObstaclePolygon& polygon);
    bool saveConfig(const std::string& path) const;
    bool saveGrid(const std::string& path) const;

private:

    float cellSize = 0.4f;
    float agentRadius = 0.45f;
    float floorY = 0.0f;
    float objectPadding = 0.25f;
    float boundaryPadding = 0.0f;

    float minX = 0.0f;
    float minZ = 0.0f;
    int gridWidth = 0;
    int gridDepth = 0;
    bool ready = false;

    std::string loadedConfigPath;
    std::string loadedZipPath;
    std::vector<ObstaclePolygon> obstaclePolygons;
    std::vector<unsigned char> walkable;
    std::vector<NavigationArea> areas;

    void loadConfig(const std::string& configPath, const std::string& zipPath);
    bool loadGrid(const std::string& path, const std::string& zipPath);
    void resetGridBounds();
    void rebuildWalkableGrid();
    void markAvailableAreasWalkable();
    void markObstaclePolygonsBlocked();

    bool worldToCell(const Eigen::Vector3f& point, Cell& out) const;
    Eigen::Vector3f cellCenter(const Cell& cell) const;
    bool isInsideGrid(const Cell& cell) const;
    bool isCellWalkable(const Cell& cell) const;
    int indexOf(const Cell& cell) const;

    bool findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out) const;
    bool hasLineOfSight(const Cell& from, const Cell& to) const;
    std::vector<Cell> smoothCells(const std::vector<Cell>& cells) const;

    static bool pointInPolygon(float x, float z, const std::vector<Eigen::Vector2f>& polygon);

	bool isCellWalkable(const Cell& cell, const std::vector<unsigned char>& walkableGrid) const;
	bool findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out, const std::vector<unsigned char>& walkableGrid) const;
	bool hasLineOfSight(const Cell& from, const Cell& to, const std::vector<unsigned char>& walkableGrid) const;
	std::vector<Cell> smoothCells(const std::vector<Cell>& cells, const std::vector<unsigned char>& walkableGrid) const;

	// A* that runs to exhaustion and returns a path to the reachable cell
	// geometrically closest to end when end itself is unreachable.
	std::vector<Eigen::Vector3f> findNearestReachableImpl(const Eigen::Vector3f& start,
	                                                      const Eigen::Vector3f& end,
	                                                      const std::vector<unsigned char>& walkableGrid) const;

    std::vector<Eigen::Vector3f> runAStar(const Eigen::Vector3f& start,
        const Eigen::Vector3f& end,
        const std::vector<unsigned char>& grid,
        bool returnNearestOnFail) const;
};

} // namespace FRG