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
}

## Side effects for these variables
# velocity - doesn't change but uses it to set base calculations
# accepts the state to determine movement, the deltatime, and an optional direction 
# Update: Actually should just return movement in PPS
##

## Could these be datatypes or a class? Sure
var _h_impulse_applied :bool = false
var _h_impulse_speed_tracking :Vector2
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
	
	var calc_inertial_dir = Vector2(sign(calc_velocity.x),sign(calc_velocity.y))
	#var calc_friction :Vector2 = Vector2.ZERO

	## 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
	var move_direction = Vector2.ZERO
	move_direction = resolve_move_direction(calc_inertia, _movement_direction)
#	else:
#		move_direction = resolve_move_direction(calc_inertia, desired_movement_vector)
	
	#var movement_parameters :MovementParameters = _state.get_movement_parameters()
#	if modifier and modifier.is_active:
#		if (_state.is_grounded and modifier.only_grounded): # Should we skip
#			modifier_indicator = '*'
#			if _state.name == 'jump':
#				var foo = 2+2 # break
#			movement_parameters.apply_state_modifier(modifier, move_direction.x)

	## 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 h_speed = resolve_h_speed(movement_parameters)
	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) 
	#var h_speed = Vector2(movement_parameters.get_speed_start(RELATIVE_DIRECTION).x , movement_parameters.get_speed_end(RELATIVE_DIRECTION).x)
	#var v_speed = resolve_v_speed(movement_parameters)
	var v_speed = Vector2(movement_parameters.get_speed_start(0).y, movement_parameters.get_speed_end(0).y)
	
	## Speed will now be expected to move in the direction of travel
	#h_speed *= move_direction.x
	v_speed *= move_direction.y
	
	## 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_range_placement == RANGE_PLACEMENT.WITHIN_RANGE and 
			_h_impulse_applied == true	):
		if debug:
			print("resetting H impulse")
		_h_impulse_applied = false
	if (v_range_placement == RANGE_PLACEMENT.WITHIN_RANGE and 
			_v_impulse_applied == true	):
		if debug:
			print("resetting V impulse")
		_v_impulse_applied = false

	## Acceleration is always postive because we use the 
	# move toward functions.
	#calc_acceleration.x = abs(resolve_h_acceleration(movement_parameters, move_direction.x))
	if sign(move_direction.x) != 0:
		calc_acceleration.x = movement_parameters.get_acceleration(0).x
	if sign(move_direction.y) != 0:
		calc_acceleration.y = movement_parameters.get_acceleration(0).y
 
	if debug and movement_parameters.debug_name == 'move': 
		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
	##TODO: The Min Max could be augmented to just be h_speed.x and prevent the sliding.
	if h_speed.x != h_speed.y and calc_acceleration.x != 0.0:
		var direction_accel :float = calc_acceleration.x * _delta #* move_direction.x
		if h_speed.x > h_speed.y:
			direction_accel *= -1
		match h_range_placement:
			RANGE_PLACEMENT.BEFORE_RANGE:
				## Also apply impulse here
				if _h_impulse_applied == false:
					calc_inertia.x += h_speed.x
					_h_impulse_applied = true
					#impulse_applied_dir = move_direction.x
				calc_inertia.x = clamp(calc_inertia.x + direction_accel,
										min(calc_inertia.x, h_speed.y),
										max(calc_inertia.x, h_speed.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
				calc_inertia.x = clamp(calc_inertia.x + direction_accel,
										min(h_speed.x, h_speed.y),
										max(h_speed.x, h_speed.y))
			RANGE_PLACEMENT.PAST_RANGE:
				if _h_impulse_applied == false and abs(h_speed.x) > abs(h_speed.y):
					calc_inertia.x = h_speed.x
					_h_impulse_applied = true
				calc_inertia.x = clamp(calc_inertia.x - direction_accel, # Friction
										min(calc_inertia.x, h_speed.y),
										max(calc_inertia.x, h_speed.y))
		
			
	elif calc_inertia.x != 0.0 and calc_acceleration.x != 0.0: ## We still have inertia but no difference in movement
		if h_speed.x < h_speed.y:
			h_speed.x = clamp(h_speed.x,0.0,h_speed.y)

		## Move back towards the base speed
		calc_inertia.x = move_toward(calc_inertia.x, h_speed.x, calc_acceleration.x * _delta)

	else:
		## inertia is just base speed
		calc_inertia.x = h_speed.x
		calc_inertial_dir.x = 0.0 # sign(calc_inertia.x) 

	## Another idea, just return the calculated velocity in PPS	
	## For now, y component of velocity is just gravity
	if v_speed.x != v_speed.y: ## For now gravity is just the default acceleration
		var direction_accel :float = movement_parameters.gravity * _delta #* move_direction.x
		if v_speed.x > v_speed.y:
			direction_accel *= -1
		match v_range_placement:
			RANGE_PLACEMENT.BEFORE_RANGE:
				## Also apply impulse here
				if _v_impulse_applied == false:
					calc_inertia.y += v_speed.x
					_v_impulse_applied = true
					#impulse_applied_dir = move_direction.x
				calc_inertia.y = clamp(calc_inertia.y + direction_accel,
										min(calc_inertia.x, v_speed.y),
										max(calc_inertia.x, v_speed.y))
			RANGE_PLACEMENT.WITHIN_RANGE:
				## If we're within the range but our speed has just changed
				if _v_impulse_applied == false and _v_impulse_speed_tracking != v_speed:
					## Set inertia to starting speed, we're already in range
					calc_inertia.y = v_speed.x
					_v_impulse_applied = true
				calc_inertia.y = clamp(calc_inertia.y + direction_accel,
										min(v_speed.x, v_speed.y),
										max(v_speed.x, v_speed.y))
			RANGE_PLACEMENT.PAST_RANGE:
				calc_inertia.y = clamp(calc_inertia.y - direction_accel, # Friction
										min(calc_inertia.y, v_speed.y),
										max(calc_inertia.y, v_speed.y))

	else:
		## The previous vertical movement methods
		calc_inertia.y += movement_parameters.gravity * _delta

	## 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_velocity

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)

	## 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

## Returns an Vector where x is MIN_SPEED and y is MAX_SPEED
func resolve_h_speed(_params :MovementParameters) -> Vector2:
#	var base_speed :float = _params.base_move_speed.x
#	## if a speed difference applies
#	if _params.move_speed_modifier.x != 0:
#		var speed_differance = base_speed + _params.move_speed_modifier.x
#
#		return(Vector2(base_speed, speed_differance))
#	return Vector2(base_speed,base_speed)
	return Vector2(0,0)

func resolve_v_speed(_params :MovementParameters) -> Vector2:
#	var base_speed :float = _params.base_move_speed.y
#	## if a speed difference applies
#	if _params.move_speed_modifier.y != 0:
#		var speed_differance = base_speed + _params.move_speed_modifier.y
#
#		return(Vector2(base_speed, speed_differance))
#	return Vector2(base_speed,base_speed)
	return Vector2(0,0)

func resolve_h_acceleration(_params :MovementParameters, _move_direction :float) -> float:
	## TODO: Adjust for jerk, determine if we're currently experiencing accel
	## if a speed difference applies
#	var _acceleration :float =  0.0
#	var base_speed :float = _params.base_move_speed.x
#	if _params.move_speed_modifier.x != 0:
#		var speed_differance = base_speed + _params.move_speed_modifier.x
#		_acceleration = _params.base_move_acceleration.x + _params.move_speed_modifier_acceleration.x
#
#	else:
#		_acceleration = _params.base_move_acceleration.x
#
#	if sign(_move_direction) != 0:
#		return _acceleration

	# No accel returned unless intended
	return 0.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)
