Pathfinder

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

public class PathFinder : MonoBehaviour {
    public float speed=10;
    public PathSite currentSite;
    public Queue<PathSite> currentRoute=new Queue<PathSite>();
    public PathSite destination;

    public double costEstimate(Node n1, Node n2){
        if(n1 is PathSite && n2 is PathSite){
            PathSite n1site=n1 as PathSite;
            PathSite n2site=n2 as PathSite;
            return (n1site.GetComponent<Transform>().position-n2site.GetComponent<Transform>().position).magnitude;
        }

        return 0;
    }

    public void routeTo(PathSite destination){
        if (currentSite!=null) {
            Debug.Log("routing");
            List<PathSite> route=PathFinding.Astar(currentSite, destination, costEstimate).Cast<PathSite>().ToList();
            currentRoute=new Queue<PathSite>(route);
            this.destination=destination;
        }
    }

    // Use this for initialization
    void Start () {

    }

    // Update is called once per frame
    void Update () {
        if(currentSite!=null){
            //check if you're at the current site
            double dist=((Vector2)currentSite.transform.position-(Vector2)transform.position).magnitude;
            if(dist<(.01*speed)){//TODO <- this is kludgy, need a better way to define closeness
                //if you are and there are more sites in the route, then go to the next site
                if (currentRoute.Count!=0){
                    currentSite=currentRoute.Dequeue();
                }
            }else{
                //if not, move towards the current site
                //transform.LookAt(currentSite.transform);
                Vector2 dir=(currentSite.transform.position-transform.position);
                transform.Translate(dir.normalized*(speed*Time.deltaTime));
            }
        }
    }
}