project-reset/Assets/Scripts/Core/Graph Tasks/DoGrapplePull.cs

using Drawing;
using NodeCanvas.Framework;
using ParadoxNotion.Design;
using ParadoxNotion.Services;
using Reset.Units;
using UnityEngine;


namespace NodeCanvas.Tasks.Actions {

	[Category("Reset/Movement")]
	[Description("Pulls the agent towards a position with a spring-like effect")]
	public class DoGrapplePull : ActionTask<UnitMovementHandler>{
		public BBParameter<Vector3> grapplePoint;
		public BBParameter<Vector3> offset;

		public BBParameter<float> pullAccelerationSpeed;
		public BBParameter<float> pullDeaccelerationSpeed;
		
		[Tooltip("X is minimum speed, Y is maximum speed")]
		public BBParameter<Vector2> pullTimeRange;
		[Tooltip("X is the distance where the curve will first be evaluated, Y is the distance where the curve will last be evaluated")]
		public BBParameter<Vector2> pullSpeedRange;

		public BBParameter<float> slowdownDistance;

		public BBParameter<AnimationCurve> pullSpeedCurve;
		public BBParameter<AnimationCurve> endDeaccelerationCurve;
		
		private float startTime;

		private Vector3 originalDirection;
		public float originalDistance;
		public Vector3 originalLocation;
		
		public float breakAtDistance;
		public float breakAtDotProduct;
			
		private float currentSpeed;
		private Vector3 smoothedInput;
		private Vector3 smoothedInputRefVelocity;

		private Vector3 gizmoHookPoint;
		private Vector3 gizmoCenterPoint;
		private Vector3 gizmoDirection;
		private Vector3 gizmoStartPos;
		private Vector3 gizmoEndPos;

		private Transform camera;

		private Vector3 finalDirection;
		
	//Use for initialization. This is called only once in the lifetime of the task.
		//Return null if init was successfull. Return an error string otherwise
		protected override string OnInit(){
			
			return null;
		}

		//This is called once each time the task is enabled.
		//Call EndAction() to mark the action as finished, either in success or failure.
		//EndAction can be called from anywhere.
		protected override void OnExecute(){
			camera = Camera.main.transform;
			MonoManager.current.onLateUpdate += DrawGrappleGizmo;
			originalLocation = agent.transform.position;
			originalDirection = agent.transform.position.DirectionTo(grapplePoint.value);
			originalDistance = Vector3.Distance(agent.transform.position, grapplePoint.value);
			startTime = Time.time;
			currentSpeed = pullSpeedCurve.value[0].value * pullSpeedRange.value.y;
			finalDirection = Vector3.zero;
		}

		//Called once per frame while the action is active.
		protected override void OnUpdate(){		
			// Add input changes
			Vector2 rawInput = agent.GetComponent<PlayerControls>().rawMoveInput;
			Vector3 input = Quaternion.LookRotation(agent.transform.position.DirectionTo(grapplePoint.value)) * new Vector3(rawInput.x, rawInput.y, 0f);
			// input = Quaternion.LookRotation(agent.transform.position.DirectionTo(grapplePoint.value)) * (camera.rotation * input);

			smoothedInput = Vector3.SmoothDamp(smoothedInput, input, ref smoothedInputRefVelocity, 5f * Time.deltaTime); 
			
			// Create the distance variables
			Vector3 dirToPoint = agent.transform.position.DirectionTo(grapplePoint.value + (smoothedInput * 6));
			float currentDist = Vector3.Distance(agent.transform.position, grapplePoint.value); 

			var center = agent.transform.position + agent.transform.position.DirectionTo(grapplePoint.value).normalized * 5f;
			
			// finalDirection = Vector3.Slerp(finalDirection, dirToPoint, originalDistance);
			// finalDirection = Vector3.Slerp(relativeStart, relativeEnd, currentDist / 5f);
			finalDirection = Vector3.Slerp(finalDirection, dirToPoint + smoothedInput, currentDist / 5f);
			
			gizmoHookPoint = grapplePoint.value + (smoothedInput * 5);
				
			float evaluatedSpeed = pullSpeedCurve.value.Evaluate(Mathf.Clamp((Time.time - startTime) / 6f, 0f, Mathf.Infinity));

			float speedAgainstCurve = Mathf.Lerp(pullSpeedRange.value.x, pullSpeedRange.value.y, evaluatedSpeed);
			
			// Find how far from 0-1 the player is from the max and minimum distance
			// Get the base distance then account for the minimum distance to the point so that being the minimum away will Lerp to 1
			// float currentDistMinimumAccounted = (currentDist - pullSpeedDistances.value.x); 

			if (currentDist < slowdownDistance.value) {
				float change = endDeaccelerationCurve.value.Evaluate((slowdownDistance.value - currentDist) / slowdownDistance.value);
				speedAgainstCurve = speedAgainstCurve * change;
				
				// Debug.Log($"prev: {speedAgainstCurve}, norm: {(slowdownDistance.value - currentDist) / slowdownDistance.value}, change: {change}, output: {speedAgainstCurve * change} ");
			}

			// Evaluate the normalized value
			// float normaled = Mathf.Lerp(0, 1f, 1f - elapsedTime / (pullSpeedDistances.value.y - pullSpeedDistances.value.x)); 
			
			// Use the curve evaluation to set the speed
			// float outputSpeed = Mathf.Lerp(pullTimeRange.value.x, pullTimeRange.value.y, evaluatedSpeed);

			// Soften the speed changes
			currentSpeed = Mathf.Lerp(currentSpeed, speedAgainstCurve, 10f * Time.deltaTime);
			
			Debug.Log(dirToPoint);
			dirToPoint.y *= 2.5f;

			gizmoDirection = finalDirection;
			// agent.GetComponent<CharacterController>().Move((dirToPoint.normalized + smoothedInput) * currentSpeed * Time.deltaTime);
			agent.AddToCurrentDirection(finalDirection, currentSpeed * Time.deltaTime);

			// Debug.Log( $"{ endDeaccelerationCurve.value.Evaluate(((slowdownDistance.value - currentDist)) )}");

			if (Vector3.Dot(originalDirection, dirToPoint) < breakAtDotProduct) {
				EndAction(true);
			} else if (currentDist < breakAtDistance) {
				EndAction(true);
			}
		}

		public void DrawGrappleGizmo(){
			using (Draw.WithColor(Color.blue)){
				Draw.Arrow(agent.transform.position, agent.transform.position + gizmoDirection);
				Draw.SolidCircle(grapplePoint.value, grapplePoint.value.DirectionTo(camera.position), 1f);
				
			}

			using (Draw.WithColor(Color.yellow)) {
				Draw.Arrow(grapplePoint.value, gizmoHookPoint);
			}

			using (Draw.WithColor(Color.red)) {
				Draw.SolidCircle(gizmoCenterPoint, gizmoCenterPoint.DirectionTo(camera.position), .2f);
			}

			using (Draw.WithColor(Color.magenta)) {
				Draw.Line(agent.transform.position, grapplePoint.value);
			}
		}
		
		//Called when the task is disabled.
		protected override void OnStop() {
			MonoManager.current.onLateUpdate -= DrawGrappleGizmo;
		}

		//Called when the task is paused.
		protected override void OnPause() {
			
		}
	}
}