using System;
using System.Collections.Generic;
using Godot;

public class ExploreNode : ProgramNode
{
    public Vector3 startPosition;
    public Vector3I targetPosition;
    public List<Vector3> pathPoints;
    public ExploreNode()
    {
        DisplayText = "Explore";
    }
    public override NodeResult Execute(Robot robot, double delta)
    {   
        if (pathPoints == null)
        {
            int safetyCounter = 0;
            int layerRange = Math.Max(GameData.lowestLayer, 1);
            while (true)
            {
                targetPosition = new Vector3I(GameData.rand.Next(GameData.layerSize), GameData.rand.Next(layerRange), GameData.rand.Next(GameData.layerSize));
                if (!GameData.map[targetPosition.Y].tiles[targetPosition.X, targetPosition.Z].wasVisited) break;
                safetyCounter++;
                if (safetyCounter > Math.Pow(GameData.layerSize, 2) * 2)
                {
                    lastExecutionMessage = "No tiles left to explore";
                    return NodeResult.SUCCESS;
                }
            }
        }

        pathPoints ??= new List<Vector3>(Pathfinding.GetPath(Pathfinding.GetClosestStartPoint(robot.Position), targetPosition));
        
        if (pathPoints.Count <= 0)
        {
            lastExecutionMessage = $"No path available {targetPosition}";
            return NodeResult.FAILURE;
        }

        startPosition = robot.Position;
        Vector3 target = pathPoints[0] - startPosition;
        float distance = target.Length();

        float movementSpeed = robot.GetMovementSpeed();

        if (distance < 0.1f * Mathf.Sqrt(movementSpeed))
        {
            robot.Position = pathPoints[0];
            Vector3I mapIndex = Pathfinding.GetClosestStartPoint(robot.Position);
            Tile tile = GameData.map[mapIndex.Y].tiles[mapIndex.X, mapIndex.Z];
            if (!tile.wasVisited)
            {
                tile.VisitTile();
            }

            pathPoints.Remove(pathPoints[0]);
            if (pathPoints.Count <= 0)
            {
                lastExecutionMessage = "Current exploration finished";
                pathPoints = null;
                return NodeResult.RUNNING;
            }

            lastExecutionMessage = "";
            return NodeResult.RUNNING;
        }

        Vector3 direction = target / distance;
        Vector3 lookDirection = new Vector3(direction.X, 0, direction.Z);
        if (lookDirection.Length() > 0.1f)
        {
            robot.LookAt(robot.GlobalPosition + lookDirection, Vector3.Up);
        }
        robot.GlobalPosition += direction * (float)delta * movementSpeed;

        return NodeResult.RUNNING;
    }

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

    public override void ReadParameters(NodeDisplay display)
    {
    }

    public override void Setup(NodeDisplay display)
    {
    }

    public override string Save()
    {
        return $"Name: {DisplayText}";
    }
}
