extends Node3D
class_name ProceduralRagdoll

var torso_body: RigidBody3D

func build_ragdoll(color: Color) -> void:
	var mat := StandardMaterial3D.new()
	mat.albedo_color = color
	mat.roughness = 0.8
	
	# Create bodies
	torso_body = _create_body(Vector3(0.4, 0.7, 0.25), mat, Vector3(0, 1.15, 0), 20.0, "Torso")
	var head_body = _create_body(Vector3(0.25, 0.25, 0.25), mat, Vector3(0, 1.65, 0), 5.0, "Head")
	
	var upper_arm_l_body = _create_body(Vector3(0.12, 0.35, 0.12), mat, Vector3(-0.28, 1.275, 0), 3.0, "UpperArmL")
	var lower_arm_l_body = _create_body(Vector3(0.1, 0.35, 0.1), mat, Vector3(-0.28, 0.925, 0), 2.0, "LowerArmL")
	var upper_arm_r_body = _create_body(Vector3(0.12, 0.35, 0.12), mat, Vector3(0.28, 1.275, 0), 3.0, "UpperArmR")
	var lower_arm_r_body = _create_body(Vector3(0.1, 0.35, 0.1), mat, Vector3(0.28, 0.925, 0), 2.0, "LowerArmR")
	
	var thigh_l_body = _create_body(Vector3(0.15, 0.45, 0.15), mat, Vector3(-0.12, 0.625, 0), 5.0, "ThighL")
	var calf_l_body = _create_body(Vector3(0.13, 0.45, 0.13), mat, Vector3(-0.12, 0.175, 0), 3.0, "CalfL")
	var thigh_r_body = _create_body(Vector3(0.15, 0.45, 0.15), mat, Vector3(0.12, 0.625, 0), 5.0, "ThighR")
	var calf_r_body = _create_body(Vector3(0.13, 0.45, 0.13), mat, Vector3(0.12, 0.175, 0), 3.0, "CalfR")
	
	add_child(torso_body)
	add_child(head_body)
	add_child(upper_arm_l_body)
	add_child(lower_arm_l_body)
	add_child(upper_arm_r_body)
	add_child(lower_arm_r_body)
	add_child(thigh_l_body)
	add_child(calf_l_body)
	add_child(thigh_r_body)
	add_child(calf_r_body)
	
	var is_server = not multiplayer.has_multiplayer_peer() or multiplayer.is_server()
	var rep = SceneReplicationConfig.new()
	for child in get_children():
		if child is RigidBody3D:
			rep.add_property(NodePath(str(child.name) + ":position"))
			rep.add_property(NodePath(str(child.name) + ":rotation"))
			if not is_server:
				child.freeze = true
				child.freeze_mode = RigidBody3D.FREEZE_MODE_KINEMATIC
				
	var sync = MultiplayerSynchronizer.new()
	sync.name = "RagdollSync"
	sync.replication_config = rep
	add_child(sync)
	
	# Need to wait a frame for paths to be valid before creating joints
	call_deferred("_setup_joints", head_body, 
		upper_arm_l_body, lower_arm_l_body, 
		upper_arm_r_body, lower_arm_r_body, 
		thigh_l_body, calf_l_body, 
		thigh_r_body, calf_r_body)

func _setup_joints(head_body, 
	u_arm_l, l_arm_l, 
	u_arm_r, l_arm_r, 
	thigh_l, calf_l, 
	thigh_r, calf_r) -> void:
		
	# Neck
	_create_joint(torso_body, head_body, Vector3(0, 1.5, 0))
	
	# Shoulders
	_create_joint(torso_body, u_arm_l, Vector3(-0.28, 1.45, 0))
	_create_joint(torso_body, u_arm_r, Vector3(0.28, 1.45, 0))
	
	# Hips
	_create_joint(torso_body, thigh_l, Vector3(-0.12, 0.85, 0))
	_create_joint(torso_body, thigh_r, Vector3(0.12, 0.85, 0))
	
	# Elbows (Hinge, bends forwards only)
	_create_hinge(u_arm_l, l_arm_l, Vector3(-0.28, 1.10, 0), -PI * 0.8, 0.0)
	_create_hinge(u_arm_r, l_arm_r, Vector3(0.28, 1.10, 0), -PI * 0.8, 0.0)
	
	# Knees (Hinge, bends backwards only)
	_create_hinge(thigh_l, calf_l, Vector3(-0.12, 0.40, 0), 0.0, PI * 0.8)
	_create_hinge(thigh_r, calf_r, Vector3(0.12, 0.40, 0), 0.0, PI * 0.8)

func apply_initial_velocities(linear_vel: Vector3, impulse: Vector3) -> void:
	for child in get_children():
		if child is RigidBody3D:
			child.linear_velocity = linear_vel
			# Add random tumbling velocity so they don't fall perfectly still
			child.angular_velocity = Vector3(randf_range(-5, 5), randf_range(-5, 5), randf_range(-5, 5))
			
	# Apply the weapon impulse mostly to the torso so it yanks the limbs
	if is_instance_valid(torso_body):
		torso_body.apply_central_impulse(impulse * 2.0)

func _create_body(size: Vector3, mat: Material, pos: Vector3, mass: float, bname: String) -> RigidBody3D:
	var body = RigidBody3D.new()
	body.name = bname
	body.position = pos
	body.mass = mass
	
	# Damping prevents insane spinning and explosion
	body.linear_damp = 0.5
	body.angular_damp = 2.0
	
	var shape = CollisionShape3D.new()
	var box_shape = BoxShape3D.new()
	box_shape.size = size
	shape.shape = box_shape
	body.add_child(shape)
	
	var mesh_inst = MeshInstance3D.new()
	var box_mesh = BoxMesh.new()
	box_mesh.size = size
	box_mesh.material = mat
	mesh_inst.mesh = box_mesh
	mesh_inst.cast_shadow = GeometryInstance3D.SHADOW_CASTING_SETTING_ON
	body.add_child(mesh_inst)
	
	return body

func _create_joint(node_a: RigidBody3D, node_b: RigidBody3D, pos: Vector3) -> void:
	var joint = Generic6DOFJoint3D.new()
	add_child(joint)
	joint.position = pos
	joint.node_a = node_a.get_path()
	joint.node_b = node_b.get_path()
	
	# Free angular axes to allow flopping, lock linear
	joint.set_flag_x(Generic6DOFJoint3D.FLAG_ENABLE_LINEAR_LIMIT, true)
	joint.set_flag_y(Generic6DOFJoint3D.FLAG_ENABLE_LINEAR_LIMIT, true)
	joint.set_flag_z(Generic6DOFJoint3D.FLAG_ENABLE_LINEAR_LIMIT, true)
	
	# Restrict angular limits so it doesn't bend 360 degrees
	joint.set_flag_x(Generic6DOFJoint3D.FLAG_ENABLE_ANGULAR_LIMIT, true)
	joint.set_param_x(Generic6DOFJoint3D.PARAM_ANGULAR_LOWER_LIMIT, -PI * 0.4)
	joint.set_param_x(Generic6DOFJoint3D.PARAM_ANGULAR_UPPER_LIMIT, PI * 0.4)
	
	joint.set_flag_y(Generic6DOFJoint3D.FLAG_ENABLE_ANGULAR_LIMIT, true)
	joint.set_param_y(Generic6DOFJoint3D.PARAM_ANGULAR_LOWER_LIMIT, -PI * 0.2)
	joint.set_param_y(Generic6DOFJoint3D.PARAM_ANGULAR_UPPER_LIMIT, PI * 0.2)
	
	joint.set_flag_z(Generic6DOFJoint3D.FLAG_ENABLE_ANGULAR_LIMIT, true)
	joint.set_param_z(Generic6DOFJoint3D.PARAM_ANGULAR_LOWER_LIMIT, -PI * 0.2)
	joint.set_param_z(Generic6DOFJoint3D.PARAM_ANGULAR_UPPER_LIMIT, PI * 0.2)

func _create_hinge(node_a: RigidBody3D, node_b: RigidBody3D, pos: Vector3, lower_lim: float, upper_lim: float) -> void:
	var joint = Generic6DOFJoint3D.new()
	add_child(joint)
	joint.position = pos
	joint.node_a = node_a.get_path()
	joint.node_b = node_b.get_path()
	
	joint.set_flag_x(Generic6DOFJoint3D.FLAG_ENABLE_LINEAR_LIMIT, true)
	joint.set_flag_y(Generic6DOFJoint3D.FLAG_ENABLE_LINEAR_LIMIT, true)
	joint.set_flag_z(Generic6DOFJoint3D.FLAG_ENABLE_LINEAR_LIMIT, true)
	
	# Allow X rotation for the hinge (knee/elbow)
	joint.set_flag_x(Generic6DOFJoint3D.FLAG_ENABLE_ANGULAR_LIMIT, true)
	joint.set_param_x(Generic6DOFJoint3D.PARAM_ANGULAR_LOWER_LIMIT, lower_lim)
	joint.set_param_x(Generic6DOFJoint3D.PARAM_ANGULAR_UPPER_LIMIT, upper_lim)
	
	# Lock Y and Z
	joint.set_flag_y(Generic6DOFJoint3D.FLAG_ENABLE_ANGULAR_LIMIT, true)
	joint.set_param_y(Generic6DOFJoint3D.PARAM_ANGULAR_LOWER_LIMIT, 0.0)
	joint.set_param_y(Generic6DOFJoint3D.PARAM_ANGULAR_UPPER_LIMIT, 0.0)
	
	joint.set_flag_z(Generic6DOFJoint3D.FLAG_ENABLE_ANGULAR_LIMIT, true)
	joint.set_param_z(Generic6DOFJoint3D.PARAM_ANGULAR_LOWER_LIMIT, 0.0)
	joint.set_param_z(Generic6DOFJoint3D.PARAM_ANGULAR_UPPER_LIMIT, 0.0)