/* 
 * Rapier bindings are defined here
 * We use crust btw
 */

extern crate rapier3d;
extern crate parry3d;
use rapier3d::{na::{Isometry, Vector3}, prelude::*, pipeline};
use self::parry3d::query::*;
use std::num::NonZeroUsize;
use std::arch::asm;

#[derive(Default)]
pub struct funnyphysics {
	rigid_body_set: RigidBodySet,
	collider_set: ColliderSet,

	physics_pipeline: PhysicsPipeline,
	island_manager: IslandManager,
	broad_phase: DefaultBroadPhase,
	narrow_phase: NarrowPhase,
	impulse_joint_set: ImpulseJointSet,
	multibody_joint_set: MultibodyJointSet,
	ccd_solver: CCDSolver,
	query_pipeline: QueryPipeline,
}

#[unsafe(no_mangle)]
pub unsafe extern "C" fn px_init() -> *mut funnyphysics {
	let mut px = Box::new(funnyphysics::default());
	px.rigid_body_set=RigidBodySet::new();
	px.collider_set=ColliderSet::new();

	px.physics_pipeline = PhysicsPipeline::new();
	px.island_manager = IslandManager::new();
	px.broad_phase = DefaultBroadPhase::new();
	px.narrow_phase = NarrowPhase::new();
	px.impulse_joint_set = ImpulseJointSet::new();
	px.multibody_joint_set = MultibodyJointSet::new();
	px.ccd_solver = CCDSolver::new();
	px.query_pipeline = QueryPipeline::new();

	return Box::into_raw(px);	
}

#[unsafe(no_mangle)]
pub unsafe extern "C" fn px_frame(px_world: *mut funnyphysics, delta:f32) {
	let gravity = vector![0.0, 0.0, -9.81];
	let mut integration_parameters = IntegrationParameters::default();
	integration_parameters.dt=delta;
	integration_parameters.min_island_size=1;
	integration_parameters.num_solver_iterations=NonZeroUsize::new(4).unwrap();
	integration_parameters.num_internal_stabilization_iterations=2;
	let physics_hooks = ();
	let event_handler = ();
	if let Some(px) = px_world.as_mut() {
		px.physics_pipeline.step(
		&gravity,
		&integration_parameters,
		&mut px.island_manager,
		&mut px.broad_phase,
		&mut px.narrow_phase,
		&mut px.rigid_body_set,
		&mut px.collider_set,
		&mut px.impulse_joint_set,
		&mut px.multibody_joint_set,
		&mut px.ccd_solver,
		Some(&mut px.query_pipeline),
		&physics_hooks,
		&event_handler,
		);
	};
}


#[repr(C)]
pub struct px_collider_params {
	friction:f32
}
#[unsafe(no_mangle)]
pub unsafe extern "C" fn px_ball(radius:f32, params:px_collider_params) -> *mut Collider {
	Box::into_raw(Box::new(ColliderBuilder::ball(radius).friction(params.friction).build()))
}
#[unsafe(no_mangle)]
pub unsafe extern "C" fn px_box(x:f32,y:f32,z:f32, params:px_collider_params) -> *mut Collider {
	Box::into_raw(Box::new(ColliderBuilder::cuboid(x, y, z).friction(params.friction).build()))
}
#[unsafe(no_mangle)]
pub unsafe extern "C" fn px_trimesh(ptr_vert: *const Point<f32>, len_vert: usize, ptr_ind: *const [u32; 3], len_ind: usize, params:px_collider_params) -> *mut Collider {
	let vertices: &[Point<f32>] = std::slice::from_raw_parts(ptr_vert, len_vert);
	let indices: &[[u32; 3]] = std::slice::from_raw_parts(ptr_ind, len_ind);

	Box::into_raw(Box::new(ColliderBuilder::trimesh(vertices.to_vec(), indices.to_vec()).unwrap().friction(params.friction).build()))
}

#[unsafe(no_mangle)]
pub unsafe extern "C" fn px_staticbody(px_world: *mut funnyphysics, collider: *mut Collider, m: px_matrix) -> *mut RigidBodyHandle {
	let c = &mut *collider;
	let px = px_world.as_mut().unwrap();
	let rigid_body = RigidBodyBuilder::fixed()
		.translation(vector![m.m[3],m.m[7],m.m[11]])
		.dominance_group(127)
		.build();
	let body = px.rigid_body_set.insert(rigid_body);
	px.collider_set.insert_with_parent(c.clone(),body,&mut px.rigid_body_set);
	Box::into_raw(Box::new(body))
}


#[repr(C)]
pub struct px_matrix {
	m: [f32; 16],
}


#[repr(C)]
#[derive(Default)]
pub struct px_vec3 {
	m: [f32; 3],
}

#[repr(C)]
pub struct px_rigidbody_params {
	velocity: [f32;3],
	angular_velocity: [f32;3],
	gravity_scale: f32,
	linear_damping: f32,
	angular_damping: f32,
	continous: u8,
	sleeping: u8,
	

	lockrotx: u8,
	lockroty: u8,
	lockrotz: u8,
	lockposx: u8,
	lockposy: u8,
	lockposz: u8,
	dominance: i8,
}

#[unsafe(no_mangle)]
pub unsafe extern "C" fn px_rigidbody(px_world: *mut funnyphysics, collider: *mut Collider, m:px_matrix,params:px_rigidbody_params) -> *mut RigidBodyHandle {
	let c = &mut *collider;
	let px = px_world.as_mut().unwrap();
	let rigid_body = RigidBodyBuilder::dynamic()
		.translation(vector![m.m[3],m.m[7],m.m[11]])
		.can_sleep(false)
		.gravity_scale(1.0)
		.enabled_rotations(params.lockrotx==0, params.lockroty==0, params.lockrotz==0)
		.enabled_translations(params.lockposx==0, params.lockposy==0, params.lockposz==0)
		.dominance_group(params.dominance)
		.linear_damping(0.0)
		.angular_damping(0.0)
		.build();
	let body = px.rigid_body_set.insert(rigid_body);
	px.collider_set.insert_with_parent(c.clone(),body,&mut px.rigid_body_set);

	Box::into_raw(Box::new(body))
}
#[unsafe(no_mangle)]
pub unsafe extern "C" fn px_getposition(px_world: *mut funnyphysics, body: *mut RigidBodyHandle) -> px_vec3 {
	let c = &mut *body;
	let px = px_world.as_mut().unwrap();
	let mut m = px_vec3 { m: [0.0,0.0,0.0]};
	let b = &px.rigid_body_set[*c];
	m.m[0]=b.translation().x;
	m.m[1]=b.translation().y;
	m.m[2]=b.translation().z;
	m
}
#[unsafe(no_mangle)]
pub unsafe extern "C" fn px_setposition(px_world: *mut funnyphysics, body: *mut RigidBodyHandle, vec: px_vec3) {
	let c = &mut *body;
	let px = px_world.as_mut().unwrap();
	let b = &mut px.rigid_body_set[*c];
	b.set_translation(vector![vec.m[0],vec.m[1],vec.m[2]],true);
	
}
#[unsafe(no_mangle)]
pub unsafe extern "C" fn px_getvelocity(px_world: *mut funnyphysics, body: *mut RigidBodyHandle) -> px_vec3 {
	let c = &mut *body;
	let px = px_world.as_mut().unwrap();
	let mut m = px_vec3 { m: [0.0,0.0,0.0]};
	let b = &px.rigid_body_set[*c];
	m.m[0]=b.linvel().x;
	m.m[1]=b.linvel().y;
	m.m[2]=b.linvel().z;
	m
}
#[unsafe(no_mangle)]
pub unsafe extern "C" fn px_setvelocity(px_world: *mut funnyphysics, body: *mut RigidBodyHandle, vec: px_vec3) {
	let c = &mut *body;
	let px = px_world.as_mut().unwrap();
	let b = &mut px.rigid_body_set[*c];
	b.set_linvel(vector![vec.m[0],vec.m[1],vec.m[2]],true);
	
}
#[unsafe(no_mangle)]
pub unsafe extern "C" fn px_getmatrix(px_world: *mut funnyphysics, body: *mut RigidBodyHandle) -> px_matrix {
	let c = &mut *body;
	let px = px_world.as_mut().unwrap();
	let mut m = px_matrix { m: [
		1.0,0.0,0.0,0.0,
		0.0,1.0,0.0,0.0,
		0.0,0.0,1.0,0.0,
		0.0,0.0,0.0,1.0,
		]};
	let b = &px.rigid_body_set[*c];
	let mat = b.rotation().to_rotation_matrix();
	m.m[0]=mat[(0,0)];
	m.m[4]=mat[(0,1)];
	m.m[8]=mat[(0,2)];
	m.m[1]=mat[(1,0)];
	m.m[5]=mat[(1,1)];
	m.m[9]=mat[(1,2)];
	m.m[2]=mat[(2,0)];
	m.m[6]=mat[(2,1)];
	m.m[10]=mat[(2,2)];
	m.m[12]=b.translation().x;
	m.m[13]=b.translation().y;
	m.m[14]=b.translation().z;
	m
}

#[repr(C)]
#[derive(Default)]
pub struct px_cast_result {
	hit: i8,
	time: f32,
	witness1: px_vec3,
	witness2: px_vec3,
	normal1: px_vec3,
	normal2: px_vec3,
}

#[unsafe(no_mangle)]
pub unsafe extern "C" fn px_box_cast(px_world: *mut funnyphysics, x:f32,y:f32,z:f32, pos:px_vec3, rot: px_vec3, vel: px_vec3, time:f32) -> px_cast_result {
	let c = Cuboid::new(vector![x,y,z]);

	let mut res = px_cast_result::default();
	let px = px_world.as_mut().unwrap();
	let filter = QueryFilter::default();
	let options = ShapeCastOptions {
		max_time_of_impact: time,
		target_distance: 0.0,
		stop_at_penetration: false,
		compute_impact_geometry_on_penetration: false,
	};
	let shape_pos = Isometry { translation: Translation{vector: vector![pos.m[0],pos.m[1],pos.m[2]]}, rotation: Rotation::from_euler_angles(rot.m[0], rot.m[1], rot.m[2])};
	let shape_vel=vector![vel.m[0],vel.m[1],vel.m[2]];

	if let Some((handle, hit)) = px.query_pipeline.cast_shape(
		&px.rigid_body_set, &px.collider_set, &shape_pos, &shape_vel, &c, options, filter
	) {
		res.time=hit.time_of_impact;
		res.hit = 1;
	}
	res
}