#include "Location.h"
#include "utils/Utils.h"
#include "render/OpenGlExtensions.h"
#include <iostream>
#include "render/TextureManager.h"
#include "TextModel.h"
#include <random>
#include <cmath>
#include <algorithm>
#include <functional>
#include <memory>
#include <cfloat>
#include "GameConstants.h"

namespace {
	Eigen::Matrix4f lookAt(const Eigen::Vector3f& eye, const Eigen::Vector3f& target, const Eigen::Vector3f& up) {
		Eigen::Vector3f f = (target - eye).normalized();
		Eigen::Vector3f s = f.cross(up).normalized();
		Eigen::Vector3f u = s.cross(f);
		Eigen::Matrix4f result;
		result << s.x(), s.y(), s.z(), -s.dot(eye),
			u.x(), u.y(), u.z(), -u.dot(eye),
			-f.x(), -f.y(), -f.z(), f.dot(eye),
			0, 0, 0, 1;
		return result;
	}
}

namespace ZL
{
	extern float x;
	extern const char* CONST_ZIP_FILE;

	static constexpr float CAMERA_FOV_Y = 1.0f / 1.5f;

	Location::Location(Renderer& iRenderer, Inventory& iInventory, const std::string& locId)
		: renderer(iRenderer)
		, inventory(iInventory)
		, locationId(locId)
		, roofObjectKey("")
		, azsRoofObjectKey("")
	{

	}

void Location::setup()
{

	

	tileMesh.data = LoadFromTextFile02("resources/e/land/land003.txt", CONST_ZIP_FILE);
	tileMesh.RefreshVBO();
	roadTexture = std::make_shared<Texture>(CreateTextureDataFromPng("resources/e/land/Gemini_Generated_Image_eq858beq858beq85.png", CONST_ZIP_FILE));
	grassTexture = std::make_shared<Texture>(CreateTextureDataFromPng("resources/e/land/Gemini_Generated_Image_321v9q321v9q321v.png", CONST_ZIP_FILE));
	
	auto playerTexture0 = std::make_shared<Texture>(CreateTextureDataFromPng("resources/e/male_packed0_diffuse.png", CONST_ZIP_FILE));
	auto playerTexture1 = std::make_shared<Texture>(CreateTextureDataFromPng("resources/e/male_packed1_diffuse.png", CONST_ZIP_FILE));
	auto playerTexture2 = std::make_shared<Texture>(CreateTextureDataFromPng("resources/e/male_packed2_diffuse.png", CONST_ZIP_FILE));

	player = std::make_unique<Character>();
	player->loadBinaryAnimation(AnimationState::STAND, "resources/e/gg_stand_idle003.anim2", CONST_ZIP_FILE);
	player->loadBinaryAnimation(AnimationState::WALK, "resources/e/gg_run003.anim2", CONST_ZIP_FILE);
	player->setTexture("Body", playerTexture0);
	player->setTexture("Bottoms", playerTexture1);
	player->setTexture("Eyelashes", playerTexture0);
	player->setTexture("Eyes", playerTexture0);
	player->setTexture("Eyewear", playerTexture0);
	player->setTexture("Gloves", playerTexture1);
	player->setTexture("Hair", playerTexture0);
	player->setTexture("Shoes", playerTexture1);
	player->setTexture("Tops", playerTexture1);
	player->walkSpeed = 3.0f;
	player->rotationSpeed = 8.0f;
	player->modelScale = 0.01f;
	player->canAttack = true;
	player->isPlayer = true;
	player->modelCorrectionRotation = Eigen::Quaternionf(Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitY()));

	//player->position = { 9.43527, 0, 0.952688 };
	//player->setTarget(player->position);
	


	auto girlfriendTexture0 = std::make_shared<Texture>(CreateTextureDataFromPng("resources/e/female_packed0_diffuse.png", CONST_ZIP_FILE));
	auto girlfriendTexture1 = std::make_shared<Texture>(CreateTextureDataFromPng("resources/e/female_packed1_diffuse.png", CONST_ZIP_FILE));
	auto girlfriendTexture2 = std::make_shared<Texture>(CreateTextureDataFromPng("resources/e/female_packed2_diffuse.png", CONST_ZIP_FILE));
	auto girlfriendTexture3 = std::make_shared<Texture>(CreateTextureDataFromPng("resources/e/female_packed3_diffuse.png", CONST_ZIP_FILE));

	girlfriend = std::make_unique<Character>();
	girlfriend->loadBinaryAnimation(AnimationState::STAND, "resources/e/woman_stand_idle003.anim2", CONST_ZIP_FILE);
	girlfriend->loadBinaryAnimation(AnimationState::WALK, "resources/e/woman_run003.anim2", CONST_ZIP_FILE);
	girlfriend->setTexture("Body", girlfriendTexture0);
	girlfriend->setTexture("Bottoms", girlfriendTexture3);
	girlfriend->setTexture("Eyelashes", girlfriendTexture0);
	girlfriend->setTexture("Eyes", girlfriendTexture0);
	girlfriend->setTexture("Hair", girlfriendTexture2);
	girlfriend->setTexture("Shoes", girlfriendTexture1);
	girlfriend->setTexture("Tops", girlfriendTexture2);
	girlfriend->walkSpeed = 3.0f;
	girlfriend->rotationSpeed = 8.0f;
	girlfriend->modelScale = 0.01f;
	girlfriend->modelCorrectionRotation = Eigen::Quaternionf(Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitY()));
	//girlfriend->position = Vector3f{ -10, -0, -10 };
	//girlfriend->setTarget(girlfriend->position);

	
	//girlfriend->canAttack = true;
	//player->isPlayer = true;
	struct ModelAsset {
		const char* meshPath;
		const char* texPath;
		Eigen::Vector3f position;
		float scale;
	};

	if (locationId == "forest")
	{

		auto salesPersonTexture0 = std::make_shared<Texture>(CreateTextureDataFromPng("resources/e/Salesperson_packed0_diffuse.png", CONST_ZIP_FILE));
		auto salesPersonTexture1 = std::make_shared<Texture>(CreateTextureDataFromPng("resources/e/Salesperson_packed1_diffuse.png", CONST_ZIP_FILE));
		auto salesPersonTexture2 = std::make_shared<Texture>(CreateTextureDataFromPng("resources/e/Salesperson_packed2_diffuse.png", CONST_ZIP_FILE));

		salesperson = std::make_unique<Character>();
		salesperson->loadBinaryAnimation(AnimationState::STAND, "resources/e/salesperson_stand_idle003.anim2", CONST_ZIP_FILE);
		salesperson->loadBinaryAnimation(AnimationState::WALK, "resources/e/salesperson_stand_idle003.anim2", CONST_ZIP_FILE);
		salesperson->setTexture("Body", salesPersonTexture0);
		salesperson->setTexture("Bottoms", salesPersonTexture1);
		salesperson->setTexture("Eyelashes", salesPersonTexture0);
		salesperson->setTexture("Eyes", salesPersonTexture0);
		salesperson->setTexture("Hair", salesPersonTexture1);
		salesperson->setTexture("Shoes", salesPersonTexture1);
		salesperson->setTexture("Tops", salesPersonTexture1);
		salesperson->walkSpeed = 3.0f;
		salesperson->rotationSpeed = 8.0f;
		salesperson->modelScale = 0.01f;
		salesperson->modelCorrectionRotation = Eigen::Quaternionf(Eigen::AngleAxisf(0, Eigen::Vector3f::UnitY()));
		salesperson->position = Vector3f{ -8.31099, -0, -3.56868 };
		salesperson->setTarget(salesperson->position);

		auto policeTexture0 = std::make_shared<Texture>(CreateTextureDataFromPng("resources/e/police_packed0_diffuse.png", CONST_ZIP_FILE));
		auto policeTexture1 = std::make_shared<Texture>(CreateTextureDataFromPng("resources/e/police_packed1_diffuse.png", CONST_ZIP_FILE));

		police = std::make_unique<Character>();
		police->loadBinaryAnimation(AnimationState::STAND, "resources/e/police_stand_idle003.anim2", CONST_ZIP_FILE);
		police->loadBinaryAnimation(AnimationState::WALK, "resources/e/police_stand_idle003.anim2", CONST_ZIP_FILE);
		police->setTexture("Body", policeTexture0);
		police->setTexture("Bottoms", policeTexture1);
		police->setTexture("Eyelashes", policeTexture0);
		police->setTexture("Eyes", policeTexture0);
		police->setTexture("Hair", policeTexture1);
		police->setTexture("Shoes", policeTexture1);
		police->setTexture("Tops", policeTexture1);
		police->walkSpeed = 3.0f;
		police->rotationSpeed = 8.0f;
		police->modelScale = 0.01f;
		police->modelCorrectionRotation = Eigen::Quaternionf(Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitY()));
		police->position = Vector3f{ 1000, -0, 10 };
		police->setTarget(police->position);

		std::cout << "[LOCATION] Setting up FOREST location (custom models only)" << std::endl;

		carPosition ={ 7, 0, -7 + 200 };
		npcCar.position = Vector3f(9, 0, -335) + Vector3f(1000,0,0);

		girlfriend->position = Vector3f{ 5, 0, 0.9 + 200 };
		girlfriend->setTarget(girlfriend->position);

		player->position = { 9.5, 0, 0.95 + 200 };
		player->setTarget(player->position);
		//player->modelCorrectionRotation = Eigen::Quaternionf(Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitY()));
		//cameraAzimuth = M_PI;


	/*	gameObjects.clear();
		interactiveObjects.clear();
		npcs.clear();
		debugNavMeshes.clear();

		roomMesh.data = VertexDataStruct();
		//roomMesh.data = LoadFromTextFile02("resources/w/road001.txt", CONST_ZIP_FILE);
		//roomMesh.RefreshVBO();
		roomTexture = std::make_unique<Texture>(CreateTextureDataFromPng("resources/w/room005.png", CONST_ZIP_FILE));
		*/
		std::vector<ModelAsset> models = {
			{"resources/out/AzsBase001.txt", "resources/ghost_avatar.png", {0.0f, 2.0f, 0.0f}, 2.0f},
			{"resources/out/AzsRoof001.txt", "resources/ghost_avatar.png", {0.0f, 2.0f, 0.0f}, 2.0f},
			{"resources/out/floor001.txt", "resources/ghost_avatar.png", {0.0f, -1.97f, 0.0f}, 2.0f},
			{"resources/out/roof001.txt", "resources/ghost_avatar.png", {0.0f, 2.0f, 0.0f}, 2.0f},
			{"resources/out/Walls001.txt", "resources/ghost_avatar.png", {0.0f, 2.0f, 0.0f}, 2.0f},
			{"resources/out/Price001.txt", "resources/w/blue.png", {0.0f, 1.0f, 5.0f}, 2.0f}
		};

		for (size_t i = 0; i < models.size(); ++i) {
			const auto& m = models[i];
			LoadedGameObject obj;
			obj.mesh.data = LoadFromTextFile02(m.meshPath, CONST_ZIP_FILE);
			obj.mesh.data.Scale(m.scale);
			obj.mesh.data.Move(m.position);
			obj.mesh.RefreshVBO();
			obj.texture = std::make_shared<Texture>(CreateTextureDataFromPng(m.texPath, CONST_ZIP_FILE));

			if (std::string(m.meshPath) == "resources/out/AzsRoof001.txt") {
				gameObjects["azsRoof"] = std::move(obj);
				azsRoofObjectKey = "azsRoof";
				azsRoofHideZones.clear();
				azsRoofHideZones.emplace_back(Eigen::Vector2f(-7.0f, -14.0f), Eigen::Vector2f(0.1f, -8.5f));
			}
			else if (std::string(m.meshPath) == "resources/out/roof001.txt") {
				gameObjects["roof"] = std::move(obj);
				roofObjectKey = "roof";
				roofHideZones.clear();
				roofHideZones.emplace_back(Eigen::Vector2f(-9.3f, -5.0f), Eigen::Vector2f(2.5f, 1.7f));
			}
			else {
				gameObjects["forest_model_" + std::to_string(i)] = std::move(obj);
			}
		}


		shadowMap = std::make_unique<ShadowMap>(2048, 40.0f, 0.1f, 100.0f);
		shadowMap->setLightDirection(Eigen::Vector3f(-0.5f, -1.0f, -0.3f));

		setupNavigation();

		scriptEngine.init(this, &inventory);
		dialogueSystem.init(renderer, CONST_ZIP_FILE);
		dialogueSystem.loadDatabase("resources/dialogue/sample_dialogues.json");

		std::cout << "[FOREST] Setup complete, loaded " << gameObjects.size() << " custom models" << std::endl;
	}

	else if (locationId == "barn")
	{
		std::cout << "[LOCATION] Setting up BARN location" << std::endl;

		carPosition = { 0, 0, -20 };
		npcCar.position = Vector3f(15, 0, 50);

		player->position = { 0, 0, -15 };
		player->setTarget(player->position);

		girlfriend->position = Vector3f{ 10, 0, -15 };
		girlfriend->setTarget(girlfriend->position);

		float meshScale = 4.0f;
		Vector3f meshPos = { 18, 0, 0 };

		std::vector<ModelAsset> models = {
			{"resources/barn/board.txt", "resources/ghost_avatar.png", meshPos, meshScale},
			{"resources/barn/books1.txt", "resources/ghost_avatar.png", meshPos, meshScale},
			{"resources/barn/books2.txt", "resources/ghost_avatar.png", meshPos, meshScale},
			{"resources/barn/cardboard1.txt", "resources/ghost_avatar.png", meshPos, meshScale},
			{"resources/barn/cardboard2.txt", "resources/ghost_avatar.png", meshPos, meshScale},
			{"resources/barn/cube.txt", "resources/ghost_avatar.png", meshPos, meshScale},
			{"resources/barn/house.txt", "resources/ghost_avatar.png", meshPos, meshScale},
			{"resources/barn/lamp.txt", "resources/ghost_avatar.png", meshPos, meshScale},
			{"resources/barn/pin1.txt", "resources/ghost_avatar.png", meshPos, meshScale},
			{"resources/barn/pin2.txt", "resources/ghost_avatar.png", meshPos, meshScale},
			{"resources/barn/pin3.txt", "resources/ghost_avatar.png",meshPos, meshScale},
			{"resources/barn/pin4.txt", "resources/ghost_avatar.png", meshPos, meshScale},
			{"resources/barn/radio.txt", "resources/ghost_avatar.png", meshPos, meshScale},
			{"resources/barn/table.txt", "resources/ghost_avatar.png", meshPos, meshScale}
		};

		for (size_t i = 0; i < models.size(); ++i) {
			const auto& m = models[i];
			LoadedGameObject obj;
			obj.mesh.data = LoadFromTextFile02(m.meshPath, CONST_ZIP_FILE);
			obj.mesh.data.Scale(m.scale);
			obj.mesh.data.Move(m.position);
			obj.mesh.RefreshVBO();
			obj.texture = std::make_shared<Texture>(CreateTextureDataFromPng(m.texPath, CONST_ZIP_FILE));
			gameObjects["bark_model_" + std::to_string(i)] = std::move(obj);
		}

		shadowMap = std::make_unique<ShadowMap>(2048, 40.0f, 0.1f, 100.0f);
		shadowMap->setLightDirection(Eigen::Vector3f(-0.5f, -1.0f, -0.3f));

		//setupNavigation();

		scriptEngine.init(this, &inventory);
		dialogueSystem.init(renderer, CONST_ZIP_FILE);
		dialogueSystem.loadDatabase("resources/dialogue/sample_dialogues.json");

		buildingFirstPersonZone = Eigen::AlignedBox<float, 2>(Eigen::Vector2f(14.0f, -8.7f), Eigen::Vector2f(26.0f, 9.5f));
		firstPersonMode = false;

		std::cout << "[BARK] Setup complete, loaded " << gameObjects.size() << " models" << std::endl;
	}

	else // default
	{
		auto banditTexture0 = std::make_shared<Texture>(CreateTextureDataFromPng("resources/e/bandit_packed0_diffuse.png", CONST_ZIP_FILE));
		auto banditTexture1 = std::make_shared<Texture>(CreateTextureDataFromPng("resources/e/bandit_packed1_diffuse.png", CONST_ZIP_FILE));
		auto banditTexture2 = std::make_shared<Texture>(CreateTextureDataFromPng("resources/e/bandit_packed2_diffuse.png", CONST_ZIP_FILE));

		bandit = std::make_unique<Character>();
		bandit->loadBinaryAnimation(AnimationState::STAND, "resources/e/bandit_stand_idle003.anim2", CONST_ZIP_FILE);
		bandit->loadBinaryAnimation(AnimationState::WALK, "resources/e/bandit_stand_idle003.anim2", CONST_ZIP_FILE);
		bandit->setTexture("Body", banditTexture0);
		bandit->setTexture("Bottoms", banditTexture1);
		bandit->setTexture("Eyelashes", banditTexture0);
		bandit->setTexture("Eyes", banditTexture0);
		bandit->setTexture("Hair", banditTexture1);
		bandit->setTexture("Shoes", banditTexture2);
		bandit->setTexture("Tops", banditTexture2);
		bandit->walkSpeed = 3.0f;
		bandit->rotationSpeed = 8.0f;
		bandit->modelScale = 0.01f;
		bandit->modelCorrectionRotation = Eigen::Quaternionf(Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitY()));
		bandit->position = Vector3f{ 12.1782, 0, 62.4014 };
		bandit->setTarget(bandit->position);

		carPosition = { -6.61929, 0, -30.7197 };
		carRotation = M_PI;
		npcCar.position = Vector3f{ 7.1782, 0, 68.4014 };

		player->position = { -6.61929, 0, -36.7197 };
		player->setTarget(player->position);
		cameraAzimuth = M_PI;

		girlfriend->position = Vector3f{ 27.6714, 0, 73.3165 };
		girlfriend->setTarget(girlfriend->position);

		float meshScale = 6.f;
		Vector3f meshPos = { 18,0,0 };

		std::vector<ModelAsset> models = {
			{"resources/out/main2/birch-2.001.txt", "resources/ghost_avatar.png", meshPos, meshScale},
{ "resources/out/main2/birch-2.002.txt", "resources/ghost_avatar.png", meshPos, meshScale },
{ "resources/out/main2/birch-2.003.txt", "resources/ghost_avatar.png", meshPos, meshScale },
{ "resources/out/main2/birch-2.004.txt", "resources/ghost_avatar.png", meshPos, meshScale },
{ "resources/out/main2/birch-2.005.txt", "resources/ghost_avatar.png", meshPos, meshScale },
{ "resources/out/main2/birch-2.006.txt", "resources/ghost_avatar.png", meshPos, meshScale },
//{ "resources/out/main2/birch-2.007.txt", "resources/ghost_avatar.png", meshPos, meshScale },
{ "resources/out/main2/birch-2.008.txt", "resources/ghost_avatar.png", meshPos, meshScale },
{ "resources/out/main2/birch-2.009.txt", "resources/ghost_avatar.png", meshPos, meshScale },
{ "resources/out/main2/birch-2.010.txt", "resources/ghost_avatar.png", meshPos, meshScale },
{ "resources/out/main2/birch-2.011.txt", "resources/ghost_avatar.png", meshPos, meshScale },
{ "resources/out/main2/birch-2.txt", "resources/ghost_avatar.png", meshPos, meshScale },
{ "resources/out/main2/Cube.001.txt", "resources/ghost_avatar.png", meshPos, meshScale },
{ "resources/out/main2/Cube.002.txt", "resources/ghost_avatar.png", meshPos, meshScale },
{ "resources/out/main2/Cube.003.txt", "resources/ghost_avatar.png", meshPos, meshScale },
{ "resources/out/main2/Cube.004.txt", "resources/ghost_avatar.png", meshPos, meshScale },
{ "resources/out/main2/Cube.005.txt", "resources/ghost_avatar.png", meshPos, meshScale },
{ "resources/out/main2/Cube.006.txt", "resources/ghost_avatar.png", meshPos, meshScale },
{ "resources/out/main2/Cube.007.txt", "resources/ghost_avatar.png", meshPos, meshScale },
{ "resources/out/main2/Cube.008.txt", "resources/ghost_avatar.png", meshPos, meshScale },
{ "resources/out/main2/Cube.009.txt", "resources/ghost_avatar.png", meshPos, meshScale },
{ "resources/out/main2/Cube.010.txt", "resources/ghost_avatar.png", meshPos, meshScale },
{ "resources/out/main2/Cube.011.txt", "resources/ghost_avatar.png", meshPos, meshScale },
{ "resources/out/main2/Cube.012.txt", "resources/ghost_avatar.png", meshPos, meshScale },
{ "resources/out/main2/Cube.013.txt", "resources/ghost_avatar.png", meshPos, meshScale },
{ "resources/out/main2/Cube.014.txt", "resources/ghost_avatar.png", meshPos, meshScale },
{ "resources/out/main2/Cube.015.txt", "resources/ghost_avatar.png", meshPos, meshScale },
{ "resources/out/main2/Cube.txt", "resources/ghost_avatar.png", meshPos, meshScale }
		};

		for (size_t i = 0; i < models.size(); ++i) {
			const auto& m = models[i];
			LoadedGameObject obj;
			obj.mesh.data = LoadFromTextFile02(m.meshPath, CONST_ZIP_FILE);
			obj.mesh.data.Scale(m.scale);
			obj.mesh.data.Move(m.position);
			obj.mesh.RefreshVBO();
			obj.texture = std::make_shared<Texture>(CreateTextureDataFromPng(m.texPath, CONST_ZIP_FILE));
			gameObjects["default_model_" + std::to_string(i)] = std::move(obj);
		}


		shadowMap = std::make_unique<ShadowMap>(2048, 40.0f, 0.1f, 100.0f);
		shadowMap->setLightDirection(Eigen::Vector3f(-0.5f, -1.0f, -0.3f));

		scriptEngine.init(this, &inventory);
		dialogueSystem.init(renderer, CONST_ZIP_FILE);
		dialogueSystem.loadDatabase("resources/dialogue/sample_dialogues.json");
	}

	carTexture = std::make_unique<Texture>(CreateTextureDataFromPng("resources/e/car002.png", CONST_ZIP_FILE));
	carMesh.data = LoadFromTextFile02("resources/e/car001.txt", CONST_ZIP_FILE);
	carMesh.data.RotateByMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(M_PI * 0.5, Eigen::Vector3f::UnitY())).toRotationMatrix());
	carMesh.RefreshVBO();

	carWheelTexture = std::make_unique<Texture>(CreateTextureDataFromPng("resources/e/Wheel_base002.png", CONST_ZIP_FILE));
	carWheelMesh.data = LoadFromTextFile02("resources/e/car_wheel001.txt", CONST_ZIP_FILE);
	carWheelMesh.data.RotateByMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(M_PI * 0.5, Eigen::Vector3f::UnitY())).toRotationMatrix());
	carWheelMesh.RefreshVBO();

	npcCar.texture = std::make_shared<Texture>(CreateTextureDataFromPng("resources/e/car_black001.png", CONST_ZIP_FILE));
	//npcCar.position = carPosition + Eigen::Vector3f(0, 0.f, 14.f);//Eigen::Vector3f(-12.f, 0.f, 8.f);
	npcCar.rotation = 0.f;
	npcCar.mode = NpcCar::Mode::NONE;
	/*npcCar.mode = NpcCar::Mode::FOLLOW_WAYPOINTS;
	npcCar.waypoints = {
		Eigen::Vector3f(-12.f, 0.f,  8.f),
		Eigen::Vector3f( 12.f, 0.f,  8.f),
		Eigen::Vector3f( 12.f, 0.f, -8.f),
		Eigen::Vector3f(-12.f, 0.f, -8.f),
	};*/
	//npcCar.currentWaypoint = 0;
}

	void Location::setupNavigation()
	{

		std::vector<PathFinder::ObstacleMesh> obstacles;

		if (locationId == "forest") {
			navigation.build(obstacles, "resources/config2/navigation2.json", CONST_ZIP_FILE);
		}
		else {
			navigation.build(obstacles, "resources/config2/navigation.json", CONST_ZIP_FILE);
		}
//#ifdef SHOW_PATH
		buildDebugNavMeshes();
		buildDebugForbiddenMeshes();
//#endif


		auto planner = [this](const Eigen::Vector3f& start, const Eigen::Vector3f& end) {
			return navigation.findPath(start, end);
			};

		if (player) {
			player->setPathPlanner(planner);
		}

		for (auto& npc : npcs) {
			if (npc) {
				npc->setPathPlanner(planner);
			}
		}
	}

//#ifdef SHOW_PATH
	void Location::buildDebugNavMeshes()
	{
		debugNavMeshes.clear();
		const auto& areas = navigation.getAreas();
		float y = navigation.getFloorY() + 0.02f;
		Eigen::Vector3f red(1.0f, 0.0f, 0.0f);

		for (const auto& area : areas) {
			if (area.polygon.size() < 3) continue;
			VertexRenderStruct mesh;
			mesh.data = CreatePolygonFloor(area.polygon, y, red);
			mesh.RefreshVBO();
			debugNavMeshes.push_back(std::move(mesh));
		}
	}

	void Location::drawDebugNavigation()
	{
		renderer.shaderManager.PushShader("defaultColor");
		renderer.SetMatrix();
		for (const auto& mesh : debugNavMeshes) {
			renderer.DrawVertexRenderStruct(mesh);
		}
		renderer.shaderManager.PopShader();
		renderer.SetMatrix();
	}

	void Location::buildDebugForbiddenMeshes()
	{
		debugForbiddenMeshes.clear();
		const auto& areas = navigation.getAreas();
		float y = navigation.getFloorY() + 0.2f;
		Eigen::Vector3f blue(0.0f, 0.0f, 1.0f);

		for (const auto& area : areas) {
			if (area.forbidden && area.polygon.size() >= 3) {
				VertexRenderStruct mesh;
				mesh.data = CreatePolygonFloor(area.polygon, y, blue);
				mesh.RefreshVBO();
				debugForbiddenMeshes.push_back(std::move(mesh));
			}
		}
	}

	void Location::drawDebugForbidden()
	{
		if (debugForbiddenMeshes.empty()) return;
		renderer.shaderManager.PushShader("defaultColor");
		renderer.SetMatrix();
		for (const auto& mesh : debugForbiddenMeshes) {
			renderer.DrawVertexRenderStruct(mesh);
		}
		renderer.shaderManager.PopShader();
		renderer.SetMatrix();
	}
//#endif


	InteractiveObject* Location::raycastInteractiveObjects(const Eigen::Vector3f& rayOrigin, const Eigen::Vector3f& rayDir) {
		if (interactiveObjects.empty()) {
			std::cout << "[RAYCAST] No interactive objects to check" << std::endl;
			return nullptr;
		}

		std::cout << "[RAYCAST] Starting raycast with " << interactiveObjects.size() << " objects" << std::endl;
		std::cout << "[RAYCAST] Ray origin: (" << rayOrigin.x() << ", " << rayOrigin.y() << ", " << rayOrigin.z() << ")" << std::endl;
		std::cout << "[RAYCAST] Ray dir: (" << rayDir.x() << ", " << rayDir.y() << ", " << rayDir.z() << ")" << std::endl;

		float closestDistance = FLT_MAX;
		InteractiveObject* closestObject = nullptr;

		for (auto& intObj : interactiveObjects) {
			std::cout << "[RAYCAST] Checking object: " << intObj.name << " (active: " << intObj.isActive << ")" << std::endl;

			if (!intObj.isActive) {
				std::cout << "[RAYCAST]   -> Object inactive, skipping" << std::endl;
				continue;
			}

			std::cout << "[RAYCAST]   Position: (" << intObj.position.x() << ", " << intObj.position.y() << ", "
				<< intObj.position.z() << "), Radius: " << intObj.interactionRadius << std::endl;

			Eigen::Vector3f toObject = intObj.position - rayOrigin;
			std::cout << "[RAYCAST]   Vector to object: (" << toObject.x() << ", " << toObject.y() << ", " << toObject.z() << ")" << std::endl;

			float distanceAlongRay = toObject.dot(rayDir);
			std::cout << "[RAYCAST]   Distance along ray: " << distanceAlongRay << std::endl;

			if (distanceAlongRay < 0.1f) {
				std::cout << "[RAYCAST]   -> Object behind camera, skipping" << std::endl;
				continue;
			}

			Eigen::Vector3f closestPointOnRay = rayOrigin + rayDir * distanceAlongRay;
			float distToObject = (closestPointOnRay - intObj.position).norm();

			std::cout << "[RAYCAST]   Distance to object: " << distToObject
				<< " (interaction radius: " << intObj.interactionRadius << ")" << std::endl;

			if (distToObject <= intObj.interactionRadius && distanceAlongRay < closestDistance) {
				std::cout << "[RAYCAST]   *** HIT DETECTED! ***" << std::endl;
				closestDistance = distanceAlongRay;
				closestObject = &intObj;
			}
		}

		if (closestObject) {
			std::cout << "[RAYCAST] *** RAYCAST SUCCESS: Found object " << closestObject->name << " ***" << std::endl;
		}
		else {
			std::cout << "[RAYCAST] No objects hit" << std::endl;
		}

		return closestObject;
	}

	Character* Location::raycastNpcs(const Eigen::Vector3f& rayOrigin, const Eigen::Vector3f& rayDir, float maxDistance) {
		Character* closestNpc = nullptr;
		float closestDist = maxDistance;

		for (auto& npc : npcs) {
			Eigen::Vector3f toNpc = npc->position - rayOrigin;
			float distAlongRay = toNpc.dot(rayDir);
			if (distAlongRay < 0.1f) continue;

			Eigen::Vector3f closestPoint = rayOrigin + rayDir * distAlongRay;
			float distToNpc = (closestPoint - npc->position).norm();

			float radius = npc->modelScale * 50.0f;

			if (distToNpc <= radius && distAlongRay < closestDist) {
				closestDist = distAlongRay;
				closestNpc = npc.get();
			}
		}
		return closestNpc;
	}


	void Location::drawGame()
	{
		glClearColor(0.0f, 0.0f, 0.0f, 1.0f);
		glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);


		renderer.shaderManager.PushShader(defaultShaderName);
		renderer.RenderUniform1i(textureUniformName, 0);

		renderer.PushPerspectiveProjectionMatrix(1.0 / 1.5,
			static_cast<float>(Environment::width) / static_cast<float>(Environment::height),
			Environment::CONST_Z_NEAR, Environment::CONST_Z_FAR);
		renderer.PushMatrix();

		/*renderer.LoadIdentity();
		renderer.TranslateMatrix({ 0,0, -1.0f * Environment::zoom });

		renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(cameraInclination, Eigen::Vector3f::UnitX())).toRotationMatrix());
		renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(cameraAzimuth, Eigen::Vector3f::UnitY())).toRotationMatrix());
		Eigen::Vector3f camTarget = inCar ? carPosition : (player ? player->position : Eigen::Vector3f::Zero());
		renderer.TranslateMatrix({ -camTarget.x(), -camTarget.y(), -camTarget.z() });
		renderer.TranslateMatrix({ 0, -1.3f, 0 });

		*/

		if (firstPersonMode && !inCar && player) {
			const float eyeHeight = 1.6f;
			Eigen::Vector3f eye = player->position + Eigen::Vector3f(0.0f, eyeHeight, 0.0f);

			float yaw = cameraAzimuth;
			float pitch = cameraInclination;
			Eigen::Vector3f forward(std::sin(yaw), 0.0f, -std::cos(yaw));
			Eigen::Vector3f direction = Eigen::Quaternionf(Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitX())) * forward;
			direction.normalize();

			Eigen::Vector3f target = eye + direction;
			Eigen::Vector3f up(0.0f, 1.0f, 0.0f);

			Eigen::Matrix4f view = lookAt(eye, target, up);
			renderer.PushSpecialMatrix(view);   // (2) добавляем матрицу первого лица в стек
		}
		else {
			// Стандартная камера от третьего лица (без дополнительного Push)
			renderer.LoadIdentity();
			renderer.TranslateMatrix({ 0, 0, -1.0f * Environment::zoom });
			renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(cameraInclination, Eigen::Vector3f::UnitX())).toRotationMatrix());
			renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(cameraAzimuth, Eigen::Vector3f::UnitY())).toRotationMatrix());
			Eigen::Vector3f camTarget = inCar ? carPosition : (player ? player->position : Eigen::Vector3f::Zero());
			renderer.TranslateMatrix({ -camTarget.x(), -camTarget.y(), -camTarget.z() });
			const float cameraYOffset = firstPersonMode ? -0.2f : -1.3f;
			renderer.TranslateMatrix({ 0, cameraYOffset, 0 });
		}

		for (int i = -7; i <= 7; i++)
		{
			for (int j = -7; j <= 7; j++)
			{
				renderer.PushMatrix();
				renderer.TranslateMatrix({ i * 100.0f, 0, j * 100.0f });
				if (i == 0)
				{
					glBindTexture(GL_TEXTURE_2D, roadTexture->getTexID());
				}
				else
				{
					glBindTexture(GL_TEXTURE_2D, grassTexture->getTexID());
				}
				renderer.DrawVertexRenderStruct(tileMesh);
				renderer.PopMatrix();
			}
		}

		if (roomMesh.data.PositionData.size() > 0)
		{
			glBindTexture(GL_TEXTURE_2D, roomTexture->getTexID());
			renderer.DrawVertexRenderStruct(roomMesh);
		}

		for (auto& [name, gameObj] : gameObjects) {
			if (name == roofObjectKey && !roofVisible) {
				continue;
			}
			if (name == azsRoofObjectKey && !azsRoofVisible) {
				continue;
			}
			//renderer.PushMatrix();
			//renderer.TranslateMatrix({ x,0,0 });
			glBindTexture(GL_TEXTURE_2D, gameObj.texture->getTexID());
			renderer.DrawVertexRenderStruct(gameObj.mesh);
			//renderer.PopMatrix();
		}

		for (auto& intObj : interactiveObjects) {
			if (intObj.isActive) {
				intObj.draw(renderer);
			}
		}

		renderer.PushMatrix();
		//renderer.LoadIdentity();
		renderer.TranslateMatrix(carPosition);
		renderer.TranslateMatrix({ 0, 0.7, 0 });
		renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(carRotation, Eigen::Vector3f::UnitY())).toRotationMatrix());
		glBindTexture(GL_TEXTURE_2D, carTexture->getTexID());
		renderer.DrawVertexRenderStruct(carMesh);

		if (carWheelTexture) {
			constexpr float track_width = 1.28f;
			constexpr float wheel_base = 2.25f;
			constexpr float shift = 0.6f;
			const Eigen::Vector3f wheelPositions[4] = {
				Eigen::Vector3f( track_width, 0.f - 0.21, -(wheel_base + shift)+1.25),  // front right
				Eigen::Vector3f(-track_width, 0.f - 0.21, -(wheel_base + shift)+1.25),  // front left
				Eigen::Vector3f( track_width, 0.f - 0.21,  (wheel_base - shift)+1.1),  // rear right
				Eigen::Vector3f(-track_width, 0.f - 0.21,  (wheel_base - shift)+1.1)   // rear left
			};
			const bool isFront[4] = { true, true, false, false };

			glBindTexture(GL_TEXTURE_2D, carWheelTexture->getTexID());
			for (int i = 0; i < 4; ++i) {
				renderer.PushMatrix();
				renderer.TranslateMatrix(wheelPositions[i]);
				if (isFront[i]) {
					renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(carSteeringAngle, Eigen::Vector3f::UnitY())).toRotationMatrix());
				}
				renderer.DrawVertexRenderStruct(carWheelMesh);
				renderer.PopMatrix();
			}
		}

		renderer.PopMatrix();

		drawNpcCar();

		// Don't draw the player mesh when in first-person mode
		if (player && !inCar && !firstPersonMode) player->draw(renderer);

		if (girlfriend && !girlfriendInCar)
		{
			girlfriend->draw(renderer);
		}

		if (salesperson)
		{
			salesperson->draw(renderer);
		}

		if (police)
		{
			police->draw(renderer);
		}

		if (bandit)
		{
			bandit->draw(renderer);
		}

		for (auto& npc : npcs) npc->draw(renderer);

//#ifdef SHOW_PATH
		drawDebugNavigation();
		drawDebugForbidden();
//#endif

	//	renderer.PopMatrix();

	//	renderer.PopProjectionMatrix();

	//	renderer.shaderManager.PopShader();

		// Восстанавливаем стек матриц
		if (firstPersonMode && !inCar && player) {
			renderer.PopMatrix();   // убираем (2) – матрицу первого лица
		}
		renderer.PopMatrix();       // убираем (1) – исходную матрицу

		renderer.PopProjectionMatrix();
		renderer.shaderManager.PopShader();

	}

	void Location::drawShadowDepthPass()
	{
		if (!shadowMap) return;

		const Eigen::Vector3f& sceneCenter = player ? player->position : Eigen::Vector3f::Zero();
		shadowMap->updateLightSpaceMatrix(sceneCenter);

		shadowMap->bind();

		glEnable(GL_DEPTH_TEST);
		glDepthFunc(GL_LESS);

		// Use front-face culling during depth pass to reduce shadow acne on lit faces
		//glCullFace(GL_FRONT);
		glEnable(GL_CULL_FACE);

		renderer.shaderManager.PushShader("shadow_depth");

		// Set up light's orthographic projection
		const Eigen::Matrix4f& lightProj = shadowMap->getLightProjectionMatrix();
		const Eigen::Matrix4f& lightView = shadowMap->getLightViewMatrix();

		// Push the light's projection matrix via the 6-param ortho overload won't
		// match our pre-computed matrix. Instead, use the raw stack approach:
		// push a dummy projection then overwrite via PushSpecialMatrix-style.
		// Simpler: push ortho then push the light view as modelview.
		renderer.PushProjectionMatrix(
			-40.0f, 40.0f,
			-40.0f, 40.0f,
			0.1f, 100.0f);

		const Eigen::Vector3f& lightDir = shadowMap->getLightDirection();
		Eigen::Vector3f lightPos = sceneCenter - lightDir * 50.0f;
		Eigen::Vector3f up(0.0f, 1.0f, 0.0f);
		if (std::abs(lightDir.dot(up)) > 0.99f) {
			up = Eigen::Vector3f(0.0f, 0.0f, 1.0f);
		}

		// Build the light view matrix and push it
		renderer.PushSpecialMatrix(lightView);

		// Draw static geometry
		renderer.DrawVertexRenderStruct(roomMesh);

		for (auto& [name, gameObj] : gameObjects) {
			if (name == roofObjectKey && !roofVisible) {
				continue;
			}
			if (name == azsRoofObjectKey && !azsRoofVisible) {
				continue;
			}
			renderer.DrawVertexRenderStruct(gameObj.mesh);
		}

		for (auto& intObj : interactiveObjects) {
			if (intObj.isActive && intObj.texture) {
				renderer.PushMatrix();
				renderer.TranslateMatrix(intObj.position);
				renderer.DrawVertexRenderStruct(intObj.mesh);
				renderer.PopMatrix();
			}
		}

		// Draw characters (they handle their own skinning shader switch internally)
		// Draw player shadow depth only if not in first-person (so depth won't be duplicated)
		if (player && !firstPersonMode) player->drawShadowDepth(renderer);

		if (girlfriend && !girlfriendInCar)
		{
			girlfriend->drawShadowDepth(renderer);
		}

		for (auto& npc : npcs) npc->drawShadowDepth(renderer);

		renderer.PopMatrix();  // light view
		renderer.PopProjectionMatrix();
		renderer.shaderManager.PopShader();

		//glCullFace(GL_BACK);
		glDisable(GL_CULL_FACE);

		shadowMap->unbind();
	}

	void Location::drawGameWithShadows()
	{
		glClearColor(0.0f, 0.0f, 0.0f, 1.0f);
		glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);

#ifdef DEBUG_LIGHT
		// Debug mode: render from the light's point of view using the plain
		// textured shader so we can see what the shadow map "sees".
		renderer.shaderManager.PushShader(defaultShaderName);
		renderer.RenderUniform1i(textureUniformName, 0);

		renderer.PushProjectionMatrix(
			-40.0f, 40.0f,
			-40.0f, 40.0f,
			0.1f, 1000.0f);

		renderer.PushSpecialMatrix(shadowMap->getLightViewMatrix());

#else
		static const std::string shadowShaderName = "default_shadow";

		renderer.shaderManager.PushShader(shadowShaderName);
		renderer.RenderUniform1i(textureUniformName, 0);
		renderer.RenderUniform1i("uShadowMap", 1);

		// Bind shadow map texture to unit 1
		glActiveTexture(GL_TEXTURE1);
		glBindTexture(GL_TEXTURE_2D, shadowMap->getDepthTexture());
		glActiveTexture(GL_TEXTURE0);

		renderer.PushPerspectiveProjectionMatrix(1.0 / 1.5,
			static_cast<float>(Environment::width) / static_cast<float>(Environment::height),
			Environment::CONST_Z_NEAR, Environment::CONST_Z_FAR);
		renderer.PushMatrix();

		/*renderer.LoadIdentity();
		renderer.TranslateMatrix({ 0,0, -1.0f * Environment::zoom });

		renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(cameraInclination, Eigen::Vector3f::UnitX())).toRotationMatrix());
		renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(cameraAzimuth, Eigen::Vector3f::UnitY())).toRotationMatrix());
		const Eigen::Vector3f& camTarget = player ? player->position : Eigen::Vector3f::Zero();
		renderer.TranslateMatrix({ -camTarget.x(), -camTarget.y(), -camTarget.z() });
		*/

		if (firstPersonMode && !inCar && player) {
			const float eyeHeight = 1.6f;
			Eigen::Vector3f eye = player->position + Eigen::Vector3f(0.0f, eyeHeight, 0.0f);
			float yaw = cameraAzimuth;
			float pitch = cameraInclination;
			Eigen::Vector3f forward(std::sin(yaw), 0.0f, -std::cos(yaw));
			Eigen::Vector3f direction = Eigen::Quaternionf(Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitX())) * forward;
			direction.normalize();
			Eigen::Vector3f target = eye + direction;
			Eigen::Vector3f up(0.0f, 1.0f, 0.0f);
			Eigen::Matrix4f view = lookAt(eye, target, up);
			renderer.PushSpecialMatrix(view);   // (2)
		}
		else {
			// стандартная камера третьего лица
			renderer.LoadIdentity();
			renderer.TranslateMatrix({ 0, 0, -1.0f * Environment::zoom });
			renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(cameraInclination, Eigen::Vector3f::UnitX())).toRotationMatrix());
			renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(cameraAzimuth, Eigen::Vector3f::UnitY())).toRotationMatrix());
			Eigen::Vector3f camTarget = inCar ? carPosition : (player ? player->position : Eigen::Vector3f::Zero());
			renderer.TranslateMatrix({ -camTarget.x(), -camTarget.y(), -camTarget.z() });
			const float cameraYOffset = firstPersonMode ? -0.2f : -1.3f;
			renderer.TranslateMatrix({ 0, cameraYOffset, 0 });
		}

		// Capture the camera view matrix and compute uLightFromCamera
		cameraViewMatrix = renderer.GetCurrentModelViewMatrix();
		Eigen::Matrix4f cameraViewInverse = cameraViewMatrix.inverse();
		Eigen::Matrix4f lightFromCamera = shadowMap->getLightSpaceMatrix() * cameraViewInverse;
		renderer.RenderUniformMatrix4fv("uLightFromCamera", false, lightFromCamera.data());

		// Light direction in camera space for diffuse lighting
		Eigen::Vector3f lightDirCamera = cameraViewMatrix.block<3, 3>(0, 0) * shadowMap->getLightDirection();
		renderer.RenderUniform3fv("uLightDir", lightDirCamera.data());
#endif

		CheckGlError(__FILE__, __LINE__);
		glBindTexture(GL_TEXTURE_2D, roomTexture->getTexID());
		renderer.DrawVertexRenderStruct(roomMesh);

		CheckGlError(__FILE__, __LINE__);
		for (auto& [name, gameObj] : gameObjects) {
			if (name == roofObjectKey && !roofVisible) {
				continue;
			}
			if (name == azsRoofObjectKey && !azsRoofVisible) {
				continue;
			}
			glBindTexture(GL_TEXTURE_2D, gameObj.texture->getTexID());
			renderer.DrawVertexRenderStruct(gameObj.mesh);
		}

		CheckGlError(__FILE__, __LINE__);
		for (auto& intObj : interactiveObjects) {
			if (intObj.isActive) {
				intObj.draw(renderer);
			}
		}
		CheckGlError(__FILE__, __LINE__);

#ifdef DEBUG_LIGHT
		// In debug-light mode characters use the plain shaders (draw normally
		// but from the light's viewpoint — projection/view already on stack).
		if (player) player->draw(renderer);
		for (auto& npc : npcs) npc->draw(renderer);
#else
		// Characters use their own shadow-aware shaders
		CheckGlError(__FILE__, __LINE__);

		// don't draw player with shadow shader if in first-person mode
		if (player && !firstPersonMode) player->drawWithShadow(renderer, lightFromCamera, shadowMap->getDepthTexture(), lightDirCamera);
		CheckGlError(__FILE__, __LINE__);

		for (auto& npc : npcs) npc->drawWithShadow(renderer, lightFromCamera, shadowMap->getDepthTexture(), lightDirCamera);
#endif
		CheckGlError(__FILE__, __LINE__);

		drawDebugNavigation();
		drawDebugForbidden();

	//	renderer.PopMatrix();
	//	renderer.PopProjectionMatrix();
	//	renderer.shaderManager.PopShader();

		// Восстанавливаем стек
		if (firstPersonMode && !inCar && player) {
			renderer.PopMatrix();   // убираем (2)
		}
		renderer.PopMatrix();       // убираем (1)

		renderer.PopProjectionMatrix();
		renderer.shaderManager.PopShader();
	}

	bool Location::setNavigationAreaAvailable(const std::string& areaName, bool available)
	{
		return navigation.setAreaAvailable(areaName, available);
	}

	bool Location::isPointInCarFootprint(const Eigen::Vector3f& point, const Eigen::Vector3f& center, float rotation) const
	{
		constexpr float carLength = 7.3f;
		constexpr float carWidth = 2.8f;
		const Eigen::Vector3f forward(-std::sin(rotation), 0.f, -std::cos(rotation));
		const Eigen::Vector3f right(std::cos(rotation), 0.f, -std::sin(rotation));
		Eigen::Vector3f delta = point - center;
		delta.y() = 0.f;
		const float alongForward = delta.dot(forward);
		const float alongRight = delta.dot(right);
		return std::abs(alongForward) <= carLength * 0.5f && std::abs(alongRight) <= carWidth * 0.5f;
	}

	void Location::pushOutOfNpcCarFootprint(Eigen::Vector3f& position) const
	{
		if (!npcCar.texture) return;

		constexpr float carLength = 7.3f;
		constexpr float carWidth = 2.8f;
		constexpr float halfLength = carLength * 0.5f;
		constexpr float halfWidth = carWidth * 0.5f;
		constexpr float skin = 0.05f;

		const Eigen::Vector3f forward(-std::sin(npcCar.rotation), 0.f, -std::cos(npcCar.rotation));
		const Eigen::Vector3f right(std::cos(npcCar.rotation), 0.f, -std::sin(npcCar.rotation));

		Eigen::Vector3f delta = position - npcCar.position;
		delta.y() = 0.f;
		float alongForward = delta.dot(forward);
		float alongRight = delta.dot(right);

		if (std::abs(alongForward) >= halfLength || std::abs(alongRight) >= halfWidth) {
			return;
		}

		const float penForward = halfLength - std::abs(alongForward);
		const float penRight = halfWidth - std::abs(alongRight);

		if (penRight <= penForward) {
			const float sign = alongRight >= 0.f ? 1.f : -1.f;
			alongRight = sign * (halfWidth + skin);
		} else {
			const float sign = alongForward >= 0.f ? 1.f : -1.f;
			alongForward = sign * (halfLength + skin);
		}

		const float originalY = position.y();
		Eigen::Vector3f resolved = npcCar.position + forward * alongForward + right * alongRight;
		resolved.y() = originalY;
		position = resolved;
	}

	bool Location::doesPlayerCarCollideWithNpcCar(const Eigen::Vector3f& center, float rotation) const
	{
		if (!npcCar.texture) return false;

		constexpr float carLength = 7.3f;
		constexpr float carWidth = 2.8f;
		constexpr float sampleSpacing = 0.5f;

		const Eigen::Vector3f delta = center - npcCar.position;
		const float approxRadius = std::sqrt(carLength * carLength + carWidth * carWidth);
		if (delta.squaredNorm() > approxRadius * approxRadius) {
			return false;
		}

		const float halfLength = carLength * 0.5f;
		const float halfWidth = carWidth * 0.5f;
		const int longSteps = max(1, static_cast<int>(std::ceil(carLength / sampleSpacing)));
		const int widthSteps = max(1, static_cast<int>(std::ceil(carWidth / sampleSpacing)));

		const Eigen::Vector3f forward(-std::sin(rotation), 0.f, -std::cos(rotation));
		const Eigen::Vector3f right(std::cos(rotation), 0.f, -std::sin(rotation));

		for (int i = 0; i <= longSteps; ++i) {
			const float fl = (static_cast<float>(i) / static_cast<float>(longSteps)) * 2.0f - 1.0f;
			const Eigen::Vector3f lineOffset = forward * (fl * halfLength);
			for (int j = 0; j <= widthSteps; ++j) {
				const float fw = (static_cast<float>(j) / static_cast<float>(widthSteps)) * 2.0f - 1.0f;
				const Eigen::Vector3f sample = center + lineOffset + right * (fw * halfWidth);
				if (isPointInCarFootprint(sample, npcCar.position, npcCar.rotation)) {
					return true;
				}
			}
		}
		return false;
	}

	bool Location::isCarFootprintWalkable(const Eigen::Vector3f& center, float rotation) const
	{
		if (!navigation.isReady()) return true;

		constexpr float carLength = 7.3f;
		constexpr float carWidth = 2.8f;
		constexpr float sampleSpacing = 0.35f;

		const float halfLength = carLength * 0.5f;
		const float halfWidth = carWidth * 0.5f;
		const int longSteps = max(1, static_cast<int>(std::ceil(carLength / sampleSpacing)));
		const int widthSteps = max(1, static_cast<int>(std::ceil(carWidth / sampleSpacing)));

		const Eigen::Vector3f forward(-std::sin(rotation), 0.f, -std::cos(rotation));
		const Eigen::Vector3f right(std::cos(rotation), 0.f, -std::sin(rotation));

		for (int i = 0; i <= longSteps; ++i) {
			const float fl = (static_cast<float>(i) / static_cast<float>(longSteps)) * 2.0f - 1.0f;
			const Eigen::Vector3f lineOffset = forward * (fl * halfLength);
			for (int j = 0; j <= widthSteps; ++j) {
				const float fw = (static_cast<float>(j) / static_cast<float>(widthSteps)) * 2.0f - 1.0f;
				const Eigen::Vector3f sample = center + lineOffset + right * (fw * halfWidth);
				if (!navigation.isWalkable(sample)) {
					return false;
				}
			}
		}
		return true;
	}

	void Location::update(int64_t delta)
	{
		const Eigen::Vector3f playerPosBefore = player ? player->position : Eigen::Vector3f::Zero();

		if (player) {
			if (!inCar) {
				player->targetFacingAngle = cameraAzimuth;
				if (playerFrozen) {
					player->clearPath();
					wasKeyForward = false;
				} else if (keyForward) {
					player->attackTarget = nullptr;
					Eigen::Vector3f forward(std::sin(cameraAzimuth), 0.f, -std::cos(cameraAzimuth));
					player->setDirectWalkTarget(player->position + forward * 5.0f);
					wasKeyForward = true;
				} else {
					if (wasKeyForward) {
						player->clearPath();
					}
					wasKeyForward = false;
				}
			}
			player->update(delta);
		    dialogueSystem.update(static_cast<int>(delta), player->position);
			
			if (!roofObjectKey.empty()) {
				bool insideAnyZone = false;
				const Eigen::Vector2f playerPos2d(player->position.x(), player->position.z());
				for (const auto& zone : roofHideZones) {
					if (zone.contains(playerPos2d)) {
						insideAnyZone = true;
						break;
					}
				}
				const float maxHeightForHide = 3.0f;
				roofVisible = !(insideAnyZone && player->position.y() < maxHeightForHide);
			}

			if (!azsRoofObjectKey.empty()) {
				bool insideAnyZone = false;
				const Eigen::Vector2f playerPos2d(player->position.x(), player->position.z());
				for (const auto& zone : azsRoofHideZones) {
					if (zone.contains(playerPos2d)) {
						insideAnyZone = true;
						break;
					}
				}
				const float maxHeightForHide = 3.0f;
				azsRoofVisible = !(insideAnyZone && player->position.y() < maxHeightForHide);
			}

			// --- first-person building zone detection ---
			{
				const Eigen::Vector2f p2d(player->position.x(), player->position.z());
				const bool insideBuilding = buildingFirstPersonZone.contains(p2d);
				if (insideBuilding && !firstPersonMode) {
					// Enter first-person
					firstPersonMode = true;
					savedCameraAzimuth = cameraAzimuth;
					savedCameraInclination = cameraInclination;
					// Align camera to player's facing so feel like first-person
					cameraAzimuth = player->targetFacingAngle;
					// a moderate inclination (slightly downwards)
					cameraInclination = M_PI * 20.f / 180.f;
					std::cout << "[FIRSTPERSON] Entered building zone, switching to first-person view" << std::endl;
				} else if (!insideBuilding && firstPersonMode) {
					// Exit first-person: restore camera
					firstPersonMode = false;
					cameraAzimuth = savedCameraAzimuth;
					cameraInclination = savedCameraInclination;
					std::cout << "[FIRSTPERSON] Exited building zone, restoring camera and third-person view" << std::endl;
				}
			}
			// --- end first-person detection ---
		}

		if (girlfriend)
		{
			updateGirlfriendFollow();
			girlfriend->update(delta);
			if (!girlfriendInCar) {
				pushOutOfNpcCarFootprint(girlfriend->position);
			}
		}

		if (salesperson) pushOutOfNpcCarFootprint(salesperson->position);
		if (police) {
			police->update(delta);
			pushOutOfNpcCarFootprint(police->position);
		}
		if (bandit) pushOutOfNpcCarFootprint(bandit->position);

		for (auto& npc : npcs) {
			npc->update(delta);
			pushOutOfNpcCarFootprint(npc->position);
		}

		if (inCar) {
			float dt = static_cast<float>(delta) / 1000.0f;

			constexpr float roadHalfWidth = 10.0f;
			constexpr float offRoadFrictionMultiplier = 4.0f;
			const bool offRoad = std::abs(carPosition.x()) > roadHalfWidth;

			if (!offRoad && !carOutOfGas) {
				if (keyForward) {
					carVelocity += carAcceleration * dt;
				}
				if (keyBackward) {
					carVelocity -= carAcceleration * dt;
				}
			}

			const bool applyThrottleFriction = offRoad || (!keyForward && !keyBackward);
			if (applyThrottleFriction) {
				const float frictionRate = offRoad ? (carFriction * offRoadFrictionMultiplier) : carFriction;
				const float resistance = frictionRate * dt;
				if (carVelocity > 0.f) {
					carVelocity = max(0.f, carVelocity - resistance);
				} else if (carVelocity < 0.f) {
					carVelocity = min(0.f, carVelocity + resistance);
				}
			}

			carVelocity = max(-carMaxReverseSpeed, min(carMaxSpeed, carVelocity));

			const float oldRotation = carRotation;
			if (std::abs(carVelocity) > 0.01f) {
				float speedFactor = carVelocity / carMaxSpeed;
				if (keyLeft)  carRotation += carTurnRate * dt * speedFactor;
				if (keyRight) carRotation -= carTurnRate * dt * speedFactor;
			}
			if (navigation.isReady() && !isCarFootprintWalkable(carPosition, carRotation)) {
				carRotation = oldRotation;
			}
			if (doesPlayerCarCollideWithNpcCar(carPosition, carRotation)) {
				carRotation = oldRotation;
			}

			float targetSteer = (keyLeft ? 1.f : 0.f) - (keyRight ? 1.f : 0.f);
			targetSteer *= carMaxSteerAngle;
			float steerLerp = min(1.f, dt * 8.f);
			carSteeringAngle += (targetSteer - carSteeringAngle) * steerLerp;

			const Eigen::Vector3f forward(-std::sin(carRotation), 0.f, -std::cos(carRotation));
			const Eigen::Vector3f totalDelta = forward * carVelocity * dt;
			const float totalDist = totalDelta.norm();
			const float maxSubstep = 0.2f;
			const int substeps = max(1, static_cast<int>(std::ceil(totalDist / maxSubstep)));
			const Eigen::Vector3f stepDelta = totalDelta / static_cast<float>(substeps);
			for (int s = 0; s < substeps; ++s) {
				Eigen::Vector3f candidate = carPosition + stepDelta;
				if (navigation.isReady() && !isCarFootprintWalkable(candidate, carRotation)) {
					carVelocity = 0.f;
					break;
				}
				if (doesPlayerCarCollideWithNpcCar(candidate, carRotation)) {
					carVelocity = 0.f;
					if (!dialoguePlayedCrash && !dialogueSystem.isActive()) {
						if (dialogueSystem.startDialogue("driving_dialogue_crash")) {
							dialoguePlayedCrash = true;
						}
					}
					break;
				}
				carPosition = candidate;
			}

			if (offRoad && std::abs(carVelocity) < 0.1f && (keyForward || keyBackward)) {
				if (!dialoguePlayedOffroad && !dialogueSystem.isActive()) {
					if (dialogueSystem.startDialogue("driving_dialogue_offroad")) {
						dialoguePlayedOffroad = true;
					}
				}
			}

			if (dialoguePlayedCrash && carVelocity > 3.0)
			{
				dialoguePlayedCrash = false;
			}

			if (player) {
				player->position = carPosition;
			}

			if (locationId == "forest")
			{
				if (girlfriendInCar)
				{
					if (!dialoguePlayedDrivingGas1 && !dialogueSystem.isActive()) {
						if (dialogueSystem.startDialogue("driving_dialogue_gas1")) {
							dialoguePlayedDrivingGas1 = true;
						}
					}
				}
				// Ran out of gas: far south without having heard the girlfriend's gas
			// reminder. Latches the stall, then plays the out-of-gas line once.
				if (!carOutOfGas && !dialoguePlayedGas1 && player->position.z() < -300.f) {
					carOutOfGas = true;
				}
				if (carOutOfGas && !dialoguePlayedDrivingGasOut && !dialogueSystem.isActive()) {
					if (dialogueSystem.startDialogue("driving_dialogue_gas_out")) {
						dialoguePlayedDrivingGasOut = true;
					}
				}

				if (!policeFollow &&
					player->position.z() - npcCar.position.z() > -10.f
					&& player->position.z() < npcCar.position.z()
					&& npcCar.position.x() < 900
					&& policeEncounterStage != PoliceEncounterStage::Done)
				{
					policeFollow = true;
					npcCar.mode = NpcCar::Mode::FOLLOW_PLAYER;
					policeDrivingDialogueTimer = 8.0f;
				}

				// While police follows and player is driving, the cop yells every 8s.
				if (policeFollow && policeEncounterStage == PoliceEncounterStage::Idle) {
					policeDrivingDialogueTimer -= static_cast<float>(delta) / 1000.f;
					if (policeDrivingDialogueTimer <= 0.f && !dialogueSystem.isActive()) {
						if (dialogueSystem.startDialogue("driving_dialogue1")) {
							policeDrivingDialogueTimer = 8.0f;
						}
					}
				}
			}
			

			
		}

		if (player && !inCar) {
			auto blocked = [&](const Eigen::Vector3f& p) {
				if (navigation.isReady() && !navigation.isWalkable(p)) return true;
				if (isPointInCarFootprint(p, carPosition, carRotation)) return true;
				if (isPointInCarFootprint(p, npcCar.position, npcCar.rotation)) return true;
				return false;
			};
			if (blocked(player->position)) {
				const Eigen::Vector3f slideX(player->position.x(), playerPosBefore.y(), playerPosBefore.z());
				const Eigen::Vector3f slideZ(playerPosBefore.x(), playerPosBefore.y(), player->position.z());
				Eigen::Vector3f resolved;
				if (!blocked(slideX)) {
					resolved = slideX;
				} else if (!blocked(slideZ)) {
					resolved = slideZ;
				} else {
					resolved = playerPosBefore;
				}
				player->position = resolved;
			}
		}

		updateNpcCar(delta);

		if (locationId == "forest")
		{
			// Track remaining driving distance toward -Z. Computed from this frame's
			// real movement (captured before the tile-shift below), so the teleport
			// never contributes to the count.
			if (player) {
				distanceRemaining += player->position.z() - playerPosBefore.z();
			}
			if (!dialoguePlayedDistance7000 && distanceRemaining < 7000.f / 5.f && !dialogueSystem.isActive()) {
				if (dialogueSystem.startDialogue("driving_dialogue_distance7000")) {
					dialoguePlayedDistance7000 = true;
				}
			}
			if (!dialoguePlayedDistance5000 && distanceRemaining < 5000.f / 5.f && !dialogueSystem.isActive()) {
				if (dialogueSystem.startDialogue("driving_dialogue_distance5000")) {
					dialoguePlayedDistance5000 = true;
				}
			}
			if (!dialoguePlayedDistance2000 && distanceRemaining < 2000.f / 5.f && !dialogueSystem.isActive()) {
				if (dialogueSystem.startDialogue("driving_dialogue_distance2000")) {
					dialoguePlayedDistance2000 = true;
				}
			}
			if (!dialoguePlayedDistance0 && distanceRemaining < 0.f && !dialogueSystem.isActive()) {
				if (dialogueSystem.startDialogue("driving_dialogue_distance0")) {
					dialoguePlayedDistance0 = true;
				}
			}
		}


		{
			constexpr float tileSize = 100.0f;
			constexpr float teleportThreshold = tileSize * 3.5f;

			const Eigen::Vector3f anchor = inCar
				? carPosition
				: (player ? player->position : Eigen::Vector3f::Zero());

			Eigen::Vector3f shift = Eigen::Vector3f::Zero();
			if (anchor.x() > teleportThreshold) shift.x() = -tileSize;
			else if (anchor.x() < -teleportThreshold) shift.x() = tileSize;
			if (anchor.z() > teleportThreshold) shift.z() = -tileSize;
			else if (anchor.z() < -teleportThreshold) shift.z() = tileSize;

			if (shift.squaredNorm() > 0.f) {
				if (inCar) {
					carPosition += shift;
					if (player) player->position = carPosition;
				}
				else if (player) {
					player->position += shift;
					player->setTarget(player->position);
					player->clearPath();
				}
				if (npcCar.mode == NpcCar::Mode::FOLLOW_PLAYER || npcCar.mode == NpcCar::Mode::NONE_STAY) {
					npcCar.position += shift;
				}
			}
		}

		if (locationId == "forest")
		{
			// After the gas-station sale, once the player has walked away from the
			// salesperson, the girlfriend chimes in — fires once.
			if (!dialoguePlayedGirlfriend1 && dialoguePlayedGas1 && salesperson && player && !dialogueSystem.isActive()) {
				constexpr float minDistance = 8.0f;
				const float dx = player->position.x() - salesperson->position.x();
				const float dz = player->position.z() - salesperson->position.z();
				if (std::hypot(dx, dz) > minDistance) {
					if (dialogueSystem.startDialogue("dialogue_girlfriend1")) {
						dialoguePlayedGirlfriend1 = true;
					}
				}
			}

			// Police encounter: once the player gets out of the car while being
			// followed, the officer walks over from the NPC car, delivers his line,
			// then walks back and despawns. Happens exactly once.
			if (player && policeFollow && !inCar && policeEncounterStage == PoliceEncounterStage::Idle && !dialogueSystem.isActive() && police) {
				Eigen::Vector3f toCar = npcCar.position - player->position;
				toCar.y() = 0.f;
				const float toCarDist = toCar.norm();

				// Spawn right next to the NPC car, on its left side.
				const Eigen::Vector3f carLeft(-std::cos(npcCar.rotation), 0.f, std::sin(npcCar.rotation));
				constexpr float carHalfWidth = 2.8f * 0.5f;
				police->position = npcCar.position + carLeft * (carHalfWidth + 1.5f);
				police->position.y() = 0.f;
				police->clearPath();

				// Walk to a spot 1.5 m short of the player, approaching from the car's side.
				Eigen::Vector3f approachTarget = player->position;
				if (toCarDist > 1e-4f) {
					approachTarget = player->position + (toCar / toCarDist) * 1.5f;
				}
				approachTarget.y() = 0.f;
				police->setTarget(approachTarget);

				if (dialogueSystem.startDialogue("dialogue_police1")) {
					playerFrozen = true;
					policeEncounterStage = PoliceEncounterStage::Approaching;
				}
			}

			if (policeEncounterStage == PoliceEncounterStage::Approaching) {
				if (!dialogueSystem.isActive()) {
					// Dialogue finished — unfreeze player and send officer back to the car.
					playerFrozen = false;
					if (police) {
						police->setTarget(npcCar.position);
					}
					policeEncounterStage = PoliceEncounterStage::Returning;
				}
			}

			if (policeEncounterStage == PoliceEncounterStage::Returning) {
				if (police) {
					const float dx = police->position.x() - npcCar.position.x();
					const float dz = police->position.z() - npcCar.position.z();
					if (std::hypot(dx, dz) <= 8.0f) {
						police.reset();
					}
				}
				if (!police) {
					policeFollow = false;
					npcCar.mode = NpcCar::Mode::NONE_STAY;
					policeEncounterStage = PoliceEncounterStage::Done;
				}
			}

			// Phone rings once the player drives far from the gas station — fires once,
			// and only after the gas-station sale.
			if (inCar && dialoguePlayedGas1 && !dialoguePlayedPhone1 && !dialogueSystem.isActive()) {
				constexpr float minDistance = 50.0f;
				const Eigen::Vector3f gasStationPos(-3.f, 0.f, -11.f);
				const float dx = carPosition.x() - gasStationPos.x();
				const float dz = carPosition.z() - gasStationPos.z();
				if (std::hypot(dx, dz) > minDistance) {
					if (dialogueSystem.startDialogue("dialogue_phone1", [this]() {
						dialoguePhone1Finished = true;
						})) {
						dialoguePlayedPhone1 = true;
					}
				}
			}

			// After the phone dialogue, once the player's car enters the ambush zone,
			// spawn the NPC (police) car at its staged position. Fires once.
			if (dialoguePhone1Finished && !npcCarSpawnedAfterPhone) {
				if (carPosition.z() > -350.f && carPosition.z() < -340.f) {
					npcCar.position = Eigen::Vector3f(9.f, 0.f, -325.f);
					npcCarSpawnedAfterPhone = true;
				}
			}
		}
	}

	void Location::updateNpcCar(int64_t deltaMs)
	{
		const float dt = static_cast<float>(deltaMs) / 1000.0f;

		Eigen::Vector3f target = npcCar.position;
		float throttle = 0.f;
		bool hasTarget = false;

		if (npcCar.mode == NpcCar::Mode::FOLLOW_WAYPOINTS) {
			if (!npcCar.waypoints.empty()) {
				if (npcCar.currentWaypoint >= npcCar.waypoints.size()) {
					npcCar.currentWaypoint = 0;
				}
				Eigen::Vector3f toTarget = npcCar.waypoints[npcCar.currentWaypoint] - npcCar.position;
				toTarget.y() = 0.f;
				if (toTarget.norm() < npcCar.waypointReachRadius) {
					npcCar.currentWaypoint = (npcCar.currentWaypoint + 1) % npcCar.waypoints.size();
				}
				target = npcCar.waypoints[npcCar.currentWaypoint];
				throttle = 1.0f;
				hasTarget = true;
			}
		}
		else if (npcCar.mode == NpcCar::Mode::FOLLOW_PLAYER) {
			
			//if (x > 0.5)
			{
				target = carPosition;
				Eigen::Vector3f toTarget = target - npcCar.position;
				toTarget.y() = 0.f;
				const float dist = toTarget.norm();

				const float targetDist = npcCar.followMinDistance;
				const float coastBuffer = 1.5f;

				if (dist > npcCar.followMaxDistance) {
					throttle = 1.0f;
				}
				else if (dist > targetDist + coastBuffer) {
					throttle = 0.3f;
				}
				else if (dist > targetDist - coastBuffer) {
					throttle = 0.f;
				}
				else {
					const float underBy = (targetDist - coastBuffer) - dist;
					const float brakeRamp = min(1.0f, underBy / 3.0f);
					throttle = -(1.0f + 1.5f * brakeRamp);
				}
				hasTarget = dist > 0.5f;
			}
		}
		else
		{
			throttle = 0.f;
		}

		float targetHeading = npcCar.rotation;
		if (hasTarget) {
			Eigen::Vector3f toTarget = target - npcCar.position;
			toTarget.y() = 0.f;
			if (toTarget.squaredNorm() > 1e-4f) {
				targetHeading = std::atan2(-toTarget.x(), -toTarget.z());
			}
		}

		float angleDiff = targetHeading - npcCar.rotation;
		while (angleDiff > M_PI) angleDiff -= 2.0f * static_cast<float>(M_PI);
		while (angleDiff < -M_PI) angleDiff += 2.0f * static_cast<float>(M_PI);

		const float turnIntent = max(-1.0f, min(1.0f, angleDiff * 2.0f));
		const float targetSteer = turnIntent * npcCar.maxSteerAngle;
		const float steerLerp = min(1.f, dt * 8.f);
		npcCar.steeringAngle += (targetSteer - npcCar.steeringAngle) * steerLerp;

		if (throttle > 0.f) {
			npcCar.velocity += npcCar.acceleration * dt * throttle;
		}
		else if (throttle < 0.f) {
			const float brakeForce = npcCar.acceleration * dt * (-throttle);
			if (npcCar.velocity > 0.f) {
				npcCar.velocity = max(0.f, npcCar.velocity - brakeForce);
			}
			else {
				npcCar.velocity = min(0.f, npcCar.velocity + brakeForce);
			}
		}
		else {
			const float resistance = npcCar.friction * dt;
			if (npcCar.velocity > 0.f) {
				npcCar.velocity = max(0.f, npcCar.velocity - resistance);
			}
			else if (npcCar.velocity < 0.f) {
				npcCar.velocity = min(0.f, npcCar.velocity + resistance);
			}
		}
		npcCar.velocity = max(-npcCar.maxReverseSpeed, min(npcCar.maxSpeed, npcCar.velocity));

		const float oldRotation = npcCar.rotation;
		if (std::abs(npcCar.velocity) > 0.01f) {
			const float speedFactor = npcCar.velocity / npcCar.maxSpeed;
			npcCar.rotation += turnIntent * npcCar.turnRate * dt * speedFactor;
		}
		if (navigation.isReady() && !isCarFootprintWalkable(npcCar.position, npcCar.rotation)) {
			npcCar.rotation = oldRotation;
		}

		const Eigen::Vector3f forward(-std::sin(npcCar.rotation), 0.f, -std::cos(npcCar.rotation));
		const Eigen::Vector3f totalDelta = forward * npcCar.velocity * dt;
		const float totalDist = totalDelta.norm();
		const float maxSubstep = 0.2f;
		const int substeps = max(1, static_cast<int>(std::ceil(totalDist / maxSubstep)));
		const Eigen::Vector3f stepDelta = totalDelta / static_cast<float>(substeps);
		for (int s = 0; s < substeps; ++s) {
			Eigen::Vector3f candidate = npcCar.position + stepDelta;
			if (navigation.isReady() && !isCarFootprintWalkable(candidate, npcCar.rotation)) {
				npcCar.velocity = 0.f;
				break;
			}
			npcCar.position = candidate;
		}
	}

	void Location::updateGirlfriendFollow()
	{
		if (!girlfriend || !player) return;

		constexpr float retargetThreshold = 0.1f;
		constexpr float stopDistance = 1.0f;
		constexpr float carEnterRadius = 3.0f;

		// Player just exited the car while girlfriend was inside — she steps out
		// right next to him, as if coming out of the rear door (same lateral side,
		// offset back along the car's length).
		if (girlfriendInCar && !inCar) {
			const Eigen::Vector3f forward(-std::sin(carRotation), 0.f, -std::cos(carRotation));
			Eigen::Vector3f exitPos = player->position - forward * 1.5f;
			exitPos.y() = 0.f;
			girlfriend->position = exitPos;
			girlfriend->clearPath();
			girlfriendInCar = false;
			girlfriendLastFollowTargetValid = false;
		}

		// While she is riding, her position is glued to the car and no pathing needed.
		if (girlfriendInCar) {
			girlfriend->position = carPosition;
			girlfriend->position.y() = 0.f;
			girlfriend->clearPath();
			return;
		}

		// Player is in car and girlfriend has caught up — she jumps in.
		if (inCar) {
			Eigen::Vector3f diff = girlfriend->position - carPosition;
			diff.y() = 0.f;
			if (diff.norm() <= carEnterRadius) {
				girlfriendInCar = true;
				girlfriend->position = carPosition;
				girlfriend->position.y() = 0.f;
				girlfriend->clearPath();
				girlfriendLastFollowTargetValid = false;
				return;
			}
		}

		if (girlfriendRescued)
		{
			// Follow anchor: the car when player is driving, otherwise a point stopDistance
			// away from the player along the line to the girlfriend (so she stops near him).
			Eigen::Vector3f anchor;
			if (inCar) {
				anchor = carPosition;
			}
			else {
				Eigen::Vector3f toPlayer = player->position - girlfriend->position;
				toPlayer.y() = 0.f;
				const float distToPlayer = toPlayer.norm();
				if (distToPlayer <= stopDistance) {
					girlfriend->clearPath();
					girlfriendLastFollowTargetValid = false;
					return;
				}
				const Eigen::Vector3f fromPlayer = -toPlayer / distToPlayer;
				anchor = player->position + fromPlayer * stopDistance;
			}
			anchor.y() = 0.f;

			const bool shouldRetarget = !girlfriendLastFollowTargetValid ||
				(anchor - girlfriendLastFollowTarget).norm() > retargetThreshold;

			if (shouldRetarget) {
				girlfriend->setTarget(anchor);
				girlfriendLastFollowTarget = anchor;
				girlfriendLastFollowTargetValid = true;
			}

		}
	}

	void Location::drawNpcCar()
	{
		if (!npcCar.texture) return;

		renderer.PushMatrix();
		renderer.TranslateMatrix(npcCar.position);
		renderer.TranslateMatrix({ 0, 0.7f, 0 });
		renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(npcCar.rotation, Eigen::Vector3f::UnitY())).toRotationMatrix());
		glBindTexture(GL_TEXTURE_2D, npcCar.texture->getTexID());
		renderer.DrawVertexRenderStruct(carMesh);

		if (carWheelTexture) {
			constexpr float track_width = 1.28f;
			constexpr float wheel_base = 2.25f;
			constexpr float shift = 0.6f;
			const Eigen::Vector3f wheelPositions[4] = {
				Eigen::Vector3f( track_width, 0.f - 0.21f, -(wheel_base + shift) + 1.25f),
				Eigen::Vector3f(-track_width, 0.f - 0.21f, -(wheel_base + shift) + 1.25f),
				Eigen::Vector3f( track_width, 0.f - 0.21f,  (wheel_base - shift) + 1.1f),
				Eigen::Vector3f(-track_width, 0.f - 0.21f,  (wheel_base - shift) + 1.1f)
			};
			const bool isFront[4] = { true, true, false, false };

			glBindTexture(GL_TEXTURE_2D, carWheelTexture->getTexID());
			for (int i = 0; i < 4; ++i) {
				renderer.PushMatrix();
				renderer.TranslateMatrix(wheelPositions[i]);
				if (isFront[i]) {
					renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(npcCar.steeringAngle, Eigen::Vector3f::UnitY())).toRotationMatrix());
				}
				renderer.DrawVertexRenderStruct(carWheelMesh);
				renderer.PopMatrix();
			}
		}

		renderer.PopMatrix();
	}

	void Location::handleDown(int64_t fingerId, int eventX, int eventY, int mx, int my)
	{
		// Calculate ray for picking
		if (dialogueSystem.blocksGameplayInput()) {
			dialogueSystem.handlePointerReleased(static_cast<float>(mx), Environment::projectionHeight - static_cast<float>(my));
			return;
		}

		if (inCar) {
			return;
		}

		if (salesperson && player && !dialogueSystem.isActive()) {
			constexpr float maxTalkDistance = 4.0f;
			// cos(45°) — salesperson must lie within the ±45° half-cone in front
			// of the player (90° total field of view).
			constexpr float coneCosThreshold = 0.7071067f;


			Eigen::Vector3f toSales = salesperson->position - player->position;
			toSales.y() = 0.f;
			const float dist = toSales.norm();

			if (dist > 1e-4f && dist <= maxTalkDistance) {
				const Eigen::Vector3f playerForward(std::sin(cameraAzimuth), 0.f, -std::cos(cameraAzimuth));
				const Eigen::Vector3f toSalesDir = toSales / dist;

				if (playerForward.dot(toSalesDir) >= coneCosThreshold) {
					const Eigen::Vector3f gasStationPos(-3.f, 0.f, -11.f);
					const float carDistToStation = std::hypot(
						carPosition.x() - gasStationPos.x(),
						carPosition.z() - gasStationPos.z());

					std::string dialogueId;
					if (dialoguePlayedGas1) {
						dialogueId = "dialogue_gas3";
					} else if (carDistToStation <= 10.f) {
						dialogueId = "dialogue_gas1";
					} else {
						dialogueId = "dialogue_gas2";
					}

					if (dialogueSystem.startDialogue(dialogueId)) {
						// Face each other. cameraAzimuth also drives the player's
						// targetFacingAngle, so updating it rotates the player toward
						// the salesperson.
						cameraAzimuth = std::atan2(toSales.x(), -toSales.z());
						salesperson->targetFacingAngle = std::atan2(-toSales.x(), toSales.z());
						if (dialogueId == "dialogue_gas1") {
							dialoguePlayedGas1 = true;
						}
					}
				}
			}
		}
	}
	void Location::handleUp(int64_t fingerId, int mx, int my)
	{

	}
	void Location::handleMotion(int64_t fingerId, int dx, int dy, int mx, int my)
	{
		
		/*if (dialogueSystem.blocksGameplayInput()) {
			dialogueSystem.handlePointerMoved(
				static_cast<float>(mx),
				Environment::projectionHeight - static_cast<float>(my)
			);
			return;
		}*/

		const float sensitivity = 0.005f;
		const float inclinationSign = invertCameraY ? -1.0f : 1.0f;
		cameraAzimuth += dx * sensitivity;
		cameraInclination += inclinationSign * dy * sensitivity;

		const float minInclination = M_PI * 10.f / 180.f;
		const float maxInclination = M_PI * 0.5f;
		cameraInclination = max(minInclination, min(maxInclination, cameraInclination));
	}

	void Location::handleKeyDown(int sdlKey)
	{
		switch (sdlKey) {
		case SDLK_l:
		{
			if (!player) break;
			if (playerFrozen) break;
			if (!inCar) {
				const Eigen::Vector3f diff(
					carPosition.x() - player->position.x(), 0.f,
					carPosition.z() - player->position.z());
				const float distToCar = diff.norm();
				const float enterMaxDistance = 4.0f;
				if (distToCar <= enterMaxDistance) {
					inCar = true;
					carVelocity = 0.f;
				} else {
					std::cout << "[CAR] Too far to enter: " << distToCar << std::endl;
				}
			} else {
				const Eigen::Vector3f left(-std::cos(carRotation), 0.f, std::sin(carRotation));
				const float halfWidth = 2.8f * 0.5f;
				const float exitBuffers[] = { 1.0f, 1.5f, 2.0f, 3.0f };
				Eigen::Vector3f exitPos = carPosition;
				bool foundExit = false;
				for (float buffer : exitBuffers) {
					Eigen::Vector3f candidate = carPosition + left * (halfWidth + buffer);
					candidate.y() = 0.f;
					if (!navigation.isReady() || navigation.isWalkable(candidate)) {
						exitPos = candidate;
						foundExit = true;
						break;
					}
				}
				if (foundExit) {
					player->position = exitPos;
					player->clearPath();
					inCar = false;
					carVelocity = 0.f;
					keyForward = false;
				} else {
					std::cout << "[CAR] No walkable exit spot" << std::endl;
				}
			}
			break;
		}
		case SDLK_w: keyForward = true; break;
		case SDLK_s: keyBackward = true; break;
		case SDLK_a: keyLeft = true; break;
		case SDLK_d: keyRight = true; break;
		case SDLK_i: invertCameraY = !invertCameraY; break;
		case SDLK_u:
			std::cout << player->position << std::endl;
			//girlfriend->setTarget(player->position);
			break;
		case SDLK_m:
			npcCar.mode = (npcCar.mode == NpcCar::Mode::FOLLOW_WAYPOINTS)
				? NpcCar::Mode::FOLLOW_PLAYER
				: NpcCar::Mode::FOLLOW_WAYPOINTS;
			std::cout << "[NPC_CAR] Mode: "
				<< (npcCar.mode == NpcCar::Mode::FOLLOW_WAYPOINTS ? "FOLLOW_WAYPOINTS" : "FOLLOW_PLAYER")
				<< std::endl;
			break;
		default: break;
		}
	}

	void Location::handleKeyUp(int sdlKey)
	{
		switch (sdlKey) {
		case SDLK_w: keyForward = false; break;
		case SDLK_s: keyBackward = false; break;
		case SDLK_a: keyLeft = false; break;
		case SDLK_d: keyRight = false; break;
		default: break;
		}
	}

	bool Location::requestDialogueStart(const std::string& dialogueId)
	{
		return dialogueSystem.startDialogue(dialogueId);
	}

	void Location::setDialogueFlag(const std::string& flag, int value)
	{
		dialogueSystem.setFlag(flag, value);
	}

	int Location::getDialogueFlag(const std::string& flag) const
	{
		return dialogueSystem.getFlag(flag);
	}

	void Location::cleanup()
	{
		std::cout << "[LOCATION] Starting cleanup..." << std::endl;

		npcs.clear();

		interactiveObjects.clear();

		gameObjects.clear();

		player.reset();

		roomTexture.reset();

		npcCar.texture.reset();

		//dialogueSystem.dialogueDatabase.clear();

		navigation = PathFinder();

		cameraAzimuth = 0.0f;
		cameraInclination = M_PI * 30.f / 180.f;

		targetInteractiveObject = nullptr;
		rightMouseDown = false;
		lastMouseX = 0;
		lastMouseY = 0;

		dialoguePlayedOffroad = false;
		dialoguePlayedCrash = false;

		std::cout << "[LOCATION] Cleanup complete" << std::endl;
	}

	void Location::unload()
	{
		std::cout << "[LOCATION] Starting unload..." << std::endl;
		cleanup();
		std::cout << "[LOCATION] Unload complete" << std::endl;
	}


} // namespace ZL