#include "stdafx.h"
#include "Behaviour.h"
#include "BehaviourTree.h"
#include <IExamInterface.h>
#include "Thinker.h"

#define TO_RAD(i) i * (M_PI / 180)

#define PRINT_FUNCTION_NAME() std::cout << __func__ << std::endl

static int randNumRange(int minRange, int maxRange) {
	std::random_device rd;
	std::mt19937 seed(rd());
	std::uniform_int_distribution<> range(minRange, maxRange);

	return range(seed);
}

namespace BT_Action
{
	BT::State SetTimer(Blackboard* blackboardPtr, const std::string& timerName, bool doOnce) {
		PRINT_FUNCTION_NAME();
		bool didOnce{};
		blackboardPtr->GetData("Timer" + timerName + "DoOnce", didOnce);

		if (doOnce && didOnce)
			return BT::State::Success;

		if (doOnce)
			blackboardPtr->ChangeData("Timer" + timerName + "DoOnce", true);

		blackboardPtr->ChangeData("Timer" + timerName, std::chrono::steady_clock::now());

		return BT::State::Success;
	}

	BT::State UnlockTimer(Blackboard* blackboardPtr, const std::string& timerName) {
		PRINT_FUNCTION_NAME();
		blackboardPtr->ChangeData("TimerLock" + timerName, false);

		return BT::State::Success;
	}

	BT::State LockTimer(Blackboard* blackboardPtr, const std::string& timerName) {
		PRINT_FUNCTION_NAME();
		blackboardPtr->ChangeData("TimerLock" + timerName, true);

		return BT::State::Success;
	}

	BT::State GoToTarget(Blackboard* blackboardPtr) {
		//PRINT_FUNCTION_NAME();
		IExamInterface* interfacePtr{};
		Elite::Vector2 target{};
		SteeringPlugin_Output steering{};

		std::chrono::steady_clock::time_point timer{};
		float maxTime{ 5.0f }; // Default value if not set
		bool doOnce{};

		blackboardPtr->GetData("Interface", interfacePtr);
		blackboardPtr->GetData("Target", target);
		blackboardPtr->GetData("Steering", steering);
		blackboardPtr->GetData("FailSafe", timer);
		blackboardPtr->GetData("MaxFailSafe", maxTime); // Make sure this is set before calling
		blackboardPtr->GetData("FailSafeDoOnce", doOnce);

		// Initialize timer if not done yet
		if (!doOnce) {
			timer = std::chrono::steady_clock::now();
			blackboardPtr->ChangeData("FailSafe", timer);
			blackboardPtr->ChangeData("FailSafeDoOnce", true);
		}

		const auto agentInfo = interfacePtr->Agent_GetInfo();
		const auto nextTargetPos = interfacePtr->NavMesh_GetClosestPathPoint(target);

		// Check success conditions first
		const std::chrono::steady_clock::time_point currentTime{ std::chrono::steady_clock::now() };
		const std::chrono::duration<float> elapsedSec{ currentTime - timer };

		// Check if we've reached the target
		if (Distance(target, agentInfo.Position) < 3.f) {
			blackboardPtr->ChangeData("FailSafeDoOnce", false);
			return BT::State::Success;
		}

		// Check if failsafe timeout has been reached
		if (elapsedSec.count() > maxTime) {
			blackboardPtr->ChangeData("FailSafeDoOnce", false);
			return BT::State::Success;
		}

		// Calculate steering if we're still running
		steering.LinearVelocity = nextTargetPos - agentInfo.Position;
		steering.LinearVelocity.Normalize();
		steering.LinearVelocity *= agentInfo.MaxLinearSpeed;
		blackboardPtr->ChangeData("Steering", steering);

		return BT::State::Running;
	}

	BT::State EnableSpin(Blackboard* blackboardPtr) {
		PRINT_FUNCTION_NAME();
		IExamInterface* interfacePtr{};
		SteeringPlugin_Output steering{};

		blackboardPtr->GetData("Interface", interfacePtr);
		blackboardPtr->GetData("Steering", steering);

		steering.AutoOrient = true;
		steering.AngularVelocity = interfacePtr->Agent_GetInfo().MaxAngularSpeed;

		blackboardPtr->ChangeData("Spin", true);
		blackboardPtr->ChangeData("Steering", steering);

		return BT::State::Success;
	}

	BT::State DisableSpin(Blackboard* blackboardPtr) {
		PRINT_FUNCTION_NAME();
		blackboardPtr->ChangeData("Spin", false);

		return BT::State::Success;
	}


	BT::State FindClosestEdge(Blackboard* blackboardPtr, int degree) {
		PRINT_FUNCTION_NAME();
		IExamInterface* interfacePtr{};
		blackboardPtr->GetData("Interface", interfacePtr);

		const Elite::Vector2 playerPos{ interfacePtr->Agent_GetInfo().Position };

		constexpr float offset{ 3.f };
		const Elite::Vector2 center{ interfacePtr->GetPurgeZonesInFOV()[0].Center };
		const float radius{ interfacePtr->GetPurgeZonesInFOV()[0].Radius + offset };

		float closestTarget{ FLT_MAX };
		Elite::Vector2 finalTarget{};

		constexpr int circleDegrees{ 360 };

		if (degree > circleDegrees) {
			degree = circleDegrees;
		}
		for (int i = 0; i <= circleDegrees; i += degree) {
			const Elite::Vector2 pointOnCircle{ center.x + radius * std::cosf(TO_RAD(i)), center.y + radius * std::sinf(TO_RAD(i)) };

			const float targetDistance{ playerPos.DistanceSquared(pointOnCircle) };

			if (closestTarget > targetDistance) {
				closestTarget = targetDistance;
				finalTarget = pointOnCircle;
			}
		}

		blackboardPtr->ChangeData("Target", finalTarget);

		return BT::State::Success;
	}

	BT::State SetZombieTarget(Blackboard* blackboardPtr) {
		PRINT_FUNCTION_NAME();
		IExamInterface* interfacePtr{};
		blackboardPtr->GetData("Interface", interfacePtr);

		if (interfacePtr->GetEnemiesInFOV().capacity() == 0) {
			return BT::State::Failure;
		}

		EnemyInfo zombieInfo{};
		float closestZombie{ FLT_MAX };

		for (const auto zombie : interfacePtr->GetEnemiesInFOV()) {
			const float distance{ interfacePtr->Agent_GetInfo().Position.DistanceSquared(zombie.Location) };

			if (closestZombie < distance)
				continue;

			closestZombie = distance;
			zombieInfo = zombie;
		}

		blackboardPtr->ChangeData("TargetZombie", zombieInfo);

		return BT::State::Success;
	}

	BT::State AvoidingZombie(Blackboard* blackboardPtr) {
		PRINT_FUNCTION_NAME();
		IExamInterface* interfacePtr{};
		SteeringPlugin_Output steering{};

		blackboardPtr->GetData("Interface", interfacePtr);
		blackboardPtr->GetData("Steering", steering);

		Elite::Vector2 evadeDirection{};

		for (auto zombie : interfacePtr->GetEnemiesInFOV()) {
			Elite::Vector2 currentPos{ interfacePtr->Agent_GetInfo().Position };
			Elite::Vector2 targetPos{ zombie.Location };
			Elite::Vector2 goingAwayVec{ currentPos - targetPos };
			float distance = goingAwayVec.MagnitudeSquared();

			evadeDirection += goingAwayVec / distance;
		}

		steering.LinearVelocity = evadeDirection.GetNormalized() * interfacePtr->Agent_GetInfo().MaxLinearSpeed;

		blackboardPtr->ChangeData("Steering", steering);

		return BT::State::Success;
	}

	BT::State RotateToZombie(Blackboard* blackboardPtr) {
		PRINT_FUNCTION_NAME();
		IExamInterface* interfacePtr{};
		EnemyInfo zombieInfo{};
		SteeringPlugin_Output steering{};

		blackboardPtr->GetData("Interface", interfacePtr);
		blackboardPtr->GetData("TargetZombie", zombieInfo);
		blackboardPtr->GetData("Steering", steering);

		const float maxAngularVelocity{ interfacePtr->Agent_GetInfo().MaxAngularSpeed };
		const float targetAngle{ VectorToOrientation((zombieInfo.Location - interfacePtr->Agent_GetInfo().Position).GetNormalized()) };
		const float angleDiff{ targetAngle - interfacePtr->Agent_GetInfo().Orientation };

		steering.AngularVelocity = angleDiff * maxAngularVelocity;

		blackboardPtr->ChangeData("Steering", steering);
		blackboardPtr->ChangeData("angleDiff", angleDiff);

		return BT::State::Success;
	}

	BT::State ReadyToShoot(Blackboard* blackboardPtr, float minAngleDiff) {
		PRINT_FUNCTION_NAME();
		float angleDiff{};

		blackboardPtr->GetData("angleDiff", angleDiff);

		return (std::abs(angleDiff) < minAngleDiff) ? BT::State::Success : BT::State::Failure;
	}

	BT::State Shoot(Blackboard* blackboardPtr, eItemType type) {
		PRINT_FUNCTION_NAME();
		IExamInterface* interfacePtr{};
		Thinker* thinkerPtr{};

		blackboardPtr->GetData("Interface", interfacePtr);
		blackboardPtr->GetData("Brain", thinkerPtr);

		const auto item = thinkerPtr->FindLeastValueItem(type);

		if (item->ItemInfo.Value <= 0)
			return BT::State::Failure;

		if (interfacePtr->Inventory_UseItem(item->invIndex)) {
			--item->ItemInfo.Value;
			return BT::State::Success;
		}

		return BT::State::Failure;
	}

	BT::State SetItemAsTarget(Blackboard* blackboardPtr) {
		PRINT_FUNCTION_NAME();
		IExamInterface* interfacePtr{};
		blackboardPtr->GetData("Interface", interfacePtr);

		if (interfacePtr->GetItemsInFOV().capacity() == 0) {
			return BT::State::Failure;
		}

		ItemInfo targetItem{};
		float closestItem{ FLT_MAX };

		for (const auto item : interfacePtr->GetItemsInFOV()) {
			const float distance{ interfacePtr->Agent_GetInfo().Position.DistanceSquared(item.Location) };

			if (closestItem < distance)
				continue;

			closestItem = distance;
			targetItem = item;
		}

		blackboardPtr->ChangeData("TargetItem", targetItem);
		blackboardPtr->ChangeData("Target", targetItem.Location);

		return BT::State::Success;
	}

	BT::State DestroyItemOnFloor(Blackboard* blackboardPtr) {
		PRINT_FUNCTION_NAME();
		IExamInterface* interfacePtr{};
		ItemInfo targetItem{};

		blackboardPtr->GetData("Interface", interfacePtr);
		blackboardPtr->GetData("TargetItem", targetItem);

		if (interfacePtr->DestroyItem(targetItem))
			return BT::State::Success;

		return BT::State::Failure;
	}

	BT::State PickUpItem(Blackboard* blackboardPtr) {
		PRINT_FUNCTION_NAME();
		IExamInterface* interfacePtr{};
		Thinker* thinkerPtr{};
		ItemInfo targetItem{};
		int freeSlot{};

		blackboardPtr->GetData("Interface", interfacePtr);
		blackboardPtr->GetData("Brain", thinkerPtr);
		blackboardPtr->GetData("TargetItem", targetItem);
		blackboardPtr->GetData("NextFreeSlot", freeSlot);

		if (interfacePtr->GrabItem(targetItem)) {
			interfacePtr->Inventory_AddItem(freeSlot, targetItem);

			blackboardPtr->ChangeData("NextFreeSlot", thinkerPtr->AddItemToMemory(targetItem));

			return BT::State::Success;
		}

		return BT::State::Failure;
	}

	BT::State SwapItem(Blackboard* blackboardPtr) {
		PRINT_FUNCTION_NAME();
		IExamInterface* interfacePtr{};
		Thinker* thinkerPtr{};
		ItemInfo targetItem{};

		blackboardPtr->GetData("Interface", interfacePtr);
		blackboardPtr->GetData("Brain", thinkerPtr);
		blackboardPtr->GetData("TargetItem", targetItem);

		if (interfacePtr->GrabItem(targetItem)) {
			const int slot = thinkerPtr->FindEmptyValue(targetItem);
			interfacePtr->Inventory_RemoveItem(slot);
			interfacePtr->Inventory_AddItem(slot, targetItem);

			return BT::State::Success;
		}

		return BT::State::Failure;
	}

	BT::State CheckItem(Blackboard* blackboardPtr) {
		PRINT_FUNCTION_NAME();
		IExamInterface* interfacePtr{};
		Thinker* thinkerPtr{};
		ItemInfo targetItem{};

		blackboardPtr->GetData("Interface", interfacePtr);
		blackboardPtr->GetData("Brain", thinkerPtr);
		blackboardPtr->GetData("TargetItem", targetItem);

		const int slotIndex{ thinkerPtr->CheckItem(targetItem) };

		if (slotIndex == interfacePtr->Inventory_GetCapacity() - 1) {
			if (targetItem.Type == eItemType::SHOTGUN || targetItem.Type == eItemType::PISTOL) {
				interfacePtr->DestroyItem(targetItem);
				return BT::State::Success;
			}

			if (interfacePtr->GrabItem(targetItem)) {
				interfacePtr->Inventory_AddItem(slotIndex, targetItem);
				interfacePtr->Inventory_UseItem(slotIndex);
				interfacePtr->Inventory_RemoveItem(slotIndex);

				return BT::State::Success;
			}
		}
		else {
			if (!(targetItem.Type == eItemType::SHOTGUN || targetItem.Type == eItemType::PISTOL)) {
				interfacePtr->Inventory_UseItem(slotIndex);
			}

			if (interfacePtr->GrabItem(targetItem)) {
				interfacePtr->Inventory_RemoveItem(slotIndex);
				interfacePtr->Inventory_AddItem(slotIndex, targetItem);
				return BT::State::Success;
			}
		}

		return BT::State::Failure;
	}

	BT::State UseItem(Blackboard* blackboardPtr, eItemType type) {
		PRINT_FUNCTION_NAME();
		IExamInterface* interfacePtr{};
		Thinker* thinkerPtr{};

		blackboardPtr->GetData("Interface", interfacePtr);
		blackboardPtr->GetData("Brain", thinkerPtr);

		const auto item = thinkerPtr->FindLeastValueItem(type);

		if (item->ItemInfo.Value <= 0)
			return BT::State::Failure;

		if (interfacePtr->Inventory_UseItem(item->invIndex)) {
			item->ItemInfo.Value = 0;
			return BT::State::Success;
		}

		return BT::State::Failure;
	}

	BT::State TryFindHouse(Blackboard* blackboardPtr, float searchRadius, int degree) {
		PRINT_FUNCTION_NAME();
		IExamInterface* interfacePtr{};
		Thinker* thinkerPtr{};

		blackboardPtr->GetData("Interface", interfacePtr);
		blackboardPtr->GetData("Brain", thinkerPtr);

		blackboardPtr->ChangeData("TargetHouse", HouseInfo{});

		const Elite::Vector2 playerPos{ interfacePtr->Agent_GetInfo().Position };

		float closestTarget{ FLT_MAX };
		Elite::Vector2 finalTarget{};

		constexpr int circleDegrees{ 360 };

		if (degree > circleDegrees) {
			degree = circleDegrees;
		}

		for (int i = 0; i <= circleDegrees; i += degree) {
			const Elite::Vector2 pointOnCircle{ playerPos.x + searchRadius * std::cosf(TO_RAD(i)), playerPos.y + searchRadius * std::sinf(TO_RAD(i)) };
			const Elite::Vector2 target = interfacePtr->NavMesh_GetClosestPathPoint(pointOnCircle);


			const float worldDimensions{ interfacePtr->World_GetInfo().Dimensions.x / 2 };
			if (std::abs(target.x) >= worldDimensions || std::abs(target.y) >= worldDimensions) {
				continue;
			}

			if (pointOnCircle != target) {
				constexpr float houseOffset{ 5.f };
				if (thinkerPtr->CheckIfTargetIsExplored(target, houseOffset))
					continue;

				const float targetDistance{ playerPos.DistanceSquared(target) };

				if (closestTarget > targetDistance) {
					closestTarget = targetDistance;
					finalTarget = target;
				}
			}
		}

		if (finalTarget == Elite::Vector2{}) {
			return BT::State::Failure;
		}

		blackboardPtr->ChangeData("Target", finalTarget);

		return BT::State::Success;
	}

	BT::State FindRandomLocation(Blackboard* blackboardPtr, float randomRadius) {
		PRINT_FUNCTION_NAME();
		IExamInterface* interfacePtr{};
		blackboardPtr->GetData("Interface", interfacePtr);

		const Elite::Vector2 playerPos{ interfacePtr->Agent_GetInfo().Position };

		const float randomAngle = Elite::ToRadians(randNumRange(0.0f, 360.0f));

		const Elite::Vector2 offset = Elite::Vector2{ cosf(randomAngle), sinf(randomAngle) } *randomRadius;

		const Elite::Vector2 target = playerPos + offset;

		blackboardPtr->ChangeData("Target", target);

		return BT::State::Success;
	}

	BT::State GetHouseAsTarget(Blackboard* blackboardPtr, float maxTravelDistance) {
		PRINT_FUNCTION_NAME();
		IExamInterface* interfacePtr{};
		Thinker* thinkerPtr{};

		blackboardPtr->GetData("Interface", interfacePtr);
		blackboardPtr->GetData("Brain", thinkerPtr);

		const HouseInfo targetHouse{ thinkerPtr->CheckHouseValidTarget(interfacePtr->Agent_GetInfo().Position, maxTravelDistance) };

		if (targetHouse.Size.x <= 0) {
			return BT::State::Failure;
		}

		blackboardPtr->ChangeData("Target", targetHouse.Center);
		blackboardPtr->ChangeData("TargetHouse", targetHouse);

		return BT::State::Success;
	}

	BT::State CheckHouses(Blackboard* blackboardPtr) {
		//PRINT_FUNCTION_NAME();
		Thinker* thinkerPtr{};
		IExamInterface* interfacePtr{};

		blackboardPtr->GetData("Brain", thinkerPtr);
		blackboardPtr->GetData("Interface", interfacePtr);

		if (thinkerPtr->CheckHousesForMemory(interfacePtr->GetHousesInFOV())) {
			return BT::State::Success;
		}

		return BT::State::Failure;
	}

	BT::State SetExpireDate(Blackboard* blackboardPtr) {
		PRINT_FUNCTION_NAME();
		Thinker* thinkerPtr{};
		HouseInfo targetHouse{};

		blackboardPtr->GetData("Brain", thinkerPtr);
		blackboardPtr->GetData("TargetHouse", targetHouse);

		thinkerPtr->SetTargetHouseExpireDate(targetHouse);

		return BT::State::Success;
	}

	BT::State GetInsideTarget(Blackboard* blackboardPtr, float offset) {
		PRINT_FUNCTION_NAME();
		HouseInfo targetHouse{};
		blackboardPtr->GetData("TargetHouse", targetHouse);

		const Elite::Vector2 houseCenter{ targetHouse.Center };
		const Elite::Vector2 houseSize{ targetHouse.Size };

		const Elite::Vector2 targetLocation(randNumRange(int(houseCenter.x - houseSize.x / 2 + offset), int(houseCenter.x + houseSize.x / 2 - offset)),
			randNumRange(int(houseCenter.y - houseSize.y / 2 + offset), int(houseCenter.y + houseSize.y / 2 - offset)));

		blackboardPtr->ChangeData("Target", targetLocation);

		return BT::State::Success;
	}
}

namespace BT_Condition
{
	bool CheckTimerLock(Blackboard* blackboardPtr, const std::string& timerName) {
		bool lock{};
		blackboardPtr->GetData(timerName + "TimerLock", lock);

		return !lock;
	}

	bool CheckTimer(Blackboard* blackboardPtr, const std::string& timerName, bool doOnce) {
		std::chrono::steady_clock::time_point timer{};
		float maxTime{};

		blackboardPtr->GetData("Timer" + timerName, timer);
		blackboardPtr->GetData("MaxTime" + timerName, maxTime);

		const std::chrono::steady_clock::time_point currentTime{ std::chrono::steady_clock::now() };
		const std::chrono::duration<float> elapsedSec{ currentTime - timer };

		if (elapsedSec.count() > maxTime) {
			if (doOnce)
				blackboardPtr->ChangeData("Timer" + timerName + "DoOnce", false);

			return true;
		}

		return false;
	}

	bool InPurgeZone(Blackboard* blackboardPtr) {
		IExamInterface* interfacePtr{};
		blackboardPtr->GetData("Interface", interfacePtr);

		const Elite::Vector2 playerPos{ interfacePtr->Agent_GetInfo().Position };

		for (const auto purgeZone : interfacePtr->GetPurgeZonesInFOV()) {
			const Elite::Vector2 purgeCenter{ purgeZone.Center };
			const float purgeRadius{ purgeZone.Radius };

			const float x{ playerPos.x - purgeCenter.x };
			const float y{ playerPos.y - purgeCenter.y };

			const float result{ x * x + y * y - purgeRadius * purgeRadius };

			if (result <= 0)
				return true;
		}

		return false;
	}

	bool SeeZombie(Blackboard* blackboardPtr) {
		IExamInterface* interfacePtr{};
		blackboardPtr->GetData("Interface", interfacePtr);

		return interfacePtr->GetEnemiesInFOV().capacity() > 0;
	}

	bool HasWeapon(Blackboard* blackboardPtr) {
		return ItemInInv(blackboardPtr, eItemType::SHOTGUN) || ItemInInv(blackboardPtr, eItemType::PISTOL);
	}

	bool InRange(Blackboard* blackboardPtr, float maxRange) {
		IExamInterface* interfacePtr{};
		EnemyInfo zombieInfo{};

		blackboardPtr->GetData("Interface", interfacePtr);
		blackboardPtr->GetData("TargetZombie", zombieInfo);

		const float dist2{ (zombieInfo.Location - interfacePtr->Agent_GetInfo().Position).MagnitudeSquared() };

		return dist2 <= maxRange * maxRange;
	}

	bool ItemInInv(Blackboard* blackboardPtr, eItemType type) {
		Thinker* thinkerPtr{};
		blackboardPtr->GetData("Brain", thinkerPtr);

		return thinkerPtr->IsItemInInv(type);
	}

	bool HpUnderThreshold(Blackboard* blackboardPtr, float threshold) {
		IExamInterface* interfacePtr{};
		blackboardPtr->GetData("Interface", interfacePtr);

		return interfacePtr->Agent_GetInfo().Health <= threshold;
	}

	bool CheckMinNeededEnergy(Blackboard* blackboardPtr) {
		IExamInterface* interfacePtr{};
		Thinker* thinkerPtr{};

		blackboardPtr->GetData("Interface", interfacePtr);
		blackboardPtr->GetData("Brain", thinkerPtr);

		const auto item = thinkerPtr->FindLeastValueItem(eItemType::FOOD);

		return interfacePtr->Agent_GetInfo().Energy <= 10.f - item->ItemInfo.Value;
	}

	bool SeeItem(Blackboard* blackboardPtr) {
		IExamInterface* interfacePtr{};
		blackboardPtr->GetData("Interface", interfacePtr);

		return interfacePtr->GetItemsInFOV().capacity() > 0;
	}

	bool IsTypeOfItem(Blackboard* blackboardPtr, eItemType type) {
		ItemInfo targetItem{};
		blackboardPtr->GetData("TargetItem", targetItem);

		return targetItem.Type == type;
	}

	bool InvIsNotFull(Blackboard* blackboardPtr) {
		Thinker* thinkerPtr{};
		blackboardPtr->GetData("Brain", thinkerPtr);
		return thinkerPtr->IsInvNotFull();
	}

	bool EmptyValue(Blackboard* blackboardPtr) {
		Thinker* thinkerPtr{};
		blackboardPtr->GetData("Brain", thinkerPtr);

		return thinkerPtr->EmptyValue();
	}

	bool InvIsFull(Blackboard* blackboardPtr) {
		return !InvIsNotFull(blackboardPtr);
	}

	bool InsideTargetHouse(Blackboard* blackboardPtr) {
		IExamInterface* interfacePtr{};
		Thinker* thinkerPtr{};
		HouseInfo targetHouse{};

		blackboardPtr->GetData("Interface", interfacePtr);
		blackboardPtr->GetData("Brain", thinkerPtr);
		blackboardPtr->GetData("TargetHouse", targetHouse);

		if (interfacePtr->Agent_GetInfo().IsInHouse) {
			return thinkerPtr->CheckIfTargetIsInside(targetHouse, interfacePtr->Agent_GetInfo().Position);
		}
		return false;
	}

	bool SeeHouse(Blackboard* blackboardPtr) {
		IExamInterface* interfacePtr{};
		blackboardPtr->GetData("Interface", interfacePtr);

		return interfacePtr->GetHousesInFOV().capacity() > 0;
	}

	bool NewHouse(Blackboard* blackboardPtr) {
		Thinker* thinkerPtr{};
		blackboardPtr->GetData("Brain", thinkerPtr);

		return thinkerPtr->NewHouseToExplore();
	}

	bool ReExploreHouse(Blackboard* blackboardPtr) {
		Thinker* thinkerPtr{};
		blackboardPtr->GetData("Brain", thinkerPtr);

		return thinkerPtr->HouseToReExplore();
	}

	bool CheckMovement(Blackboard* blackboardPtr, float timeout) {
		IExamInterface* interfacePtr{};
		blackboardPtr->GetData("Interface", interfacePtr);

		// Get current position
		const Elite::Vector2 currentPosition = interfacePtr->Agent_GetInfo().Position;

		// Get or initialize last position and timer from blackboard
		Elite::Vector2 lastPosition;
		std::chrono::steady_clock::time_point lastMovementTime;

		// Try to get last position from blackboard, or initialize if not present
		if (!blackboardPtr->GetData("LastPosition", lastPosition)) {
			lastPosition = currentPosition;
			blackboardPtr->AddData("LastPosition", lastPosition);
		}

		// Try to get last movement time from blackboard, or initialize if not present
		if (!blackboardPtr->GetData("LastMovementTime", lastMovementTime)) {
			lastMovementTime = std::chrono::steady_clock::now();
			blackboardPtr->AddData("LastMovementTime", lastMovementTime);
		}

		// Check if we've moved significantly (more than a small threshold)
		const float movementThreshold = 0.5f; // Adjust this based on your game scale
		const float distanceMoved = currentPosition.DistanceSquared(lastPosition);

		if (distanceMoved > movementThreshold * movementThreshold) {
			// We've moved significantly, update last position and time in blackboard
			blackboardPtr->ChangeData("LastPosition", currentPosition);
			blackboardPtr->ChangeData("LastMovementTime", std::chrono::steady_clock::now());
			return false;
		}

		// Check how long we've been stuck
		const auto currentTime = std::chrono::steady_clock::now();
		const auto elapsedTime = std::chrono::duration_cast<std::chrono::seconds>(currentTime - lastMovementTime).count();

		// Return true if we've been stuck for longer than the timeout
		return elapsedTime >= timeout;
	}

}

namespace BT_Grid {
	BT::State UpdateExplorationGrid(Blackboard* blackboardPtr) {
		PRINT_FUNCTION_NAME();
		IExamInterface* interfacePtr{};
		Thinker* thinkerPtr{};

		if (!blackboardPtr->GetData("Interface", interfacePtr) || !blackboardPtr->GetData("Brain", thinkerPtr)) {
			return BT::State::Failure;
		}

		const AgentInfo agentInfo = interfacePtr->Agent_GetInfo();
		//Visible houses, enemies, items, and purge zones
		const std::vector<EnemyInfo>& enemiesInFOV = interfacePtr->GetEnemiesInFOV();
		const std::vector<ItemInfo>& itemsInFOV = interfacePtr->GetItemsInFOV();
		const std::vector<PurgeZoneInfo>& purgeZonesInFOV = interfacePtr->GetPurgeZonesInFOV();


		const std::vector<HouseInfo>& housesInFOV = interfacePtr->GetHousesInFOV();
		thinkerPtr->m_ExplorationGrid.Update(agentInfo, housesInFOV);

		return BT::State::Success;
	}
	
	BT::State FindUnexploredCell(Blackboard* blackboardPtr) {
		PRINT_FUNCTION_NAME();
		Thinker* thinkerPtr{};
		blackboardPtr->GetData("Brain", thinkerPtr);

		IExamInterface* interfacePtr{};
		if (!blackboardPtr->GetData("Interface", interfacePtr)) {
			return BT::State::Failure;
		}

		Elite::Vector2 playerPos = interfacePtr->Agent_GetInfo().Position;
		const Elite::Vector2 target{ thinkerPtr->m_ExplorationGrid.FindNearestUnexplored(playerPos)};
		if (target == Elite::Vector2{})
			return BT::State::Failure;
		blackboardPtr->ChangeData("Target", target);
		return BT::State::Success;
	}


}
