extends Node

export var trailer_house: PackedScene
export var palace: PackedScene
export var car: PackedScene

onready var obj_names = {
	"palace": palace,
	"trailer_house": trailer_house
}

var done = false

var side_wall: PackedScene = preload("res://objects/wall-side.scn")
var bottom_side: PackedScene = preload("res://objects/bottom-side.scn")
var bottom: PackedScene = preload("res://objects/bottom.scn")
var bottom_wheels: PackedScene = preload("res://objects/bottom-wheels.scn")
var entry: PackedScene = preload("res://objects/entry.scn")
var roof_floor: PackedScene = preload("res://objects/roof-floor.scn")
var roof_floor_range: PackedScene = preload("res://objects/roof-floor-range.scn")
var wall_internal:  PackedScene = preload("res://objects/wall-internal.scn")
var window_narrow: PackedScene = preload("res://objects/window-narrow.scn")
var window_wide: PackedScene = preload("res://objects/window-wide.scn")
var wall_solid: PackedScene = preload("res://objects/wall-solid.scn")

var courtyard_tile: PackedScene = preload("res://objects/courtyard-tile.scn")
var foundation_tile: PackedScene = preload("res://objects/foundation.scn")
var room_tile: PackedScene = preload("res://objects/block-room-corridoor.scn")
var tower_walls_tile: PackedScene = preload("res://objects/tower-walls.scn")
var tower_floor_tile: PackedScene = preload("res://objects/tower_floor.scn")
var stairs_tile: PackedScene = preload("res://objects/stairs.scn")
var gate_bottom_tile: PackedScene = preload("res://objects/gate_bottom.scn")
var gate_top_tile: PackedScene = preload("res://objects/gate-top.scn")
var entry_tile: PackedScene = preload("res://objects/block-room-entry.scn")
var roof_tile: PackedScene = preload("res://objects/roof.scn")
var tower_roof_tile: PackedScene = preload("res://objects/tower-roof.scn")

onready var palace_map_data = {
	"courtyard_tile": courtyard_tile,
	"foundation_tile": foundation_tile,
	"room_tile": room_tile,
	"tower_walls_tile": tower_walls_tile,
	"tower_floor_tile": tower_floor_tile,
	"stairs_tile": stairs_tile,
	"gate_bottom_tile": gate_bottom_tile,
	"gate_top_tile": gate_top_tile,
	"entry_tile": entry_tile,
	"roof_tile": roof_tile,
	"tower_roof_tile": tower_roof_tile
}


#class radial_grid:
#	var radial_points = []
#	var max_r = 0.0
#	var grid = []
#	var width: int = 0
#	var height: int = 0
#	var nodes = []
#	func build(poly, center):
#		var height = center.y
#		max_r = 0.0
#		for p in range(poly.size()):
#			var ep1 = poly[p]
#			ep1.y = height
#			var ep2 = poly[(p + 1) % poly.size()]
#			ep2.y = height
#			var d = (ep2 - ep1).normalized()
#			radial_points.push_back(ep1)
#			var dst =  ep1.distance_to(ep2)
#			while dst > 32:
#				ep1 += d * 32
#				radial_points.push_back(ep1)
#				dst -= 32
#		for p in range(radial_points.size()):
#			var ep = radial_points[p]
#			var dst = ep.distance_to(center)
#			if max_r < dst:
#				max_r = dst
#		width = radial_points.size()
#		height = int(max_r / 32)
#		grid.resize(width * height)
#		for p in range(width):
#			var start = radial_points[p]
#			var end = center
#			var step = (end - start).normalized() * (end - start).length() / float(height + 1)
#			var val = start
#			for q in range(height):
#				grid[p * height + q] = {"position": val, "node": null}
#	func get_grid_node(x, y):
#		return grid[x * height + y].node
#	func set_grid_node(x, y, node):
#		if !node in nodes:
#			nodes.push_back(node)
#		grid[x * height + y].node = node
#	func place_grid(aabb: AABB, node) -> void:
#		for u in range(width):
#			for v in range(height):
#				if aabb.has_point(grid[u * height + v].position):
#					set_grid_node(u, v, node)
#	func get_pos(x, y):
#		return grid[x * height + y].position

# onready var grid = radial_grid.new()

#func debug_poly(poly):
#	for k in range(poly.size()):
#		var p1 = poly[k]
#		var p2 = poly[(k + 1) % poly.size()]
#		var l = p1.distance_to(p2)
#		var d = (p2 - p1).normalized()
#		var pt = p1
#		while l > 0.0:
#			for e in range(0, 30, 2):
#				var mi = MeshInstance.new()
#				mi.mesh = CubeMesh.new()
#				get_tree().root.add_child(mi)
#				mi.global_transform.origin = pt + Vector3(0, e, 0)
#			pt += d * 5.0
#			l -= 5.0
#
#func calc_border(poly, offt):
#	var height = RoadsData.get_site_avg_height(0)
#	var border = []
#	border.resize(poly.size())
#	for k in range(poly.size()):
#		var i = k - 1
#		if i < 0:
#			i += poly.size()
#		var p1 = poly[i]
#		var p2 = poly[k]
#		var p3 = poly[(k + 1) % poly.size()]
#		var p1x = Vector2(p1.x, p1.z)
#		var p2x = Vector2(p2.x, p2.z)
#		var p3x = Vector2(p2.x, p2.z)
#		var p4x = Vector2(p3.x, p3.z)
#		var n1 = (p2x - p1x).tangent().normalized()
#		var n2 = (p4x - p3x).tangent().normalized()
#		p1x -= n1 * offt
#		p2x -= n1 * offt
#		p3x -= n2 * offt
#		p4x -= n2 * offt
#
#		var xp = Geometry.segment_intersects_segment_2d(p1x, p2x, p3x, p4x)
#		if !xp:
#			xp = p2x.linear_interpolate(p3x, 0.5) - (n1 + n2).normalized() * offt
#		var tp = Vector3(xp.x, height, xp.y)
#		border[k] = tp
#	return border
#
var traffic_rnd: RandomNumberGenerator
var traffic_astar: AStar
var towns = 0
func setup_town(site):
	if !RoadsData.site_is_town(site):
		print("site ", site, " type: ", RoadsData.get_site_type(site))
		return
	var poly = RoadsData.get_site_polygon_3d(site)
	var height = RoadsData.get_site_avg_height(site)
	var border2 = RoadsData.get_site_border(site, 60)
	var aabbs = []
	var poly2 = []
	poly2.resize(border2.size())
	for p in range(border2.size()):
		poly2[p] = Vector2(border2[p].x, border2[p].z)
	var center = Vector3()
	for p in poly:
		center += p
	center /= poly.size()
	center.y = height
#	grid.build(border2, center)
	var radial_points = RoadsData.get_site_radial_points(site, 32.0, 64.0)
	var max_r = 0.0
	for p in range(radial_points.size()):
		var ep = radial_points[p]
		var dst = ep.distance_to(center)
		if max_r < dst:
			max_r = dst
	for p in range(radial_points.size()):
		var ep = radial_points[p]
		var d = (center - ep).normalized()
		var dst = ep.distance_to(center)
		print(dst)
		if dst < 64.0 + 12 + 8 + 4:
			continue
		var step = 16.0
		var pstart = ep
		while dst > 0.0:
			var ok = true
			if !Geometry.is_point_in_polygon(Vector2(pstart.x, pstart.z), poly2):
				ok = false
			if ok:
				for b in aabbs:
					if b.has_point(pstart):
						ok = false
			if ok:
				var xform = Transform(Basis(), pstart).looking_at(pstart + d, Vector3.UP)
				stream_obj("trailer_house", xform)
				var aabb = AABB(pstart, Vector3())
				aabb = aabb.grow(32)
				aabbs.push_back(aabb)
				print("placed to: ", pstart)
			pstart = pstart + d * step
			dst -= step
	towns += 1

func setup_first_town():
	assert(!done)
	var poly = RoadsData.get_site_polygon_3d(0)
	var height = RoadsData.get_site_avg_height(0)
	var border = RoadsData.get_site_border(0, 32)
	var border1a = RoadsData.get_site_border(0, 42)
	var border2 = RoadsData.get_site_border(0, 60)

	var poly2 = []
	poly2.resize(border2.size())
	for p in range(border2.size()):
		poly2[p] = Vector2(border2[p].x, border2[p].z)
	var center = Vector3()
	for p in poly:
		center += p
	center /= poly.size()
	center.y = height
#	grid.build(border2, center)
	var radial_points = RoadsData.get_site_radial_points(0, 32.0, 64.0)
	var max_r = 0.0
	for p in range(radial_points.size()):
		var ep = radial_points[p]
		var dst = ep.distance_to(center)
		if max_r < dst:
			max_r = dst
	var u = radial_points.size()
	var v = int(max_r / 32)
	var grid = []
	grid.resize(u * v)
	for p in range(u):
		var start = radial_points[p]
		var end = center
		var step = (end - start).normalized() * (end - start).length() / float(v + 1)
		var val = start
		for q in range(v):
			grid[p * v + q] = {"position": val}
	var aabbs = []
	var palace_aabb = AABB(center, Vector3())
	palace_aabb = palace_aabb.grow(48)
	print(palace_aabb)
	aabbs.push_back(palace_aabb)
	stream_obj("palace", Transform(Basis(), center))
	
	for p in range(radial_points.size()):
		var ep = radial_points[p]
		var d = (center - ep).normalized()
		var dst = ep.distance_to(center)
		print(dst)
		if dst < 64.0 + 12 + 8 + 4:
			continue
		var step = 16.0
		var pstart = ep
		while dst > 0.0:
			var ok = true
			if !Geometry.is_point_in_polygon(Vector2(pstart.x, pstart.z), poly2):
				ok = false
			if ok:
				for b in aabbs:
					if b.has_point(pstart):
						ok = false
			if ok:
				var xform = Transform(Basis(), pstart).looking_at(pstart + d, Vector3.UP)
				stream_obj("trailer_house", xform)
				var aabb = AABB(pstart, Vector3())
				aabb = aabb.grow(32)
				aabbs.push_back(aabb)
				print("placed to: ", pstart)
			pstart = pstart + d * step
			dst -= step
	towns += 1
	done = true

func setup_traffic(site):
	var poly = RoadsData.get_site_polygon_3d(site)
	var lp = get_tree().root
	for p in range(poly.size()):
		var p1 = poly[p]
		var p2 = poly[(p + 1) % poly.size()]
		var n = (p2 - p1).cross(Vector3.UP).normalized()
		var t = (p2 - p1).normalized()
		var l = p1.distance_to(p2)
		var xpos = p1 + t * 8.0
		var xe = 128.0
		if l < xe + 16.0:
			xpos = p1.linear_interpolate(p2, 0.5)
			var x = xpos + n * 1.5 + Vector3.UP * 0.5
			var xform = Transform(Basis(), x).looking_at(x + t * 3.0, Vector3.UP)
			var c = Spatial.new()
			lp.add_child(c)
			c.transform = xform
			c.add_to_group("spawn")
			c.add_to_group("keep")
			c.add_to_group("traffic_spawn")
		else:
			while l > xe + 16.0:
				var x = xpos + n * 4.0 + Vector3.UP * 0.5
				var xform = Transform(Basis(), x).looking_at(x + t * 3.0, Vector3.UP)
				var c = Spatial.new()
				lp.add_child(c)
				c.transform = xform
				c.add_to_group("spawn")
				c.add_to_group("keep")
				c.add_to_group("traffic_spawn")
				xpos += t * xe
				l -= xe

func _ready():
	traffic_rnd = RandomNumberGenerator.new()
	traffic_rnd.randomize()
	traffic_astar = AStar.new()
	for k in obj_names.keys():
		Spawner.add_scene(k, obj_names[k])
	for k in palace_map_data.keys():
		Spawner.add_scene(k, palace_map_data[k])
	var parts = {
		"side_wall": side_wall,
		"bottom_side": bottom_side,
		"bottom": bottom,
		"bottom_wheels": bottom_wheels,
		"entry": entry,
		"roof_floor": roof_floor,
		"roof_floor_range": roof_floor_range,
		"wall_internal": wall_internal,
		"window_narrow": window_narrow,
		"window_wide": window_wide,
		"wall_solid": wall_solid
	}
	for k in parts.keys():
		Spawner.add_scene(k, parts[k])
	Traffic.add_traffic_vehicle(car)

# Called every frame. 'delta' is the elapsed time since the previous frame.
var delay = 3.0
var state = 0
func _process(delta):
	match state:
		0:
			delay -= delta
			if delay < 0:
				state = 1
		1:
			Spawner.update_view(self, 200)
			var sc = get_tree().root
			var viewport: = get_viewport()
			if !viewport:
				return
			var cam: = viewport.get_camera()
			if !cam:
				return
			var cam_xform: = cam.global_transform
			# building parts
			call_deferred("real_spawn_child")

func stream_obj(obj: String, xform: Transform):
	Spawner.place_scene(obj, xform)
func switch_to_distant_vehicle(n):
	var sp = PhysicsServer.body_get_space(n.get_rid())
	var l = n.get_linear_velocity()
	n.set_meta("velocity", l)
	n.set_meta("space", sp)
	PhysicsServer.body_set_space(n.get_rid(), RID())
func switch_to_close_vehicle(n):
	var sp = n.get_meta("space")
	PhysicsServer.body_set_space(n.get_rid(), sp)
	n.remove_meta("space")
	if n.has_meta("velocity"):
		n.set_linear_velocity(n.get_meta("velocity"))
		n.remove_meta("velocity")
func _physics_process(delta):
	var cam = get_viewport().get_camera()
	if !cam:
		return
	match state:
		1:
			# convert to distant traffic here
			for n in get_tree().get_nodes_in_group("traffic_vehicle"):
				var p1 = cam.global_transform.origin
				var p2 = n.global_transform.origin
				var check_coords = cam.global_transform.xform_inv(p2)
				var steer = 0.0
				var x_target = Vector3()
				var next_target = Vector3()
				var orientation = n.global_transform
				orientation.origin = Vector3()
				var direction = orientation.xform(Vector3(0, 0, -1))
				var good_path = false
				var engine_force = n.engine_force
				if !n.has_meta("curve"):
					create_path(n, p2, direction * 10.0)
				if n.has_meta("curve"):
					var curve = n.get_meta("curve")
					if curve.get_point_count() > 0:
						var plength = n.get_meta("curve_length")
						var offt = curve.get_closest_offset(p2)
						var offt_ext = 8.0
						if n.has_meta("velocity"):
							offt_ext = n.get_meta("velocity").length()
						offt = clamp(offt + offt_ext, 0, plength)
						var p0 = curve.interpolate_baked(offt, false)
						next_target = p0
						if false: # p0.distance_squared_to(p2) > 16.0:
							n.remove_meta("curve")
							steer = 0.0
							x_target = Vector3()
							next_target = p2 + direction * 4.0
						else:
							var xt0 = n.global_transform.xform_inv(p0)
							x_target = xt0
							if xt0.z < 0.0:
								steer = xt0.x
								if steer == 0.0:
									steer = -1
							else:
								steer = x_target.x
							steer = sign(steer)
#							if abs(steer) < 0.005:
#								steer = 0.0
							n.set_meta("x_target", x_target)
							print("position: ", p2, " direction: ", direction, " next_target: ", next_target, " x_target: ", x_target, " steer: ", steer, " ! ", (next_target - p2).normalized())
					else:
						assert(false)
#				if check_coords.z > 160 || check_coords.z < -120 || abs(check_coords.x) > 100:
#					n.queue_free()
				if false: # check_coords.z > 40.0 || check_coords.z < -25.0 || abs(check_coords.x) > 20.0:
					if !n.has_meta("space"):
						switch_to_distant_vehicle(n)
						var xvel = (next_target - p2).normalized() * 6.0
						n.set_meta("velocity", xvel)
					var pos = n.global_transform.origin
					var vel = (next_target - p2).normalized() * 6.0
					if n.has_meta("velocity"):
						vel = n.get_meta("velocity")
					var speed = vel.length()
					
					var stf = (next_target - pos).normalized() * speed
#					stf.y = vel.y
					vel = vel.linear_interpolate(stf, delta)
					var target_pos = pos + vel
#					vel.y = vel.y * delta
					var newpos = pos.linear_interpolate(target_pos, delta)
					n.set_meta("velocity", vel)
					# why?
					var xform = Transform(Basis(), newpos).looking_at(newpos - vel * 4.0, Vector3.UP)
					n.global_transform = xform
				else:
					if n.has_meta("space"):
						switch_to_close_vehicle(n)
					var v = n.get_linear_velocity()
					n.set_meta("velocity", v)
					var l = v.length()
#					if abs(steer) > 3.0 || !good_path:
#						# vehicle totally lost
#						n.brake = 60000
#						n.engine_force = 0
#					elif abs(steer) > 2.0:
#						# vehicle lost
#						n.engine_force = 0
					if next_target.length_squared() == 0:
						engine_force = 0.0
						steer = 0.0
					else:
						if abs(steer) > 3.0:
							engine_force *= 0.9
							engine_force = clamp(engine_force, 2500.0, max(2500, engine_force))
						else:
							if abs(steer) > 4.0 && l > 5.0:
								engine_force = engine_force * 0.9996
							if l > 7.0:
								engine_force = engine_force * 0.9996
							elif l < 1.1:
								engine_force = clamp(engine_force * 1.004, 6000, 8000)
							elif l < 2.5:
								engine_force = clamp(engine_force * 1.002, 5000, 7000)
							elif l < 5.0:
								engine_force = clamp(engine_force * 1.001, 4000, 6000)
							elif l < 6.0:
								engine_force = clamp(engine_force * 1.001, 3000, 5000)
#					engine_force = 0.0
					engine_force = clamp(engine_force, 0, 8500)
					print("engine_force: ", engine_force)
					n.engine_force = engine_force
					n.brake = 0
#					n.steering = clamp(steer, -1, 1)
					var base_steering = n.steering
					var main_steering = base_steering * 0.95 + steer * 0.05
					n.steering = clamp(main_steering, -1, 1)
					n.set_meta("steering", n.steering)
			var space_state = get_viewport().get_world().direct_space_state
			# probaly should not be here
			for n in get_tree().get_nodes_in_group("spawn"):
				var ok = false
				if !n.is_in_group("keep"):
					var where = n.get_global_transform().origin
					var from = where
					var to = where
					from.y -= 8.0
					to.y += 8.0
					var result = space_state.intersect_ray(from, to)
					if result.empty() || !result.has("collider"):
						continue
					if result.collider:
						n.global_transform.origin = result.position
						ok = true
				if ok || n.is_in_group("keep"):
					if n.is_in_group("male"):
						characters.replace_character(n, "male", ["cmdq", "marker", "hurtboxes", "student"])
					elif n.is_in_group("female"):
						characters.replace_character(n, "female", ["cmdq", "marker", "hurtboxes", "student"])
					elif n.is_in_group("car"):
						var p1 = cam.global_transform.origin
						var p2 = n.global_transform.origin
						if p1.distance_squared_to(p2) < 5000.0:
							var c = car.instance()
							c.add_to_group("saved_vehicle")
							var p = get_tree().root
							p.add_child(c)
							c.global_transform = n.global_transform
							n.queue_free()
#					elif n.is_in_group("traffic_spawn"):
#						var p1 = cam.global_transform.origin
#						var p2 = n.global_transform.origin
#						if !n.has_meta("cooldown") && p1.distance_squared_to(p2) < 10000.0:
#							var c = car.instance()
#							c.add_to_group("traffic_vehicle")
#							var p = get_tree().root
#							p.add_child(c)
#							c.global_transform = n.global_transform
#							c.parked = false
#							c.mode = c.MODE_RIGID
#							c.engine_force = 2500
#							c.steering = 0
#							var xf = c.global_transform
#							xf.origin = Vector3()
#							var vel = xf.xform(Vector3(0, 0, -10))
#							c.set_linear_velocity(vel)
#							var sp = PhysicsServer.body_get_space(c.get_rid())
#							c.set_meta("space", sp)
#							PhysicsServer.body_set_space(c.get_rid(), RID())
#							n.set_meta("cooldown", 2.0 + randi() % 8)
#							# create_path(c, p2, vel)
#						elif n.has_meta("cooldown"):
#							var cd = n.get_meta("cooldown")
#							cd -= delta
#							if cd < 0:
#								n.remove_meta("cooldown")
func get_curve_closest(curve, p2):
	var test_offt = curve.get_closest_offset(p2)
	var testp = curve.interpolate_baked(test_offt, false)
	return testp
func test_curve(curve, p2):
	var testp = get_curve_closest(curve, p2)
	return testp.distance_squared_to(p2) < 16.0

func create_path(c, p2, vel):
	c.set_meta("velocity", vel)
	print("create_path: velocity: ", vel)
	var rangle = traffic_rnd.randf() * PI * 2.0
	var randa = 500.0 + traffic_rnd.randf() * 500.0
	var xt = cos(rangle) * randa
	var yt = cos(rangle) * randa
	var rv = Vector3(xt, 0, yt)
	var target = RoadsData.get_closest_point(p2 + rv, false)
	var cur = RoadsData.get_closest_point(p2, false)
	while cur == target:
		rangle = traffic_rnd.randf() * PI * 2.0
		randa = 500.0 + traffic_rnd.randf() * 500.0
		xt = cos(rangle) * randa
		yt = sin(rangle) * randa
		rv = Vector3(xt, 0, yt)
		target = RoadsData.get_closest_point(p2 + rv, false)
	c.set_meta("target", target)
	var path = RoadsData.get_point_path(cur, target);
	assert(cur != target)
	assert(path.size() > 0)
	var curve = Curve3D.new()
	for e in range(path.size() - 1):
		var pt1 = path[e]
		var pt2 = path[e + 1]
		var nt = (pt2 - pt1).cross(Vector3.UP).normalized()
		var d = (pt2 - pt1).normalized()
		var l = pt1.distance_to(pt2)
		while l > 16.0:
			pt1 += d * 8.0
			curve.add_point(pt1 + nt * 3.0 + Vector3.UP * 0.5)
			l -= 8.0
	if (!test_curve(curve, p2)):
		var testp = get_curve_closest(curve, p2)
		var e = p2 + (testp - p2).normalized() * 2.0
		var nt = (testp - p2).cross(Vector3.UP).normalized()
		curve.add_point(e + nt * 3.0 + Vector3.UP * 0.5, Vector3(), Vector3(), 0)
	assert(test_curve(curve, p2))
	c.set_meta("curve", curve)
	c.set_meta("curve_length", curve.get_baked_length())

# buildings

var spawn = []

func spawn_child(n: String, xform: Transform) -> void:
	var sp = {"node": n, "xform": xform}
	spawn.push_back(sp)
func real_spawn_child():
	var count = 0
	while spawn.size() > 0:
		var e = spawn.pop_front()
		Spawner.place_scene(e.node, e.xform)
		count += 1

func update_node_position(n):
	var space_state = get_viewport().get_world().direct_space_state
	var where = n.get_global_transform().origin
	var from = where
	var to = where
	from.y -= 8.0
	to.y += 8.0
	var result = space_state.intersect_ray(from, to)
	if result.empty() || !result.has("collider"):
		return
	else:
		n.global_transform.origin = result.position