#include "ClientState.h"


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 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();
	}

	// 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()
{
	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;
}