public void Search(NavigationGraphNode startNode, NodeGoalBounds nodeGoalBounds)
        {
            NodeRecord startNodeRecord = this.NodeRecordArray.GetNodeRecord(startNode);

            startNodeRecord.StartNodeOutConnectionIndex = -1;
            startNodeRecord.fValue = 0;
            startNodeRecord.parent = null;

            for (int i = 0; i < startNode.OutEdgeCount; i++)
            {
                NodeRecord nodeChildRecord = this.NodeRecordArray.GetNodeRecord(startNode.EdgeOut(i).ToNode);
                nodeChildRecord.parent = startNodeRecord;
                nodeChildRecord.StartNodeOutConnectionIndex = i;
                nodeChildRecord.fValue = (startNodeRecord.node.Position - nodeChildRecord.node.Position).magnitude;

                this.Open.AddToOpen(nodeChildRecord);
            }

            this.Closed.AddToClosed(startNodeRecord);


            while (this.Open.CountOpen() > 0)
            {
                var bestRecord = this.Open.GetBestAndRemove();
                for (int i = 0; i < bestRecord.node.OutEdgeCount; i++)
                {
                    this.ProcessChildNode(bestRecord, bestRecord.node.EdgeOut(i), i);
                }
                nodeGoalBounds.connectionBounds [bestRecord.StartNodeOutConnectionIndex].UpdateBounds(bestRecord.node.Position);
                this.Closed.AddToClosed(bestRecord);
            }
        }
        private void Initialize(NavigationGraphNode startNode)
        {
            var startNodeRecord = new NodeRecord
            {
                fValue = 0,
                gValue = 0,
                hValue = 0,
                StartNodeOutConnectionIndex = -1,
                node   = startNode,
                status = NodeStatus.Closed
            };

            this.Open.Initialize();
            this.Closed.Initialize();

            this.Closed.AddToClosed(startNodeRecord);

            for (var i = 0; i < startNode.OutEdgeCount; i++)
            {
                var outEdge         = startNode.EdgeOut(i);
                var childNodeRecord = this.NodeRecordArray.GetNodeRecord(outEdge.ToNode);

                var gValue = startNodeRecord.gValue + (childNodeRecord.node.LocalPosition - startNodeRecord.node.LocalPosition).magnitude;

                childNodeRecord.hValue = 0;
                childNodeRecord.gValue = gValue;
                childNodeRecord.fValue = gValue;
                childNodeRecord.StartNodeOutConnectionIndex = i;
                childNodeRecord.parent = startNodeRecord;

                this.UpdateBoundingBoxes(childNodeRecord, i);

                this.Open.AddToOpen(childNodeRecord);
            }
        }
    public override ActionResult Execute(AI ai)
    {
        if (!MoveTargetVariable.IsValid || !MoveTargetVariable.IsVariable)
        {
            return(ActionResult.FAILURE);
        }

        WaypointSet waypointSet = GetWaypointSetFromExpression(ai);

        if (waypointSet == null)
        {
            return(ActionResult.FAILURE);
        }

        if (waypointSet != lastWaypointSet)
        {
            lastWaypoint    = -1;
            lastWaypointSet = waypointSet;
        }

        if (lastWaypoint == -1)
        {
            lastWaypoint = waypointSet.GetClosestWaypointIndex(ai.Kinematic.Position);
            if (lastWaypoint < 0)
            {
                return(ActionResult.FAILURE);
            }

            moveTarget.VectorTarget        = waypointSet.Waypoints[lastWaypoint].position;
            moveTarget.CloseEnoughDistance = Mathf.Max(waypointSet.Waypoints[lastWaypoint].range, ai.Motor.CloseEnoughDistance);
            if (!ai.Motor.IsAt(moveTarget))
            {
                ai.WorkingMemory.SetItem <MoveLookTarget>(MoveTargetVariable.VariableName, moveTarget);
                return(ActionResult.SUCCESS);
            }
        }

        //**REMOVED EXTRA LINE HERE
        NavigationGraphNode tNode = waypointSet.Graph.GetNode(lastWaypoint);

        if (tNode.OutEdgeCount > 0)
        {
            int tRandomEdge = UnityEngine.Random.Range(0, tNode.OutEdgeCount); //**FIXED THIS LINE
            lastWaypoint = ((VectorPathNode)tNode.EdgeOut(tRandomEdge).ToNode).NodeIndex;
        }
        moveTarget.VectorTarget        = waypointSet.Waypoints[lastWaypoint].position;
        moveTarget.CloseEnoughDistance = Mathf.Max(waypointSet.Waypoints[lastWaypoint].range, ai.Motor.CloseEnoughDistance);
        ai.WorkingMemory.SetItem <MoveLookTarget>(MoveTargetVariable.VariableName, moveTarget);

        return(ActionResult.SUCCESS);
    }
        //cria rectangulos
        public void Search(NavigationGraphNode startNode, NodeGoalBounds nodeGoalBounds)
        {
            //TODO: Implement the algorithm that calculates the goal bounds using a dijkstra
            //Given that the nodes in the graph correspond to the edges of a polygon, we won't be able to use the vertices of the polygon to update the bounding boxes


            // mete os vizinhos do no inicial. Inicializacao da lista
            var outConnections = startNode.OutEdgeCount;

            for (int i = 0; i < outConnections; i++)
            {
                NavigationGraphEdge edge = startNode.EdgeOut(i);

                var childNode       = edge.ToNode;
                var childNodeRecord = this.NodeRecordArray.GetNodeRecord(childNode);
                //adicionar ao open
                NodeRecordArray.AddToOpen(childNodeRecord);

                //transformar em vector3 para inicializar cada rectangulo
                childNodeRecord.StartNodeOutConnectionIndex = i;
            }

            //giro:  var startTime = Time.realtimeSinceStartup;

            //enquanto houver nos no conj open
            while (this.Open.CountOpen() > 0)
            {
                NodeRecord bestNode = this.Open.GetBestAndRemove();

                //aumentar o rectangulo
                nodeGoalBounds.connectionBounds[bestNode.StartNodeOutConnectionIndex].UpdateBounds(bestNode.node.LocalPosition); //isto e a cor do rectangulo. falta updateBounds

                this.Closed.AddToClosed(bestNode);

                //para ver as ligacoes do no que acabamos de ver
                var outConnections2 = bestNode.node.OutEdgeCount;
                for (int j = 0; j < outConnections2; j++)
                {
                    this.ProcessChildNode(bestNode, bestNode.node.EdgeOut(j), bestNode.StartNodeOutConnectionIndex);
                }
                // giro: this.MaxOpenNodes = Mathf.Max(this.Open.CountOpen(), this.MaxOpenNodes);
            }
        }
    public override ActionResult Execute(AI ai)
    {
        NavigationTarget finish = NavigationManager.Instance.GetNavigationTarget("Finish");

        moveTarget.VectorTarget        = finish.Position;
        moveTarget.CloseEnoughDistance = Mathf.Max(finish.Range, ai.Motor.CloseEnoughDistance);
        if (ai.Motor.IsAt(moveTarget))
        {
            return(ActionResult.FAILURE);
        }

        if (!MoveTargetVariable.IsValid || !MoveTargetVariable.IsVariable)
        {
            return(ActionResult.FAILURE);
        }

        WaypointSet waypointSet = NavigationManager.Instance.GetWaypointSet("Network");

        if (waypointSet == null)
        {
            return(ActionResult.FAILURE);
        }


        lastWaypoint = waypointSet.GetClosestWaypointIndex(ai.Kinematic.Position);
        if (lastWaypoint < 0)
        {
            return(ActionResult.FAILURE);
        }
        Debug.Log(lastWaypoint);

        visited.Add(lastWaypoint);
        moveTarget.VectorTarget        = waypointSet.Waypoints[lastWaypoint].Position;
        moveTarget.CloseEnoughDistance = Mathf.Max(waypointSet.Waypoints[lastWaypoint].Range, ai.Motor.CloseEnoughDistance);
        if (!ai.Motor.IsAt(moveTarget))
        {
            ai.WorkingMemory.SetItem <MoveLookTarget>(MoveTargetVariable.VariableName, moveTarget);
            return(ActionResult.SUCCESS);
        }
        //If currently at a waypoint
        NavigationGraphNode tNode = waypointSet.Graph.GetNode(lastWaypoint);

        if (tNode.OutEdgeCount > 0)
        {
            var numbers = new List <int>();
            for (int i = 0; i < tNode.OutEdgeCount; i++)
            {
                numbers.Add(i);
            }
            for (int i = 0; i < tNode.OutEdgeCount; i++)
            {
                int j    = UnityEngine.Random.Range(0, tNode.OutEdgeCount - 1);
                int temp = numbers[i];
                numbers[i] = numbers[j];
                numbers[j] = temp;
            }
            for (int i = 0; i < tNode.OutEdgeCount; i++)
            {
                lastWaypoint = ((VectorPathNode)tNode.EdgeOut(numbers[i]).ToNode).NodeIndex;
                if (visited.Contains(lastWaypoint))
                {
                    continue;
                }
                visited.Add(lastWaypoint);
                stack.Push(lastWaypoint);
                moveTarget.VectorTarget        = waypointSet.Waypoints[lastWaypoint].Position;
                moveTarget.CloseEnoughDistance = Mathf.Max(waypointSet.Waypoints[lastWaypoint].Range, ai.Motor.CloseEnoughDistance);
                ai.WorkingMemory.SetItem <MoveLookTarget>(MoveTargetVariable.VariableName, moveTarget);
                return(ActionResult.SUCCESS);
            }
        }
        if (stack.Count == 0)
        {
            return(ActionResult.FAILURE);
        }
        stack.Pop();
        Debug.Log(stack.Count);
        if (stack.Count > 0)
        {
            moveTarget.VectorTarget        = waypointSet.Waypoints[stack.Peek()].Position;
            moveTarget.CloseEnoughDistance = Mathf.Max(waypointSet.Waypoints[lastWaypoint].Range, ai.Motor.CloseEnoughDistance);
            ai.WorkingMemory.SetItem <MoveLookTarget>(MoveTargetVariable.VariableName, moveTarget);
            return(ActionResult.SUCCESS);
        }
        else
        {
            return(ActionResult.FAILURE);
        }
    }