#include "scene/flying_camera.hpp"

#include <SFML/Window/Keyboard.hpp>
#include <cmath>
#include <numbers>
#include "glm/gtx/string_cast.hpp"
#include "glm/gtx/euler_angles.hpp"

#include "util/logger.hpp" // TODO remove
#include "glm/gtx/vector_angle.hpp"

flying_camera::flying_camera(const float yaw, const float pitch, const float roll) {
	m_world_rotation = glm::mat3(glm::eulerAngleYXZ<float>(yaw, pitch, roll));
	m_world_up = m_world_rotation * glm::vec3(0, 1, 0);

	m_velocity = glm::vec3(0.f, 0.f, 0.f);
	m_pitch = 0.f;
	m_yaw = glm::radians(-90.f);
	m_roll = 0.f;
}


void flying_camera::update(
	const float time_delta,
	const glm::vec2 mouse_pos_delta,
	const float mouse_wheel_delta,
	camera_view& view
) {
	static constexpr auto pi = std::numbers::pi_v<float>;
	static constexpr auto epsilon = std::numeric_limits<float>::epsilon();
	static constexpr auto world_up = glm::vec3(0.0f, 1.0f, 0.0f);

	static constexpr auto friction_coefficient = 25.0f;
	static constexpr auto walk_acceleration = 3000.0f;
	static constexpr auto max_velocity = 4000.0f;

	static constexpr float max_pitch = (pi / 2.0f) - epsilon;
	static constexpr float min_fov = 0.01f * pi;
	static constexpr float max_fov = 0.8f * pi;

	m_yaw += mouse_pos_delta.x;
	m_pitch -= mouse_pos_delta.y;

	m_yaw = std::fmod(m_yaw, 2.0f * pi);
	m_pitch = std::clamp(m_pitch, -max_pitch, max_pitch);

	view.front.x = std::cos(m_yaw) * std::cos(m_pitch);
	view.front.y = std::sin(m_pitch);
	view.front.z = std::sin(m_yaw) * std::cos(m_pitch);
	view.front = glm::normalize(view.front);

	view.right = glm::normalize(glm::cross(view.front, world_up));
	view.up = glm::normalize(glm::cross(view.right, view.front));

	view.front = m_world_rotation * view.front;
	view.up = m_world_rotation * view.up;
	view.right = m_world_rotation * view.right;

	view.fov *= 1.0f + mouse_wheel_delta;
	view.fov = std::clamp(view.fov, min_fov, max_fov);

	using kb = sf::Keyboard;
	auto acceleration = glm::vec3{ 0.0f };

	if (kb::isKeyPressed(kb::W)) acceleration += view.front;
	if (kb::isKeyPressed(kb::S)) acceleration -= view.front;

	if (kb::isKeyPressed(kb::A)) acceleration -= view.right;
	if (kb::isKeyPressed(kb::D)) acceleration += view.right;

	if (kb::isKeyPressed(kb::Space)) acceleration += m_world_up; // TODO fix
	if (kb::isKeyPressed(kb::LControl)) acceleration -= m_world_up;

	acceleration *= walk_acceleration;

	const auto acc = glm::length(acceleration);
	if (acc > epsilon)
	{
		acceleration *= walk_acceleration / acc;
	}

	if (kb::isKeyPressed(kb::LShift)) acceleration *= 2.0f;

	m_velocity += acceleration * time_delta;

	const float drag = std::exp(-friction_coefficient * time_delta);
	m_velocity *= drag;


	const auto speed = time_delta * glm::length(m_velocity);
	if (speed > max_velocity) {
		m_velocity *= (max_velocity / speed) * max_velocity;
	}

	view.position += m_velocity * time_delta;
}

void flying_camera::look_at(
	const glm::vec3& origin,
	const glm::vec3& target,
	camera_view& view
) {
	static constexpr auto world_up = glm::vec3(0.0f, 1.0f, 0.0f);
	// TODO inverted matrix

	view.position = origin;
	view.front = glm::normalize(target - origin);
	view.right = glm::normalize(glm::cross(view.front, world_up));
	view.up = glm::normalize(glm::cross(view.right, view.front));
	view.fov = std::numbers::pi_v<float> / 2.0f;

	m_velocity = { 0.0f, 0.0f, 0.0f };

	m_pitch = std::asin(glm::dot(view.front, world_up));
	m_yaw = std::atan2(view.front.z, view.front.x);
	m_roll = std::atan2(view.up.x, view.up.y);
}