#load "gjk.jai";

WORLD_UP :: Vector3.{0,1,0};
GRAVITY :: -20.8;

Collider_Type :: enum {
    AABB;
    SPHERE;
    MESH;
    BOX;
}

Sphere :: struct {
    radius: float;
}

Mesh_Collider :: struct {
    vertices : [..] Vector3;
    is_baked: bool;
}

Trigger_Overlap :: struct {
    entity: *Entity;
    frame_index: u64;
}

MAX_TRIGGER_OVERLAPS :: 16;

Collision_Layers :: enum_flags {
    NONE;

    LAYER1;
    LAYER2;
    LAYER3;
    LAYER4;
    LAYER5;
    LAYER6;
    LAYER7;
    LAYER8;
    LAYER9;
    LAYER10;

    ALL :: .LAYER1 | LAYER2 | .LAYER3 | .LAYER4 | .LAYER5 | .LAYER6 | .LAYER7 | .LAYER8 | .LAYER9 | .LAYER10;
}

Collider_Bake_Mode :: enum {
    AABB;

    FULL_MESH;

    ONLY_FACING_UP;
}

Collider :: struct {
    type : Collider_Type;

    bake_mode: Collider_Bake_Mode;

    layer: Collision_Layers = .LAYER1;
    collides_with_layers: Collision_Layers = .LAYER1;

    override_aabb: bool;
    render_aabb: bool;
    aabb_color: Vector4 = .{0,1,0,1};
    aabb: AABB;

    union {
        sphere: Sphere; @DontSerialize
        mesh : Mesh_Collider; @DontSerialize
    }

    overlaps: [MAX_TRIGGER_OVERLAPS] Trigger_Overlap; @DontSerialize
    num_overlaps: s64; @DontSerialize

    ignore: bool; @DontSerialize
}

Physics_Body :: struct {
    enabled: bool = true;
    velocity: Vector3; @DontSerialize

    friction : float = 0.0; @DontSerialize
    bounciness : float = 0.0; @DontSerialize
    linear_damping : float = 0.0; @DontSerialize

    check_for_grounded: bool; @DontSerialize
	grounded: bool; @DontSerialize
}

update_mesh_collider :: (e: *Entity) {
    if e.collider.bake_mode == {
        case .AABB; {
            if e.collider.mesh.vertices.count == 0 {
                array_resize(*e.collider.mesh.vertices, 8);
            }

            m := e.transform.model_matrix;
            e.collider.mesh.vertices[0] = transform_position(e.collider.aabb.min, m);
            e.collider.mesh.vertices[1] = transform_position(e.collider.aabb.max, m);
            e.collider.mesh.vertices[2] = transform_position(.{e.collider.aabb.min.x, e.collider.aabb.min.y, e.collider.aabb.max.z}, m);
            e.collider.mesh.vertices[3] = transform_position(.{e.collider.aabb.max.x, e.collider.aabb.min.y, e.collider.aabb.max.z}, m);
            e.collider.mesh.vertices[4] = transform_position(.{e.collider.aabb.max.x, e.collider.aabb.min.y, e.collider.aabb.min.z}, m);
            e.collider.mesh.vertices[5] = transform_position(.{e.collider.aabb.min.x, e.collider.aabb.max.y, e.collider.aabb.max.z}, m);
            e.collider.mesh.vertices[6] = transform_position(.{e.collider.aabb.max.x, e.collider.aabb.max.y, e.collider.aabb.min.z}, m);
            e.collider.mesh.vertices[7] = transform_position(.{e.collider.aabb.min.x, e.collider.aabb.max.y, e.collider.aabb.min.z}, m);
        }
        case .FULL_MESH; {
            //update_entity_transform(e);

            model := get_model_by_handle(e.renderable.model);
            if model {
                for i: 0..e.renderable.num_nodes-1 {
                    render_node := e.renderable.nodes[i];
                    model_node := model.nodes[i];

                    for mesh_handle: model_node.meshes {
                        mesh := parray_get(*engine.renderer.meshes, mesh_handle);

                        for p: mesh.positions { 
                            position := transform_position(p, render_node.transform.world_matrix);
                            array_add(*e.collider.mesh.vertices, position);
                        }
                    }
                }
            }
        }
    }

    e.collider.mesh.is_baked = true;
}

update_trigger_mesh_colliders :: (scene: *Scene) {
    for e: scene.entities {
        if e.flags & .TRIGGER {
            if e.collider.type == .MESH {
                update_mesh_collider(e);
            }
        }
    }
}

update_mesh_colliders :: (scene: *Scene) {
    for e: scene.entities {
        if e.flags & .COLLISION {
            if e.collider.type == .MESH {
                if e.flags & .STATIC && e.collider.mesh.is_baked continue;

                update_mesh_collider(e);
            }
        }
    }
}

make_sure_entity_does_not_collide :: (e: *Entity, scene: *Scene) {
    if e.flags & .PHYSICS {
        aabb := e.collider.aabb;
        aabb.min += e.transform.position;
        aabb.max += e.transform.position;

        for other_e: scene.entities {
            if e == other_e continue;

            if other_e.flags & .COLLISION {
                other_aabb := other_e.collider.aabb;
                other_aabb.min += other_e.transform.position;
                other_aabb.max += other_e.transform.position;
                if aabb_vs_aabb(aabb, other_aabb) {
                    offset := resolve_aabb_vs_aabb(aabb, other_aabb);
                    set_position(*e.transform, e.transform.position + offset * 1.0001);
                }
            }
        }
    }
}

make_sure_nothing_collides :: (scene: *Scene) {
    for e: scene.entities {
        make_sure_entity_does_not_collide(e, scene);
    }
}

update_gravity :: (scene: *Scene, dt: float) {
    for e: scene.entities {
        if !e.enabled continue;
        if !e.body.enabled continue;

        if e.flags & .PHYSICS {
            #if NETWORKING { if e.is_proxy continue; }

            if e.collider.ignore continue;
            e.body.velocity.y += GRAVITY * dt;
        }
    }
}

update_positions :: (scene: *Scene, dt: float) {
    for e: scene.entities {
        if !e.enabled continue;
        if !e.body.enabled continue;

        #if NETWORKING { if e.is_proxy continue; }

        if e.collider.ignore continue;

        if e.flags & .PHYSICS {
            delta := e.body.velocity * dt;
            set_position(*e.transform, e.transform.position + delta);

            // @Speed: Only do this, if we actually moved
            m := e.transform.model_matrix;
            if e.collider.mesh.vertices.count < 8 continue;

            e.collider.mesh.vertices[0] = transform_position(e.collider.aabb.min, m);
            e.collider.mesh.vertices[1] = transform_position(e.collider.aabb.max, m);
            e.collider.mesh.vertices[2] = transform_position(.{e.collider.aabb.min.x, e.collider.aabb.min.y, e.collider.aabb.max.z}, m);
            e.collider.mesh.vertices[3] = transform_position(.{e.collider.aabb.max.x, e.collider.aabb.min.y, e.collider.aabb.max.z}, m);
            e.collider.mesh.vertices[4] = transform_position(.{e.collider.aabb.max.x, e.collider.aabb.min.y, e.collider.aabb.min.z}, m);
            e.collider.mesh.vertices[5] = transform_position(.{e.collider.aabb.min.x, e.collider.aabb.max.y, e.collider.aabb.max.z}, m);
            e.collider.mesh.vertices[6] = transform_position(.{e.collider.aabb.max.x, e.collider.aabb.max.y, e.collider.aabb.min.z}, m);
            e.collider.mesh.vertices[7] = transform_position(.{e.collider.aabb.min.x, e.collider.aabb.max.y, e.collider.aabb.min.z}, m);
        }
    }
}

add_trigger_overlap_if_new :: (triggered_entity: *Entity, triggered_by_entity: *Entity) {
    for 0..triggered_entity.collider.num_overlaps-1 {
        overlap := *triggered_entity.collider.overlaps[it];
        if overlap.entity == triggered_by_entity {
            overlap.frame_index = frame_index;
            return;
        }
    }

    if engine.procs.on_trigger_enter != null {
        engine.procs.on_trigger_enter(triggered_entity, triggered_by_entity);
    }

    triggered_entity.collider.overlaps[triggered_entity.collider.num_overlaps] = .{ triggered_by_entity, frame_index };
    triggered_entity.collider.num_overlaps += 1;
}

can_collide :: (e: *Entity, other: *Entity) -> bool {
    return xx (e.collider.collides_with_layers & other.collider.layer);
}

physics_step :: (scene: *Scene, timestep: float) {
    update_gravity(scene, timestep);
    update_positions(scene, timestep);

    for e: scene.entities {
        if !e.enabled continue;
        if !e.body.enabled continue;
        #if NETWORKING { if e.is_proxy continue;}
        if e.collider.ignore continue;

        if e.flags & .PHYSICS {
            if e.body.check_for_grounded {
                e.body.grounded = false;
            }

            for other_e: scene.entities {
                if e == other_e continue;
                if other_e.collider.ignore continue;
                if !other_e.enabled continue;
                if !other_e.body.enabled continue;
                if !can_collide(e, other_e) continue;

                if other_e.flags & .COLLISION {
                    if other_e.collider.type == .AABB {
                        success :, inv_matrix := inverse(other_e.transform.model_matrix);
                        aabb := other_e.collider.aabb;
                        if point_inside_aabb(aabb, transform_position(e.transform.position, inv_matrix)) {
                            add_trigger_overlap_if_new(other_e, e);
                        }
                    } else {
                        point := gjk(e.collider, other_e.collider);  

                        if point.has_collision {
                            if other_e.flags & .TRIGGER {
                                // TRIGGER CALLBACK
                                add_trigger_overlap_if_new(other_e, e);
                            } else {
                                n := -point.normal;
                                speed_along_normal := dot(e.body.velocity, n);

                                restitution := e.body.bounciness;
                                impulse := n * (-(1.0 + restitution) * speed_along_normal);
                                e.body.velocity += impulse;

                                percent := 0.1;
                                slop := 0.005;
                                correction := n * max(point.penetration_depth - slop, 0.0) / (1.0 / percent);
                                set_position(*e.transform, e.transform.position + correction);

                                if e.body.check_for_grounded {
                                    e.body.grounded = dot(n, WORLD_UP) > 0.6; // @Incomplete: Add allowed angle variable at some point?
                                }

                                // @Incomplete: This shouldn't be in here
                                //if e.type == Diamond && length(impulse) > 2.0 {
                                    //    play_audio_event(sfx_diamond_hit);
                                    //}
                            }
                        }
                    }
                }
            }
        }
    }
}

update_physics :: (scene: *Scene, dt: float) {
    for scene.entities {
        if it.collider.type == .MESH {
            if !it.collider.mesh.is_baked {
                update_mesh_collider(it);
            }
        }
    }

    iterations := 4;
    step_time := dt / cast(float)iterations;
    for 0..iterations-1 {
        physics_step(scene, step_time);

        for e: scene.entities {
            if e.flags & .PHYSICS {
                //if e.body.friction > 0.0 {
                //    e.body.velocity *= 1.0 - (e.body.friction / cast(float)iterations);
                //}

                e.body.velocity *= (1.0 - e.body.linear_damping / cast(float)iterations);
            }
        }
    }

    for e: scene.entities {
        index := 0;
        while index < e.collider.num_overlaps {
            defer index += 1;

            if e.collider.overlaps[index].frame_index < frame_index {
                if engine.procs.on_trigger_exit != null {
                    engine.procs.on_trigger_exit(e, e.collider.overlaps[index].entity);
                }

                if e.collider.num_overlaps > 1 {
                    e.collider.overlaps[index] = e.collider.overlaps[e.collider.num_overlaps-1];
                }

                e.collider.num_overlaps -= 1;
            }
        }
    }
}

// DISCRETE
aabb_vs_aabb :: (box1: AABB, box2: AABB) -> bool {
    // Check for no overlap along any axis
    if box1.max.x < box2.min.x || box1.min.x > box2.max.x return false;
    if box1.max.y < box2.min.y || box1.min.y > box2.max.y return false;
    if box1.max.z < box2.min.z || box1.min.z > box2.max.z return false;
    
    return true;
}

resolve_aabb_vs_aabb :: (moving_box: AABB, static_box: AABB) -> Vector3 {
    overlap_x := min(moving_box.max.x, static_box.max.x) - max(moving_box.min.x, static_box.min.x);
    overlap_y := min(moving_box.max.y, static_box.max.y) - max(moving_box.min.y, static_box.min.y);
    overlap_z := min(moving_box.max.z, static_box.max.z) - max(moving_box.min.z, static_box.min.z);

    offset : Vector3;

    // Resolve overlap on each axis
    if (overlap_x > 0 && overlap_y > 0 && overlap_z > 0) {
        // Determine which axis has the smallest overlap
        if (overlap_x <= overlap_y && overlap_x <= overlap_z) {
            // Resolve overlap on X-axis
            if (moving_box.max.x < static_box.max.x) {
                offset.x -= overlap_x;
            } else {
                offset.x += overlap_x;
            }
        } else if (overlap_y <= overlap_x && overlap_y <= overlap_z) {
            // Resolve overlap on Y-axis
            if (moving_box.max.y < static_box.max.y) {
                offset.y -= overlap_y;
            } else {
                offset.y += overlap_y;
            }
        } else {
            // Resolve overlap on Z-axis
            if (moving_box.max.z < static_box.max.z) {
                offset.z -= overlap_z;
            } else {
                offset.z += overlap_z;
            }
        }
    }

    return offset;
}

// SWEPT
// Check for collision between two AABBs over a specified time interval
swept_aabb_collision :: (box1: AABB, box2: AABB, delta: Vector3) -> bool, t_enter: float, t_exit: float {
    d_inv := 1.0 / delta;

    tx_enter, tx_exit, ty_enter, ty_exit, tz_enter, tz_exit : float;

    if (delta.x >= 0) {
        tx_enter = (box2.min.x - box1.max.x) * d_inv.x;
        tx_exit = (box2.max.x - box1.min.x) * d_inv.x;
    } else {
        tx_enter = (box2.max.x - box1.min.x) * d_inv.x;
        tx_exit = (box2.min.x - box1.max.x) * d_inv.x;
    }

    if (delta.y >= 0) {
        ty_enter = (box2.min.y - box1.max.y) * d_inv.y;
        ty_exit = (box2.max.y - box1.min.y) * d_inv.y;
    } else {
        ty_enter = (box2.max.y - box1.min.y) * d_inv.y;
        ty_exit = (box2.min.y - box1.max.y) * d_inv.y;
    }

    if (delta.z >= 0) {
        tz_enter = (box2.min.z - box1.max.z) * d_inv.z;
        tz_exit = (box2.max.z - box1.min.z) * d_inv.z;
    } else {
        tz_enter = (box2.max.z - box1.min.z) * d_inv.z;
        tz_exit = (box2.min.z - box1.max.z) * d_inv.z;
    }

    t_enter := max(max(tx_enter, ty_enter), tz_enter);
    t_exit := min(min(tx_exit, ty_exit), tz_exit);

    return t_enter <= t_exit && t_exit >= 0 && t_enter <= 1, t_enter, t_exit;
}

calculate_aabbs :: (scene: *Scene) {
    for e: scene.entities {
        if e.flags & .COLLISION && e.flags & .RENDERABLE {
            if e.collider.override_aabb continue;
            if e.renderable.model == 0 continue;

            aabb : AABB;            
            model := get_model_by_handle(e.renderable.model);

            for n : model.nodes {
                if n.parent == 0 {
                    bake_aabb(*aabb, Matrix4_Identity, e, n);
                }
            }

            e.collider.aabb = aabb;
        }
    }
}

bake_aabb :: (aabb: *AABB, parent_matrix: Matrix4, e: *Entity, n: Node) {
    update_matrix(*n.transform);
	node_matrix := parent_matrix * n.transform.model_matrix;
	
	for handle : n.meshes {
		index := 0;
        m := parray_get(*engine.renderer.meshes, handle);
		if m.indices.count > 0 {
			while index < m.indices.count {
				i1 := m.indices[index];
				i2 := m.indices[index + 1];
				i3 := m.indices[index + 2];
				p0 := to_v3(node_matrix * to_v4(m.positions[i1]));
				p1 := to_v3(node_matrix * to_v4(m.positions[i2]));
				p2 := to_v3(node_matrix * to_v4(m.positions[i3]));

				apply_min_max(*aabb.min, *aabb.max, p0);
				apply_min_max(*aabb.min, *aabb.max, p1);
				apply_min_max(*aabb.min, *aabb.max, p2);

				index += 3;
			}
			// assert("Meshes with indices currently for aabb collision baking." && false);
		} else {
			while index < m.positions.count - 1 {
				p0 := to_v3(node_matrix * to_v4(m.positions[index]));
				p1 := to_v3(node_matrix * to_v4(m.positions[index + 1]));
				p2 := to_v3(node_matrix * to_v4(m.positions[index + 2]));

				apply_min_max(*aabb.min, *aabb.max, p0);
				apply_min_max(*aabb.min, *aabb.max, p1);
				apply_min_max(*aabb.min, *aabb.max, p2);

				index += 3;
			}
		}
	}
    
    model := get_model_by_handle(e.renderable.model);

	for n.children {
		child := *model.nodes[it - 1];
		bake_aabb(aabb, node_matrix, e, child);
	}

    if abs(aabb.min.y - aabb.max.y) < 0.00001 {
        aabb.min.y -= 0.001;
    }
}