Unity Character pathfinding

using UnityEngine;
using System.Collections;
using System.Collections.Generic;


public class CharacterControls : MonoBehaviour {

    public bool moveDestSet = false;


    public int moveDistance = 3; //How far can the character move


    public List<PathNode> direction = new List<PathNode>();

    float moveSpeed = 1.0f;
    public int currentlyOccupiedNodeIndex = 0;
    public GameObject endPoint;
    public PathNode occupiedNode;
    RaycastHit obj_ray_hit;
    Ray obj_ray;

    void Update ()
    {
        if (!occupiedNode)
        {
            foreach(GameObject tile in GameManager.FloorTiles)
            {
                if (tile.transform.position.x == transform.position.x && tile.transform.position.z == transform.position.z)
                {
                    occupiedNode = tile.GetComponent<PathNode>();
                }
            }
        }
        if (!moveDestSet)
        {
            obj_ray = Camera.main.ScreenPointToRay(Input.mousePosition);
            if (Physics.Raycast(obj_ray, out obj_ray_hit, 1000))
            {
                if (endPoint == obj_ray_hit.collider.gameObject)
                {
                    Debug.Log("End point(" +endPoint.transform.position.x +"," +endPoint.transform.position.z +") is same as hit gO (" +obj_ray_hit.collider.gameObject.transform.position.x +"," +obj_ray_hit.collider.gameObject.transform.position.z);
                }
                else
                {
                    //Now is when Unity Freezes (as long as player has moved once).
                    Debug.Log("End point is not the same as hit gO");
                }
                if (obj_ray_hit.collider.gameObject.tag == "Floor" && obj_ray_hit.collider.gameObject != endPoint)
                {
                    Debug.Log("Ray hit new floor tile, cleaning tiles & setting new endPoint");
                    endPoint = obj_ray_hit.collider.gameObject;

                    foreach(GameObject tile in GameManager.FloorTiles)
                    {
                        tile.renderer.material.color = Color.white;
                    }

                    Debug.Log("Setting new direction from ("+occupiedNode.transform.position.x +"," +occupiedNode.transform.position.z +") to (" +endPoint.transform.position.x +"," +endPoint.transform.position.z);

                    direction.Clear();
                    direction = GetPath(occupiedNode,endPoint.GetComponent<PathNode>());

                    if (direction.Count <= moveDistance)
                    {
                        foreach(PathNode tile in direction)
                        {
                            tile.renderer.material.color = Color.green;
                        }
                    }
                    else
                    {
                        foreach(PathNode tile in direction)
                        {
                            tile.renderer.material.color = Color.red;
                        }
                    }
                    Debug.Log("Got new endpoint " +endPoint +" & colored path");
                }
            }
            //If the user inputs a mouse button then we can start moving.
            if (Input.GetMouseButtonDown(0))
            {
                if (direction.Count <= moveDistance && endPoint.GetComponent<PathNode>() != occupiedNode)
                {
                    moveDestSet = true;
                    currentlyOccupiedNodeIndex = direction.Count-1;
                }
                else
                {
                    Debug.Log("Can't move here");
                }
            }

        }
        else //moveDestSet == true (Move destination has been set)
        {
            Debug.Log("Move destination has been set, checking if we have reached it, if not moving to it.");
            if (currentlyOccupiedNodeIndex >= 0)
            {
                Debug.Log("Moving to node " +currentlyOccupiedNodeIndex);
                if (transform.position.x != direction[currentlyOccupiedNodeIndex].transform.position.x || transform.position.z != direction[currentlyOccupiedNodeIndex].transform.position.z)
                {
                    transform.position = Vector3.MoveTowards(transform.position, new Vector3(direction[currentlyOccupiedNodeIndex].transform.position.x, transform.position.y, direction[currentlyOccupiedNodeIndex].transform.position.z), Time.deltaTime * moveSpeed);
                }
                if (transform.position.x == direction[currentlyOccupiedNodeIndex].transform.position.x && transform.position.z == direction[currentlyOccupiedNodeIndex].transform.position.z)
                {
                    Debug.Log("Arrived at node ("+transform.position.x +", " +transform.position.z +") "+currentlyOccupiedNodeIndex +" :"  +direction[currentlyOccupiedNodeIndex] +" (" +direction[currentlyOccupiedNodeIndex].transform.position.x +"," +direction[currentlyOccupiedNodeIndex].transform.position.z +") Moving on to next node");
                    currentlyOccupiedNodeIndex --;
                }
            }
            else
            {
                Debug.Log("You have arrived at your destination " +currentlyOccupiedNodeIndex +" (" +transform.position.x +"," +transform.position.z +")");
                endPoint = null;
                occupiedNode = direction[currentlyOccupiedNodeIndex+1]; //so that next point the occupied node does not have to be set.
                moveDestSet = false;
            }
        }
    }


    List<PathNode> GetPath(PathNode startNode, PathNode endNode)
    {
        List<PathNode> openList = new List<PathNode>();
        List<PathNode> closedList = new List<PathNode>();
        PathNode currentNode = startNode;
        bool foundTarget = false;
        int movementCost = 1;
        List<PathNode> PathToTake = new List<PathNode>();

        while (!foundTarget)
        {
            if (startNode == endNode || currentNode == endNode)
            {
                foundTarget = true;
                continue;
            }
            foreach (PathNode nextNode in currentNode.neighbors)
            {
                if (nextNode != null) {
                    if (nextNode == endNode)
                    {
                        endNode.Parent = currentNode;
                        foundTarget = true;
                        continue;
                    }
                    if (!closedList.Contains (nextNode))
                    {
                        checkNext(nextNode,currentNode,openList,movementCost,endNode);
                    }
                }
            }


            if (!foundTarget) {
                closedList.Add (currentNode);
                openList.Remove (currentNode);
                //Get smallest f node.
                int smallestF = 10000;
                PathNode smallestNode = null;
                foreach (PathNode node in openList)
                {
                    if (node.heuristicValue < smallestF)
                    {
                        smallestNode = node;
                        smallestF = node.heuristicValue;
                    }
                }
                currentNode = smallestNode;
            }
        }
        PathNode endPathNode = endNode;
        while (endPathNode != null)
        {
            if (!PathToTake.Contains(endPathNode))
            {
                PathToTake.Add(endPathNode);
            }
            endPathNode = endPathNode.Parent;
        }
        return PathToTake;
    }



    void checkNext (PathNode NextNode, PathNode currentNode, List<PathNode> openList, int movementCost, PathNode endNode)
    {
        if (openList.Contains (NextNode))
        {
            int newMovementCost = currentNode.movementCost + movementCost;
            if (newMovementCost < NextNode.movementCost)
            {
                NextNode.Parent = currentNode;
                NextNode.movementCost = newMovementCost;
                NextNode.calculateFValue (endNode);
            }
        }
        else
        {
            NextNode.Parent = currentNode;
            NextNode.movementCost = currentNode.movementCost + movementCost;
            NextNode.calculateFValue(endNode);
            openList.Add(NextNode);
        }
    }
}