#include <math.h>
#include "transform.h"
#include "mons_math/vec3.h"
#include "mons_math/quat.h"
#include <stdio.h>

mons_mat4 translation_to_mat4(mons_vec3 translation) {
    mons_mat4 result = MONS_MAT4_IDENTITY;
    result.m1.w = translation.x;
    result.m2.w = translation.y;
    result.m3.w = translation.z;
    return result;
}

mons_mat4 scale_to_mat4(mons_vec3 scale) {
    mons_mat4 result = MONS_MAT4_IDENTITY;
    result.m1.x = scale.x;
    result.m2.y = scale.y;
    result.m3.z = scale.z;
    return result;
}

void mons_transform_translate(mons_transform *transform,
                              mons_vec3 translation) {
    mons_vec3_add_inplace(&transform->translation, translation);
}

void mons_transform_rotate(mons_transform *transform, mons_quat rotation) {
    mons_quat_mul_inplace(&transform->rotation, rotation);
}

void mons_transform_scale(mons_transform *transform, mons_vec3 scale) {
    mons_vec3_mul_memberwise_inplace(&transform->scale, scale);
}

mons_mat4 mons_transform_matrix(mons_transform transform) {
    mons_mat4 translation = translation_to_mat4(transform.translation);
    mons_mat4 result = translation;
    mons_mat4_mul_inplace(&result, mons_quat_to_mat4(transform.rotation));
    mons_mat4_mul_inplace(&result, scale_to_mat4(transform.scale));
    return result;
}

mons_vec3 mons_transform_forward(mons_transform transform) {
    return mons_quat_transform_vec3(transform.rotation, MONS_VEC3_Z);
}

mons_vec3 mons_transform_up(mons_transform transform) {
    return mons_quat_transform_vec3(transform.rotation, MONS_VEC3_Y);
}

mons_vec3 mons_transform_right(mons_transform transform) {
    return mons_quat_transform_vec3(transform.rotation, MONS_VEC3_X);
}

void mons_transform_look_at(mons_transform *transform, mons_vec3 point, mons_vec3 up) {
    mons_vec3 forward = mons_vec3_normalize(mons_vec3_sub(transform->translation, point));
    mons_vec3 right = mons_vec3_normalize(mons_vec3_cross(up, forward));
    mons_vec3 new_up = mons_vec3_cross(forward, right);
    float m00 = right.x;
    float m01 = right.y;
    float m02 = right.z;
    float m10 = new_up.x;
    float m11 = new_up.y;
    float m12 = new_up.z;
    float m20 = forward.x;
    float m21 = forward.y;
    float m22 = forward.z;

    float num8 = (m00 + m11) + m22;
    mons_quat quaternion = {0};
    if (num8 > 0.0f) {
        float num = sqrt(num8 + 1.0f);
        quaternion.w = num * 0.5f;
        num = 0.5f / num;
        quaternion.x = (m12 - m21) * num;
        quaternion.y = (m20 - m02) * num;
        quaternion.z = (m01 - m10) * num;
        transform->rotation = quaternion;
        return;
    }
    if ((m00 >= m11) && (m00 >= m22))
    {
        float num7 = (float)sqrt(((1.0f + m00) - m11) - m22);
        float num4 = 0.5f / num7;
        quaternion.x = 0.5f * num7;
        quaternion.y = (m01 + m10) * num4;
        quaternion.z = (m02 + m20) * num4;
        quaternion.w = (m12 - m21) * num4;
        transform->rotation = quaternion;
        return;
    }
    if (m11 > m22)
    {
        float num6 = (float)sqrt(((1.0f + m11) - m00) - m22);
        float num3 = 0.5f / num6;
        quaternion.x = (m10 + m01) * num3;
        quaternion.y = 0.5f * num6;
        quaternion.z = (m21 + m12) * num3;
        quaternion.w = (m20 - m02) * num3;
        transform->rotation = quaternion;
        return;
    }
    float num5 = (float)sqrt(((1.0f + m22) - m00) - m11);
    float num2 = 0.5f / num5;
    quaternion.x = (m20 + m02) * num2;
    quaternion.y = (m21 + m12) * num2;
    quaternion.z = 0.5f * num5;
    quaternion.w = (m01 - m10) * num2;
    transform->rotation = quaternion;
    return;
}