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

/// <summary>
/// Componente que habilita a un objeto para calcular rutas sobre un AStarGraph
/// </summary>
public class AStarNavigator : MonoBehaviour {
    protected AStarGraph _graph; // Estructura con los nodos del grafo que recorreremos
    public List<Vector2> lista = new List<Vector2>();
    // Use this for initialization
    void Start () {
        _graph = FindObjectOfType<AStarGraph>();
    }
    //MASL
    /// <summary>
    /// Devuelve el camino para ir desde la posición start hasta end
    /// </summary>
    /// <returns>The path.</returns>
    /// <param name="start">Posición inicial del camino.</param>
    /// <param name="end">Posición objetivo.</param>
    public List<Vector2> GetPath(Vector2 start, Vector2 end) {

        //f estimacion hasta el final
        //h camino recorrido
        //g estimacion actual al final
        // TODO: Implementar
        List<Vector2> listaAbierta = new List<Vector2>();
        List<Vector2> listaCerrada = new List<Vector2>();
        listaAbierta.Add(start);

        while(){
            Vector2 posicionActual = listaAbierta[0];
            AStarNode nodoActual = _graph.nodes[(int)posicionActual.x, (int)posicionActual.y];
            listaAbierta.RemoveAt(0);
            listaCerrada.Add(posicionActual);
            List<Vector2> hijos = new List<Vector2>();
            //calcular factores para cada hijo
            if (nodoActual.north != null)
            {
                hijos.Add(nodoActual.north.otherNode.position);
            }
            if (nodoActual.south != null)
            {
                hijos.Add(nodoActual.south.otherNode.position);
            }
            if (nodoActual.east != null)
            {
                hijos.Add(nodoActual.east.otherNode.position);
            }
            if (nodoActual.west != null)
            {
                hijos.Add(nodoActual.west.otherNode.position);
            }
            if (nodoActual.northWest != null)
            {
                hijos.Add(nodoActual.northWest.otherNode.position);
            }
            if (nodoActual.northEast != null)
            {
                hijos.Add(nodoActual.northEast.otherNode.position);
            }
            if (nodoActual.southEast != null)
            {
                hijos.Add(nodoActual.southEast.otherNode.position);
            }
            if (nodoActual.southWest != null)
            {
                hijos.Add(nodoActual.southWest.otherNode.position);
            }
            int posicionMejorHijo=0;
            foreach (var hijo in hijos)
            {
                AStarNode nodoHijo = _graph.nodes[(int)hijo.x, (int)hijo.y];
                if (hijo == end) break;
                if (listaCerrada.Contains(hijo))
                {
                    break;
                }
                else if (listaAbierta.Contains(hijo))
                {
                    if (nodoHijo.distanciaRecorrida < _graph.nodes[(int)listaAbierta[listaAbierta.BinarySearch(hijo)].x, (int)listaAbierta[listaAbierta.BinarySearch(hijo)].y].distanciaRecorrida) {
                        //nuevoPadre=nodoActual
                        //recalcular factores
                    }
                }
                else
                {
                    //padre=nodoActual
                    //recalcularFactores
                }
            }

        }


        //if (start == end) return lista;
        //List<Vector2> listaAbierta = new List<Vector2>();
        //List<Vector2> listaCerrada = new List<Vector2>();
        //listaAbierta.Add(start);
        //_graph.nodes[(int)start.x, (int)start.y].distanciaEstimada = Mathf.Abs(start.x - end.x) + Mathf.Abs(start.y - end.y);
        //_graph.nodes[(int)start.x, (int)start.y].distanciaReal = _graph.nodes[(int)start.x, (int)start.y].distanciaEstimada;
        //_graph.nodes[(int)start.x, (int)start.y].distanciaRecorrida = 0;
        //while (listaAbierta.Count!=0)
        //{
        //    Vector2 mejorVector=listaAbierta[0];
        //    for (int i = 0; i < listaAbierta.Count; i++) {
        //        if (i == 0) { }
        //        else if(_graph.nodes[(int)listaAbierta[i].x, (int)listaAbierta[i].y].distanciaReal< _graph.nodes[(int)listaAbierta[i-1].x, (int)listaAbierta[i - 1].y].distanciaReal)
        //        {
        //            mejorVector = listaAbierta[i];
        //        }
        //    }
        //    _graph.nodes[(int)mejorVector.x, (int)mejorVector.y].padre = _graph.nodes[(int)_graph.nodes[(int)mejorVector.x, (int)mejorVector.y].posicionPadre.x, (int)_graph.nodes[(int)mejorVector.x, (int)mejorVector.y].posicionPadre.y];


        //    listaCerrada.Add(mejorVector);
        //    listaAbierta.Remove(mejorVector);
        //    if (mejorVector == end) return listaCerrada;
        //    List<Vector2> hijos = new List<Vector2>();
        //    AStarNode nodo = _graph.nodes[(int)listaCerrada[listaCerrada.Count-1].x, (int)listaCerrada[listaCerrada.Count - 1].y];
        //    if (nodo.north!=null)
        //    {
        //        Vector2 comprobante = nodo.north.otherNode.position;
        //        if (!listaCerrada.Contains(comprobante))
        //        {
        //            nodo.north.otherNode.distanciaRecorrida = nodo.distanciaRecorrida + nodo.north.distance;
        //            nodo.north.otherNode.distanciaEstimada = Mathf.Abs(nodo.north.otherNode.position.x - end.x) + Mathf.Abs(nodo.north.otherNode.position.y - end.y);
        //            nodo.north.otherNode.distanciaReal = nodo.north.otherNode.distanciaRecorrida + nodo.north.otherNode.distanciaEstimada;
        //        }
        //        else if (nodo.north.otherNode.distanciaRecorrida < _graph.nodes[(int)nodo.north.otherNode.position.x, (int)nodo.north.otherNode.position.y].distanciaRecorrida) {
        //            listaAbierta.Remove(comprobante);
        //        }
        //        hijos.Add(nodo.north.otherNode.position);
        //        nodo.north.otherNode.posicionPadre = nodo.position;

        //    }
        //    if (nodo.south!=null)
        //    {
        //        Vector2 comprobante = nodo.south.otherNode.position;
        //        if (!listaCerrada.Contains(comprobante))
        //        {
        //            nodo.south.otherNode.distanciaRecorrida = nodo.distanciaRecorrida + nodo.south.distance;
        //            nodo.south.otherNode.distanciaEstimada = Mathf.Abs(nodo.south.otherNode.position.x - end.x) + Mathf.Abs(nodo.south.otherNode.position.y - end.y);
        //            nodo.south.otherNode.distanciaReal = nodo.south.otherNode.distanciaRecorrida + nodo.south.otherNode.distanciaEstimada;
        //        }
        //        else if (nodo.south.otherNode.distanciaRecorrida < _graph.nodes[(int)nodo.south.otherNode.position.x, (int)nodo.south.otherNode.position.y].distanciaRecorrida)
        //        {
        //            listaAbierta.Remove(comprobante);
        //        }
        //        hijos.Add(nodo.east.otherNode.position);
        //        nodo.south.otherNode.posicionPadre = nodo.position;

        //    }
        //    if (nodo.east!=null)
        //    {
        //        Vector2 comprobante = nodo.east.otherNode.position;
        //        if (!listaCerrada.Contains(comprobante))
        //        {
        //            nodo.east.otherNode.distanciaRecorrida = nodo.distanciaRecorrida + nodo.east.distance;
        //            nodo.east.otherNode.distanciaEstimada = Mathf.Abs(nodo.east.otherNode.position.x - end.x) + Mathf.Abs(nodo.east.otherNode.position.y - end.y);
        //            nodo.east.otherNode.distanciaReal = nodo.east.otherNode.distanciaRecorrida + nodo.east.otherNode.distanciaEstimada;
        //        }
        //        else if (nodo.east.otherNode.distanciaRecorrida < _graph.nodes[(int)nodo.east.otherNode.position.x, (int)nodo.east.otherNode.position.y].distanciaRecorrida)
        //        {
        //            listaAbierta.Remove(comprobante);
        //        }
        //        hijos.Add(nodo.east.otherNode.position);
        //        nodo.east.otherNode.posicionPadre = nodo.position;

        //    }
        //    if (nodo.west!=null)
        //    {
        //        Vector2 comprobante = nodo.west.otherNode.position;
        //        if (!listaCerrada.Contains(comprobante))
        //        {
        //            nodo.west.otherNode.distanciaRecorrida = nodo.distanciaRecorrida + nodo.west.distance;
        //            nodo.west.otherNode.distanciaEstimada = Mathf.Abs(nodo.west.otherNode.position.x - end.x) + Mathf.Abs(nodo.west.otherNode.position.y - end.y);
        //            nodo.west.otherNode.distanciaReal = nodo.west.otherNode.distanciaRecorrida + nodo.west.otherNode.distanciaEstimada;
        //        }
        //        else if (nodo.west.otherNode.distanciaRecorrida < _graph.nodes[(int)nodo.west.otherNode.position.x, (int)nodo.west.otherNode.position.y].distanciaRecorrida)
        //        {
        //            listaAbierta.Remove(comprobante);
        //        }
        //        hijos.Add(nodo.west.otherNode.position);
        //        nodo.west.otherNode.posicionPadre = nodo.position;

        //    }
        //    if (nodo.northWest!=null)
        //    {
        //        Vector2 comprobante = nodo.northWest.otherNode.position;
        //        if (!listaCerrada.Contains(comprobante))
        //        {
        //            nodo.northWest.otherNode.distanciaRecorrida = nodo.distanciaRecorrida + nodo.northWest.distance;
        //            nodo.northWest.otherNode.distanciaEstimada = Mathf.Abs(nodo.northWest.otherNode.position.x - end.x) + Mathf.Abs(nodo.northWest.otherNode.position.y - end.y);
        //            nodo.northWest.otherNode.distanciaReal = nodo.northWest.otherNode.distanciaRecorrida + nodo.northWest.otherNode.distanciaEstimada;
        //        }
        //        else if (nodo.northWest.otherNode.distanciaRecorrida < _graph.nodes[(int)nodo.northWest.otherNode.position.x, (int)nodo.northWest.otherNode.position.y].distanciaRecorrida)
        //        {
        //            listaAbierta.Remove(comprobante);
        //        }
        //        hijos.Add(nodo.northWest.otherNode.position);
        //        nodo.northWest.otherNode.posicionPadre = nodo.position;

        //    }
        //    if (nodo.northEast!=null)
        //    {
        //        Vector2 comprobante = nodo.northEast.otherNode.position;
        //        if (!listaCerrada.Contains(comprobante))
        //        {
        //            nodo.northEast.otherNode.distanciaRecorrida = nodo.distanciaRecorrida + nodo.northEast.distance;
        //            nodo.northEast.otherNode.distanciaEstimada = Mathf.Abs(nodo.northEast.otherNode.position.x - end.x) + Mathf.Abs(nodo.northEast.otherNode.position.y - end.y);
        //            nodo.northEast.otherNode.distanciaReal = nodo.northEast.otherNode.distanciaRecorrida + nodo.northEast.otherNode.distanciaEstimada;
        //        }
        //        else if (nodo.northEast.otherNode.distanciaRecorrida < _graph.nodes[(int)nodo.northEast.otherNode.position.x, (int)nodo.northEast.otherNode.position.y].distanciaRecorrida)
        //        {
        //            listaAbierta.Remove(comprobante);
        //        }
        //        hijos.Add(nodo.northEast.otherNode.position);
        //        nodo.northEast.otherNode.posicionPadre = nodo.position;

        //    }
        //    if (nodo.southWest!=null)
        //    {
        //        Vector2 comprobante = nodo.southWest.otherNode.position;
        //        if (!listaCerrada.Contains(comprobante))
        //        {
        //            nodo.southWest.otherNode.distanciaRecorrida = nodo.distanciaRecorrida + nodo.southWest.distance;
        //            nodo.southWest.otherNode.distanciaEstimada = Mathf.Abs(nodo.southWest.otherNode.position.x - end.x) + Mathf.Abs(nodo.southWest.otherNode.position.y - end.y);
        //            nodo.southWest.otherNode.distanciaReal = nodo.southWest.otherNode.distanciaRecorrida + nodo.southWest.otherNode.distanciaEstimada;
        //        }
        //        if (nodo.southWest.otherNode.distanciaRecorrida < _graph.nodes[(int)nodo.southWest.otherNode.position.x, (int)nodo.southWest.otherNode.position.y].distanciaRecorrida)
        //        {
        //            listaAbierta.Remove(comprobante);
        //        }
        //        hijos.Add(nodo.southWest.otherNode.position);
        //        nodo.southWest.otherNode.posicionPadre = nodo.position;

        //    }
        //    if (nodo.southEast!=null)
        //    {
        //        Vector2 comprobante = nodo.southEast.otherNode.position;
        //        if (!listaCerrada.Contains(comprobante))
        //        {
        //            nodo.southEast.otherNode.distanciaRecorrida = nodo.distanciaRecorrida + nodo.southEast.distance;
        //            nodo.southEast.otherNode.distanciaEstimada = Mathf.Abs(nodo.southEast.otherNode.position.x - end.x) + Mathf.Abs(nodo.southEast.otherNode.position.y - end.y);
        //            nodo.southEast.otherNode.distanciaReal = nodo.southEast.otherNode.distanciaRecorrida + nodo.southEast.otherNode.distanciaEstimada;
        //        }
        //        if (nodo.southEast.otherNode.distanciaRecorrida < _graph.nodes[(int)nodo.southEast.otherNode.position.x, (int)nodo.southEast.otherNode.position.y].distanciaRecorrida)
        //        {
        //            listaAbierta.Remove(comprobante);
        //        }
        //        hijos.Add(nodo.southEast.otherNode.position);
        //        nodo.southEast.otherNode.posicionPadre = nodo.position;


        //    }
        //    listaAbierta.AddRange(hijos);
        //}
        //AStarNode nodoActual = _graph.nodes[(int)end.x, (int)end.y];
        //List<Vector2> listaFinal = new List<Vector2>();
        //while (nodoActual.position != start) {
        //    Debug.Log("asdasd");
        //    listaFinal.Add(nodoActual.position);
        //    nodoActual = nodoActual.padre;
        //}
        //return listaFinal;
    }

    /// <summary>
    /// Devuelve el camino para ir desde la posición start hasta end. Es un método helper para facilitar la conversión entre Vector2 y Vector3 de los parámetros
    /// </summary>
    /// <returns>The path.</returns>
    /// <param name="start">Posición inicial del camino.</param>
    /// <param name="end">Posición objetivo.</param>
    //public List<Vector2> GetPath(Vector3 start, Vector3 end) {
    //    return GetPath(new Vector2(start.x, start.y), new Vector2(end.x, end.y));
    //}

}