#include "LocalClient.h"
#include <iostream>
#include <sstream>
#include <algorithm>
#include <cmath>
#define _USE_MATH_DEFINES
#include <math.h>

namespace ZL {

	void LocalClient::Connect(const std::string& host, uint16_t port) {
		generateBoxes();
		initializeNPCs();
		lastUpdateMs = std::chrono::duration_cast<std::chrono::milliseconds>(
			std::chrono::system_clock::now().time_since_epoch()).count();
	}

	void LocalClient::generateBoxes() {
		serverBoxes.clear();

		std::random_device rd;
		std::mt19937 gen(rd());

		const float MIN_COORD = -100.0f;
		const float MAX_COORD = 100.0f;
		const float MIN_DISTANCE = 3.0f;
		const float MIN_DISTANCE_SQUARED = MIN_DISTANCE * MIN_DISTANCE;
		const int MAX_ATTEMPTS = 1000;

		std::uniform_real_distribution<> posDistrib(MIN_COORD, MAX_COORD);
		std::uniform_real_distribution<> angleDistrib(0.0, M_PI * 2.0);

		for (int i = 0; i < 50; i++) {
			bool accepted = false;
			int attempts = 0;

			while (!accepted && attempts < MAX_ATTEMPTS) {
				LocalServerBox box;
				box.position = Eigen::Vector3f(
					(float)posDistrib(gen),
					(float)posDistrib(gen),
					(float)posDistrib(gen)
				);

				accepted = true;
				for (const auto& existingBox : serverBoxes) {
					Eigen::Vector3f diff = box.position - existingBox.position;
					if (diff.squaredNorm() < MIN_DISTANCE_SQUARED) {
						accepted = false;
						break;
					}
				}

				if (accepted) {
					float randomAngle = (float)angleDistrib(gen);
					Eigen::Vector3f axis = Eigen::Vector3f::Random().normalized();
					box.rotation = Eigen::AngleAxisf(randomAngle, axis).toRotationMatrix();
					serverBoxes.push_back(box);
				}

				attempts++;
			}
		}

		std::cout << "LocalClient: Generated " << serverBoxes.size() << " boxes\n";
	}

	Eigen::Vector3f LocalClient::generateRandomPosition() {
		std::random_device rd;
		std::mt19937 gen(rd());
		std::uniform_real_distribution<> distrib(-500.0, 500.0);

		return Eigen::Vector3f(
			(float)distrib(gen),
			(float)distrib(gen),
			(float)distrib(gen) + 45000.0f
		);
	}

	void LocalClient::initializeNPCs() {
		npcs.clear();
		for (int i = 0; i < 3; ++i) {
			LocalNPC npc;
			npc.id = 100 + i;
			npc.currentState.id = npc.id;
			npc.currentState.position = generateRandomPosition();
			npc.currentState.rotation = Eigen::Matrix3f::Identity();
			npc.currentState.velocity = 0.0f;
			npc.currentState.selectedVelocity = 0;
			npc.currentState.discreteMag = 0.0f;
			npc.currentState.discreteAngle = -1;
			npc.currentState.currentAngularVelocity = Eigen::Vector3f::Zero();

			npc.targetPosition = generateRandomPosition();
			npc.lastStateUpdateMs = std::chrono::duration_cast<std::chrono::milliseconds>(
				std::chrono::system_clock::now().time_since_epoch()).count();
			npc.destroyed = false;

			npc.stateHistory.add_state(npc.currentState);
			npcs.push_back(npc);
		}
	}

	void LocalClient::updateNPCs() {
		auto now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
			std::chrono::system_clock::now().time_since_epoch()).count();

		for (auto& npc : npcs) {
			if (npc.destroyed) continue;

			uint64_t deltaMs = now_ms - npc.lastStateUpdateMs;
			if (deltaMs == 0) {
				npc.lastStateUpdateMs = now_ms;
				continue;
			}
			npc.lastStateUpdateMs = now_ms;

			Eigen::Vector3f toTarget = npc.targetPosition - npc.currentState.position;
			float distance = toTarget.norm();

			const float ARRIVAL_THRESHOLD = 100.0f;

			if (distance < ARRIVAL_THRESHOLD) {
				npc.targetPosition = generateRandomPosition();
				toTarget = npc.targetPosition - npc.currentState.position;
				distance = toTarget.norm();
			}

			Eigen::Vector3f forwardWorld = -npc.currentState.rotation.col(2);
			forwardWorld.normalize();

			Eigen::Vector3f desiredDir = (distance > 0.001f) ? toTarget.normalized() : Eigen::Vector3f::UnitZ();
			float dot = forwardWorld.dot(desiredDir);
			float angleErrorRad = std::acos(std::clamp(dot, -1.0f, 1.0f));

			const float ALIGN_TOLERANCE = 0.15f;

			const float HYSTERESIS_FACTOR = 1.35f;
			const float SOFT_THRUST_ANGLE = ALIGN_TOLERANCE * HYSTERESIS_FACTOR;

			if (angleErrorRad < ALIGN_TOLERANCE) {
				npc.currentState.selectedVelocity = 1;
				npc.currentState.discreteMag = 0.0f;
			}
			else if (angleErrorRad < SOFT_THRUST_ANGLE) {
				npc.currentState.selectedVelocity = 1;
				npc.currentState.discreteMag = std::min(0.50f, (angleErrorRad - ALIGN_TOLERANCE) * 10.0f);
			}
			else {
				npc.currentState.selectedVelocity = 0;

				Eigen::Vector3f localDesired = npc.currentState.rotation.transpose() * desiredDir;
				float dx = localDesired.x();
				float dy = localDesired.y();
				float dz = localDesired.z();

				float turnX = dy;
				float turnY = -dx;
				float turnLen = std::sqrt(turnX * turnX + turnY * turnY);

				if (turnLen > 0.0001f) {
					turnX /= turnLen;
					turnY /= turnLen;

					float rad = std::atan2(turnX, turnY);
					int angleDeg = static_cast<int>(std::round(rad * 180.0f / M_PI));
					if (angleDeg < 0) angleDeg += 360;

					npc.currentState.discreteAngle = angleDeg;
					npc.currentState.discreteMag = std::min(1.0f, angleErrorRad * 2.2f);
				}
				else if (angleErrorRad > 0.1f) {
					npc.currentState.discreteAngle = 0;
					npc.currentState.discreteMag = 1.0f;
				}
				else {
					npc.currentState.discreteMag = 0.0f;
				}
			}

			npc.currentState.simulate_physics(static_cast<size_t>(deltaMs));
			npc.currentState.lastUpdateServerTime = std::chrono::system_clock::time_point(
				std::chrono::milliseconds(now_ms));
			npc.stateHistory.add_state(npc.currentState);
		}
	}

	void LocalClient::Poll() {
		updatePhysics();
		updateNPCs();
		checkCollisions();
	}

	void LocalClient::updatePhysics() {
		auto now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
			std::chrono::system_clock::now().time_since_epoch()).count();

		if (lastUpdateMs == 0) {
			lastUpdateMs = now_ms;
			return;
		}

		uint64_t deltaMs = now_ms - lastUpdateMs;
		float dt = deltaMs / 1000.0f;
		lastUpdateMs = now_ms;

		std::vector<int> indicesToRemove;

		for (size_t i = 0; i < projectiles.size(); ++i) {
			auto& pr = projectiles[i];
			pr.pos += pr.vel * dt;

			if (now_ms > pr.spawnMs + static_cast<uint64_t>(pr.lifeMs)) {
				indicesToRemove.push_back(static_cast<int>(i));
			}
		}

		if (!indicesToRemove.empty()) {
			std::sort(indicesToRemove.rbegin(), indicesToRemove.rend());
			for (int idx : indicesToRemove) {
				if (idx >= 0 && idx < (int)projectiles.size()) {
					projectiles.erase(projectiles.begin() + idx);
				}
			}
		}
	}

	void LocalClient::checkCollisions() {
		auto now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
			std::chrono::system_clock::now().time_since_epoch()).count();

		const float projectileHitRadius = 1.5f;
		const float boxCollisionRadius = 2.0f;
		const float shipCollisionRadius = 15.0f;
		const float npcCollisionRadius = 5.0f;

		std::vector<std::pair<size_t, size_t>> boxProjectileCollisions;

		for (size_t bi = 0; bi < serverBoxes.size(); ++bi) {
			if (serverBoxes[bi].destroyed) continue;

			Eigen::Vector3f boxWorld = serverBoxes[bi].position + Eigen::Vector3f(0.0f, 0.0f, 45000.0f);

			for (size_t pi = 0; pi < projectiles.size(); ++pi) {
				const auto& pr = projectiles[pi];
				Eigen::Vector3f diff = pr.pos - boxWorld;
				float thresh = boxCollisionRadius + projectileHitRadius;

				if (diff.squaredNorm() <= thresh * thresh) {
					boxProjectileCollisions.push_back({ bi, pi });
				}
			}
		}

		std::vector<int> projIndicesToRemove;
		for (const auto& [boxIdx, projIdx] : boxProjectileCollisions) {
			if (!serverBoxes[boxIdx].destroyed) {
				serverBoxes[boxIdx].destroyed = true;

				Eigen::Vector3f boxWorld = serverBoxes[boxIdx].position + Eigen::Vector3f(0.0f, 0.0f, 45000.0f);

				BoxDestroyedInfo destruction;
				destruction.boxIndex = static_cast<int>(boxIdx);
				destruction.serverTime = now_ms;
				destruction.position = boxWorld;
				destruction.destroyedBy = projectiles[projIdx].shooterId;

				pendingBoxDestructions.push_back(destruction);

				std::cout << "LocalClient: Box " << boxIdx << " destroyed by projectile from player "
					<< projectiles[projIdx].shooterId << std::endl;

				if (std::find(projIndicesToRemove.begin(), projIndicesToRemove.end(), (int)projIdx)
					== projIndicesToRemove.end()) {
					projIndicesToRemove.push_back(static_cast<int>(projIdx));
				}
			}
		}

		std::vector<std::pair<size_t, size_t>> npcProjectileCollisions;

		for (size_t ni = 0; ni < npcs.size(); ++ni) {
			if (npcs[ni].destroyed) continue;

			for (size_t pi = 0; pi < projectiles.size(); ++pi) {
				const auto& pr = projectiles[pi];
				Eigen::Vector3f diff = pr.pos - npcs[ni].currentState.position;
				float thresh = npcCollisionRadius + projectileHitRadius;

				if (diff.squaredNorm() <= thresh * thresh) {
					npcProjectileCollisions.push_back({ ni, pi });
				}
			}
		}

		for (const auto& [npcIdx, projIdx] : npcProjectileCollisions) {
			if (!npcs[npcIdx].destroyed) {
				npcs[npcIdx].destroyed = true;

				DeathInfo death;
				death.targetId = npcs[npcIdx].id;
				death.serverTime = now_ms;
				death.position = npcs[npcIdx].currentState.position;
				death.killerId = projectiles[projIdx].shooterId;

				pendingDeaths.push_back(death);

				std::cout << "LocalClient: NPC " << npcs[npcIdx].id << " destroyed by projectile from player "
					<< projectiles[projIdx].shooterId << " at position ("
					<< npcs[npcIdx].currentState.position.x() << ", "
					<< npcs[npcIdx].currentState.position.y() << ", "
					<< npcs[npcIdx].currentState.position.z() << ")" << std::endl;

				if (std::find(projIndicesToRemove.begin(), projIndicesToRemove.end(), (int)projIdx)
					== projIndicesToRemove.end()) {
					projIndicesToRemove.push_back(static_cast<int>(projIdx));
				}
			}
		}

		if (!projIndicesToRemove.empty()) {
			std::sort(projIndicesToRemove.rbegin(), projIndicesToRemove.rend());
			for (int idx : projIndicesToRemove) {
				if (idx >= 0 && idx < (int)projectiles.size()) {
					projectiles.erase(projectiles.begin() + idx);
				}
			}
		}

		if (hasLocalPlayerState) {
			for (size_t bi = 0; bi < serverBoxes.size(); ++bi) {
				if (serverBoxes[bi].destroyed) continue;

				Eigen::Vector3f boxWorld = serverBoxes[bi].position + Eigen::Vector3f(0.0f, 0.0f, 45000.0f);
				Eigen::Vector3f diff = localPlayerState.position - boxWorld;
				float thresh = shipCollisionRadius + boxCollisionRadius;

				if (diff.squaredNorm() <= thresh * thresh) {
					serverBoxes[bi].destroyed = true;

					BoxDestroyedInfo destruction;
					destruction.boxIndex = static_cast<int>(bi);
					destruction.serverTime = now_ms;
					destruction.position = boxWorld;
					destruction.destroyedBy = GetClientId();

					pendingBoxDestructions.push_back(destruction);

					std::cout << "LocalClient: Box " << bi << " destroyed by ship collision with player "
						<< GetClientId() << std::endl;
				}
			}
		}
	}

	void LocalClient::Send(const std::string& message) {
		auto parts = [](const std::string& s, char delimiter) {
			std::vector<std::string> tokens;
			std::string token;
			std::istringstream tokenStream(s);
			while (std::getline(tokenStream, token, delimiter)) {
				tokens.push_back(token);
			}
			return tokens;
			}(message, ':');

		if (parts.empty()) return;

		std::string type = parts[0];

		if (type == "FIRE") {
			if (parts.size() < 10) return;

			uint64_t clientTime = std::stoull(parts[1]);
			Eigen::Vector3f pos{
				std::stof(parts[2]), std::stof(parts[3]), std::stof(parts[4])
			};
			Eigen::Quaternionf dir(
				std::stof(parts[5]), std::stof(parts[6]), std::stof(parts[7]), std::stof(parts[8])
			);
			float velocity = std::stof(parts[9]);

			int shotCount = 2;
			if (parts.size() >= 11) {
				try { shotCount = std::stoi(parts[10]); }
				catch (...) { shotCount = 2; }
			}

			const std::vector<Eigen::Vector3f> localOffsets = {
				Eigen::Vector3f(-1.5f, 0.9f - 6.f, 5.0f),
				Eigen::Vector3f(1.5f, 0.9f - 6.f, 5.0f)
			};

			uint64_t now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
				std::chrono::system_clock::now().time_since_epoch()).count();

			for (int i = 0; i < std::min(shotCount, (int)localOffsets.size()); ++i) {
				LocalProjectile pr;
				pr.shooterId = GetClientId();
				pr.spawnMs = now_ms;
				Eigen::Vector3f shotPos = pos + dir.toRotationMatrix() * localOffsets[i];
				pr.pos = shotPos;
				Eigen::Vector3f localForward(0.0f, 0.0f, -1.0f);
				Eigen::Vector3f worldForward = dir.toRotationMatrix() * localForward;
				float len = worldForward.norm();
				if (len > 1e-6f) worldForward /= len;
				pr.vel = worldForward * velocity;
				pr.lifeMs = 5000.0f;
				projectiles.push_back(pr);

				ProjectileInfo pinfo;
				pinfo.shooterId = pr.shooterId;
				pinfo.clientTime = clientTime;
				pinfo.position = pr.pos;
				pinfo.rotation = dir.toRotationMatrix();
				pinfo.velocity = velocity;
				pendingProjectiles.push_back(pinfo);

				std::cout << "LocalClient: Created projectile at pos (" << shotPos.x() << ", "
					<< shotPos.y() << ", " << shotPos.z() << ") vel (" << pr.vel.x() << ", "
					<< pr.vel.y() << ", " << pr.vel.z() << ")" << std::endl;
			}
		}
	}

	std::vector<ProjectileInfo> LocalClient::getPendingProjectiles() {
		auto result = pendingProjectiles;
		pendingProjectiles.clear();
		return result;
	}

	std::vector<DeathInfo> LocalClient::getPendingDeaths() {
		auto result = pendingDeaths;
		pendingDeaths.clear();
		return result;
	}

	std::unordered_map<int, ClientStateInterval> LocalClient::getRemotePlayers() {
		std::unordered_map<int, ClientStateInterval> result;
		for (const auto& npc : npcs) {
			if (!npc.destroyed) {
				result[npc.id] = npc.stateHistory;
			}
		}
		return result;
	}

	std::vector<std::pair<Eigen::Vector3f, Eigen::Matrix3f>> LocalClient::getServerBoxes() {
		std::vector<std::pair<Eigen::Vector3f, Eigen::Matrix3f>> result;
		for (const auto& box : serverBoxes) {
			result.push_back({ box.position, box.rotation });
		}
		return result;
	}

	std::vector<BoxDestroyedInfo> LocalClient::getPendingBoxDestructions() {
		auto result = pendingBoxDestructions;
		pendingBoxDestructions.clear();
		return result;
	}
}