extends Actor
const PhysicsFunc = preload("res://src/Utilities/Physic/PhysicsFunc.gd")

onready var players = get_tree().get_nodes_in_group("player")


onready var vision_raycast: RayCast2D = $VisionRayCast
onready var orientation: RayCast2D = $Orientation 
onready var feeler_raycast: RayCast2D = $FeelerRayCast
onready var tilemap: TileMap = $"../%TileMap"
onready var state_machine = $Statemachine
onready var jump_timer: Timer
onready var target_lost_timer: Timer
onready var rng = RandomNumberGenerator.new()

# Is given in blocks
export var vision_distance := 6.0
export var attack_jump_range := 6.0
export var loose_target_seconds := 3.0
# Jump distance in blocks
export var default_jump_distance := 3.0
export var default_jump_angle := 70.0
export var jump_time_search := 0.7
export var jump_time_hunt := 0.3
export var jump_time_standard_deviation := 0.1


# Also in blocks
var movement_radius: float
var anchor: Node2D
var is_bound := false
var was_restricted := false
var barely_held_back_counter := 0
var has_reversed := false

var food_sources = []
var target: Object = null
var food_target: Object = null

var start_x := 0.0
var in_air := false
var is_hurt := false
var stored_x_vel = 0.0

var current_delta = 0.0
var detect_timer := 0.0

var reversing_possible_searching := true

# TODO Make parameters tunable!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!1111!!
func _ready():
	default_jump_distance = default_jump_distance * tilemap.cell_size.x
	jump_timer = Timer.new()
	jump_timer.set_one_shot(true)
	jump_timer.connect("timeout", self, "jump")
	target_lost_timer = Timer.new()
	target_lost_timer.set_one_shot(true)
	target_lost_timer.connect("timeout", self, "loose_target")
	add_child(jump_timer)
	add_child(target_lost_timer)


func bind_to_anchor(anchor_node: Node2D, radius: float ) -> void:
	anchor = anchor_node
	movement_radius = radius * 24
	is_bound = true


func _on_StompDetector_body_entered(body: Node) -> void:
	if body.global_position.y > get_node("StompDetector").global_position.y:
		return
	if body.is_in_group("player"):
		remove_from_group("harmful")
		$StompDetector.remove_from_group("weakpoint")
		is_hurt = true


func execute_movement(delta: float) -> void:
	# Navigation2DServer.map_get_path()
	current_delta = delta
	detect_timer += delta
	velocity.y += _gravity * delta
	if(is_bound):
		var next_position = global_position + velocity * current_delta
		var current_distance = global_position.distance_to(anchor.global_position)
		var new_distance = next_position.distance_to(anchor.global_position)
		# TODO Fix this in respects to x and y distances and movement dampening
		# Maybe use mathemathematics or something idfc
		if(current_distance >= movement_radius && new_distance > current_distance):
			velocity.x = velocity.x * 0.8
			velocity.y = velocity.y * 0.8
			was_restricted = true
	velocity = move_and_slide(velocity, FLOOR_NORMAL, false, 4, 0.785398,false)
	if(is_on_floor()):
		velocity = Vector2(0,0)
	
	# Reverse direction when hitting limit


func die() -> void:
	levelState.kills += 1
	queue_free()


func _on_EnemySkin_area_entered(area:Area2D) -> void:
	if area.is_in_group("harmful"):
		get_node("EnemyBody").disabled = true
		die()


func _on_EnemySkin_body_entered(body: Node) -> void:
	if body.is_in_group("frogfood"):
			loose_target()
			body.die()


func searching() -> Vector2:
	if(detect_timer > 0.333):
		search_next_target()
		detect_timer = 0.0
	if(is_on_floor()):
		if(jump_timer.is_stopped()):
			jump_timer.start(rng.randfn(jump_time_search, jump_time_standard_deviation))
		if(in_air):
			in_air = false
	else:
		if(!in_air):
			start_x = global_position.x
		reversing_possible_searching = true
		jump_timer.stop()
		in_air = true
	return velocity


func search_next_target():
	if(target != null && !weakref(target).get_ref()):
		return
	detect_food()
	if(food_target == null):
		detect_player()


func hunting() -> Vector2:
	var was_target_freed = !weakref(target).get_ref()
	if(detect_timer > 0.333):
		search_next_target()
		detect_timer = 0.0
	#TODO Dependent on block size
	elif(is_on_floor() && food_target != null && !was_target_freed &&
			global_position.distance_to(food_target.global_position) <= attack_jump_range * 24):
				
		var collider = check_feeler(food_target.global_position - global_position)
		if(!was_restricted && collider != null && collider.is_in_group("frogfood")):
			jump_timer.stop()
			return attack_jump(food_target.global_position)
			
	if(is_on_floor()):
		if(jump_timer.is_stopped()):
			jump_timer.start(rng.randfn(jump_time_hunt, jump_time_standard_deviation))
		if(in_air):
			in_air = false
			
	else:
		if(!in_air):
			start_x = global_position.x
		reversing_possible_searching = true
		jump_timer.stop()
		in_air = true

	if(barely_held_back_counter > 1):
		 barely_held_back_counter = 0
		 loose_target()

 if(target != null && !was_target_freed &&
		sign((target.global_position - global_position).x) != get_facing_direction()):
		# TODO Waits in front of too small tunnels if it sees the target on the other side
		# It's ok behavior for now
		reverse_facing_direction()

	return velocity


func detect_food() -> void:
		# TODO What if food spawns in
		food_sources = get_tree().get_nodes_in_group("frogfood")
		if(food_sources.empty()):
			return 
		var i = 0
		var min_dist_f_index = 0
		var min_dist = (food_sources[0].global_position - global_position).length()
		var food_node = null
		for f in food_sources:
			var new_dist = (food_sources[i].global_position - global_position).length()
			min_dist = new_dist if new_dist < min_dist else min_dist
			min_dist_f_index = i if new_dist < min_dist else min_dist_f_index
			i += 1
		food_node = food_sources[min_dist_f_index]
		#TODO Depends on height of blobby sprite since blobbys bottom and not his middle is on y=0
		vision_raycast.cast_to = (food_node.global_position - global_position).normalized() * 24 * vision_distance
		var ray_angle_to_facing = vision_raycast.cast_to.angle_to(orientation.cast_to)
		vision_raycast.force_raycast_update()
		var collider = vision_raycast.get_collider()
		if(abs(ray_angle_to_facing) < PI/3 && collider != null && collider.is_in_group("frogfood")):
			target_lost_timer.stop()
			target = collider
			food_target = collider
		elif(target != null && target_lost_timer.is_stopped()):
			target_lost_timer.start(loose_target_seconds)


func detect_player() -> void:
		var player
		if(players.empty()):
			print("no player found")
			return 
		player = players[0]
		#TODO Depends on height of blobby sprite since blobbys bottom and not his middle is on y=0
		vision_raycast.cast_to = (player.global_position - global_position - Vector2(0, 9)).normalized() * 24 * vision_distance
		var ray_angle_to_facing = vision_raycast.cast_to.angle_to(orientation.cast_to)
		vision_raycast.force_raycast_update()
		var collider = vision_raycast.get_collider()
		if(abs(ray_angle_to_facing) < PI/4 && collider != null && collider.is_in_group("player")):
			target_lost_timer.stop()
			target = collider
		elif(target != null && target_lost_timer.is_stopped()):
			target_lost_timer.start(loose_target_seconds)


func sleeping() -> Vector2:
	jump_timer.stop()
	detect_player()
	return velocity


func loose_target() -> void:
	print("frog target lost")
	target = null
	food_target = null


func jump():
	# print("jump calculation initiated")
	# Can only reverse once per jump calculation
	has_reversed = false
	var zero_vector = Vector2(0,0)
	var v: Vector2 = velocity_for_jump_distance(default_jump_distance, deg2rad(default_jump_angle))
	v = correct_jump_direction(v)

	if(is_bound):
		var next_position = global_position + v * current_delta
		var current_distance = global_position.distance_to(anchor.global_position)
		var new_distance = next_position.distance_to(anchor.global_position)
		# print(current_distance)
		# print(new_distance)
		# Would go out of distance
		if((new_distance >= movement_radius && new_distance > current_distance) || (new_distance > current_distance && was_restricted)):
			if(state_machine.state == "hunting"):
				barely_held_back_counter += 1
			if can_reverse_facing_direction() && (barely_held_back_counter == 0 || barely_held_back_counter > 1):
				reverse_facing_direction()
		was_restricted = false
	
	if (get_facing_direction() < 0 && $Left_Wallcast.is_colliding()):
		v = zero_vector
	if (get_facing_direction() > 0 && $Right_Wallcast.is_colliding()):
		v = zero_vector
	if ($Right_Wallcast.is_colliding() && $Left_Wallcast.is_colliding()):
		print("help this is a really tight space :(")
		return velocity
	
	v = correct_jump_direction(v)
	if(v != zero_vector):
		v = consider_jump_headspace(v)
	if(v != zero_vector):
		v = consider_jump_landing_space(v)
	if(v == zero_vector):
		# TODO fix that you could call jump from jumping on top
		# and let it fail if the top is dangerous for jump height or not safe
		v = consider_jumping_on_top()
	if(v == zero_vector && can_reverse_facing_direction()):
		reverse_facing_direction()
	velocity = v
	

func correct_jump_direction(v: Vector2) -> Vector2:
	if sign(v.x) != get_facing_direction():
		v.x *= -1
	return v


#	Cast a ray to the highest point of the jump
# Check the highest point for collision
# Calculate safe jump height and then a safe jump velocity
func consider_jump_headspace(v: Vector2) -> Vector2:
	var height = calculate_jump_height(v) 
	var distance = calculate_jump_distance(v)
	var angle = (v * get_facing_direction()).angle()
	# Half distance is an estimate of the jumps apex()
	#TODO Consider sprite size for height
	var height_collider = check_feeler(Vector2(get_facing_direction()*(distance/2), -(height+23)))
	if(height_collider != null):
		var collision_point = feeler_raycast.get_collision_point()
		var target_height = collision_point.y - (feeler_raycast.global_position.y - 23)
		print(feeler_raycast.global_position)
		var new_angle = angle * (0.75 if target_height > -26 else 0.95)
		var new_distance = default_jump_distance * (0.66 if target_height > -26 else 0.75)
		v = velocity_for_jump_distance(new_distance, abs(new_angle))
		v = correct_jump_direction(v)
		height = calculate_jump_height(v) * -1
		distance = calculate_jump_distance(v) * get_facing_direction()
		if(height < target_height && can_reverse_facing_direction()):
			print("no safe height for frog jump")
			return Vector2(0,0)
	return v


# Check the block in jump distance for danger or height
# If danger check neighboring blocks: if still danger, then jump closer (or jump over)
# If height move to distance which allows 1 block high jump
func consider_jump_landing_space(v: Vector2) -> Vector2:
	var jump_distance = calculate_jump_distance(v)
	var jump_height = calculate_jump_height(v)
	var collider = check_feeler(Vector2(jump_distance * get_facing_direction(), - jump_height/2))
	# TODO Unpacked loop, make function or something?
	# Shortens the jump in steps to make it more safe
	if(!is_jump_path_safe(v, global_position) || collider != null):
		jump_distance = calculate_jump_distance(v) - 18
		v = change_jump_distance(jump_distance, v)
		jump_height = calculate_jump_height(v)
		v = correct_jump_direction(v)
		collider = check_feeler(Vector2(jump_distance * get_facing_direction(), - jump_height/2))
	if(!is_jump_path_safe(v, global_position) || collider != null):
		jump_distance = calculate_jump_distance(v) - 12
		v = change_jump_distance(jump_distance, v)
		jump_height = calculate_jump_height(v)
		v = correct_jump_direction(v)
		collider = check_feeler(Vector2(jump_distance * get_facing_direction(), - jump_height/2))
	if((!is_jump_path_safe(v, global_position) || collider != null) && can_reverse_facing_direction()):
		print("no safe landing space found")
		return Vector2(0,0)
	return v


func consider_jumping_on_top() -> Vector2:
	var collider = check_feeler(Vector2(42 * get_facing_direction(),0))
	var facing = 0 if get_facing_direction() >= 0 else - 1
	if (collider == null):
		return Vector2(0,0)
	var local_position = tilemap.to_local(feeler_raycast.get_collision_point())
	var map_position = tilemap.world_to_map(local_position)
	var tile_position = Vector2(map_position.x + facing, map_position.y)
	# TODO Here the climb height of frog is limited to one constantly
	if (tilemap.get_cell(tile_position.x, tile_position.y - 1) != -1 &&
			#TODO 9 is the navigation tile!
			tilemap.get_cell(tile_position.x, tile_position.y - 1) != 9):
		return Vector2(0,0)
	var tile_upper_left_corner = tilemap.to_global(tilemap.map_to_world(tile_position))
	var tile_upper_right_corner = Vector2(tile_upper_left_corner.x + tilemap.cell_size.x, tile_upper_left_corner.y)
	
	var jump_angle = 0
	if(facing < 0):
		var frog_bottom_left_corner = Vector2($EnemyBody.global_position.x - $EnemyBody.shape.extents.x,
																					$EnemyBody.global_position.y + $EnemyBody.shape.extents.y)
		jump_angle = frog_bottom_left_corner.angle_to_point(tile_upper_right_corner)
	else:
		var frog_bottom_right_corner = Vector2($EnemyBody.global_position.x + $EnemyBody.shape.extents.x,
																					$EnemyBody.global_position.y + $EnemyBody.shape.extents.y)
		jump_angle = frog_bottom_right_corner.angle_to_point(tile_upper_left_corner) - PI
	
	if(abs(rad2deg(jump_angle)) < 78):
		return correct_jump_direction(velocity_for_jump_distance(default_jump_distance/2, abs(deg2rad(80))))
	else:
		return velocity_for_jump_distance(10, abs(deg2rad(45))) * -1 * facing


# Tries to shorten the jump, so that it lands in a tiles center
func jump_to_tile_center(v: Vector2) -> Vector2:
	var distance = stepify(calculate_jump_distance(v), 0.01)
	if !is_equal_approx(fmod(abs(global_position.x + distance * get_facing_direction()), 24), 12):
		# print(distance)
		# print(global_position.x + distance)
		# print(fmod((global_position.x + distance), 24))
		var new_distance = distance
		if(get_facing_direction() < 0):
			new_distance = fmod((global_position.x + distance), 24) - 12 + distance
		else:
			new_distance = distance + 12 - fmod((global_position.x + distance), 24)
		# print("centering distance")
		# print(new_distance)
		v = change_jump_distance(abs(new_distance), v)
	v = correct_jump_direction(v)
	return v


# TODO Depends on Frog Shape and Tile Shape
func is_jump_path_safe(v: Vector2, pos: Vector2) -> bool:
	var v0 = v.length()
	var angle = v.angle()
	var jump_distance = calculate_jump_distance(v)
	var harmful_nodes = get_tree().get_nodes_in_group("harmful")
	for node in harmful_nodes:
		var node_pos = node.global_position
		if abs(node_pos.x - pos.x) > abs(jump_distance) * 3 || abs(node_pos.x - pos.x) < 1:
			continue
		var node_y = node_pos.y - 12
		var initial_throw_height = node_y - (global_position.y + 9)
		var term1 = (pow(v0, 2) * sin(2 * angle)) / (2 * _gravity)
		var term2 = ((v0 * cos(angle))/_gravity) * sqrt(pow(v0, 2) * pow(sin(angle), 2) + 2 * _gravity * initial_throw_height)
		var distance = abs(term1) + abs(term2)
		# print("distance to next spike")
		# print(pos.x + sign(v.x) * distance - node_pos.x)
		var safe_distance = 12
		if (sign(initial_throw_height) < 0):
			safe_distance = 24
		if(abs(pos.x + sign(v.x) * distance - node_pos.x) < safe_distance):
			return false
	return true


func calculate_jump_height(v: Vector2) -> float:
	return abs((pow(v.length(), 2) * pow(sin(v.angle()), 2))/(2*_gravity))


# Only works for jumps on straight ground
func calculate_jump_distance(v: Vector2) -> float:
	return abs((pow(v.length(), 2) * sin(-1 * 2 * v.angle()))/(_gravity))


func jump_height_to_velocity(target_height: float, v: Vector2) -> Vector2:
	var initial_height = calculate_jump_height(v)
	return v.normalized() * sqrt(pow(v.length(),2)/(initial_height/target_height))


# Changes a Vector for a jump to the targeted distance, keeping the angle
func change_jump_distance(target_distance: float, v: Vector2) -> Vector2:
	var initial_distance = calculate_jump_distance(v)
	return v.normalized() * sqrt(pow(v.length(),2)/(initial_distance/target_distance))


# Takes an angle and a distance to calculate a jump launching at that angle and covering the distance
func velocity_for_jump_distance(distance: float = default_jump_distance*24, angle: float = deg2rad(default_jump_angle)) -> Vector2:
	var abs_velocity = sqrt((distance * _gravity)/sin(2*angle))
	return Vector2(abs_velocity,0).rotated(-1*angle)


func can_reverse_facing_direction() -> bool:
	if(is_on_floor() && !has_reversed):
			return true
	return false


# Returns a jump velocity that has the target_position in it's path
func attack_jump(target_position: Vector2) -> Vector2:
	var target_vector = target_position - global_position
	target_vector = Vector2(abs(target_vector.x), target_vector.y)
	var jump_angle = target_vector.angle()
	var v = Vector2()
	# TODO Tunable parameters
	if jump_angle < deg2rad(-30):
		v = velocity_for_jump_distance(target_vector.x, deg2rad(default_jump_angle))
		v = jump_height_to_velocity(abs(target_vector.y), v)
	else:
		v = velocity_for_jump_distance(target_vector.x * 1.5,deg2rad(45))
	v = correct_jump_direction(v)
	return v


# Checks the feeler ray for collisions and returns collider or null
func check_feeler(v: Vector2, _offset = Vector2(0,0)) -> Object:
	var prev_position = feeler_raycast.position
	feeler_raycast.position += _offset
	feeler_raycast.cast_to = v
	feeler_raycast.force_raycast_update()
	var collider = feeler_raycast.get_collider()
	feeler_raycast.position = prev_position
	return collider


func reverse_facing_direction() -> void:
	has_reversed = true
	print("reversing direction")
	orientation.cast_to.x *= -1
	pass


func get_facing_direction() -> float:
	return orientation.cast_to.x