#include "player_controller.h"
#include "core/render.h"
#include "core/input.h"
#include "utils/typeclass_helpers.h"
#include "utils/debug.h"
#include "raylib.h"

START_REFLECT(PlayerController);
REFLECT_TYPECLASS(PlayerController, Drop);
REFLECT_TYPECLASS(PlayerController, SceneNodeEntity);
END_REFLECT(PlayerController);

impl_Drop_for(PlayerController,
    DestroyPlayerController
)

impl_SceneNodeEntity_for(PlayerController,
    PlayerControllerEnterTree,
    PlayerControllerExitTree,
    PlayerControllerTick
)

SceneNode *CreatePlayerController() {
    PlayerController *self = new(PlayerController);
    *self = (PlayerController) {
        .node = NULL,
        .transform = tc_null(Transformable),
        .rotation = Vector3Zero(),
        .fly_input = Vector3Zero(),
        .speed = 10.f,
        .brake = false
    };
    return CreateSceneNode(PlayerController_as_SceneNodeEntity(self));
}

void DestroyPlayerController(PlayerController *self) {
    free(self);
}

void PlayerControllerEnterTree(PlayerController *self) {
    self->transform = TC_CAST(self->node->parent->entity, Transformable);
    DisableCursor();
    AddListener("pitch_up", ButtonInputListener(self, PlayerControllerInputPitchUp));
    AddListener("pitch_down", ButtonInputListener(self, PlayerControllerInputPitchDown));
    AddListener("roll_left", ButtonInputListener(self, PlayerControllerInputRollLeft));
    AddListener("roll_right", ButtonInputListener(self, PlayerControllerInputRollRight));
    AddListener("yaw_left", ButtonInputListener(self, PlayerControllerInputYawLeft));
    AddListener("yaw_right", ButtonInputListener(self, PlayerControllerInputYawRight));
    AddListener("stop", ButtonInputListener(self, PlayerControllerInputBrake));
}

void PlayerControllerExitTree(PlayerController *self) {
    RemoveAllListeners(self);
}

static
float MoveTowards(float from, float to, float delta) {
    if(from == to) return to;
    float const diff = to - from;
    float const sign = signbit(diff) ? -1.f : 1.f;
    return from + sign * fminf(delta, fabsf(diff));
}

//! angular acceleration limited to local X and Z axes
static
void PlayerControllerTickAngularAcceleration(PlayerController *self, double delta) {
    self->rotation.x = MoveTowards(self->rotation.x, self->fly_input.x, delta * 4.f);
    self->rotation.y = MoveTowards(self->rotation.y, self->fly_input.y, delta * 4.f);
    self->rotation.z = MoveTowards(self->rotation.z, self->fly_input.z, delta * 4.f);
}

//! linear acceleration limited to the local Z axis
static
void PlayerControllerTickLinearAcceleration(PlayerController *self, double delta) {
    float const target = self->brake ? 5.f : (self->rotation.y == 0.f ? 30.f : 10.f);
    float const speed_diff = target - self->speed;
    self->speed = self->speed + copysignf(fminf(fabsf(speed_diff), 15.f * delta), speed_diff);
}

//! Update linear transform based on velocities
static
void PlayerControllerTickTransform(PlayerController *self, double delta) {
    Transform global_transform = self->transform.tc->get_global_transform(self->transform.data);
    Matrix global_matrix = TransformGetMatrix(global_transform);
    global_transform.translation = Vector3Add(global_transform.translation, Vector3Scale(MATRIX_FORWARD(global_matrix), self->speed * delta));
    global_transform.rotation = QuaternionMultiply(QuaternionFromAxisAngle(MATRIX_FORWARD(global_matrix), self->rotation.x * 2.f * delta), global_transform.rotation);
    global_transform.rotation = QuaternionMultiply(QuaternionFromAxisAngle(MATRIX_RIGHT(global_matrix), self->rotation.y * 2.5f * delta), global_transform.rotation);
    global_transform.rotation = QuaternionMultiply(QuaternionFromAxisAngle(MATRIX_UP(global_matrix), self->rotation.z * 0.5f * delta), global_transform.rotation);
    self->transform.tc->set_global_transform(self->transform.data, global_transform);
}

void PlayerControllerTick(PlayerController *self, double delta) {
    PlayerControllerTickAngularAcceleration(self, delta);
    PlayerControllerTickLinearAcceleration(self, delta);
    PlayerControllerTickTransform(self, delta);
}

void PlayerControllerInputRollLeft(PlayerController *self, bool value) {
    self->fly_input.x += value ? -1 : +1;
}

void PlayerControllerInputRollRight(PlayerController *self, bool value) {
    self->fly_input.x += value ? +1 : -1;
}

void PlayerControllerInputPitchUp(PlayerController *self, bool value) {
    self->fly_input.y += value ? -1 : +1;
}

void PlayerControllerInputPitchDown(PlayerController *self, bool value) {
    self->fly_input.y += value ? +1 : -1;
}

void PlayerControllerInputYawRight(PlayerController *self, bool value) {
    self->fly_input.z += value ? -1 : +1;
}

void PlayerControllerInputYawLeft(PlayerController *self, bool value) {
    self->fly_input.z += value ? +1 : -1;
}

void PlayerControllerInputBrake(PlayerController *self, bool value) {
    self->brake = value;
}