class_name SonarService
extends RefCounted

const SonarMaterial = preload("res://game/domain/sonar/sonar_material.gd")
const SonarScanState = preload("res://game/domain/sonar/sonar_scan_state.gd")
const SonarSample = preload("res://game/domain/sonar/sonar_sample.gd")
const SonarSampleBuffer = preload(
	"res://game/domain/sonar/sonar_sample_buffer.gd"
)
const SonarContact = preload("res://game/domain/sonar/sonar_contact.gd")
const SonarScan = preload("res://game/domain/sonar/sonar_scan.gd")
const SonarCaptureRequest = preload(
	"res://game/application/sonar/sonar_capture_request.gd"
)
const SonarCaptureResult = preload(
	"res://game/application/sonar/sonar_capture_result.gd"
)
const SonarAcquisitionPort = preload(
	"res://game/application/interfaces/sonar_acquisition_port.gd"
)
const SonarCommandResult = preload(
	"res://game/application/sonar/sonar_command_result.gd"
)

const MINIMUM_RANGE_M := 5.0
const MAXIMUM_RANGE_M := 95.0
const HORIZONTAL_OPENING_RADIANS := deg_to_rad(76.0)
const VERTICAL_OPENING_RADIANS := deg_to_rad(64.0)
const COLUMNS := 49
const ROWS := 31
const REVEAL_DURATION_SECONDS := 2.5
const RECOVERY_DURATION_SECONDS := 0.35
const FIRST_RETURN_PROGRESS := 0.04
const PREVIOUS_SCAN_STARTING_OPACITY := 0.52
const MINIMUM_COLOR_SPAN_M := 6.0

var _acquisition: SonarAcquisitionPort
var _session_id: StringName
var _scan_sequence := 0
var _phase_elapsed_seconds := 0.0
var _current_scan: SonarScan
var _previous_scan: SonarScan


func _init(p_acquisition: SonarAcquisitionPort, p_session_id: StringName) -> void:
	_acquisition = p_acquisition
	_session_id = p_session_id


func request_scan(sensor_transform: Transform3D) -> SonarCommandResult:
	if _acquisition == null:
		return SonarCommandResult.new(false, "sonar acquisition port is missing")
	if _session_id.is_empty():
		return SonarCommandResult.new(false, "sonar session id is empty")
	if _current_scan != null and _current_scan.state != SonarScanState.Value.READY:
		return SonarCommandResult.new(false, "sonar is already scanning")

	var scan_id := StringName("%s-scan-%d" % [_session_id, _scan_sequence])
	var request := SonarCaptureRequest.new(
		scan_id,
		_session_id,
		sensor_transform,
		MINIMUM_RANGE_M,
		MAXIMUM_RANGE_M,
		HORIZONTAL_OPENING_RADIANS,
		VERTICAL_OPENING_RADIANS,
		COLUMNS,
		ROWS
	)
	var request_error := request.validation_error()
	if not request_error.is_empty():
		return SonarCommandResult.new(false, request_error)
	var capture: SonarCaptureResult = _acquisition.capture(request)
	if capture == null or not capture.accepted:
		return SonarCommandResult.new(
			false,
			"sonar acquisition failed" if capture == null else capture.message
		)
	var built = _build_scan(request, capture)
	if built is String:
		return SonarCommandResult.new(false, built)

	_previous_scan = _current_scan
	_current_scan = built as SonarScan
	_phase_elapsed_seconds = 0.0
	_scan_sequence += 1
	return SonarCommandResult.new(true, "", _current_scan)


func advance(elapsed_seconds: float) -> SonarScan:
	if _current_scan == null or not is_finite(elapsed_seconds) or elapsed_seconds < 0.0:
		return _current_scan
	if _current_scan.state == SonarScanState.Value.REVEALING:
		_phase_elapsed_seconds += elapsed_seconds
		var progress := clampf(
			_phase_elapsed_seconds / REVEAL_DURATION_SECONDS,
			0.0,
			1.0
		)
		if progress >= 1.0:
			_current_scan = _current_scan.with_phase(
				SonarScanState.Value.RECOVERY,
				1.0
			)
			_phase_elapsed_seconds = 0.0
			_previous_scan = null
		else:
			_current_scan = _current_scan.with_phase(
				SonarScanState.Value.REVEALING,
				progress
			)
	elif _current_scan.state == SonarScanState.Value.RECOVERY:
		_phase_elapsed_seconds += elapsed_seconds
		if _phase_elapsed_seconds >= RECOVERY_DURATION_SECONDS:
			_current_scan = _current_scan.with_phase(
				SonarScanState.Value.READY,
				1.0
			)
			_phase_elapsed_seconds = 0.0
	return _current_scan


func current_scan() -> SonarScan:
	return _current_scan


func previous_scan() -> SonarScan:
	return _previous_scan


func visible_row_count() -> int:
	if _current_scan == null:
		return 0
	if _current_scan.state != SonarScanState.Value.REVEALING:
		return _current_scan.samples.rows
	if _current_scan.reveal_progress < FIRST_RETURN_PROGRESS:
		return 0
	var normalized := inverse_lerp(
		FIRST_RETURN_PROGRESS,
		1.0,
		_current_scan.reveal_progress
	)
	return clampi(
		floori(normalized * float(_current_scan.samples.rows - 1)) + 1,
		1,
		_current_scan.samples.rows
	)


func previous_scan_opacity() -> float:
	if _previous_scan == null or _current_scan == null:
		return 0.0
	return PREVIOUS_SCAN_STARTING_OPACITY * (1.0 - _current_scan.reveal_progress)


func _build_scan(request: SonarCaptureRequest, capture: SonarCaptureResult) -> Variant:
	var buffer := SonarSampleBuffer.new(request.columns, request.rows)
	var minimum_elevation := INF
	var maximum_elevation := -INF
	for captured_sample in capture.samples:
		if captured_sample == null:
			return "sonar acquisition returned a missing sample"
		var capture_error: String = captured_sample.validation_error(
			request.columns,
			request.rows
		)
		if not capture_error.is_empty():
			return capture_error
		var sample := SonarSample.new(
			captured_sample.position_m,
			captured_sample.normal,
			captured_sample.intensity,
			captured_sample.elevation_m,
			captured_sample.material,
			true
		)
		var write_error := buffer.set_sample(
			captured_sample.row,
			captured_sample.column,
			sample
		)
		if not write_error.is_empty():
			return write_error
		minimum_elevation = minf(minimum_elevation, sample.elevation_m)
		maximum_elevation = maxf(maximum_elevation, sample.elevation_m)
	var seal_error := buffer.seal()
	if not seal_error.is_empty():
		return seal_error
	if buffer.observed_count() == 0:
		minimum_elevation = -MINIMUM_COLOR_SPAN_M * 0.5
		maximum_elevation = MINIMUM_COLOR_SPAN_M * 0.5
	elif maximum_elevation - minimum_elevation < MINIMUM_COLOR_SPAN_M:
		var center := (minimum_elevation + maximum_elevation) * 0.5
		minimum_elevation = center - MINIMUM_COLOR_SPAN_M * 0.5
		maximum_elevation = center + MINIMUM_COLOR_SPAN_M * 0.5

	var scan := SonarScan.new(
		request.scan_id,
		request.session_id,
		request.sensor_transform,
		request.minimum_range_m,
		request.maximum_range_m,
		request.horizontal_opening_radians,
		request.vertical_opening_radians,
		REVEAL_DURATION_SECONDS,
		RECOVERY_DURATION_SECONDS,
		buffer,
		_cluster_contacts(request.scan_id, buffer),
		minimum_elevation,
		maximum_elevation,
		SonarScanState.Value.REVEALING,
		0.0
	)
	var scan_error := scan.validation_error()
	return scan if scan_error.is_empty() else scan_error


func _cluster_contacts(
	scan_id: StringName,
	buffer: SonarSampleBuffer
) -> Array[SonarContact]:
	var metal_indices: Dictionary = {}
	for row in buffer.rows:
		for column in buffer.columns:
			var sample: SonarSample = buffer.sample_at(row, column)
			if sample.observed and sample.material == SonarMaterial.Value.METAL:
				metal_indices[row * buffer.columns + column] = true

	var contacts: Array[SonarContact] = []
	var visited: Dictionary = {}
	for seed_index: int in metal_indices:
		if visited.has(seed_index):
			continue
		var queue: Array[int] = [seed_index]
		var cluster_indices: Array[int] = []
		visited[seed_index] = true
		while not queue.is_empty():
			var index: int = queue.pop_front()
			cluster_indices.append(index)
			var row: int = index / buffer.columns
			var column: int = index % buffer.columns
			for row_offset: int in range(-1, 2):
				for column_offset: int in range(-1, 2):
					var neighbor_row: int = row + row_offset
					var neighbor_column: int = column + column_offset
					if (
						neighbor_row < 0
						or neighbor_row >= buffer.rows
						or neighbor_column < 0
						or neighbor_column >= buffer.columns
					):
						continue
					var neighbor_index: int = neighbor_row * buffer.columns + neighbor_column
					if metal_indices.has(neighbor_index) and not visited.has(neighbor_index):
						visited[neighbor_index] = true
						queue.append(neighbor_index)
		contacts.append(
			_contact_from_indices(scan_id, contacts.size(), buffer, cluster_indices)
		)
	return contacts


func _contact_from_indices(
	scan_id: StringName,
	contact_index: int,
	buffer: SonarSampleBuffer,
	indices: Array[int]
) -> SonarContact:
	var center := Vector3.ZERO
	var minimum := Vector3(INF, INF, INF)
	var maximum := Vector3(-INF, -INF, -INF)
	var maximum_intensity := 0.0
	for index: int in indices:
		var sample: SonarSample = buffer.sample_at(
			index / buffer.columns,
			index % buffer.columns
		)
		center += sample.position_m
		minimum = Vector3(
			minf(minimum.x, sample.position_m.x),
			minf(minimum.y, sample.position_m.y),
			minf(minimum.z, sample.position_m.z)
		)
		maximum = Vector3(
			maxf(maximum.x, sample.position_m.x),
			maxf(maximum.y, sample.position_m.y),
			maxf(maximum.z, sample.position_m.z)
		)
		maximum_intensity = maxf(maximum_intensity, sample.intensity)
	center /= float(indices.size())
	return SonarContact.new(
		StringName("%s-contact-%d" % [scan_id, contact_index]),
		SonarMaterial.Value.METAL,
		center,
		AABB(minimum, maximum - minimum),
		maximum_intensity,
		indices.size()
	)