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

public class main : MonoBehaviour {

	public Transform prefab;
	public Transform prefab2;
	List<Vertex> graph = new List<Vertex>();
	Vertex target;

	// Use this for initialization
	void Start () {
		for (int j= -2;j<=2;j+=2) {
			for (int i = -6;i<=6;i+=2) {
				graph.Add (new Vertex(i,0,j));
			}
		}

		for (int j= 0;j<21;j++) {
			if(j<14) addEdge (graph[j],graph[j+7]);
		}

		addEdge (graph[7],graph[8]);
		addEdge (graph[15],graph[16]);
		addEdge (graph[9],graph[10]);
		addEdge (graph[10],graph[11]);
		addEdge (graph[4],graph[5]);
		addEdge (graph[19],graph[20]);
		addEdge (graph[1],graph[9]);
		addEdge (graph[18],graph[12]);

		foreach (Vertex vert in graph) {
			Instantiate (prefab,vert.loc,Quaternion.identity);
		}

		target = graph[20];
		List<Node> result = aStar (new Node(graph[0],0,null));
		if(result != null) {
			foreach (Node n in result) {
				Instantiate (prefab2,n.vert.loc+Vector3.up,Quaternion.identity);
			}
		}
		else Debug.LogError ("FAILED!");

	}

	// Update is called once per frame
	void Update () {
		foreach (Vertex vert in graph) {
			foreach (Vertex vert2 in vert.adjacent) {
				Debug.DrawLine (vert.loc,vert2.loc);
			}
		}
	}

	List<Node> aStar(Node init) {
		List<Node> openList = new List<Node>();
		List<Vertex> closedList = new List<Vertex>();
		openList.Add (init);
		while (openList.Count != 0) {
			openList.Sort (compareNodes);
			Node current = openList[0];
			if (current.vert == target) return generatePath(current);
			openList.RemoveAt (0);
			closedList.Add (current.vert);
			List<Node> newNodes =  current.expand ();
			foreach (Node newNode in newNodes) {
				if(!(closedList.Contains (newNode.vert))) {
					//Insert Replacement code here
					openList.Add (newNode);
				}
			}
		}
		return null;
	}

	List<Node> generatePath(Node final) {
		List<Node> res = new List<Node>();
		Node cur = final;
		res.Add (cur);
		while(cur.parent != null) {
			res.Add (cur.parent);
			cur=cur.parent;
		}
		res.Reverse ();
		return res;
	}

	void addEdge (Vertex v1, Vertex v2) {
		v1.adjacent.Add(v2);
		v2.adjacent.Add(v1);
	}

	class Vertex {

		public Vector3 loc;
		public List<Vertex> adjacent = new List<Vertex>();

		public Vertex(int x, int y, int z) {
			loc = new Vector3(x,y,z);
		}

	}

	class Node {
		public Vertex vert;
		public Node parent;
		public float g = 0;
		public Node(Vertex v, float cost, Node p) {
			vert = v; g = cost; parent = p;
		}
		public List<Node> expand() {
			List<Node> ret = new List<Node>();
			foreach( Vertex v in vert.adjacent) {
				ret.Add (new Node(v,g+Vector3.Distance(vert.loc,v.loc),this));
			}
			return ret;
		}
	}

	float h (Node a,Vertex b) {
		return Vector3.Distance (a.vert.loc,b.loc);
	}

	private int compareNodes(Node x, Node y) {
		if (x == null) {
			if (y == null) return 0;
			else return -1;
		}
		else {
			if (y == null) return 1;
			else {
				float fx = x.g + h (x,target);
				float fy = y.g + h (y,target);
				return fx.CompareTo (fy);
			}
		}
	}
}