#include "player_states.h"
#include "core/config/engine.h"
#include "core/math/math_funcs.h"
#include "core/typedefs.h"
#include "going/player_body.h"


PlayerBody *PlayerState::get_body() const {
	return this->body;
}

PlayerState::StateID StandingState::get_next_state() const {
	if(!this->get_body()->is_on_floor()) {
		return FallingState::get_class_static();
	} else {
		return !this->get_body()->get_movement_input().is_zero_approx()
			? RunningState::get_class_static()
			: self_type::get_class_static();
	}
}

void StandingState::state_entered() {
	this->get_body()->get_anim()->play("RESET", 0.1);
}

void StandingState::physics_process(double delta) {
	Vector3 const current{this->get_body()->get_velocity()};
	float const speed_delta{float(this->get_body()->get_stopping_deceleration() * delta)};
	this->get_body()->set_velocity(current.move_toward(Vector3(), speed_delta));
}

PlayerState::StateID RunningState::get_next_state() const {
	if(!this->get_body()->is_on_floor()) {
		return FallingState::get_class_static();
	} else if(this->get_body()->get_velocity().is_zero_approx()) {
		return StandingState::get_class_static();
	} else if(Input::get_singleton()->is_action_just_pressed(PlayerBody::split_step_action) && this->get_body()->get_velocity().length() > this->get_body()->get_min_step_speed()) {
		return SplitStepState::get_class_static();
	} else {
		return self_type::get_class_static();
	}
}

void RunningState::state_entered() {
	this->get_body()->get_anim()->play("run", 0.1);
}

void RunningState::process(double delta) {
	if(!this->get_body()->get_velocity().is_zero_approx()) {
		Vector3 const current{this->get_body()->get_velocity()};
		Vector3 const cross{Vector3{0.f, 1.f, 0.f}.cross(current).normalized()};
		Vector3 const desired_direction{this->get_body()->get_desired_direction()};
		this->lean_modifier = Math::move_toward(
			this->lean_modifier,
			cross.dot(desired_direction) * this->get_body()->get_model_lean(),
			float(this->get_body()->get_model_lean_speed() * delta)
		);
		Vector3 up{Vector3{0.0, 1.0, 0.0} + cross * this->lean_modifier};
		this->get_body()->get_model()->look_at(this->get_body()->get_global_position() - current, up);
	}
}

void RunningState::physics_process(double delta) {
	Vector3 const current{this->get_body()->get_velocity()};
	Vector3 const desired{this->get_body()->get_desired_velocity()};
	float const dot{current.dot(desired)};
	float const speed_delta{desired.is_zero_approx() || dot < 0.f // if we're stopping or making a full turnaround
		? float(this->get_body()->get_stopping_deceleration() * delta) // deceleration
		: (current.is_zero_approx() // if we're starting from standstill
			? this->get_body()->get_start_speed() // startup boost
			: float(this->get_body()->get_acceleration() * delta) // in-motion velocity
		)
	};
	this->get_body()->set_velocity(current.move_toward(desired, speed_delta));
}

void RunningState::state_exited() {
	this->get_body()->get_model()->look_at(this->get_body()->get_global_position() - this->get_body()->get_velocity());
	this->lean_modifier = 0.f;
}

PlayerState::StateID SplitStepState::get_next_state() const {
	if(!this->get_body()->is_on_floor()) {
		return FallingState::get_class_static();
	} else {
		return this->timer <= 0.0
			? RunningState::get_class_static()
			: self_type::get_class_static();
	}
}

void SplitStepState::state_entered() {
	this->last_velocity = this->get_body()->get_velocity();
	this->timer = this->get_body()->get_split_step_time();
	this->get_body()->set_velocity(last_velocity.normalized() * this->get_body()->get_target_speed() * 0.75f);
	this->get_body()->get_anim()->play("split-step");
}

void SplitStepState::process(double delta) {
	this->timer -= delta;
	this->get_body()->set_velocity(this->get_body()->get_velocity()
		.move_toward(Vector3(), this->get_body()->get_target_speed() / this->get_body()->get_split_step_stop_time() * delta));
}

void SplitStepState::state_exited() {
	if(this->get_body()->is_on_floor()) {
		Vector3 const desired_direction{this->get_body()->get_desired_direction()};
		float const dot{this->last_velocity.normalized().dot(desired_direction)};
		this->get_body()->set_velocity(dot > -0.8f
			? desired_direction * MAX(last_velocity.length(), this->get_body()->get_step_boost())
			: Vector3()
		);
	}
}

PlayerState::StateID FallingState::get_next_state() const {
	if(this->get_body()->is_on_floor()) {
		return RunningState::get_class_static();
	} else {
		return self_type::get_class_static();
	}
}

void FallingState::state_entered() {
	this->get_body()->get_anim()->play("falling", 0.1);
}

void FallingState::process(double delta) {
	Vector3 const current{this->get_body()->get_velocity()};
	Vector3 const flattened{current.x, 0.f, current.z};
	this->get_body()->set_velocity((flattened - (flattened * 0.025f)) + Vector3{0.f, current.y - float(9.8 * delta), 0.f});
}

void PlayerStateMachine::_bind_methods() {
}

void PlayerStateMachine::_notification(int what) {
	if(Engine::get_singleton()->is_editor_hint()) {
		return;
	}
	switch(what) {
	default:
		return;
	case NOTIFICATION_READY:
		this->ready();
		break;
	case NOTIFICATION_PROCESS:
		this->current_state->process(this->get_process_delta_time());
		this->try_transition();
		break;
	case NOTIFICATION_PHYSICS_PROCESS:
		this->current_state->physics_process(this->get_physics_process_delta_time());
		break;
	}
}

void PlayerStateMachine::ready() {
	this->set_process(true);
	this->set_physics_process(true);
	this->body = Object::cast_to<PlayerBody>(this->get_parent());
	this->add_state<StandingState>();
	this->add_state<RunningState>();
	this->add_state<SplitStepState>();
	this->add_state<FallingState>();
}

void PlayerStateMachine::try_transition() {
	PlayerState::StateID next{this->current_state->get_next_state()};
	if(next != this->current_state->get_class()) {
		this->current_state->state_exited();
		this->current_state = this->states[next];
		this->current_state->state_entered();
	}
}
