﻿using System;
using Sirenix.OdinInspector;
using System.Collections;
using System.Collections.Generic;
using System.Threading;
using CielaSpike;
using UnityEditor;
using UnityEngine;
using UnityEngine.UI;
using Random = UnityEngine.Random;

public class RERTManager : MonoBehaviour
{
    private static RERTManager instance;
    public static RERTManager Instance { get { return instance; } private set { } }

    private Cell[,] Map = null;

    [OnValueChanged("CreateNewMap")]
    public float Tilesize = 1;
    public float MaxFalldownHeight;
    public float ClimbLimit;
    public float HighestWalkable = 5;
    public float HighestPoint = 100F;
    public float LowestPoint = -50F;

    public Transform MapStartPosition;
    public Transform MapEndPosition;

    public List<string> DisallowedTags;
    
    public bool DrawSquares = false;
    public bool DrawLines = false;

    [HideInInspector]
    public List<Vector3> vertex = new List<Vector3>();
    [HideInInspector]
    public List<Vector3> possiblePlaces = new List<Vector3>();
    [HideInInspector]
    public Dictionary<int, int> edge = new Dictionary<int, int>();
    [HideInInspector]
    public List<Vector2> desiredEdges = new List<Vector2>();
    [HideInInspector]
    public List<Vector3> desiredVertex = new List<Vector3>();

    private bool canSeeTested = false;
    private float lastTimeTested;

    public virtual void OnDrawGizmos()
    {
        if (!Application.isEditor) return;
        Gizmos.color = Color.red;
        for (int _i = 0; _i < nodes.Count; _i++)
        {
            Gizmos.DrawSphere(nodes[_i].pos, 0.2f);
        }

        for (int i = 0; i < AllLines.Count; i++)
        {
            Gizmos.DrawLine(AllLines[i].posStart, AllLines[i].posEnd);
        }
        Gizmos.color = Color.yellow;
        for (int i = 0; i < PathLines.Count; i++)
        {
            Gizmos.DrawLine(PathLines[i].posStart, PathLines[i].posEnd);
        }

        //for (int _i = 0; _i < vertex.Count; _i++)
        //{
        //    Gizmos.DrawSphere(vertex[_i], 0.2f);
        //}

        //foreach (var thisedge in edge)
        //{
        //    Gizmos.DrawLine(vertex[thisedge.Key], vertex[thisedge.Value]);
        //}

        //Gizmos.color = Color.green;

        //for (int _i = 0; _i < possiblePlaces.Count; _i++)
        //{
        //    Gizmos.DrawSphere(possiblePlaces[_i], 0.1f);
        //}

        Gizmos.color = Color.red;

        //for (int _i = 0; _i < desiredVertex.Count; _i++)
        //{
        //    Gizmos.DrawSphere(desiredVertex[_i], 0.4f);
        //}

        //for (int _i = 0; _i < desiredEdges.Count; _i++)
        //{
        //    Gizmos.DrawLine(vertex[(int)desiredEdges[_i].x], vertex[(int)desiredEdges[_i].y]);
        //}

        if (Map == null) return;

        if (start != null && goal != null && lastTimeTested + 0.3f < Time.time)
        {
            lastTimeTested = Time.time;
            canSeeTested = TestCanSee(startPosition, goalPosition);
        }
        if (lineCells.Count > 0)
        {
            for (int i = 0; i < lineCells.Count; i++)
            {
                Gizmos.color = canSeeTested ? Color.blue : Color.black;
                Gizmos.DrawCube(new Vector3(lineCells[i].xCoord, lineCells[i].yCoord + 0.5f, lineCells[i].zCoord),
                    new Vector3(Tilesize * 0.8f, 0.01f, Tilesize * 0.8f));
            }
        }
        for (int i = 0; i < Map.GetLength(1); i++)
        {
            for (int j = 0; j < Map.GetLength(0); j++)
            {
                if (DrawSquares)
                {
                    if (!Map[j, i].walkable)
                        Gizmos.color = Color.red;
                    else Gizmos.color = lineCells.Contains(Map[j,i]) ? (canSeeTested ? Color.blue : Color.black) : Color.green;
                    Gizmos.DrawCube(new Vector3(Map[j, i].xCoord, Map[j, i].yCoord, Map[j, i].zCoord),
                        new Vector3(Tilesize*0.8f, 0.01f, Tilesize*0.8f));
                } else if (DrawLines)
                {
                    if (!Map[j, i].walkable)
                        continue;

                    for (int y = i - 1; y < i + 2; y++)
                    {
                        for (int x = j - 1; x < j + 2; x++)
                        {
                            if (y < 0 || x < 0 || y >= Map.GetLength(1) || x >= Map.GetLength(0))
                                continue;

                            if (!Map[x, y].walkable)
                                continue;

                            if (Map[j, i].yCoord > Map[x, y].yCoord && Mathf.Abs(Map[j, i].yCoord - Map[x, y].yCoord) > MaxFalldownHeight)
                                continue;

                            if (Map[j, i].yCoord < Map[x, y].yCoord && Mathf.Abs(Map[x, y].yCoord - Map[j, i].yCoord) > ClimbLimit)
                                continue;

                            Vector3 startPos = new Vector3(Map[j, i].xCoord, Map[j, i].yCoord + 0.1f, Map[j, i].zCoord);
                            Vector3 endPos = new Vector3(Map[x, y].xCoord, Map[x, y].yCoord + 0.1f, Map[x, y].zCoord);

                            UnityEngine.Debug.DrawLine(startPos, endPos, Color.green);
                        }
                    }
                }
            }
        }
    }

    class
        Node
    {
        public Vector3 pos;
        public Vector3 parentPos;
        public int parentInd;
        public float heightOffset = 0.1f; //lift up a little to prevent clipping
        public Line mLine;

        public Node(Vector3 _pos, Vector3 _parentPos, int _parentInd)
        {
            pos = _pos;
            parentPos = _parentPos;
            parentInd = _parentInd;
            if (parentInd >= 0)
            {
                if (mLine != null)
                {
                    if (AllLines.Contains(mLine)) AllLines.Remove(mLine);
                    if (PathLines.Contains(mLine)) PathLines.Remove(mLine);
                    mLine = null;
                }
                mLine = new Line(pos + new Vector3(0f, heightOffset, 0f), parentPos + new Vector3(0f, heightOffset, 0f));
                AllLines.Add(mLine);
            }
        }

        public void ConvertToPath()
        {
            if (mLine != null)
            {
                if (AllLines.Contains(mLine)) AllLines.Remove(mLine);
                mLine = null;
            }
            mLine = new Line(pos + new Vector3(0f, heightOffset, 0f), parentPos + new Vector3(0f, heightOffset, 0f));
            PathLines.Add(mLine);
        }
    }

    class Line
    {
        public Vector3 posStart;
        public Vector3 posEnd;

        public Line(Vector3 _posStart, Vector3 _posEnd)
        {
            posStart = _posStart;
            posEnd = _posEnd;
        }
    }

    public float stepSize;
    public Text coordText;
    public Text statusText;
    private List<Node> nodes = new List<Node>();
    private static List<Line> AllLines = new List<Line>();
    private static List<Line> PathLines = new List<Line>();
    public GameObject start;
    public GameObject goal;
    private bool solving = false;
    
    private float tx = 0f, tz = 0f; //target location to expand towards

    private bool needNewTarget = true; //keep track of whether our random sample is expired or still valid
    private int closestInd = 0;
    private int goalInd = 0; //when success is achieved, remember which node is close to goal
    private float extendAngle = 0f;

    private float temperature = 1e-6f;
    private const float temperatureAdjustFactor = 2.0f;
    private const float MIN_TEMPERATURE = 1e-15f;
    private int numTransitionFails = 0;
    private const int MAX_TRANSITION_FAILS = 20;

    private float pGoToGoal = 0.1f;
    private const int MAX_NUM_NODES = 10000;

    private int solvingSpeed = 1; //number of attempts to make per frame
    private float pathCost;

    [OnValueChanged("CreateNewMap")]
    [Range(0,5)]
    public int EdgeRemoval = 2;
    private int startX,startZ,endX,endZ;

    [Button]
    void CreateNewMap()
    {
        if (Tilesize <= 0)
        {
            Tilesize = 1;
        }

        if (instance == null) instance = this;
        CreateMap();
    }

    void Start()
    {
        CreateMap();
        //stepSize = Tilesize; //TODO experiment

        //Debug.Log(minX + " " + maxX + " " + minZ + " " + maxZ + " " + minHeight + " " + maxHeight + " " + stepSize);
    }
    #region map
    //-------------------------------------------------INSTANIATE MAP-----------------------------------------------//

    private void CreateMap()
    {
        //Find positions for start and end of map
        startX = (int) (Mathf.Min(MapStartPosition.position.x, MapEndPosition.position.x));
        startZ = (int) (Mathf.Min(MapStartPosition.position.z, MapEndPosition.position.z));
        endX = (int) (Mathf.Max(MapStartPosition.position.x, MapEndPosition.position.x));
        endZ = (int) (Mathf.Max(MapStartPosition.position.z, MapEndPosition.position.z));

        //Find tile width and height
        int width = (int) ((endX - startX)/Tilesize);
        int height = (int) ((endZ - startZ)/Tilesize);

        if (width < 1 || height < 1)
        {
            Debug.Log("Wrong widght/height <1");
            return;
        }
        //Set map up
        Map = new Cell[width, height];
        int size = width*height;

        //Fill up Map
        for (int i = 0; i < height; i++)
        {
            for (int j = 0; j < width; j++)
            {
                float x = startX + (j*Tilesize) + (Tilesize/2); //Position from where we raycast - X
                float y = startZ + (i*Tilesize) + (Tilesize/2); //Position from where we raycast - Z
                int ID = (i*width) + j; //ID we give to our Cell!

                float dist = Mathf.Abs(HighestPoint) + Mathf.Abs(LowestPoint);
                RaycastHit[] hit;
                hit = Physics.SphereCastAll(new Vector3(x, HighestPoint, y), Tilesize/5, Vector3.down, dist);
                bool free = true;
                float maxY = -Mathf.Infinity;

                foreach (RaycastHit h in hit)
                {
                    if (DisallowedTags.Contains(h.transform.tag) || h.point.y > HighestWalkable)
                    {
                        if (h.point.y > maxY)
                        {
                            //It is a disallowed walking tile, make it false
                            Map[j, i] = new Cell(j, i, 0, ID, x, y, false); //Non walkable tile!
                            free = false;
                            maxY = h.point.y;
                        }
                    }
                    else
                    {
                        if (h.point.y > maxY)
                        {
                            //It is allowed to walk on this tile, make it walkable!
                            Map[j, i] = new Cell(j, i, h.point.y, ID, x, y, true); //walkable tile!
                            free = false;
                            maxY = h.point.y;
                        }
                    }
                }
                //We hit nothing set tile to false
                if (free == true)
                {
                    Map[j, i] = new Cell(j, i, 0, ID, x, y, false); //Non walkable tile! 
                }
            }
        }
        
        List<Cell> cellsToChange = new List<Cell>();

        if (EdgeRemoval > 1 && EdgeRemoval < 8)
        {
            for (int x = 0; x < Map.GetLength(0); x++)
            {
                for (int y = 0; y < Map.GetLength(1); y++)
                {
                    if (Map[x, y] != null && GetAdjacentSpots(x, y, EdgeRemoval, EdgeRemoval) > 1)
                    {
                        cellsToChange.Add(Map[x, y]);
                    }
                }
            }
            for (int i = 0; i < cellsToChange.Count; i++)
            {
                cellsToChange[i].walkable = false;
            }
        }

        for (int x = 0; x < Map.GetLength(0); x++)
        {
            for (int y = 0; y < Map.GetLength(1); y++)
            {
                if (Map[x, y] != null && !Map[x, y].walkable)
                {
                    RaycastHit hit = new RaycastHit();
                    if (Physics.Raycast(Map[x, y].position + Vector3.up * 400, -Vector3.up, out hit,600))
                    {
                        Map[x, y].yCoord = hit.point.y;
                    }
                }
            }
        }
    }

    #endregion //End map
    void Update()
    {
        RaycastHit hitInfo;
        Plane plane = new Plane(Vector3.up, Vector3.zero);
        float hitDist = 0f;
        Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);
        
        if (plane.Raycast(ray, out hitDist))
        {   
            if (Map != null && Map.GetLength(0) > 1 && Map.GetLength(1) > 1)
            {
                Cell closestCell = FindClosestWalkableCell(ray.GetPoint(hitDist));
                if (closestCell != null)
                {
                    coordText.text = closestCell.x+" / "+closestCell.y;

                    if (Input.GetMouseButtonUp(0))
                    {
                        start.transform.position = closestCell.position;
                    }

                    if (Input.GetMouseButtonUp(1))
                    {
                        goal.transform.position = closestCell.position;
                    }

                }
            }
        }

        if ((Map != null && Map.GetLength(0) > 1 && Map.GetLength(1) > 1) && Input.GetKeyUp(KeyCode.Space)) GetPathMultithreading();
        //if (solving)
        //{
        //    if (nodes.Count < MAX_NUM_NODES)
        //    {
        //        if (statusText != null) statusText.text = "Solving... (nodes=" + nodes.Count + ", temp=" + temperature.ToString("0.00E00") + ")";
        //        TRRTGrow();
        //    }
        //}
    }

    [Button]
    public void GetPathMultithreading()
    {
        StartCoroutine(GetPath());
    }

    private Vector3 goalPosition;
    private Vector3 startPosition;

    IEnumerator GetPath()
    {
        startPosition = start.transform.position;
        goalPosition = goal.transform.position;
        solving = false;
        ClearTree();
        solvingSpeed = 500;
        if (!solving)
        {
            solving = true;
            if (nodes.Count < 1)
            {
                //add initial node
                Node n = new Node(startPosition, startPosition, -1);
                nodes.Add(n);

                if (TestCanSee(startPosition, goalPosition))
                {
                    Node endnode = new Node(goalPosition, startPosition, 0);
                    nodes.Add(endnode);
                    closestInd = nodes.Count - 1;
                    FoundGoal();
                }
                //Debug.Log("Added node " + nodes.Count + ": " + n.pos.x + ", " + n.pos.y + ", " + n.pos.z);
            }
            // StartCoroutine(GetPath());
        }
        Task task;
        this.StartCoroutineAsync(DoFindPath(), out task);
        yield return StartCoroutine(task.Wait());
        
        if (statusText != null) statusText.text = "Solved! with " + nodes.Count + " nodes, cost=" + pathCost;
        //DONE FINDING PATH
    }

    IEnumerator DoFindPath()
    {
        while (solving)
        {
            TRRTGrow();
        }
        yield return 0;
    }

    [Button]
    void TryBeginSolving()
    {
        solving = false;
        BeginSolving(10);
    }

    void BeginSolving(int speed)
    {
        ClearTree();
        solvingSpeed = speed;
        if (!solving)
        {
            solving = true;
            if (nodes.Count < 1)
            {
                //add initial node
                Node n = new Node(start.transform.position, start.transform.position, -1);
                nodes.Add(n);

                //Debug.Log("Added node " + nodes.Count + ": " + n.pos.x + ", " + n.pos.y + ", " + n.pos.z);
            }
           // StartCoroutine(GetPath());
        }
    }

    [Button]
    void PauseSolving()
    {
        solving = false;
        if (statusText != null) statusText.text = "Solving paused.";
    }

    [Button]
    void ClearTree()
    {
        solving = false;
        if (statusText != null) statusText.text = "Tree cleared.";
        //FIXME

        nodes.Clear();
        AllLines.Clear();
        PathLines.Clear();
        needNewTarget = true;
    }


    void FoundGoal()
    {
        goalInd = closestInd;
        solving = false;
        
        //trace path backwards to highlight navigation path
        int i = goalInd;
        Node n;

        pathCost = GetSegmentCost(nodes[goalInd].pos, goalPosition);

        while (i != 0)
        {
            n = nodes[i];
            n.ConvertToPath();

            pathCost += GetSegmentCost(n.parentPos, n.pos);

            i = n.parentInd;
        }
    }

    float GetSegmentCost(Vector3 posA, Vector3 posB)
    {
        float dCost = posB.y - posA.y;
        float dx = posB.x - posA.x;
        float dz = posB.z - posA.z;

        float dist = Mathf.Sqrt(dx * dx + dz * dz);

        float cost = 0.001f * dist; //arbitrary, small distance component

        if (dCost > 0)
        {
            cost += dist * dCost;
        }

        return cost;
    }

    void TRRTGrow()
    {
        int numAttempts = 0;

        float dx, dz;

        Vector3 pos;
        Node n;

        float minDistSq;
        float distSq;

        bool goingToGoal;

        ReseedRandom();
        while (numAttempts < solvingSpeed && nodes.Count < MAX_NUM_NODES)
        {
            if (needNewTarget)
            {
                if (GetRandom(0,1) < pGoToGoal)
                {
                    goingToGoal = true;
                    tx = goalPosition.x;
                    tz = goalPosition.z;
                }
                else
                {
                    goingToGoal = false;
                    tx = GetRandom(startX, endX);
                    tz = GetRandom(startZ, endZ);
                }
                needNewTarget = false;

                //Debug.Log("New target: " + tx + ", " + tz);

                //Find which node is closest to (tx,tz)
                minDistSq = float.MaxValue;
                for (int i = 0; i < nodes.Count; i++)
                {
                    dx = tx - nodes[i].pos.x;
                    dz = tz - nodes[i].pos.z;
                    distSq = dx * dx + dz * dz;
                    if (distSq < minDistSq)
                    {
                        closestInd = i;
                        minDistSq = distSq;
                    }
                }

                if (closestInd > -1 && closestInd < nodes.Count - 1)
                {
                    if (IsInRange(nodes[closestInd].pos, goalPosition, stepSize*5))
                    {
                        if (TestCanSee(nodes[closestInd].pos, goalPosition))
                        {
                            FoundGoal();
                            break;
                        }
                    }
                }

                if (Mathf.Sqrt(minDistSq) <= stepSize)
                {
                    //random sample is already "close enough" to tree to be considered reached
                    if (goingToGoal)
                    {
                        FoundGoal();
                        break;
                    }
                    else
                    {
                        needNewTarget = true;
                        continue;
                    }
                }

                //Debug.Log("closestInd: " + closestInd);

                dx = tx - nodes[closestInd].pos.x;
                dz = tz - nodes[closestInd].pos.z;

                extendAngle = Mathf.Atan2(dz, dx);

                //Debug.Log("dx dz a: " + dx + " " + dz + " " + extendAngle*180/Mathf.PI);
            }

            pos = new Vector3(nodes[closestInd].pos.x + stepSize * Mathf.Cos(extendAngle), 0f, nodes[closestInd].pos.z + stepSize * Mathf.Sin(extendAngle));
            Cell findClosestCell = FindClosestCell(pos);
            if (findClosestCell == null)
            {
                needNewTarget = true;
                numAttempts++;
            }
            else
            {
                if (findClosestCell == null || !findClosestCell.walkable) pos.y = 500;
                else
                    pos.y = findClosestCell.yCoord; //get y value from terrain

                if (TransitionTest(nodes[closestInd].pos, findClosestCell.position))
                {
                    if (!findClosestCell.walkable && !TestCanSee(nodes[closestInd].pos,findClosestCell.position))
                    {
                        needNewTarget = true;
                        numAttempts++;
                    }
                    else
                    {
                        n = new Node(pos, nodes[closestInd].pos, closestInd);
                        nodes.Add(n);

                        //Debug.Log("Added node " + nodes.Count + ": " + n.pos.x + ", " + n.pos.y + ", " + n.pos.z);

                        //Determine whether we are close enough to goal
                        dx = goalPosition.x - n.pos.x;
                        dz = goalPosition.z - n.pos.z;
                        if (Mathf.Sqrt(dx*dx + dz*dz) <= stepSize)
                        {
                            //Reached the goal!
                            FoundGoal();
                            return;
                        }

                        //Determine whether we are close enough to target, or need to keep extending
                        dx = tx - n.pos.x;
                        dz = tz - n.pos.z;
                        if (Mathf.Sqrt(dx*dx + dz*dz) <= stepSize)
                        {
                            //we've reached our target point, need a new target
                            needNewTarget = true;
                        }
                        else
                        {
                            //keep extending from the latest node
                            closestInd = nodes.Count - 1;
                        }
                        numAttempts++;
                    }
                }
                else
                {
                    //this extension is aborted due to transition test, need a new target
                    //Debug.Log("Failed transition test");
                    needNewTarget = true;
                    numAttempts++;
                }
            }
        }
    }

    bool TransitionTest(Vector3 posA, Vector3 posB)
    {
        float dx = posB.x - posA.x;
        float dz = posB.z - posA.z;
        float dist = Mathf.Sqrt(dx * dx + dz * dz);

        float slope = (posB.y - posA.y) / dist;

        float pTransition; //transition probability, 0 to 1

        if (slope <= 0)
        {
            pTransition = 1.0f; //always go "downhill"
        }
        else
        {
            pTransition = Mathf.Exp(-slope / (temperature)); //FIXME
        }

        bool pass = GetRandom(0, 1) < pTransition;

        Cell findNewPosCell = FindClosestCell(posB);
        if (findNewPosCell != null)
        {
            if (!findNewPosCell.walkable) return false;
        }
        else return false;

        if (!pass)
        {
            if (numTransitionFails > MAX_TRANSITION_FAILS)
            {
                //Heat the temperature up
                temperature = temperature * temperatureAdjustFactor;
                numTransitionFails = 0; //restart counter
            }
            else
            {
                numTransitionFails++;
            }
        }
        else
        {
            //Cool the temperature down
            if (temperature > MIN_TEMPERATURE)
            { //prevent slim chance of temp becoming 0
                temperature = temperature / temperatureAdjustFactor;
            }
            numTransitionFails = 0;
        }

        return pass;
    }

    [Button]
    public void SpitBunchOfRandoms()
    {
        Debug.Log("Custom Random:" + GetRandom(0, 30.1f)+" Unity Random:"+Random.Range(0, 30.1f) +" Custom Value:"+GetRandom(0,1)+" Unity Value:"+Random.value);
    }

    System.Random myRandom = new System.Random();

    public void ReseedRandom()
    {
        myRandom = new System.Random(Guid.NewGuid().GetHashCode());
    }

    public float GetRandom(float minRandom, float maxRandom)
    {
        return myRandom.Next(Mathf.CeilToInt(minRandom), Mathf.FloorToInt(maxRandom)) + (float)myRandom.NextDouble();
    }
    
    public int GetAdjacentSpots(int x, int y, int scopeX, int scopeY, bool walkable = false)
    {
        int startX = x - scopeX;
        int startY = y - scopeY;
        int endX = x + scopeX;
        int endY = y + scopeY;

        int iX = startX;
        int iY = startY;

        int foundCounter = 0;

        for (iY = startY; iY <= endY; iY++)
        {
            for (iX = startX; iX <= endX; iX++)
            {
                if (!(iX == x && iY == y))
                {
                    if (walkable)
                    {
                        if (!IsWall(iX, iY))
                        {
                            foundCounter += 1;
                        }
                    }
                    else
                    {
                        if (IsWall(iX, iY))
                        {
                            foundCounter += 1;
                        }
                    }
                }
            }
        }
        return foundCounter;
    }

    [Button]
    public void CanSeePointTest()
    {
       bool canSee = TestCanSee(start.transform.position, goal.transform.position);
        Debug.Log(canSee ? "<color=green>CAN SEE</color>": "<color=red>CAN'T SEE</color>");
    }

    List<Cell> lineCells = new List<Cell>();
    
    bool TestCanSee(Vector3 pos1, Vector3 pos2)
    {
        lineCells.Clear();
        Cell cell1 = FindClosestCell(pos1);
        Cell cell2 = FindClosestCell(pos2);
        if (cell1 == null || cell2 == null || !cell1.walkable || !cell2.walkable) return false;
        float xPix = cell1.x;
        float yPix = cell1.y;

        float width = cell2.x - cell1.x;
        float height = cell2.y - cell1.y;
        float length = Mathf.Abs(width);
        if (Mathf.Abs(height) > length) length = Mathf.Abs(height);
        int intLength = (int)length;
        float dx = width / (float)length;
        float dy = height / (float)length;
        for (int i = 0; i <= intLength; i++)
        {
            //a_Texture.SetPixel((int)xPix, (int)yPix, a_Color);
            xPix += dx;
            yPix += dy;
            if (!IsOutOfBounds((int)xPix, (int)yPix))
            {
                if (!lineCells.Contains(Map[(int)xPix, (int)yPix])) lineCells.Add(Map[(int)xPix, (int)yPix]);
                if (!Map[(int)xPix, (int)yPix].walkable) return false;
            }
        }
        return true;
    }


    bool IsWall(int x, int y)
    {
        // Consider out-of-bound a wall
        if (IsOutOfBounds(x, y)) return false;
        return (!Map[x, y].walkable);
    }
    
    bool IsOutOfBounds(int x, int y)
    {
       // return (x > -1 && x < Map.GetLength(1) && y > -1 && y < Map.GetLength(0));
        if (x < 0 || y < 0)
        {
            return true;
        }
        else if (x > Map.GetLength(0) - 1 || y > Map.GetLength(1) - 1)
        {
            return true;
        }
        return false;
    }

    public bool IsInRange(Vector3 ObjectPosition, Vector3 targetPosition, float range)
    {
        return (targetPosition - ObjectPosition).sqrMagnitude < (range * range);
    }

    private Cell FindClosestCell(Vector3 pos)
    {
        int x = (startX < 0F) ? Mathf.FloorToInt(((pos.x + Mathf.Abs(startX)) / Tilesize)) : Mathf.FloorToInt((pos.x - startX) / Tilesize);
        int z = (startZ < 0F) ? Mathf.FloorToInt(((pos.z + Mathf.Abs(startZ)) / Tilesize)) : Mathf.FloorToInt((pos.z - startZ) / Tilesize);

        if (x < 0 || z < 0 || x > Map.GetLength(0) || z > Map.GetLength(1))
            return null;

        if (IsOutOfBounds(x, z)) return null;

        Cell n = Map[x, z];
        
        return n;
    }

    private Cell FindClosestWalkableCell(Vector3 pos)
    {
        int x = (startX < 0F) ? Mathf.FloorToInt(((pos.x + Mathf.Abs(startX)) / Tilesize)) : Mathf.FloorToInt((pos.x - startX) / Tilesize);
        int z = (startZ < 0F) ? Mathf.FloorToInt(((pos.z + Mathf.Abs(startZ)) / Tilesize)) : Mathf.FloorToInt((pos.z - startZ) / Tilesize);

        if (x < 0 || z < 0 || x > Map.GetLength(0) || z > Map.GetLength(1))
            return null;

        if (IsOutOfBounds(x, z)) return null;

        Cell n = Map[x, z];

        if (n.walkable)
        {
            return n;
        }
        else
        {
            //If we get a non walkable tile, then look around its neightbours
            for (int i = z - 1; i < z + 2; i++)
            {
                for (int j = x - 1; j < x + 2; j++)
                {
                    //Check they are within bounderies
                    if (i > -1 && i < Map.GetLength(1) && j > -1 && j < Map.GetLength(0))
                    {
                        if (Map[j, i].walkable)
                        {
                            return Map[j, i];
                        }
                    }
                }
            }
            return null;
        }
    }
}

public class Cell
{
    public int x = 0;
    public int y = 0;
    public float yCoord = 0;
    public float xCoord = 0;
    public float zCoord = 0;
    public int ID = 0;
    public bool walkable = true;
    public Cell parent = null;

    public Vector3 position { get {  return new Vector3(xCoord,yCoord,zCoord); } }

    public Cell(int indexX, int indexY, float height, int idValue, float xcoord, float zcoord, bool w, Cell p = null)
    {
        x = indexX;
        y = indexY;
        yCoord = height;
        ID = idValue;
        xCoord = xcoord;
        zCoord = zcoord;
        walkable = w;
        parent = p;
    }
}