#include "mons_math/quat.h"
#include "mons_math/mat4.h"
#include "mons_math/vec3.h"
#include "mons_math/vec4.h"
#include <math.h>
#include <stdlib.h>
#include <stdio.h>

mons_quat mons_quat_from_axis_angle(mons_vec3 axis, float angle) {
    float half_angle = 0.5f * angle;
    float s = sinf(half_angle);
    return (mons_quat){
        axis.x * s,
        axis.y * s,
        axis.z * s,
        cosf(half_angle),
    };
}

mons_quat mons_quat_mul(mons_quat a, mons_quat b) {
    return (mons_quat){
        (a.w * b.x) + (a.x * b.w) + (a.y * b.z) - (a.z * b.y),
        (a.w * b.y) + (a.y * b.w) + (a.z * b.x) - (a.x * b.z),
        (a.w * b.z) + (a.z * b.w) + (a.x * b.y) - (a.y * b.x),
        (a.w * b.w) - (a.x * b.x) - (a.y * b.y) - (a.z * b.z),
    };
}

void mons_quat_mul_inplace(mons_quat *a, mons_quat b) {
    *a = mons_quat_mul(*a, b);
}

mons_quat mons_quat_mul_f(mons_quat a, float b) {
    return (mons_quat){
        a.x * b,
        a.y * b,
        a.z * b,
        a.w * b,
    };
}

void mons_quat_mul_f_inplace(mons_quat *a, float b) {
    a->x *= b;
    a->y *= b;
    a->z *= b;
    a->w *= b;
}

mons_quat mons_quat_mul_i(mons_quat a, int b) {
    return (mons_quat){
        a.x * b,
        a.y * b,
        a.z * b,
        a.w * b,
    };
}

void mons_quat_mul_i_inplace(mons_quat *a, int b) {
    a->x *= b;
    a->y *= b;
    a->z *= b;
    a->w *= b;
}

mons_mat4 mons_quat_to_mat4(mons_quat a) {
    return (mons_mat4){
        {
            1.0 - (2.0 * a.y * a.y) - (2.0 * a.z * a.z),
            (2.0 * a.x * a.y) - (2.0 * a.w * a.z),
            (2.0 * a.x * a.z) + (2.0 * a.w * a.y),
            0.0,
        },
        {
            (2.0 * a.x * a.y) + (2.0 * a.w * a.z),
            1.0 - (2.0 * a.x * a.x) - (2.0 * a.z * a.z),
            (2.0 * a.y * a.z) - (2.0 * a.w * a.x),
            0.0,
        },
        {
            (2.0 * a.x * a.z) - (2.0 * a.w * a.y),
            (2.0 * a.y * a.z) + (2.0 * a.w * a.x),
            1.0 - (2.0 * a.x * a.x) - (2.0 * a.y * a.y),
            0.0,
        },
        {
            0.0,
            0.0,
            0.0,
            1.0,
        },
    };
}

mons_quat mons_quat_normalize(mons_quat a) {
    mons_vec4 normalized = mons_vec4_normalize(*((mons_vec4 *)&a));
    return *((mons_quat *)&normalized);
}

void mons_quat_normalize_inplace(mons_quat *a) {
    mons_vec4_normalize_inplace((mons_vec4 *)a);
}

mons_vec3 mons_quat_transform_vec3(mons_quat quat, mons_vec3 vec) {
    mons_vec3 quat_vec = (mons_vec3){quat.x, quat.y, quat.z};
    float s = quat.w;

    return mons_vec3_add(
        mons_vec3_add(
            mons_vec3_mul_f(quat_vec, mons_vec3_dot(quat_vec, vec) * 2.0f),
            mons_vec3_mul_f(vec, s * s - mons_vec3_dot(quat_vec, quat_vec))),
        mons_vec3_mul_f(mons_vec3_cross(quat_vec, vec), 2.0f * s));
}

mons_quat mons_quat_conjugate(mons_quat quat) {
    return (mons_quat){
        .x = -quat.x,
        .y = -quat.y,
        .z = -quat.z,
        .w = quat.w,
    };
}

mons_quat mons_quat_looking_at(mons_vec3 eye, mons_vec3 target, mons_vec3 up) {
    mons_vec3 f = mons_vec3_normalize(mons_vec3_sub(target, eye));
    mons_vec3 side = mons_vec3_normalize(mons_vec3_cross(up, f));
    mons_vec3 b = mons_vec3_normalize(mons_vec3_add(f, MONS_VEC3_NEG_Z));
    mons_quat p = mons_quat_from_axis_angle(mons_vec3_cross(b, f), mons_vec3_dot(b,f));
    mons_vec3 r = (mons_vec3) {
        p.w*p.w + p.x*p.x - p.y*p.y - p.z*p.z,
        (2.0f * p.x * p.y) - (2.0f * p.w * p.z),
        (2.0f * p.x * p.z) + (2.0f * p.w * p.y),
    };
    b = mons_vec3_normalize(mons_vec3_add(side, r));
    mons_quat q = mons_quat_from_axis_angle(mons_vec3_cross(b,side), mons_vec3_dot(b, side));

    return mons_quat_mul(p, q);
}

void mons_quat_print(mons_quat quat) {
    mons_vec4_print(*(mons_vec4*)&quat);
}