#include "all.h"
#include "raymath.h"
#include <float.h>

static void PlayerRotate(void) {
	if (!game.cursor_captured) return;

	Vector2 mouseDelta = GetMouseDelta();
	game.yaw += -mouseDelta.x * PLAYER_MOUSE_SENSITIVITY;
	game.pitch += mouseDelta.y * PLAYER_MOUSE_SENSITIVITY;

	// Clamp pitch to avoid gimbal lock/flipping (approx 89 degrees)
	if (game.pitch > 89.0f * DEG2RAD) game.pitch = 89.0f * DEG2RAD;
	if (game.pitch < -89.0f * DEG2RAD) game.pitch = -89.0f * DEG2RAD;
}

static bool CheckWorldCollision(Vector3 start, Vector3 end, RayCollision *outCollision) {
	Vector3 diff = Vector3Subtract(end, start);
	float maxDist = Vector3Length(diff);
	if (maxDist < 0.001f) return false;

	Ray ray = { 0 };
	ray.position = start;
	ray.direction = Vector3Scale(diff, 1.0f / maxDist);

	RayCollision closest = { 0 };
	closest.distance = FLT_MAX;
	closest.hit = false;

	for (int i = 0; i < game.world_model_count; i++) {
		Model model = game.world_models[i];
		for (int m = 0; m < model.meshCount; m++) {
			RayCollision col = GetRayCollisionMesh(ray, model.meshes[m], model.transform);
			if (col.hit && col.distance < closest.distance && col.distance <= maxDist) {
				closest = col;
			}
		}
	}

	if (closest.hit) {
		if (outCollision) *outCollision = closest;
		return true;
	}
	return false;
}

static void MoveNormal(float dt) {
	float eyeHeight = PLAYER_EYE_HEIGHT - (game.crouch_amount * PLAYER_CROUCH_OFFSET);

	// Movement vectors based on yaw
	Vector3 forward = { sinf(game.yaw), 0, cosf(game.yaw) };
	Vector3 right = { sinf(game.yaw - PI/2.0f), 0, cosf(game.yaw - PI/2.0f) };

	Vector3 moveDir = { 0 };
	if (IsKeyDown(KEY_W)) moveDir = Vector3Add(moveDir, forward);
	if (IsKeyDown(KEY_S)) moveDir = Vector3Subtract(moveDir, forward);
	if (IsKeyDown(KEY_D)) moveDir = Vector3Add(moveDir, right);
	if (IsKeyDown(KEY_A)) moveDir = Vector3Subtract(moveDir, right);

	float speed = PLAYER_MOVE_SPEED;
	if (IsKeyDown(KEY_LEFT_SHIFT) || IsKeyDown(KEY_RIGHT_SHIFT)) speed *= PLAYER_SPRINT_MULTIPLIER;

	// Apply crouch speed penalty
	speed *= Lerp(1.0f, PLAYER_CROUCH_MOVE_MULTIPLIER, game.crouch_amount);

	if (Vector3Length(moveDir) > 0) {
		moveDir = Vector3Scale(Vector3Normalize(moveDir), speed * dt);
	}

	// Apply gravity
	game.velocity.y -= PLAYER_GRAVITY * dt;

	// Jump
	if (game.is_grounded && IsKeyPressed(KEY_SPACE)) {
		game.velocity.y = PLAYER_JUMP_FORCE;
		game.is_grounded = false;
	}

	// Horizontal movement with sliding collision
	Vector3 remainingMove = moveDir;
	for (int iter = 0; iter < 4 && Vector3Length(remainingMove) > 0.001f; iter++) {
		RayCollision closestHit = { 0 };
		closestHit.distance = FLT_MAX;
		bool hitFound = false;

		float heights[] = { -eyeHeight + 10.0f, -eyeHeight / 2.0f, 0.0f };
		Vector3 currentPos = game.pos;

		for (int i = 0; i < 3; i++) {
			Vector3 start = Vector3Add(currentPos, (Vector3){0, heights[i], 0});
			Vector3 dir = Vector3Normalize(remainingMove);
			float dist = Vector3Length(remainingMove) + PLAYER_RADIUS;
			Vector3 end = Vector3Add(start, Vector3Scale(dir, dist));

			RayCollision col;
			if (CheckWorldCollision(start, end, &col)) {
				// Adjust distance to be relative to the player boundary
				float adjustedDist = col.distance - PLAYER_RADIUS;
				if (adjustedDist < closestHit.distance) {
					closestHit = col;
					closestHit.distance = adjustedDist;
					hitFound = true;
				}
			}
		}

		if (hitFound) {
			// Move as far as possible
			float moveDist = fmaxf(0, closestHit.distance - 0.1f);
			Vector3 moveStep = Vector3Scale(Vector3Normalize(remainingMove), moveDist);
			game.pos.x += moveStep.x;
			game.pos.z += moveStep.z;

			// Project remaining movement onto the plane of the wall
			Vector3 slideNormal = { closestHit.normal.x, 0, closestHit.normal.z };
			if (Vector3Length(slideNormal) > 0.001f) {
				slideNormal = Vector3Normalize(slideNormal);
				remainingMove = Vector3Subtract(remainingMove, moveStep);
				float dot = Vector3DotProduct(remainingMove, slideNormal);
				remainingMove = Vector3Subtract(remainingMove, Vector3Scale(slideNormal, dot));
			} else {
				remainingMove = (Vector3){0, 0, 0};
			}
		} else {
			// No collision found, move the rest of the way
			game.pos.x += remainingMove.x;
			game.pos.z += remainingMove.z;
			break;
		}
	}

	// Vertical movement with collision
	float verticalMove = game.velocity.y * dt;
	Vector3 vStart = game.pos;

	if (verticalMove < 0) { // Falling/Down
		Vector3 start = vStart;
		// Check slightly below feet
		Vector3 end = Vector3Add(vStart, (Vector3){0, verticalMove - eyeHeight, 0});
		RayCollision vCol;
		if (CheckWorldCollision(start, end, &vCol) && vCol.normal.y > 0.5f) {
			game.pos.y = vCol.point.y + eyeHeight;
			game.velocity.y = 0;
			game.is_grounded = true;
		} else {
			game.pos.y += verticalMove;
			game.is_grounded = false;
		}
	} else if (verticalMove > 0) { // Jumping/Up
		Vector3 start = vStart;
		// Check above head
		float headHeight = PLAYER_HEIGHT - PLAYER_EYE_HEIGHT;
		Vector3 end = Vector3Add(vStart, (Vector3){0, verticalMove + headHeight, 0});
		RayCollision vCol;
		if (CheckWorldCollision(start, end, &vCol)) {
			game.pos.y = vCol.point.y - headHeight - 1.0f;
			game.velocity.y = 0;
		} else {
			game.pos.y += verticalMove;
		}
		game.is_grounded = false;
	} else {
		// Not moving vertically, but check if we are still on ground
		Vector3 start = vStart;
		Vector3 end = Vector3Add(vStart, (Vector3){0, -eyeHeight - 2.0f, 0});
		RayCollision vCol;
		if (CheckWorldCollision(start, end, &vCol) && vCol.normal.y > 0.5f) {
			game.is_grounded = true;
			// Snap to floor if very close
			if (vCol.distance < eyeHeight + 1.0f) {
				game.pos.y = vCol.point.y + eyeHeight;
			}
		} else {
			game.is_grounded = false;
		}
	}
}

static void MoveFly(float dt) {
	// Full 3D movement based on yaw/pitch
	Vector3 forward = { 
		cosf(game.pitch) * sinf(game.yaw), 
		-sinf(game.pitch), 
		cosf(game.pitch) * cosf(game.yaw) 
	};
	Vector3 right = { sinf(game.yaw - PI/2.0f), 0, cosf(game.yaw - PI/2.0f) };

	Vector3 move = { 0 };
	if (IsKeyDown(KEY_W)) move = Vector3Add(move, forward);
	if (IsKeyDown(KEY_S)) move = Vector3Subtract(move, forward);
	if (IsKeyDown(KEY_D)) move = Vector3Add(move, right);
	if (IsKeyDown(KEY_A)) move = Vector3Subtract(move, right);

	// Fly up/down
	if (IsKeyDown(KEY_SPACE)) move.y += 1.0f;
	if (IsKeyDown(KEY_LEFT_CONTROL)) move.y -= 1.0f;

	float speed = PLAYER_FLY_SPEED;
	if (IsKeyDown(KEY_LEFT_SHIFT) || IsKeyDown(KEY_RIGHT_SHIFT)) speed *= PLAYER_SPRINT_MULTIPLIER;

	if (Vector3Length(move) > 0) {
		move = Vector3Scale(Vector3Normalize(move), speed * dt);
		game.pos = Vector3Add(game.pos, move);
	}

	game.velocity = (Vector3){0, 0, 0};
	game.is_grounded = false;
}

void UpdatePlayer(void) {
	float dt = GetFrameTime();
	Vector3 oldPos = game.pos;

	if (IsKeyPressed(KEY_F)) {
		game.move_mode = (game.move_mode == MOVE_NORMAL) ? MOVE_FLY : MOVE_NORMAL;
		TraceLog(LOG_INFO, "Movement mode: %s", (game.move_mode == MOVE_NORMAL) ? "NORMAL" : "FLY");
	}

	PlayerRotate();

	// Crouching logic
	bool canStand = true;
	if (game.crouch_amount > 0.1f) {
		Vector3 headPos = game.pos;
		Vector3 headEnd = Vector3Add(headPos, (Vector3){0, PLAYER_HEIGHT - PLAYER_EYE_HEIGHT + PLAYER_CROUCH_OFFSET, 0});
		RayCollision col;
		if (CheckWorldCollision(headPos, headEnd, &col)) {
			canStand = false;
		}
	}

	float targetCrouch = 0.0f;
	if (IsKeyDown(KEY_LEFT_CONTROL) || !canStand) targetCrouch = 1.0f;
	game.crouch_amount = Lerp(game.crouch_amount, targetCrouch, PLAYER_CROUCH_SPEED * dt);

	// Leaning logic
	float targetLean = 0.0f;
	if (IsKeyDown(KEY_Q)) targetLean -= 1.0f;
	if (IsKeyDown(KEY_E)) targetLean += 1.0f;

	game.lean_amount = Lerp(game.lean_amount, targetLean, PLAYER_LEAN_SPEED * dt);

	if (game.move_mode == MOVE_FLY) {
		MoveFly(dt);
	} else {
		MoveNormal(dt);
	}

	// Apply lean as a pivot from the neck/waist
	float leanAngle = game.lean_amount * PLAYER_LEAN_ANGLE * DEG2RAD;
	Vector3 bodyForward = { sinf(game.yaw), 0, cosf(game.yaw) };

	float currentEyeHeight = PLAYER_EYE_HEIGHT - (game.crouch_amount * PLAYER_CROUCH_OFFSET);
	float pivotDist = fminf(PLAYER_LEAN_PIVOT_DISTANCE, currentEyeHeight * 0.8f);

	Vector3 neckToEye = { 0, pivotDist, 0 };
	Vector3 rotatedOffset = Vector3RotateByAxisAngle(neckToEye, bodyForward, leanAngle);
	Vector3 pivot = Vector3Subtract(game.pos, (Vector3){ 0, pivotDist, 0 });

	// Update camera based on physical pos + visual lean pivot
	game.camera.position = Vector3Add(pivot, rotatedOffset);

	// Apply roll to up vector
	Vector3 forward = {
		cosf(game.pitch) * sinf(game.yaw),
		-sinf(game.pitch),
		cosf(game.pitch) * cosf(game.yaw)
	};

	// Use a stable world-up vector and apply lean roll
	Vector3 up = { 0, 1, 0 };
	up = Vector3RotateByAxisAngle(up, forward, leanAngle);
	game.camera.up = up;

	// Use a longer target distance for better precision in the projection matrix
	game.camera.target = Vector3Add(game.camera.position, Vector3Scale(forward, 20.0f));

	// Calculate horizontal speed for UI
	if (dt > 0) {
		Vector2 velH = { game.pos.x - oldPos.x, game.pos.z - oldPos.z };
		game.horizontal_speed = Vector2Length(velH) / dt;
	} else {
		game.horizontal_speed = 0;
	}
}