#include "ClientState.h"

uint32_t fnv1a_hash(const std::string& data) {
	uint32_t hash = 0x811c9dc5;
	for (unsigned char c : data) {
		hash ^= c;
		hash *= 0x01000193;
	}
	return hash;
}

void ClientState::simulate_physics(size_t delta) {
	if (discreteMag > 0.01f)
	{
		float rad = static_cast<float>(discreteAngle) * static_cast<float>(M_PI) / 180.0f;

		// Целевая угловая скорость (дискретная сила определяет модуль вектора)
		// Вектор {cos, sin, 0} дает нам направление отклонения джойстика
		Eigen::Vector3f targetAngularVelDir(sinf(rad), cosf(rad), 0.0f);
		Eigen::Vector3f targetAngularVelocity = targetAngularVelDir * discreteMag;

		Eigen::Vector3f diffVel = targetAngularVelocity - currentAngularVelocity;
		float diffLen = diffVel.norm();

		if (diffLen > 0.0001f) {
			// Вычисляем, на сколько мы можем изменить скорость в этом кадре
			float maxChange = ANGULAR_ACCEL * static_cast<float>(delta);

			if (diffLen <= maxChange) {
				// Если до цели осталось меньше, чем шаг ускорения — просто прыгаем в цель
				currentAngularVelocity = targetAngularVelocity;
			}

			else {
				// Линейно двигаемся в сторону целевого вектора
				currentAngularVelocity += (diffVel / diffLen) * maxChange;
			}
		}
	}
	else
	{
		float currentSpeed = currentAngularVelocity.norm();

		if (currentSpeed > 0.0001f) {
			float drop = ANGULAR_ACCEL * static_cast<float>(delta);
			if (currentSpeed <= drop) {
				currentAngularVelocity = Eigen::Vector3f::Zero();
			}
			else {
				// Уменьшаем модуль вектора, сохраняя направление
				currentAngularVelocity -= (currentAngularVelocity / currentSpeed) * drop;
			}
		}
	}


	float distToCenter = position.norm(); // Расстояние до {0,0,0}
	float landingZone = PLANET_RADIUS * PLANET_ALIGN_ZONE;

	if (distToCenter <= landingZone) {
		Eigen::Vector3f planetNormal = position.normalized();

		// --- 1. ВЫРАВНИВАНИЕ КРЕНА (Roll - ось Z) ---
		Eigen::Vector3f localX = rotation.col(0);
		float rollError = localX.dot(planetNormal);

		if (std::abs(rollError) > 0.001f) {
			currentAngularVelocity.z() -= rollError * PLANET_ANGULAR_ACCEL * delta;
			currentAngularVelocity.z() = std::max(-PLANET_MAX_ANGULAR_VELOCITY,
				std::min(currentAngularVelocity.z(), PLANET_MAX_ANGULAR_VELOCITY));
		}

		// --- 2. ОГРАНИЧЕНИЕ ТАНГАЖА (Pitch - ось X) ---
		// Нос корабля в локальных координатах — это -Z (третий столбец матрицы)
		Eigen::Vector3f forwardDir = -rotation.col(2);

		// В твоем случае dot < 0 означает, что нос направлен К планете
		float pitchSin = forwardDir.dot(planetNormal);
		float currentPitchAngle = asinf(std::clamp(pitchSin, -1.0f, 1.0f));

		// Лимит у нас M_PI / 6.0 (примерно 0.523)
		// По твоим данным: -0.89 < -0.523, значит мы превысили наклон вниз
		if (currentPitchAngle < -PITCH_LIMIT) {
			// Вычисляем ошибку (насколько мы ушли "ниже" лимита)
			// -0.89 - (-0.52) = -0.37
			float pitchError = currentPitchAngle + PITCH_LIMIT;

			// Теперь важно: нам нужно ПОДНЯТЬ нос. 
			// Если pitchError отрицательный, а нам нужно уменьшить вращение, 
			// пробуем прибавлять или вычитать в зависимости от твоей оси X.
			// Судя по стандартной логике Eigen, нам нужно ПЛЮСОВАТЬ:
			currentAngularVelocity.x() -= pitchError * PLANET_ANGULAR_ACCEL * delta;
		}
	}
	else {
		// Вне зоны: тормозим Z (крен) И X (тангаж), если они были активны
		float drop = ANGULAR_ACCEL * delta;
		
			if (std::abs(currentAngularVelocity[2]) > 0.0001f) {
				if (std::abs(currentAngularVelocity[2]) <= drop) {
					currentAngularVelocity[2] = 0.0f;
				}
				else {
					currentAngularVelocity[2] -= (currentAngularVelocity[2] > 0 ? 1.0f : -1.0f) * drop;
				}
			}
	}

	// Ограничение скорости
	currentAngularVelocity.x() = std::max(-PLANET_MAX_ANGULAR_VELOCITY,
		std::min(currentAngularVelocity.x(), PLANET_MAX_ANGULAR_VELOCITY));
	// Ограничение скорости
	currentAngularVelocity.y() = std::max(-PLANET_MAX_ANGULAR_VELOCITY,
		std::min(currentAngularVelocity.y(), PLANET_MAX_ANGULAR_VELOCITY));
	// Ограничение скорости
	currentAngularVelocity.z() = std::max(-PLANET_MAX_ANGULAR_VELOCITY,
		std::min(currentAngularVelocity.z(), PLANET_MAX_ANGULAR_VELOCITY));

	float speedScale = currentAngularVelocity.norm();
	if (speedScale > 0.0001f) {
		// Коэффициент чувствительности вращения

		float deltaAlpha = speedScale * static_cast<float>(delta) * ROTATION_SENSITIVITY;

		Eigen::Vector3f axis = currentAngularVelocity.normalized();
		Eigen::Quaternionf rotateQuat(Eigen::AngleAxisf(deltaAlpha, axis));

		rotation = rotation * rotateQuat.toRotationMatrix();
	}


	// 4. Линейное изменение линейной скорости
	float shipDesiredVelocity = selectedVelocity * 100.f;

	if (velocity < shipDesiredVelocity)
	{
		velocity += delta * SHIP_ACCEL;
		if (velocity > shipDesiredVelocity)
		{
			velocity = shipDesiredVelocity;
		}
	}
	else if (velocity > shipDesiredVelocity)
	{
		velocity -= delta * SHIP_ACCEL;
		if (velocity < shipDesiredVelocity)
		{
			velocity = shipDesiredVelocity;
		}
	}

	if (fabs(velocity) > 0.01f)
	{
		Eigen::Vector3f velocityDirection = { 0,0, -velocity * delta / 1000.f };
		Eigen::Vector3f velocityDirectionAdjusted = rotation * velocityDirection;
		position = position + velocityDirectionAdjusted;
	}
}

void ClientState::apply_lag_compensation(std::chrono::system_clock::time_point nowTime) {

	// 2. Вычисляем задержку
	long long deltaMs = 0;
	if (nowTime > lastUpdateServerTime) {
		deltaMs = std::chrono::duration_cast<std::chrono::milliseconds>(nowTime - lastUpdateServerTime).count();
	}

	long long deltaMsLeftover = deltaMs;

	while (deltaMsLeftover > 0)
	{
		long long miniDelta = std::min(50LL, deltaMsLeftover);
		simulate_physics(miniDelta);
		deltaMsLeftover -= miniDelta;
	}

	/*
	// 3. Защита от слишком больших скачков (Clamp)
	// Если лаг более 500мс, ограничиваем его, чтобы избежать резких рывков
	long long final_lag_ms = deltaMs;//min(deltaMs, 500ll);

	if (final_lag_ms > 0) {
		// Доматываем симуляцию на величину задержки
		// Мы предполагаем, что за это время параметры управления не менялись
		simulate_physics(final_lag_ms);
	}*/
}

void ClientState::handle_full_sync(const std::vector<std::string>& parts, int startFrom) {
	// Позиция
	position = { std::stof(parts[startFrom]), std::stof(parts[startFrom + 1]), std::stof(parts[startFrom + 2]) };

	Eigen::Quaternionf q(
		std::stof(parts[startFrom + 3]),
		std::stof(parts[startFrom + 4]),
		std::stof(parts[startFrom + 5]),
		std::stof(parts[startFrom + 6]));
	rotation = q.toRotationMatrix();

	currentAngularVelocity = Eigen::Vector3f{
		std::stof(parts[startFrom + 7]),
		std::stof(parts[startFrom + 8]),
		std::stof(parts[startFrom + 9]) };
	velocity = std::stof(parts[startFrom + 10]);
	selectedVelocity = std::stoi(parts[startFrom + 11]);
	discreteMag = std::stof(parts[startFrom + 12]);
	discreteAngle = std::stoi(parts[startFrom + 13]);
}

std::string ClientState::formPingMessageContent() const
{
	Eigen::Quaternionf q(rotation);

	std::string pingMsg = std::to_string(position.x()) + ":"
		+ std::to_string(position.y()) + ":"
		+ std::to_string(position.z()) + ":"
		+ std::to_string(q.w()) + ":"
		+ std::to_string(q.x()) + ":"
		+ std::to_string(q.y()) + ":"
		+ std::to_string(q.z()) + ":"
		+ std::to_string(currentAngularVelocity.x()) + ":"
		+ std::to_string(currentAngularVelocity.y()) + ":"
		+ std::to_string(currentAngularVelocity.z()) + ":"
		+ std::to_string(velocity) + ":"
		+ std::to_string(selectedVelocity) + ":"
		+ std::to_string(discreteMag) + ":" // Используем те же static переменные из блока ROT
		+ std::to_string(discreteAngle);

	return pingMsg;
}


void ClientStateInterval::add_state(const ClientState& state)
{
	auto nowTime = std::chrono::system_clock::now();

	if (timedStates.size() > 0 && timedStates[timedStates.size() - 1].lastUpdateServerTime == state.lastUpdateServerTime)
	{
		timedStates[timedStates.size() - 1] = state;
	}
	else
	{
		timedStates.push_back(state);
	}

	auto cutoff_time = nowTime - std::chrono::milliseconds(CUTOFF_TIME);

	while (timedStates.size() > 0 && timedStates[0].lastUpdateServerTime < cutoff_time)
	{
		timedStates.erase(timedStates.begin());
	}
}

bool ClientStateInterval::canFetchClientStateAtTime(std::chrono::system_clock::time_point targetTime) const
{
	if (timedStates.empty())
	{
		return false;
	}
	if (timedStates[0].lastUpdateServerTime > targetTime)
	{
		return false;
	}

	return true;
}

ClientState ClientStateInterval::fetchClientStateAtTime(std::chrono::system_clock::time_point targetTime) const {

	ClientState closestState;

	if (timedStates.empty())
	{
		throw std::runtime_error("No timed client states available");
		return closestState;
	}
	if (timedStates[0].lastUpdateServerTime > targetTime)
	{
		throw std::runtime_error("Found time but it is in future");
		return closestState;
	}
	if (timedStates.size() == 1)
	{
		closestState = timedStates[0];
		closestState.apply_lag_compensation(targetTime);
		return closestState;
	}


	for (size_t i = 0; i < timedStates.size() - 1; ++i)
	{
		const auto& earlierState = timedStates[i];
		const auto& laterState = timedStates[i + 1];
		if (earlierState.lastUpdateServerTime <= targetTime && laterState.lastUpdateServerTime >= targetTime)
		{
			closestState = earlierState;
			closestState.apply_lag_compensation(targetTime);
			return closestState;
		}
	}

	closestState = timedStates[timedStates.size() - 1];
	closestState.apply_lag_compensation(targetTime);
	return closestState;
}
