using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using System;
using System.Net;
using UnityEditor;

public class AStarSolver
{
    public Node[,] grid;
    int gridLength;
    int gridWidth;
    float gridSize;

    Vector3 startPos;
    Vector3 endPos;
    public Node curNode;

    List<Node> openList = new List<Node>();
    List<Node> closedList = new List<Node>();

    public Bounds bounds;

    List<Node> pathNodes = new List<Node>();

    public AStarSolver(GameObject bounds)
    {
        this.bounds = bounds.renderer.bounds;
        GenerateGraph(5);
    }

    public void Solve(Vector3 startPos, Vector3 endPos)
    {
        this.startPos = new Vector3(Mathf.FloorToInt(startPos.x),Mathf.FloorToInt(startPos.y),Mathf.FloorToInt(startPos.z));
        this.endPos = new Vector3(Mathf.FloorToInt(endPos.x), Mathf.FloorToInt(endPos.y),Mathf.FloorToInt(endPos.z));

        bool canSearch = true;

        try
        {
            if(!GetNode(startPos).walkable || !GetNode(endPos).walkable)
                canSearch = false;
        }catch{}

        if(canSearch)
        {
            curNode = GetNode(startPos);
            openList.Add(curNode);

            while(openList.Count != 0)
            {
                curNode = GetNodeWithLowestTotal(openList);

                if(curNode == GetNode(endPos))
                    break; //Path completed;
                else
                {
                    openList.Remove(curNode);
                    closedList.Add(curNode);

                    if(openList.Count == 0)
                        break; //Impossible path;

                    List<Node> adjacentNodes = GetAdjacentNodes(curNode);

                    foreach(Node adjNode in adjacentNodes)
                    {
                        if(!openList.Contains(adjNode) && !closedList.Contains(adjNode))
                        {
                            openList.Add(adjNode);

                            Node node = adjNode;
                            node.cost = curNode.cost + 10;
                            node.heuristic = ManhattanDistance(adjNode);
                            node.total = node.cost + node.heuristic;
                        }
                    }
                }
            }
        }
    }

    public int ManhattanDistance(Node adjNode)
    {
        int manhattan = Math.Abs((int)(endPos.x - adjNode.pos.x)) + Math.Abs((int)(endPos.y - adjNode.pos.y));
            return manhattan;
    }

    public void TracePath()
    {
        bool startFound = false;

        Node currentNode = GetNode(endPos);
        pathNodes = new List<Node>();

        while(!startFound)
        {
            List<Node> adjacentNodes = GetAdjacentNodes(currentNode);

            foreach(Node adjNode in adjacentNodes)
            {
                if(adjNode.pos == startPos)
                    startFound = true;

                if(closedList.Contains(adjNode) || openList.Contains(adjNode))
                {
                    if(adjNode.cost <= currentNode.cost && adjNode.cost > 0)
                    {
                        curNode = adjNode;
                        pathNodes.Add(adjNode);
                        break;
                    }
                }
            }
        }
    }

    public Node GetNodeWithLowestTotal(List<Node> openList)
    {
        Vector3 nodePos = Vector3.zero;
        int lowestTotal = int.MaxValue;


        foreach(Node openNode in openList)
        {
            if(openNode.total <= lowestTotal)
            {
                lowestTotal = openNode.total;
                nodePos = openNode.pos;
            }
        }
        return GetNode(nodePos);
    }

    public List<Vector3> GetPath()
    {
        //Returns a list of points for an agent to follow.
        List<Vector3> points = new List<Vector3>();

        for(int i = 0; i < pathNodes.Count; i++)
        {
            points[i] = pathNodes[i].pos;
        }
        return points;
    }

    public Node GetNode(Vector3 pos)
    {
        //Searches for a node with a specific vector.
        Vector3 roundedPos = new Vector3(Mathf.FloorToInt(pos.x), Mathf.FloorToInt(pos.y), Mathf.FloorToInt(pos.z));

        for(int i = 0; i < gridLength; i++)
        {
            for(int j = 0; j < gridWidth; j++)
            {
                if(grid[i,j].pos == roundedPos)
                    return grid[i,j];
            }
        }
        return null;
    }

    public List<Node> GetAdjacentNodes(Node curNode)
    {
        List<Node> adjacentNodes = new List<Node>();

        try
        {
            //Node in front
            Vector3 adjacentNode = curNode.pos + new Vector3(0,0,gridSize);
            if(GetNode(adjacentNode).walkable)
                adjacentNodes.Add(GetNode(adjacentNode));
        }
        catch{}

        try
        {
            //Node behind
            Vector3 adjacentNode = curNode.pos + new Vector3(0,0,-gridSize);
            if(GetNode(adjacentNode).walkable)
                adjacentNodes.Add(GetNode(adjacentNode));
        }
        catch{}

        try
        {
            //Node to the left
            Vector3 adjacentNode = curNode.pos + new Vector3(-gridSize,0,0);
            if(GetNode(adjacentNode).walkable)
                adjacentNodes.Add(GetNode(adjacentNode));
        }
        catch{}

        try
        {
            //Node to the right
            Vector3 adjacentNode = curNode.pos + new Vector3(gridSize,0,0);
            if(GetNode(adjacentNode).walkable)
                adjacentNodes.Add(GetNode(adjacentNode));
        }
        catch{}

        return adjacentNodes;
    }

    public void GenerateGraph(float gridScale)
    {
        //Generates the node graph used in the grid.
        //Gridsize is how much distance between cells, smaller = more precise

        this.gridSize = gridScale;

        //top left corner of cube
        Vector3 topLeftCorner = bounds.center - bounds.extents + new Vector3(0, bounds.size.y,0);
        gridWidth = Mathf.RoundToInt(bounds.size.x / gridSize);
        gridLength = Mathf.RoundToInt(bounds.size.z / gridSize);

        grid = new Node[gridWidth, gridLength];

        for(int x = 0; x < gridWidth; x++)
        {
            for(int z = 0; z < gridLength; z++)
            {
                Vector3 currentPosition = topLeftCorner + new Vector3(x * gridSize, 0, z * gridSize);
                RaycastHit hit;

                //create new node for grid
                grid[x,z] = new Node(new Vector3((int)(topLeftCorner.x + x * gridSize),0,(int)(topLeftCorner.z + z * gridSize)), false, new Vector2(x,z));

                if(Physics.Raycast(currentPosition, -Vector3.up, out hit, bounds.size.y))
                {
                    grid[x,z].pos.y = hit.point.y;
                    if(Vector3.Angle(hit.normal, currentPosition) < 50)
                    {
                        if(hit.collider.gameObject.tag == "Walkable")
                        {
                            grid[x,z].walkable = true;
                        }
                    }
                }
            }
        }
    }

    public void DrawNodeGraph()
    {
        for(int x = 0; x < gridWidth; x++)
        {
            for(int z = 0; z < gridLength; z++)
            {
                Gizmos.DrawSphere(grid[x,z].pos, 0.5f);
            }
        }
    }

    public void DrawPath()
    {
        List<Vector3> path = GetPath();

        for(int i = 0; i < path.Count; i++)
        {
            Gizmos.DrawSphere(path[i], 1f);
            if(i+1 < path.Count)
                Gizmos.DrawLine(path[i], path[i+1]);
        }
    }
}