class_name VehicleTestEnvironment
extends Node3D

const TERRAIN_COLUMNS := 61
const TERRAIN_ROWS := 69
const TERRAIN_WIDTH_M := 180.0
const TERRAIN_LENGTH_M := 200.0
const TERRAIN_NEAR_Z := 28.0


func _ready() -> void:
	_add_bathymetric_terrain()
	_place_calibration_target()
	_add_suspended_particles()


func _add_suspended_particles() -> void:
	var particles := GPUParticles3D.new()
	particles.name = "SuspendedParticles"
	particles.position = Vector3(0.0, -34.0, -42.0)
	particles.amount = 360
	particles.lifetime = 14.0
	particles.preprocess = 14.0
	particles.visibility_aabb = AABB(
		Vector3(-50.0, -35.0, -78.0),
		Vector3(100.0, 70.0, 156.0)
	)

	var motion := ParticleProcessMaterial.new()
	motion.emission_shape = ParticleProcessMaterial.EMISSION_SHAPE_BOX
	motion.emission_box_extents = Vector3(45.0, 28.0, 70.0)
	motion.direction = Vector3(0.1, 0.04, 0.0)
	motion.spread = 180.0
	motion.initial_velocity_min = 0.02
	motion.initial_velocity_max = 0.09
	motion.gravity = Vector3.ZERO
	motion.scale_min = 0.025
	motion.scale_max = 0.08
	particles.process_material = motion

	var particle_mesh := SphereMesh.new()
	particle_mesh.radius = 0.035
	particle_mesh.height = 0.07
	particles.draw_pass_1 = particle_mesh
	add_child(particles)


func _add_bathymetric_terrain() -> void:
	var surface := SurfaceTool.new()
	surface.begin(Mesh.PRIMITIVE_TRIANGLES)
	for row in range(TERRAIN_ROWS - 1):
		for column in range(TERRAIN_COLUMNS - 1):
			var top_left := _terrain_vertex(row, column)
			var top_right := _terrain_vertex(row, column + 1)
			var bottom_left := _terrain_vertex(row + 1, column)
			var bottom_right := _terrain_vertex(row + 1, column + 1)
			_add_terrain_triangle(surface, top_left, top_right, bottom_left)
			_add_terrain_triangle(surface, top_right, bottom_right, bottom_left)
	surface.generate_normals()
	var mesh := surface.commit()

	var body := StaticBody3D.new()
	body.name = "BathymetricTerrain"
	body.set_meta("sonar_material", 1)
	body.set_meta("sonar_reflectivity", 0.72)
	var mesh_instance := MeshInstance3D.new()
	mesh_instance.name = "Mesh"
	mesh_instance.mesh = mesh
	var material := StandardMaterial3D.new()
	material.albedo_color = Color(0.075, 0.19, 0.19)
	material.roughness = 0.92
	mesh_instance.material_override = material
	body.add_child(mesh_instance)
	var collision := CollisionShape3D.new()
	collision.name = "Collision"
	var height_map := HeightMapShape3D.new()
	height_map.map_width = TERRAIN_COLUMNS
	height_map.map_depth = TERRAIN_ROWS
	var height_data := PackedFloat32Array()
	height_data.resize(TERRAIN_COLUMNS * TERRAIN_ROWS)
	for row in TERRAIN_ROWS:
		for column in TERRAIN_COLUMNS:
			var vertex := _terrain_vertex(row, column)
			height_data[row * TERRAIN_COLUMNS + column] = vertex.y
	height_map.map_data = height_data
	collision.shape = height_map
	collision.position = Vector3(
		0.0,
		0.0,
		TERRAIN_NEAR_Z - TERRAIN_LENGTH_M * 0.5
	)
	collision.scale = Vector3(
		TERRAIN_WIDTH_M / float(TERRAIN_COLUMNS - 1),
		1.0,
		TERRAIN_LENGTH_M / float(TERRAIN_ROWS - 1)
	)
	body.add_child(collision)
	add_child(body)


func _terrain_vertex(row: int, column: int) -> Vector3:
	var x := lerpf(
		-TERRAIN_WIDTH_M * 0.5,
		TERRAIN_WIDTH_M * 0.5,
		float(column) / float(TERRAIN_COLUMNS - 1)
	)
	var z := TERRAIN_NEAR_Z - (
		TERRAIN_LENGTH_M * float(row) / float(TERRAIN_ROWS - 1)
	)
	return Vector3(x, _terrain_height(x, z), z)


func _terrain_height(x: float, z: float) -> float:
	var distance_ratio := clampf(
		(TERRAIN_NEAR_Z - z) / TERRAIN_LENGTH_M,
		0.0,
		1.0
	)
	var base := lerpf(-40.0, -78.0, distance_ratio)
	var left_ridge := 13.0 * exp(
		-(pow(x + 24.0, 2.0) / 210.0 + pow(z + 48.0, 2.0) / 980.0)
	)
	var right_ridge := 15.0 * exp(
		-(pow(x - 25.0, 2.0) / 250.0 + pow(z + 58.0, 2.0) / 1150.0)
	)
	var channel := -8.5 * exp(
		-(pow(x, 2.0) / 120.0 + pow(z + 66.0, 2.0) / 2500.0)
	)
	var rear_rise := 8.0 * exp(
		-(pow(x + 5.0, 2.0) / 900.0 + pow(z + 125.0, 2.0) / 620.0)
	)
	var undulation := 1.8 * sin(x * 0.075) * cos(z * 0.048)
	return base + left_ridge + right_ridge + channel + rear_rise + undulation


func _add_terrain_triangle(
	surface: SurfaceTool,
	a: Vector3,
	b: Vector3,
	c: Vector3
) -> void:
	surface.set_uv(Vector2(a.x, a.z) * 0.02)
	surface.add_vertex(a)
	surface.set_uv(Vector2(b.x, b.z) * 0.02)
	surface.add_vertex(b)
	surface.set_uv(Vector2(c.x, c.z) * 0.02)
	surface.add_vertex(c)


func _place_calibration_target() -> void:
	var target := get_node_or_null("SonarCalibrationTarget") as Node3D
	if target != null:
		target.position.y = _terrain_height(target.position.x, target.position.z) + 2.5