class_name ReferenceBathymetryAcquisition
extends "res://game/application/interfaces/sonar_acquisition_port.gd"

const SonarMaterial = preload("res://game/domain/sonar/sonar_material.gd")
const SonarCaptureSample = preload(
	"res://game/application/sonar/sonar_capture_sample.gd"
)

const PROBE_HEIGHT_M := 48.0
const PROBE_DEPTH_M := 92.0
const COLLISION_MASK := 1

var _world: World3D
var _excluded_rids: Array[RID] = []


func _init(p_world: World3D, excluded_body: CollisionObject3D = null) -> void:
	_world = p_world
	if excluded_body != null:
		_excluded_rids.append(excluded_body.get_rid())


func capture(request: SonarCaptureRequest) -> SonarCaptureResult:
	if _world == null:
		return SonarCaptureResult.new(false, [], "sonar world is missing")
	var request_error := request.validation_error()
	if not request_error.is_empty():
		return SonarCaptureResult.new(false, [], request_error)

	var samples: Array[SonarCaptureSample] = []
	var inverse_sensor := request.sensor_transform.affine_inverse()
	for row in request.rows:
		var distance_ratio := float(row) / float(request.rows - 1)
		var forward_distance := lerpf(
			request.minimum_range_m,
			request.maximum_range_m,
			distance_ratio
		)
		var half_width := tan(request.horizontal_opening_radians * 0.5) * forward_distance
		for column in request.columns:
			var lateral_ratio := float(column) / float(request.columns - 1)
			var lateral_offset := lerpf(-half_width, half_width, lateral_ratio)
			var ray_start := request.sensor_transform * Vector3(
				lateral_offset,
				PROBE_HEIGHT_M,
				forward_distance
			)
			var ray_end := request.sensor_transform * Vector3(
				lateral_offset,
				-PROBE_DEPTH_M,
				forward_distance
			)
			var query := PhysicsRayQueryParameters3D.create(
				ray_start,
				ray_end,
				COLLISION_MASK,
				_excluded_rids
			)
			query.hit_back_faces = true
			var hit := _world.direct_space_state.intersect_ray(query)
			if hit.is_empty():
				continue
			var world_position: Vector3 = hit["position"]
			var world_normal: Vector3 = hit["normal"]
			var local_position := inverse_sensor * world_position
			var local_normal := (inverse_sensor.basis * world_normal).normalized()
			var collider := hit.get("collider") as Object
			var material := SonarMaterial.Value.ROCK
			var reflectivity := 0.72
			if collider != null:
				if collider.has_method("sonar_material_at"):
					material = int(collider.call("sonar_material_at", world_position))
					reflectivity = float(
						collider.call("sonar_reflectivity_at", world_position)
					)
				else:
					material = int(collider.get_meta("sonar_material", material))
					reflectivity = float(
						collider.get_meta("sonar_reflectivity", reflectivity)
					)
			var incidence := clampf(absf(world_normal.dot(Vector3.UP)), 0.18, 1.0)
			var attenuation := lerpf(1.0, 0.55, distance_ratio)
			var intensity := clampf(incidence * reflectivity * attenuation, 0.0, 1.0)
			samples.append(
				SonarCaptureSample.new(
					row,
					column,
					local_position,
					local_normal,
					intensity,
					local_position.y,
					material
				)
			)
	return SonarCaptureResult.new(true, samples)