#include "physics.h"
#include <math.h>

Vector3 apply_gravity(Vector3 velocity, float delta) {
	velocity.y += GRAVITY * delta;
	return velocity;
}

Vector3 apply_friction(Vector3 velocity, float delta, bool grounded) {
	if (!grounded) return velocity;
	velocity.x *= 1.0f - delta * 6.0f;
	velocity.z *= 1.0f - delta * 6.0f;
	return velocity;
}

Vector3 ground_accelerate(Vector3 velocity, Vector3 wishDir, float wishSpeed, float delta) {
	float accel = 10.0f;
	Vector3 change = Vector3Scale(wishDir, accel * delta);
	return Vector3Add(velocity, change);
}

Vector3 air_accelerate(Vector3 velocity, Vector3 wishDir, float wishSpeed, float delta) {
	float accel = 2.5f;
	Vector3 change = Vector3Scale(wishDir, accel * delta);
	return Vector3Add(velocity, change);
}

Vector3 calculate_wish_direction(InputState *input, float yaw) {
	Vector3 forward = { sinf(yaw), 0, cosf(yaw) };
	Vector3 right = { -forward.z, 0, forward.x };
	Vector3 wish = {0};

	if (input->forward) wish = Vector3Add(wish, forward);
	if (input->backward) wish = Vector3Subtract(wish, forward);
	if (input->left) wish = Vector3Subtract(wish, right);
	if (input->right) wish = Vector3Add(wish, right);
	return Vector3Normalize(wish);
}