use cgmath::{InnerSpace, Matrix4, SquareMatrix, Vector3, Vector4, vec3, vec4};
use vulkano::buffer::TypedBufferAccess;

use crate::vulkan::{Mesh, gameobject::GameObject};

#[allow(dead_code)]
pub fn print_matrix(mat: Matrix4<f32>) {
    let cols = [
        [mat.x.x, mat.x.y, mat.x.z, mat.x.w],
        [mat.y.x, mat.y.y, mat.y.z, mat.y.w],
        [mat.z.x, mat.z.y, mat.z.z, mat.z.w],
        [mat.w.x, mat.w.y, mat.w.z, mat.w.w],
    ].map(|v| v.map(|e| format!("{:.2}", e)));

    let col_sizes: Vec<usize> = cols.iter().map(|col| col.iter().map(|s| s.len()).max().unwrap()).collect();
    
    println!("Mat4: {}", mat.determinant());
    for row_index in 0..4 {
        for col_index in 0..4 {
            let mut str = format!("{}", cols[col_index][row_index]);
            while str.len() < col_sizes[col_index] {
                str.insert(0, ' ');
            }
            print!("{}", str);
            if col_index < 3 { print!(", "); }
        }
        println!();
    }
}

pub fn intersection_distance(ray_origin: Vector3<f32>, ray_direction: Vector3<f32>, mesh: &Mesh<crate::vulkan::Vertex>, game_object: &GameObject) -> Option<f32> {
    let index_lock = mesh.index_buffer.read().unwrap();
    let vertex_lock = mesh.vertex_buffer.read().unwrap();
    (0..mesh.index_buffer.len() / 3).map(|tri_idx| {
        let vtx_a = game_object.get_model_matrix() * vec4_from_pos(vertex_lock[index_lock[tri_idx as usize * 3    ] as usize].position);
        let vtx_b = game_object.get_model_matrix() * vec4_from_pos(vertex_lock[index_lock[tri_idx as usize * 3 + 1] as usize].position);
        let vtx_c = game_object.get_model_matrix() * vec4_from_pos(vertex_lock[index_lock[tri_idx as usize * 3 + 2] as usize].position);
        intersect_triangle(ray_origin, ray_direction, vtx_a.truncate().into(), vtx_b.truncate().into(), vtx_c.truncate().into())
    }).fold(None, |acc, x| {
        if let Some(smallest) = acc {
            if let Some(new) = x {
                if new < smallest {
                    Some(new)
                } else {
                    Some(smallest)
                }
            } else {
                acc
            }
        } else {
            x
        }
    })
}

// https://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm
pub fn intersect_triangle(ray_origin: Vector3<f32>, ray_direction: Vector3<f32>, vtx_a: [f32; 3], vtx_b: [f32; 3], vtx_c: [f32; 3]) -> Option<f32> {
    let edge_1 = vec3(vtx_b[0] - vtx_a[0], vtx_b[1] - vtx_a[1], vtx_b[2] - vtx_a[2]);
    let edge_2 = vec3(vtx_c[0] - vtx_a[0], vtx_c[1] - vtx_a[1], vtx_c[2] - vtx_a[2]);
    let h = ray_direction.cross(edge_2);
    let a = edge_1.dot(h);
    if a > -f32::EPSILON && a < f32::EPSILON { return None; }

    let f = 1. / a;
    let s: Vector3<f32> = ray_origin - Vector3::from(vtx_a);
    let u = f * s.dot(h);
    if u < 0. || u > 1. { return None; }

    let q = s.cross(edge_1);
    let v = f * ray_direction.dot(q);
    if v < 0. || u + v > 1. { return None; }

    let t = f * edge_2.dot(q);
    if t > f32::EPSILON { return Some(t); }

    None
}

pub fn vec4_from_pos(pos: [f32; 3]) -> Vector4<f32> {
    vec4(pos[0], pos[1], pos[2], 1.0)
}