#include "transformable.h"
#include "utils/debug.h"

Transform TransformIdentity() {
    return (Transform){
        .translation = { 0.f, 0.f, 0.f },
        .rotation = QuaternionIdentity(),
        .scale = { 1.f, 1.f, 1.f }
    };
}

Matrix TransformGetMatrix(Transform self) {
    Matrix mat = MatrixScale(self.scale.x, self.scale.y, self.scale.z);
    mat = MatrixMultiply(mat, QuaternionToMatrix(self.rotation));
    return MatrixMultiply(mat, MatrixTranslate(self.translation.x, self.translation.y, self.translation.z));
}

Vector3 TransformPosition(Transform self, Vector3 local_pos) {
    return Vector3Add(TransformDirection(self, local_pos), self.translation);
}

Vector3 TransformDirection(Transform self, Vector3 local_direction) {
    Matrix const m = TransformGetMatrix(self);
    return Vector3Add(Vector3Add(Vector3Scale(MATRIX_RIGHT(m), local_direction.x), Vector3Scale(MATRIX_UP(m), local_direction.y)), Vector3Scale(MATRIX_FORWARD(m), local_direction.z));
}

Vector3 TransformScale(Transform self, Vector3 local_scale) {
    return Vector3Multiply(local_scale, self.scale);
}

Quaternion TransformRotation(Transform self, Quaternion local_rotation) {
    return QuaternionMultiply(self.rotation, local_rotation);
}

Transform TransformTransform(Transform self, Transform other) {
    return (Transform) {
        .translation = TransformPosition(self, other.translation),
        .scale = TransformScale(self, other.scale),
        .rotation = TransformRotation(self, other.rotation)
    };
}

Vector3 InverseTransformPosition(Transform self, Vector3 global_pos) {
    return Vector3Subtract(InverseTransformDirection(self, global_pos), self.translation);
}

Vector3 InverseTransformDirection(Transform self, Vector3 global_direction) {
    Matrix const mat = TransformGetMatrix(self);
    return (Vector3){
        Vector3DotProduct(MATRIX_RIGHT(mat), Vector3Scale(VECTOR3_RIGHT, global_direction.x)),
        Vector3DotProduct(MATRIX_UP(mat), Vector3Scale(VECTOR3_UP, global_direction.y)),
        Vector3DotProduct(MATRIX_FORWARD(mat), Vector3Scale(VECTOR3_FORWARD, global_direction.z))
    };
}

Vector3 InverseTransformScale(Transform self, Vector3 global_scale) {
    return Vector3Multiply(global_scale, Vector3Invert(self.scale));
}

Quaternion InverseTransformRotation(Transform self, Quaternion global_rotation) {
    return QuaternionMultiply(QuaternionInvert(self.rotation), global_rotation);
}

Transform InverseTransformTransform(Transform self, Transform other) {
    return (Transform) {
        .translation = InverseTransformPosition(self, other.translation),
        .scale = InverseTransformScale(self, other.scale),
        .rotation = InverseTransformRotation(self, other.rotation)
    };
}