using System.Collections.Generic;
using Godot;

public class MoveNode : ProgramNode
{
	public Vector3 startPosition;
	public Vector3I targetPosition;
	public List<Vector3> pathPoints;

	public MoveNode()
	{
		DisplayText = "Move";
	}

	public override NodeResult Execute(Robot robot, double delta)
	{
		Vector3I closestPosition = Pathfinding.GetClosestStartPoint(robot.Position);
		if (pathPoints == null)
		{
			pathPoints = new List<Vector3>(Pathfinding.GetPath(closestPosition, targetPosition));
		}

		if (pathPoints.Count <= 0)
		{
			if ((closestPosition - targetPosition).Length() == 0)
			{
				lastExecutionMessage = "";
				return NodeResult.SUCCESS;
			}

			lastExecutionMessage = "No path available";
			return NodeResult.FAILURE;
		}

		return MoveAlongPath(robot, delta);
	}

	private NodeResult MoveAlongPath(Robot robot, double delta)
	{
		startPosition = robot.Position;
		Vector3 target = pathPoints[0] - startPosition;
		float distance = target.Length();
		float movementSpeed = robot.GetMovementSpeed();

		if (distance < 0.1f * Mathf.Sqrt(movementSpeed))
		{
			return FinishCurrentStep(robot);
		}

		Vector3 direction = target / distance;
		RotateRobot(robot, direction);
		robot.GlobalPosition += direction * (float)delta * movementSpeed;

		return NodeResult.RUNNING;
	}

	private NodeResult FinishCurrentStep(Robot robot)
	{
		robot.Position = pathPoints[0];
		VisitCurrentTile(robot);
		pathPoints.RemoveAt(0);
		lastExecutionMessage = "";

		if (pathPoints.Count > 0) return NodeResult.RUNNING;

		pathPoints = null;
		return NodeResult.SUCCESS;
	}

	private void VisitCurrentTile(Robot robot)
	{
		Vector3I mapIndex = Pathfinding.GetClosestStartPoint(robot.Position);
		Tile tile = GameData.map[mapIndex.Y].tiles[mapIndex.X, mapIndex.Z];
		if (!tile.wasVisited)
		{
			tile.VisitTile();
		}
	}

	private void RotateRobot(Robot robot, Vector3 direction)
	{
		Vector3 lookDirection = new Vector3(direction.X, 0, direction.Z);
		if (lookDirection.Length() <= 0.1f) return;

		robot.LookAt(robot.GlobalPosition + lookDirection, Vector3.Up);
	}

	public override ProgramNode Duplicate()
	{
		return new MoveNode
		{
			targetPosition = targetPosition
		};
	}

	public override string Save()
	{
		return $"Name: {DisplayText}, Position: ({targetPosition.X}|{targetPosition.Y}|{targetPosition.Z})";
	}
}