using System;
using Drawing;
using NodeCanvas.Framework;
using ParadoxNotion.Design;
using ParadoxNotion.Services;
using Reset.Core.Tools;
using Reset.Units;
using Sirenix.Serialization;
using Unity.Cinemachine;
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;
		
		private Vector3 velocityOnStart;
		private Vector3 directionOnStart;
		private Vector3 originalDirection;
		
		public float speed;
		public float minDistance;
		[Tooltip("The dot product between the current direction to the grapple point, and the direction to the grapple point when started. Starts at 1 and gradually gets closer to -1, with 0 being 90 degrees perpendicular. Only for Y")]
		public float horizontalDotBreak;
		[Tooltip("The dot product between the current direction to the grapple point, and the direction to the grapple point when started. Starts at 1 and gradually gets closer to -1, with 0 being 90 degrees perpendicular. Only for XZ")]
		public float verticalDotBreak;
		
			
		private Vector3 smoothedInput;
		private Vector3 smoothedInputRefVelocity;

		private Vector3 gizmoSwingDirection;
		private Vector3 gizmoPointDirection;
		private Vector3 gizmoFinalDirection;
		private Vector3 gizmosSmoothedInput;
		private float gizmoVertValue;

		private Transform camera;
		
		Vector3 smoothedSwingDirection;

		private Vector3 finalDirection;
		private float yChangeMultipler;

		//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;
			// Set the initial direction
			directionOnStart = agent.transform.position.DirectionTo(grapplePoint.value);
			
			// Get the current move direction
			velocityOnStart = agent.outputMoveDirection;
			
			// For setting finalDirection's initial value, first compose the swing variables one-time
			Vector3 velocityWhenMoving = CalculateSwingDirections(Vector3.Distance(agent.transform.position, grapplePoint.value), directionOnStart);
			
			// Lerp the initial direction more towards the point of the grapple and less towards current momentum if not moving fast
			finalDirection = Vector3.Lerp(velocityOnStart, velocityWhenMoving, velocityOnStart.magnitude / 2f); // This isn't working
			
			// Set the intial swing direction to the same thing, so it starts swinging withing snapping on start
			smoothedSwingDirection = finalDirection.Flatten(null, .4f, null);

			// smoothedInput = agent.GetComponent<CharacterController>().velocity.normalized.Flatten(null, 0f, 0f);
			smoothedInput = Vector3.zero;
		}

		//Called once per frame while the action is active.
		protected override void OnUpdate(){		
			agent.outputMoveDirection = Vector3.MoveTowards(agent.outputMoveDirection, Vector3.zero, .5f);
			
			// Basic variables, direction to point and current distnace
			Vector3 directionToPoint = agent.transform.position.DirectionTo(grapplePoint.value);
			float currentDist = Vector3.Distance(agent.transform.position, grapplePoint.value);
			
			// Calculate input
			Vector2 rawInput = agent.GetComponent<PlayerControls>().rawMoveInput;
			Vector3 input = new(rawInput.x, rawInput.y, 0f);
			
			smoothedInput = Vector3.SmoothDamp(smoothedInput, input, ref smoothedInputRefVelocity, 1f); 
			DebugOverlayDrawer.ChangeValue("Grapple", "Smoothed Input", smoothedInput.ToString());
			
			// The swing angle needs to change for the downwards swing, based on distance to the ground
			Physics.Raycast(agent.transform.position, Vector3.down, out RaycastHit hit);
			float distanceToGround = hit.distance;
			
			float downwardsSwingAngle = Mathf.Lerp(5, 100, distanceToGround / 20f);
			
			// Altered swing angle based on distance to the grapple point, used to keep the player not too close or far
			float inwardsAngle = Mathf.Lerp(0f, -60f, currentDist / -15f);
			float outwardsAngle = Mathf.Lerp(0f, -60f, currentDist / 15f);
			float outputAngle = inwardsAngle + outwardsAngle;
			
			DebugOverlayDrawer.ChangeValue("Grapple", "Output Angle", outputAngle.ToString() + $"({inwardsAngle.ToString()} + {outwardsAngle.ToString()})");
			
			// Calculate the swing direction.
			// Vector3 swingDirection = Quaternion.LookRotation(smoothedInput) * directionToPoint * smoothedInput.magnitude; // Old
			Vector3 pointDirectionXZStable = agent.transform.position.DirectionTo(grapplePoint.value.Flatten(null, agent.transform.position.y));
			Vector3 rightSwingDirectin =  Quaternion.AngleAxis(100f + outputAngle, Vector3.up) * pointDirectionXZStable; // Working
			Vector3 leftSwingDirectin =  Quaternion.AngleAxis(-100f - outputAngle, Vector3.up) * pointDirectionXZStable; // Working
			Vector3 upwardsSwingDirection = Quaternion.AngleAxis(-140f - outputAngle, Quaternion.LookRotation(directionToPoint) * Vector3.right) * directionToPoint; // Working
			Vector3 downwardsSwingDirection = Quaternion.AngleAxis(downwardsSwingAngle, Quaternion.LookRotation(directionOnStart) * Vector3.right) * directionToPoint; // WORKING NOW!! Note: this has to rotate by directionOnStart because else it just moves towards the point
			
			// Get the target swing direction. This is the direction "around" the point based on context
			Vector3 targetSwingDirection;
			
			if (Vector3.Dot(-directionOnStart, directionToPoint) > 0) { // More than 90 degrees from the start angle, just start going forward from the swing
				targetSwingDirection = finalDirection;	
			} else {
				// Start with up and down
				Vector3 yAxisTargetDirection; 
				
				if (Mathf.Abs(input.y) > 0.1f) { // Input exists on up and down, switch direction based on input
					yAxisTargetDirection = Vector3.Slerp(upwardsSwingDirection, downwardsSwingDirection, Mathf.Abs((input.y - 1f) / 2f));
				} else { // No input on up/down controller, so swing relative to the point
					if (directionToPoint.y < 0) { // Since you're under the point swing downwards
						yAxisTargetDirection = downwardsSwingDirection;
					} else { // Since you're over the point, swing upwards
						yAxisTargetDirection = upwardsSwingDirection;
					}
				}
				
				// Finalize the up and down by setting it based on y input
				targetSwingDirection = yAxisTargetDirection * Mathf.Abs((input.y));
				
				// Start doing left and right now
				if (Mathf.Abs(input.x) > 0.1f) {
					// Pick between swinging left or right
					Vector3 xAxisTargetDirection = Vector3.Lerp(rightSwingDirectin, leftSwingDirectin, Mathf.Abs((input.x - 1f) / 2f));
					
					// Add a little guaranteed up swing to the left and right swings
					xAxisTargetDirection += (Vector3.up * .1f) * Mathf.Abs((input.x));

					// Now finalize the left and right, adding it to the target swing direciton based on x input
					targetSwingDirection = Vector3.Slerp(targetSwingDirection, xAxisTargetDirection, Mathf.Abs((input.x)));
				}

				// Normalize
				targetSwingDirection = targetSwingDirection.normalized;
			}
			
			// Turn the difference in direction between what's now and what's targetted, to kneecap the smoothing
			float newDirDot = Vector3.Dot(smoothedSwingDirection, targetSwingDirection);
			
			// Normalize the dot
			newDirDot = (newDirDot + 1f) / 2f;
			
			// Smooth the swinging
			smoothedSwingDirection = Vector3.Slerp(smoothedSwingDirection, targetSwingDirection, 2f * Time.deltaTime * newDirDot); 
			
			// Set the output direction direction
			finalDirection = Vector3.Slerp(finalDirection, smoothedSwingDirection, (elapsedTime / 1f) + Mathf.Max(0f, smoothedInput.magnitude)); 
			
			// Gizmos
			gizmoVertValue = finalDirection.y;
			gizmosSmoothedInput = smoothedInput;
			gizmoPointDirection = targetSwingDirection;
			gizmoSwingDirection = smoothedSwingDirection; // Set to smoothedSwingDirection when done testing
			gizmoFinalDirection = finalDirection;
			
			// Finalize the movement to the controller
			agent.SetNewDirection(Vector3.Lerp(agent.additionalMoveDirection, finalDirection.Flatten(null, 0), 1f * Time.deltaTime));
			agent.SetNewGravity(finalDirection.y);
			agent.SmoothToSpeed(speed, 25f * Time.deltaTime);
			
			// Calculate dot products for using to end the action
			float xzDot = Vector3.Dot(-directionOnStart.Flatten(null, 0).normalized, -directionToPoint.Flatten(null, 0).normalized);
			float yDot = Vector3.Dot( // This one has to be rotated around the XZ
				Quaternion.LookRotation(directionToPoint) * -directionOnStart.Flatten(null, null, 0).normalized, 
				Quaternion.LookRotation(directionToPoint) * -directionToPoint.Flatten(null, null, 0).normalized
				);

			DebugOverlayDrawer.ChangeValue("Grapple", "Horizontal Dot", xzDot.ToString());
			DebugOverlayDrawer.ChangeValue("Grapple", "Vertical Dot", yDot.ToString());

			// Check if done
			if (xzDot < horizontalDotBreak || yDot < verticalDotBreak) {
				if (elapsedTime > 2f){
					EndAction(true);
				}
			} else if (currentDist < minDistance) {
				EndAction(true);
			}
		}

		Vector3 CalculateSwingDirections(float currentDist, Vector3 directionToPoint){
			// Get input
			Vector2 rawInput = agent.GetComponent<PlayerControls>().rawMoveInput;
			Vector3 input = new(rawInput.x, rawInput.y, 0f);
			
			// The swing angle needs to change for the downwards swing, based on distance to the ground
			Physics.Raycast(agent.transform.position, Vector3.down, out RaycastHit hit);
			float distanceToGround = hit.distance;

			float downwardsSwingAngle = Mathf.Lerp(30, 100, distanceToGround / 20f);
			
			// Altered swing angle based on distance to the grapple point, used to keep the player not too close or far
			float inwardsAngle = Mathf.Lerp(0f, -60f, currentDist / -15f);
			float outwardsAngle = Mathf.Lerp(0f, -60f, currentDist / 15f);
			float outputAngle = inwardsAngle + outwardsAngle;
			
			DebugOverlayDrawer.ChangeValue("Grapple", "Output Angle", outputAngle.ToString() + $"({inwardsAngle.ToString()} + {outwardsAngle.ToString()})");
			
			Vector3 pointDirectionXZStable = agent.transform.position.DirectionTo(grapplePoint.value.Flatten(null, agent.transform.position.y));
			Vector3 rightSwingDirectin =  Quaternion.AngleAxis(100f + outputAngle, Vector3.up) * pointDirectionXZStable; // Working
			Vector3 leftSwingDirectin =  Quaternion.AngleAxis(-100f - outputAngle, Vector3.up) * pointDirectionXZStable; // Working
			Vector3 upwardsSwingDirection = Quaternion.AngleAxis(-140f - outputAngle, Quaternion.LookRotation(directionToPoint) * Vector3.right) * directionToPoint; // Working
			Vector3 downwardsSwingDirection = Quaternion.AngleAxis(downwardsSwingAngle, Quaternion.LookRotation(directionOnStart) * Vector3.right) * directionToPoint; // WORKING NOW!! Note: this has to rotate by directionOnStart because else it just moves towards the point
			
			// Get the target swing direction. This is the direction "around" the point based on context
			Vector3 targetSwingDirection;
			
			// Start with up and down
			Vector3 yAxisTargetDirection; 
			
			if (Mathf.Abs(input.y) > 0.1f) { // Input exists on up and down, switch direction based on input
				yAxisTargetDirection = Vector3.Slerp(upwardsSwingDirection, downwardsSwingDirection, Mathf.Abs((input.y - 1f) / 2f));
			} else { // No input on up/down controller, so swing relative to the point
				if (directionToPoint.y < 0) { // Since you're under the point swing downwards
					yAxisTargetDirection = downwardsSwingDirection;
				} else { // Since you're over the point, swing upwards
					yAxisTargetDirection = downwardsSwingDirection;
				}
			}

			if (directionToPoint.y > -.5f) {
				// yAxisTargetDirection += Vector3.up * 4f; // This works but it's making downward motion not work
			}
			
			targetSwingDirection = yAxisTargetDirection * Mathf.Abs((input.y));
			
			if (Mathf.Abs(input.x) > 0.1f) {
				Vector3 xAxisTargetDirection = Vector3.Lerp(rightSwingDirectin, leftSwingDirectin, Mathf.Abs((input.x - 1f) / 2f));
				targetSwingDirection = Vector3.Slerp(targetSwingDirection, xAxisTargetDirection, Mathf.Abs((input.x)));
				// targetSwingDirection = xAxisTargetDirection;
				DebugOverlayDrawer.ChangeValue("Grapple", "LR Input Dot", Mathf.Abs((input.x - 1f) / 2f).ToString());
			}
			
			return targetSwingDirection.normalized;
		}

		public void DrawGrappleGizmo(){
			// Destination gizmos
			using (Draw.WithColor(Color.blue)){
				Vector3 offsetTowardsCamera = grapplePoint.value.DirectionTo(camera.transform.position);
				
				// Grapple Point
				Draw.SolidCircle(grapplePoint.value + offsetTowardsCamera, grapplePoint.value.DirectionTo(camera.transform.position), .4f);
				Draw.Label2D(grapplePoint.value + offsetTowardsCamera * 2f  + Vector3.up, "Grapple Point");

				using (Draw.WithLineWidth(1.5f)) {
					// Final Direction
					Draw.Line(agent.transform.position + Vector3.up, agent.transform.position + Vector3.up + gizmoFinalDirection.normalized * 2f);
					Draw.ArrowheadArc(agent.transform.position + Vector3.up, gizmoFinalDirection.normalized, 2f, 15f);

					// Colors for faded arrows
					Color swingColor = Color.Lerp(Color.blue, Color.blue.Alpha(.4f), (elapsedTime * .6f));
					Color dirColor = Color.Lerp(Color.blue.Alpha(.4f), Color.blue, (elapsedTime * .6f));

					// Swing Direction
					using (Draw.WithColor(swingColor)) {
						float swingLength = 2.2f;
						Vector3 swingStart = agent.transform.position + Vector3.up * .4f;
						Vector3 swingDir = swingStart + gizmoSwingDirection.normalized * swingLength;
						Draw.DashedLine(swingStart, swingDir, .2f, .2f);
						Draw.ArrowheadArc(swingStart, gizmoSwingDirection.normalized, swingLength, 15f);
						Draw.Label2D(swingDir + Vector3.up * .4f, "Swing Direction");
					}

					// Point Direction
					using (Draw.WithColor(dirColor)) {
						float pointLength = 1.2f;
						Vector3 pointStart = agent.transform.position + Vector3.up * .2f;
						Vector3 pointDir = grapplePoint.value;
						Draw.DashedLine(pointStart, pointDir, .2f, .2f);
						Draw.ArrowheadArc(pointStart, gizmoPointDirection.normalized, pointLength, 15f);
						Draw.Label2D(pointDir + Vector3.up * .4f, "Grapple Point Direction");
					}
				}
			}
			
			// Input Gizmos
			using (Draw.WithColor(Color.red)) {
				// Input left and right, up and down for size 
				Vector3 inputGizmoOffset = (agent.transform.position + camera.rotation * Vector3.back + Vector3.up * .5f);
				Vector3 inputGizmoPosition = inputGizmoOffset + camera.rotation * Vector3.Lerp(Vector3.left, Vector3.right, (gizmosSmoothedInput.x - 1 ) / 2f + 1f);
				Draw.Line(inputGizmoOffset + camera.rotation * Vector3.left, inputGizmoOffset + camera.rotation * Vector3.right);
				Draw.SolidCircle(inputGizmoPosition, inputGizmoPosition.DirectionTo(camera.position), Mathf.Lerp(.2f, .6f, (gizmosSmoothedInput.y - 1 ) / 2f + 1f));
			}
			
			// Up and down
			using (Draw.WithColor(Color.yellow)) {
				Vector3 vertGizmoPosition = agent.transform.position + camera.rotation * Vector3.left;
				float vertGizmoHeight = 1.75f;

				Vector3 vertGizmoStart = vertGizmoPosition + Vector3.up * vertGizmoHeight;
				Vector3 vertGizmoEnd = vertGizmoPosition + Vector3.up * .35f;

				Vector3 circlePos = Vector3.Lerp(vertGizmoEnd, vertGizmoStart, gizmoVertValue);

				Draw.Line(vertGizmoStart, vertGizmoEnd);
				Draw.SolidCircle(circlePos, circlePos.DirectionTo(camera.position), .4f);
				Draw.Label2D(vertGizmoStart + camera.rotation * Vector3.left * 1.5f, gizmoVertValue.ToString());
				
				Vector3 vertArrowUpPosition = vertGizmoStart + camera.rotation * Vector3.left * 1f + Vector3.up * .2f;
				Vector3 vertArrowDownPosition = vertGizmoStart + camera.rotation * Vector3.left * 1f + Vector3.down * .2f;

				if (gizmoVertValue > 0) {
					Draw.SolidTriangle(vertArrowUpPosition + camera.rotation * Vector3.left/4, vertArrowUpPosition + Vector3.up/2, vertArrowUpPosition + camera.rotation * Vector3.right/4);
				} else {
					Draw.SolidTriangle(vertArrowDownPosition + camera.rotation * Vector3.left/4, vertArrowDownPosition + Vector3.down/2, vertArrowDownPosition + camera.rotation * Vector3.right/4);

				}
			}		
		}
		
		//Called when the task is disabled.
		protected override void OnStop() {
			MonoManager.current.onLateUpdate -= DrawGrappleGizmo;
		}

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