#import "Math";

Transform_Identity : Transform : .{ .{0,0,0}, .{0,0,0,1}, .{1,1,1}, Matrix4_Identity, Matrix4_Identity, false };

Transform :: struct {
    position: Vector3;
    orientation: Quaternion;
    scale   : Vector3;

    model_matrix: Matrix4 = Matrix4_Identity; @DontSerialize

    world_matrix: Matrix4; @DontSerialize

    dirty: bool; @DontSerialize
}

create_identity_transform :: () -> Transform {
    transform : Transform;

    transform.position = .{};
    transform.orientation = .{0,0,0,1};
    transform.scale = .{1,1,1};
    transform.model_matrix = Matrix4_Identity;

    update_matrix(*transform);

    return transform;
}

make_matrix :: (position: Vector3, orientation: Quaternion, scale: Vector3) -> Matrix4 {
    trans_mat := make_translation_matrix4(position);
    scale_mat := make_scale_matrix4(scale);
    rot_mat := rotation_matrix(Matrix4, orientation);
    return trans_mat * rot_mat * scale_mat;
}

create_transform :: (position: Vector3=.{0,0,0}, orientation: Quaternion = .{0,0,0,1}, scale: Vector3=.{1,1,1}) -> Transform {
    transform : Transform;

    transform.position = position;
    transform.orientation = orientation;
    transform.scale = scale;
    transform.model_matrix = make_matrix(position, orientation, scale);

    return transform;
}

update_matrix :: (transform: *Transform) {
    transform.model_matrix = make_matrix(transform.position, transform.orientation, transform.scale);
    transform.dirty = true;
}

set_position :: (e: *Entity, position: Vector3, calculate_matrix: bool = true) {
    set_position(*e.transform, position, calculate_matrix);
}

set_position :: (transform: *Transform, position: Vector3, calculate_matrix: bool = true) {
    transform.position = position;
    if calculate_matrix update_matrix(transform);
}

set_position :: (transform: *Transform, x: float, y: float, z: float, calculate_matrix: bool = true) {
    transform.position.x = x;
    transform.position.y = y;
    transform.position.z = z;
    if calculate_matrix update_matrix(transform);
}

translate :: (transform: *Transform, translation: Vector3, calculate_matrix: bool = true) {
    transform.position += translation;
    if calculate_matrix update_matrix(transform);
}

euler_to_quaternion :: (value: Vector3) -> Quaternion {
    return euler_to_quaternion(value.y, value.x, value.z);
}

euler_to_quaternion :: (yaw: float, pitch: float, roll: float) -> Quaternion {
    cy := cos(yaw * 0.5);
    sy := sin(yaw * 0.5);
    cp := cos(pitch * 0.5);
    sp := sin(pitch * 0.5);
    cr := cos(roll * 0.5);
    sr := sin(roll * 0.5);

    q: Quaternion;
    q.w = cr * cp * cy + sr * sp * sy;
    q.x = sr * cp * cy - cr * sp * sy;
    q.y = cr * sp * cy + sr * cp * sy;
    q.z = cr * cp * sy - sr * sp * cy;

    return q;
}

sign :: (val: $T) -> T {
    if val < 0 return -1.0;
    if val > 0 return 1.0;
    return 0.0;
}

quaternion_to_euler_v3 :: (q: Quaternion) -> Vector3 {
    yaw, pitch, roll := quaternion_to_euler(q);

    v : Vector3 = ---;
    v.x = pitch;
    v.y = yaw;
    v.z = roll;

    return v;
}

quaternion_to_euler :: (q: Quaternion) -> yaw: float, pitch: float, roll: float {
    yaw: float;
    pitch: float;
    roll: float;

    sinr_cosp := 2.0 * (q.w * q.x + q.y * q.z);
    cosr_cosp := 1.0 - 2.0 * (q.x * q.x + q.y * q.y);
    roll = atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    sinp := 2.0 * (q.w * q.y - q.z * q.x);
    if abs(sinp) >= 1.0
        pitch = sign(sinp) * (PI / 2); // use 90 degrees if out of range
    else
        pitch = asin(sinp);

    // yaw (z-axis rotation)
    siny_cosp := 2.0 * (q.w * q.z + q.x * q.y);
    cosy_cosp := 1.0 - 2.0 * (q.y * q.y + q.z * q.z);
    yaw = atan2(siny_cosp, cosy_cosp);

    return yaw, pitch, roll;
}

set_rotation :: (e: *Entity, orientation: Quaternion, calculate_matrix: bool = true) {
    set_rotation(*e.transform, orientation, calculate_matrix);
}

set_rotation :: (transform: *Transform, orientation: Quaternion, calculate_matrix: bool = true) {
    transform.orientation = orientation;
    if calculate_matrix update_matrix(transform);
}

set_rotation :: (transform: *Transform, euler_angles: Vector3, calculate_matrix: bool = true) {
    orientation := euler_to_quaternion(degrees_to_radians(euler_angles.y), degrees_to_radians(euler_angles.x), degrees_to_radians(euler_angles.z));
    transform.orientation = orientation;
    if calculate_matrix update_matrix(transform);
}

set_rotation :: (e: *Entity, euler_angles: Vector3, calculate_matrix: bool = true) {
    set_rotation(*e.transform, euler_angles, calculate_matrix);
}

set_scale :: (e: *Entity, scale: Vector3, calculate_matrix: bool = true) {
    set_scale(*e.transform, scale, calculate_matrix);
}

set_scale :: (e: *Entity, scale: float, calculate_matrix: bool = true) {
    set_scale(*e.transform, scale, calculate_matrix);
}

set_scale :: (transform: *Transform, scale: Vector3, calculate_matrix: bool = true) {
    transform.scale = scale;
    if calculate_matrix update_matrix(transform);
}

set_scale :: (transform: *Transform, scale: float, calculate_matrix: bool = true) {
    transform.scale = .{scale, scale, scale};
    if calculate_matrix update_matrix(transform);
}

set_position_rotation_scale :: (transform: *Transform, position: Vector3, rotation: Quaternion, scale: Vector3, calculate_matrix: bool = true) {
    transform.position = position;
    transform.orientation = rotation;
    transform.scale = scale;
    if calculate_matrix update_matrix(transform);
}

set_position_rotation_scale :: (e: *Entity, position: Vector3, rotation: Quaternion, scale: Vector3, calculate_matrix: bool = true) {
    set_position_rotation_scale(*e.transform, position, rotation, scale, calculate_matrix);
}

get_forward :: (transform: Transform) -> Vector3 {
    v := rotation_matrix(Matrix4, transform.orientation) * Vector4.{0,0,1,0};
    return .{v.x, v.y, v.z};
}

get_right :: (transform: Transform) -> Vector3 {
    v := rotation_matrix(Matrix4, transform.orientation) * Vector4.{1,0,0,0};
    return .{v.x, v.y, v.z};
}

get_up :: (transform: Transform) -> Vector3 {
    v := rotation_matrix(Matrix4, transform.orientation) * Vector4.{0,1,0,0};
    return .{v.x, v.y, v.z};
}

get_position :: (transform: Transform) -> Vector3 {
    return .{transform.model_matrix._14, transform.model_matrix._24, transform.model_matrix._34};
}

get_rotation :: (transform: Transform) -> Vector3 {

    return .{transform.model_matrix._14, transform.model_matrix._24, transform.model_matrix._34};
}