class_name VelocityController

var velocity :Vector2
var debug :bool = false

const RELATIVE_DIRECTION = 0
const POSITIVE_DIRECTION = 1
const NEGATIVE_DIRECTION = -1

enum RANGE_PLACEMENT {
	BEFORE_RANGE = -1,
	WITHIN_RANGE = 0,
	PAST_RANGE = 1
}

enum IMPULSE_PLACEMENT {
	LEFT_SIDE,
	RIGHT_SIDE
}


## Could these be datatypes or a class? Sure
var _h_impulse_applied :bool = false
var _h_impulse_speed_tracking :Vector2
var _h_impulse_placement_tracking :int = -1
var _v_impulse_applied :bool = false
var _v_impulse_speed_tracking :Vector2

func calculate_velocity(_delta :float,
		movement_parameters :MovementParameters,
		_movement_direction := Vector2(0,0),
		_velocity_override := Vector2(0,0)) -> Vector2:
	
	var calc_velocity = Vector2.ZERO
	if _velocity_override.x != 0.0:
		calc_velocity.x = _velocity_override.x
	else:
		calc_velocity.x = velocity.x
	if _velocity_override.y != 0.0:
		calc_velocity.y = _velocity_override.y
	else:
		calc_velocity.y = velocity.y

	var calc_acceleration = Vector2.ZERO
	var calc_inertia :Vector2
	#calc_inertia.x = abs(calc_velocity.x)
	## We're now toing to preserve inertia direction
	calc_inertia.x = calc_velocity.x
	calc_inertia.y = calc_velocity.y

	## Determine movement direction
	# If there is an inertia direction from existing velocity and 
	# no desired movement we'll continue to travel in that direction.
	##
	## If an override has been passed (we're ignoring input direction
	##TODO: Should preserve initia apply here
	var move_direction = Vector2.ZERO
	move_direction = resolve_move_direction(calc_inertia, _movement_direction)

	## Acceleration is always postive because we use the current inertia
	## placement to the movement range to determine direction of movement. 
	if sign(move_direction.x) != 0:
		#calc_acceleration.x = abs(movement_parameters.get_acceleration(0).x)
		calc_acceleration.x = movement_parameters.get_acceleration().x
		assert(calc_acceleration.x >= 0, "Negative X Acceleration shouln't happen")
	if is_zero_approx(calc_acceleration.x):
		move_direction.x = _movement_direction.x
	
	if sign(move_direction.y) != 0:
		calc_acceleration.y = movement_parameters.get_acceleration().y
		assert(calc_acceleration.y >= 0, "Negative Y Acceleration shouln't happen")
	if is_zero_approx(calc_acceleration.y):
		move_direction.y = _movement_direction.y


	## Inertia only applies if there is a difference between the 
	# base move speed and a derived move speed. This makes it so all you
	# have to do is provide a move speed and that is the speed we will travel.
	# but if a modifier applies, or an accelleration is given.
	# an entirely differant process occurs.
	# h_speed.x can be thought of as impulse speed. While h_speed.y
	# is the destination speed. 
	# We move towards h_speed.y at the acceleration rate
	##
	var start_speed :float = (
		(movement_parameters.get_speed_start(RELATIVE_DIRECTION).x * move_direction.x) + 
		(movement_parameters.get_speed_start(POSITIVE_DIRECTION).x * POSITIVE_DIRECTION) +
		(movement_parameters.get_speed_start(NEGATIVE_DIRECTION).x * NEGATIVE_DIRECTION)
	)
	var end_speed :float = (
		(movement_parameters.get_speed_end(RELATIVE_DIRECTION).x * move_direction.x) + 
		(movement_parameters.get_speed_end(POSITIVE_DIRECTION).x * POSITIVE_DIRECTION) +
		(movement_parameters.get_speed_end(NEGATIVE_DIRECTION).x * NEGATIVE_DIRECTION)
	)	
	var h_speed = Vector2(start_speed, end_speed) 

	start_speed  = (
		(movement_parameters.get_speed_start(RELATIVE_DIRECTION).y * move_direction.y) + 
		(movement_parameters.get_speed_start(POSITIVE_DIRECTION).y * POSITIVE_DIRECTION) +
		(movement_parameters.get_speed_start(NEGATIVE_DIRECTION).y * NEGATIVE_DIRECTION)
	)
	end_speed = (
		(movement_parameters.get_speed_end(RELATIVE_DIRECTION).y * move_direction.y) + 
		(movement_parameters.get_speed_end(POSITIVE_DIRECTION).y * POSITIVE_DIRECTION) +
		(movement_parameters.get_speed_end(NEGATIVE_DIRECTION).y * NEGATIVE_DIRECTION)
	)
	var v_speed = Vector2(start_speed, end_speed) 
	
	
	## Now determine placement of current velocity to speed range
	var h_range_placement = placement_to_speed_range(calc_velocity.x, h_speed)
	var v_range_placement = placement_to_speed_range(calc_velocity.y, v_speed)
	
	## We don't want to be able to scoot our impulse speed to cheat the movement
	# Any non zero speed that goes opposite to our inertial direction
	# should only be allowed once. 
	## Reset the impulse when back in range
	# may also want to reset when hspeed is static
	if _h_impulse_applied == true:
		if (h_range_placement == RANGE_PLACEMENT.WITHIN_RANGE):
			_h_impulse_applied = false
			if debug:
				print("Resetting H impulse (Within Range)")
		elif (calc_inertia.x < h_speed.x and _h_impulse_placement_tracking != IMPULSE_PLACEMENT.LEFT_SIDE or
			 calc_inertia.x >= h_speed.x and _h_impulse_placement_tracking != IMPULSE_PLACEMENT.RIGHT_SIDE):
			_h_impulse_applied = false
			if debug:
				print("Resetting H impulse (Impulse Placement Change)")
		
#		if (h_range_placement == RANGE_PLACEMENT.WITHIN_RANGE and 
#				_h_impulse_speed_tracking != h_speed):
#			if debug:
#				print("Resetting H impulse (Within Range)")
#			_h_impulse_applied = false
#		elif(h_range_placement == RANGE_PLACEMENT.BEFORE_RANGE):
#			if debug:
#				print("Resetting H impulse (Before Range) ", _h_impulse_speed_tracking )
		
	if (v_range_placement == RANGE_PLACEMENT.WITHIN_RANGE and 
			_v_impulse_applied == true and
			_v_impulse_speed_tracking != v_speed):
		if debug:
			print("resetting V impulse")
		_v_impulse_applied = false

	## Separate impulse function
	if _h_impulse_applied == false and is_zero_approx(h_speed.x) == false:# and _h_impulse_speed_tracking != h_speed:
		#(range_placement :int, inertia :float, impulse_speed_range :Vector2)
		#_h_impulse_speed_tracking = apply_impulse(h_range_placement, calc_inertia.x, h_speed )			
		var impulse_inertia = apply_impulse(h_range_placement, _h_impulse_speed_tracking , h_speed )
		if h_range_placement == RANGE_PLACEMENT.PAST_RANGE and abs(h_speed.x) > abs(h_speed.y):
			if debug:
				print("Applying H impulse (Past Range): Imp: ", impulse_inertia, ", In:", calc_inertia.x)
			_h_impulse_speed_tracking = h_speed
			_h_impulse_applied = true
			## This might not make sense
			if (calc_inertia.x + h_speed.x) < h_speed.x:
				_h_impulse_placement_tracking = IMPULSE_PLACEMENT.LEFT_SIDE
			else:
				_h_impulse_placement_tracking = IMPULSE_PLACEMENT.RIGHT_SIDE
			## We don't blow past the acceleration
			calc_inertia.x = h_speed.x
		elif h_range_placement == RANGE_PLACEMENT.BEFORE_RANGE:
			if debug:
				print("Applying H impulse (Before Range): Imp: ", impulse_inertia, ", In:", calc_inertia.x)
			_h_impulse_speed_tracking = h_speed
			_h_impulse_applied = true
			if calc_inertia.x < h_speed.x:
				_h_impulse_placement_tracking = IMPULSE_PLACEMENT.LEFT_SIDE
			else:
				_h_impulse_placement_tracking = IMPULSE_PLACEMENT.RIGHT_SIDE
			if sign(h_speed.x) == sign(h_speed.y):
				calc_inertia.x += h_speed.x
			else:
				##TODO: Maybe this should be something else
				calc_inertia.x = 0
		
		## F this, didn't work well
#		if impulse_inertia != calc_inertia.x: ## We have an impulse to apply
#			if debug:
#				print("Applying H impulse: Imp: ", impulse_inertia, ", In:", calc_inertia.x)
#			#var foo = 2+2
#			_h_impulse_speed_tracking = h_speed
#			_h_impulse_applied = true
#			## We want to add impulse in the direction of movement so we need to determine
#			## 	what that is.
#			calc_inertia.x += impulse_inertia
	
	if _v_impulse_applied == false:# and _v_impulse_speed_tracking != v_speed:
		var impulse_inertia = apply_impulse(v_range_placement, _v_impulse_speed_tracking, v_speed)
		if impulse_inertia != calc_inertia.y:
			_v_impulse_speed_tracking = v_speed
			_v_impulse_applied = true
			calc_inertia.y += impulse_inertia
 
	if debug and movement_parameters.debug_name == 'roll': 
		var foo = 2+2
	## We are always moving from h_speed.x towards y at a given rate
	## if we have a difference of speed and an acceleration
	if is_zero_approx(calc_acceleration.x) == false:
		calc_inertia.x = resolve_inertia(
							h_range_placement, 
							calc_inertia.x ,
							h_speed, 
							(calc_acceleration.x * _delta)
		)
	else:
		## Using static move directions insteead of inertial move directions
#		if is_zero_approx(_movement_direction.x) == false:
#			calc_inertia.x = h_speed.x
#		else:
#			calc_inertia.x = 0.0
		calc_inertia.x = h_speed.x
		if debug and movement_parameters.debug_name == 'jump': 
			var foo = 2+2

		
	if is_zero_approx(calc_acceleration.y) == false:
		calc_inertia.y = resolve_inertia(
							v_range_placement, 
							calc_inertia.y ,
							v_speed, 
							(calc_acceleration.y * _delta)
		)
	else:
		## Using static move directions insteead of inertial move directions
#		if is_zero_approx(_movement_direction.y) == false:
#			calc_inertia.y = v_speed.x
		calc_inertia.y = v_speed.x

	## Track or last speed for in range impulses
#	_h_impulse_speed_tracking = h_speed
#	_v_impulse_speed_tracking = v_speed

	#calc_velocity.y = calc_inertia.y
	#calc_velocity.x = calc_inertia.x
	
	if debug:
		UiManager.debug_text =	( #"H_Speed x Dir: " + str(h_speed) + str('duh') +
			"H_Speed  Fm:{0} To:{1}".format({"0":"%5.2f" % h_speed.x, "1":"%5.2f" % h_speed.y}) + 
			"\nV_Speed  Fm:{0} To:{1}".format({"0":"%5.2f" % v_speed.x, "1":"%5.2f" % v_speed.y}) + 
			"\nSpeedRange   :   {0}, {1}".format({"0":"%5.2f" % h_range_placement, "1":"%5.2f" % v_range_placement}) +
			"\nVelocity_Calc:   {0}, {1}".format({"0":"%5.2f" % calc_velocity.x, "1":"%5.2f" % calc_velocity.y}) +
			"\nInertia_Calc:   {0}, {1}".format({"0":"%5.2f" % calc_inertia.x, "1":"%5.2f" % calc_inertia.y}) +
			"\nVelocity_Real:   {0}, {1}".format({"0":"%5.2f" % velocity.x, "1":"%5.2f" % velocity.y}) +
			#"\nFriction:   {0}, {1}".format({"0":"%5.2f" % calc_friction.x, "1":"%5.2f" % calc_friction.y}) +
			"\nAccelCalc   :   {0}, {1}".format({"0":"%5.2f" % calc_acceleration.x, "1":"%5.2f" % calc_acceleration.y}) +
			# h_range_placement
			#"\nLength: " + str(velocity.length()) +
			"\nMoveDir: {0}, {1}".format({"0":"%4.1f" % move_direction.x, "1":"%4.1f" % move_direction.y})
			)
		
	return calc_inertia

func is_out_of_range(value: float, a: float, b: float) -> bool:
	var lower = min(a, b)
	var upper = max(a, b)
	return value <= lower or value >= upper

## Determine the trend of direction and where our current speed is
# in relation to it. This should help determine the direction of
# acceleration and whether we speed up or slow down.
##
func placement_to_speed_range(speed: float, speed_range: Vector2) -> int:
	## Actually we don't care about mix or min
	#var range_start :float = min(speed_range.x, speed_range.y)
	#var range_end :float = max(speed_range.x, speed_range.y)

	##TODO: Do I also need an equivalent

	if is_equal_approx(speed, speed_range.x) or is_equal_approx(speed, speed_range.y):
		return RANGE_PLACEMENT.WITHIN_RANGE
	## We can be at the base range to ensure impulse can happen
	## Direction <-- that way
	if speed_range.x > speed_range.y:
		if speed < speed_range.x and speed > speed_range.y:
			return RANGE_PLACEMENT.WITHIN_RANGE
		elif speed < speed_range.y:
			return RANGE_PLACEMENT.PAST_RANGE
		else:
			return RANGE_PLACEMENT.BEFORE_RANGE
	else: ## Direction --> that way
		if speed > speed_range.x and speed < speed_range.y:
			return RANGE_PLACEMENT.WITHIN_RANGE
		elif speed > speed_range.y:
			return RANGE_PLACEMENT.PAST_RANGE
		else:
			return RANGE_PLACEMENT.BEFORE_RANGE
	return 0


func resolve_move_direction(_momentum :Vector2,
							_movement_direction :Vector2) -> Vector2:
	var horizontal_movement_direction:float
	if sign(_movement_direction.x): ## We're trying to move
		horizontal_movement_direction = sign(_movement_direction.x)
	elif sign(_momentum.x): ## We still have momentum but not trying to move
		horizontal_movement_direction = sign(_momentum.x)
	
	var vertical_movement_direction:float
	if sign(_movement_direction.y):
		vertical_movement_direction = sign(_movement_direction.y)
	elif sign(_momentum.y):
		vertical_movement_direction = sign(_momentum.y)
	
	return Vector2(horizontal_movement_direction, vertical_movement_direction)

func apply_impulse(range_placement :int, tracking_range :Vector2, impulse_speed_range :Vector2) -> float:
	match range_placement:
		RANGE_PLACEMENT.BEFORE_RANGE:
#			inertia += impulse_speed_range.x
#			return inertia
			return impulse_speed_range.x
		RANGE_PLACEMENT.WITHIN_RANGE:
			var track_stuff = placement_to_speed_range(tracking_range.x, impulse_speed_range)
			if tracking_range  != impulse_speed_range:
				## Set inertia to starting speed, we're already in range
				return impulse_speed_range.x
		RANGE_PLACEMENT.PAST_RANGE:
			## If this is a reduced speed move otherwise we usually want to slow down
			## 	because we're past the range
			if abs(impulse_speed_range.x) > abs(impulse_speed_range.y):
#				inertia = impulse_speed_range.x
#				return inertia
				return impulse_speed_range.x
	return 0.0

func resolve_inertia(range_placement :int, 
						inertia :float ,
						speed_range :Vector2, delta_acceleration :float) -> float:

	if speed_range.x != speed_range.y and is_zero_approx(delta_acceleration) == false:
		var direction_accel :float = delta_acceleration #* move_direction.x
		if speed_range.x > speed_range.y:
			direction_accel *= -1
		match range_placement:
			RANGE_PLACEMENT.BEFORE_RANGE:
				## Also apply impulse here
#				if _h_impulse_applied == false:
#					inertia += speed_range.x
#					_h_impulse_applied = true
					#impulse_applied_dir = move_direction.x
				inertia = clamp(inertia + direction_accel,
										min(inertia, speed_range.y),
										max(inertia, speed_range.y))
			RANGE_PLACEMENT.WITHIN_RANGE:
				## If we're within the range but our speed has just changed
#				if _h_impulse_applied == false and _h_impulse_speed_tracking != h_speed:
#					## Set inertia to starting speed, we're already in range
#					calc_inertia.x = h_speed.x
#					_h_impulse_applied = true
				inertia = clamp(inertia + direction_accel,
										min(inertia, speed_range.y),
										max(inertia, speed_range.y))
			RANGE_PLACEMENT.PAST_RANGE:
#				if _h_impulse_applied == false and abs(speed_range.x) > abs(speed_range.y):
#					inertia = speed_range.x
#					_h_impulse_applied = true
				inertia = clamp(inertia - direction_accel, # Friction
										min(inertia, speed_range.y),
										max(inertia, speed_range.y))


	elif is_zero_approx(inertia) == false: ## We still have inertia but no difference in movement
		var clamped_speed = speed_range.x
		if speed_range.x < speed_range.y:
			clamped_speed = clamp(speed_range.x,0.0,speed_range.y)

		## Move back towards the base speed
		inertia = move_toward(inertia, clamped_speed, delta_acceleration)
	##TODO: This hopefully won't happen (the commented else statement) but I wonder 
	##	if I should do anyting here now.
#	else:
#		## inertia is just base speed
#		inertia = speed_range.x
#		if debug and speed_range.x == -90: 
#			var foo = 2+2

	return inertia