﻿using System;
using Sirenix.OdinInspector;
using System.Collections.Generic;
using CI.TaskParallel;
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;
   
    //private bool canSeeTested = false;
    //private float lastTimeTested;
    List<Color> colors = new List<Color>();

    public void Awake()
    {
        instance = this;
    }

    public virtual void OnDrawGizmos()
    {
        if (!Application.isEditor) return;
        Gizmos.color = Color.red;
        Gizmos.color = Color.yellow;

        //if(rertPaths != null)
        //for (int i = 0; i < rertPaths.Count; i++)
        //{
        //    if(rertPaths[i] != null && rertPaths[i].PathNodes != null)
        //        for (int j = 0; j < rertPaths[i].PathNodes.Count-1; j++)
        //        {
        //            if (rertPaths[i].PathNodes[j] != null)
        //            {
        //                if (i < colors.Count)
        //                {
        //                    Gizmos.color = colors[i];
        //                }
        //                Gizmos.DrawLine(rertPaths[i].PathNodes[j].pos,
        //                    rertPaths[i].PathNodes[j + 1].pos);
        //            }
        //        }
        //} 

        if (Map == null) return;
        
        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 = 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) || !Map[x, y].walkable || Map[j, i].yCoord > Map[x, y].yCoord && Mathf.Abs(Map[j, i].yCoord - Map[x, y].yCoord) > MaxFalldownHeight|| 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);
                            Gizmos.color = Color.green;
                            Gizmos.DrawLine(startPos, endPos);
                        }
                    }
                }
            }
        }
    }

    public float stepSize;
    public Text coordText;
    public Text statusText;

    public GameObject start;
    public GameObject goal;
     
    //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()
    {
        UnityTask.InitialiseDispatcher();
        CreateMap();
        //stepSize = Tilesize; //TODO experiment

        //Debug.Log(minX + " " + maxX + " " + minZ + " " + maxZ + " " + minHeight + " " + maxHeight + " " + stepSize);
    }

    //-------------------------------------------------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) 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; 
                } 
    }
    
    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) && 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();
    }

    private int frameNumber = 0;

    public void LateUpdate()
    {
        frameNumber++;
        if (RequestQueue.Count > 0)
        {
            int maxToProcess = Mathf.Min(RequestQueue.Count, ThreadsPerFrame);
            for (int i = 0; i < maxToProcess; i++)
            {
                RunRequest(RequestQueue.Dequeue());
            }
        }
    }

    private int runningThreads = 0;
    public RertRequest RunRequest(RertRequest requestToRun)
    {
        UnityTask.Run(() =>
        {
            runningThreads++;
            return ThreadFindPath(requestToRun);
        }).ContinueOnUIThread((r) =>
        {
            runningThreads--;
            if (requestToRun.requestFinishedCallback != null) requestToRun.requestFinishedCallback(requestToRun);
            //if (runningThreads<1)
            //Debug.Log(" Finished running in "+(frameNumber-frameStart));
            //rertPaths.Add(requestToRun.pathResult);
            //Debug.Log("[" + thisRequestNumber + "]" + "[" + frameNumber + "] " + PrimeToGet + " prime number result " + r.Result.ToString() + " total elapsed " + (frameNumber - firstFrame));
            //ResultText.text = "The result is: " + r.Result.ToString();
        });
        return requestToRun;
    }

    Queue<RertRequest> RequestQueue = new Queue<RertRequest>();

    private int frameStart;
    [Range(1,100)]
    public int PathsAtATime = 100;
    [Range(1, 50)]
    public int ThreadsPerFrame = 15;

    [Button]
    public void GetPathMultithreading()
    {
        frameStart = frameNumber;
        //rertPaths.Clear();
        //colors.Clear();
        for (int i = 0; i < PathsAtATime; i++)
        {
            colors.Add(new Color(Random.value, Random.value, Random.value, 1.0f));
            SetupRequest(start.transform.position,goal.transform.position);
            //StartCoroutine(GetPath());
        }
    }

    public void SetupRequest(Vector3 fromPosition, Vector3 toPosition, Action<RertRequest> requestFinishedCallback = null)
    {
        RertRequest rertRequest = new RertRequest();
        rertRequest.startPosition = fromPosition;
        rertRequest.goalPosition = toPosition;
        rertRequest.requestFinishedCallback = requestFinishedCallback;
        rertRequest.InitiateRequest(this);
        if (!rertRequest.Solved)
        {
            RequestQueue.Enqueue(rertRequest);
        }
        else
        {
            if (rertRequest.requestFinishedCallback != null) rertRequest.requestFinishedCallback(rertRequest);
        }
    }

    private RertRequest ThreadFindPath(RertRequest rertRequest)
    {
        //Debug.Log(frameNumber+" started running");
        while (!rertRequest.Solved)
        {
            RertRequest foundPath = TRRTGrow(rertRequest);
        }
        return rertRequest;
    }
    
    //IEnumerator GetPath()
    //{
    //    startPosition = start.transform.position;
    //    goalPosition = goal.transform.position;
    //    bool isSolving = false;
    //    RertRequest rertRequest = new RertRequest();
    //    rertRequest.startPosition = startPosition;
    //    rertRequest.goalPosition = goalPosition;
    //    rertRequest.InitiateRequest(this);
        
    //    if (isSolving)
    //    {
            
    //    }
    //    else yield return 0;

    //    if (rertRequest != null && rertRequest.pathResult != null && rertRequest.solvingNodes != null)
    //    {
    //        Debug.Log("["+frameNumber+ "] Solved! with " + rertRequest.solvingNodes.Count + " nodes, cost=" + rertRequest.pathResult.cost);
    //        if (statusText != null) statusText.text = "Solved! with " + rertRequest.solvingNodes.Count + " nodes, cost=" + rertRequest.pathResult.cost;
    //    }
    //    //DONE FINDING PATH
    //}
    
    //IEnumerator DoFindPath(bool IsSolving, RertRequest rertRequest)
    //{
    //    RertPath foundPath = null;
    //    while (IsSolving)
    //    {
    //        foundPath = TRRTGrow(rertRequest);
    //        if (foundPath != null) IsSolving = false;
    //    }
    //    yield return null;
    //    rertPaths.Add(rertRequest.pathResult);
    //}

    public RertRequest FoundGoal(RertRequest rertRequest)
    {
        rertRequest.goalInd = rertRequest.closestInd;
        //Debug.Log("found goal, goal index " + rertRequest.goalInd);
        //trace path backwards to highlight navigation path
        int i = rertRequest.goalInd;
        RertNode n;

        var pathCost = GetSegmentCost(rertRequest.solvingNodes[rertRequest.goalInd].pos, rertRequest.goalPosition);
        if(rertRequest.pathResult == null) rertRequest.pathResult = new List<Vector3>();
        rertRequest.pathResult.Clear();
        rertRequest.pathResult.Add(rertRequest.goalPosition);
        while (i != 0)
        {
            n = rertRequest.solvingNodes[i];
            rertRequest.pathResult.Add(n.pos);
            //n.ConvertToPath();

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

            i = n.parentInd;
        }
        rertRequest.pathResult.Add(rertRequest.solvingNodes[0].pos);
        //rertRequest.pathResult.PathNodes.Add(rertRequest.solvingNodes[0]);
        //rertRequest.pathResult.cost = pathCost;
        rertRequest.Solved = true;

        return rertRequest;
    }

    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;
    }

    public RertRequest TRRTGrow(RertRequest rertRequest)
    {
        int numAttempts = 0;

        float dx, dz;

        Vector3 pos;
        RertNode n;

        float minDistSq;
        float distSq;

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

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

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

                if (rertRequest.closestInd > -1 && rertRequest.closestInd < rertRequest.solvingNodes.Count - 1 &&
                    (IsInRange(rertRequest.solvingNodes[rertRequest.closestInd].pos, rertRequest.goalPosition, stepSize*5) &&
                     TestCanSee(rertRequest.solvingNodes[rertRequest.closestInd].pos, rertRequest.goalPosition)))
                    return FoundGoal(rertRequest);

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

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

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

                rertRequest.extendAngle = Mathf.Atan2(dz, dx);

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

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

                if (TransitionTest(rertRequest, rertRequest.solvingNodes[rertRequest.closestInd].pos, findClosestCell.position))
                {
                    if (!findClosestCell.walkable && !TestCanSee(rertRequest.solvingNodes[rertRequest.closestInd].pos,findClosestCell.position))
                    {
                        rertRequest.needNewTarget = true;
                        numAttempts++;
                    }
                    else
                    {
                        n = new RertNode(pos, rertRequest.solvingNodes[rertRequest.closestInd].pos, rertRequest.closestInd);
                        rertRequest.solvingNodes.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 = rertRequest.goalPosition.x - n.pos.x;
                        dz = rertRequest.goalPosition.z - n.pos.z;
                        if (Mathf.Sqrt(dx*dx + dz*dz) <= stepSize)
                        {
                            return FoundGoal(rertRequest);
                        }

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

    bool TransitionTest(RertRequest rertRequest, 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 / (rertRequest.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 (rertRequest.numTransitionFails > MAX_TRANSITION_FAILS)
            {
                //Heat the temperature up
                rertRequest.temperature = rertRequest.temperature * temperatureAdjustFactor;
                rertRequest.numTransitionFails = 0; //restart counter
            }
            else
            {
                rertRequest.numTransitionFails++;
            }
        }
        else
        {
            //Cool the temperature down
            if (rertRequest.temperature > MIN_TEMPERATURE)
            { //prevent slim chance of temp becoming 0
                rertRequest.temperature = rertRequest.temperature / temperatureAdjustFactor;
            }
            rertRequest.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);
    }
    
    public float GetRandom(float minRandom, float maxRandom)
    {
        System.Random myRandom = new System.Random(Guid.NewGuid().GetHashCode());
        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>();

    public 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 < 0 || y < 0) || (x > Map.GetLength(0) - 1 || y > Map.GetLength(1) - 1);
    }

    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; 

        //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++) 
                if (i > -1 && i < Map.GetLength(1) && j > -1 && j < Map.GetLength(0) && 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;
    }
}

public class RertNode
{
    public Vector3 pos;
    public Vector3 parentPos;
    public int parentInd;

    public RertNode(Vector3 _pos, Vector3 _parentPos, int _parentInd)
    {
        pos = _pos;
        parentPos = _parentPos;
        parentInd = _parentInd;
    }
}

public class RertRequest
{
    public Action<RertRequest> requestFinishedCallback = null;
    public Vector3 startPosition;
    public Vector3 goalPosition;

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

    public float temperature = 1e-6f;
    public int numTransitionFails = 0;

    public float pGoToGoal = 0.1f;

    public float tx = 0f, tz = 0f; //target location to expand towards

    public List<RertNode> solvingNodes = new List<RertNode>();
    public List<Vector3> pathResult = new List<Vector3>();
    public float pathCost = 0;

    public float solvingSpeed = 500;

    public bool Solved = false;

    public void InitiateRequest(RERTManager rertManager)
    {
        solvingSpeed = 500;
        if (Solved || solvingNodes.Count >= 1) return;
        RertNode n = new RertNode(startPosition, startPosition, -1);
        solvingNodes.Add(n);
        if (rertManager.TestCanSee(startPosition, goalPosition))
        {
            RertNode endnode = new RertNode(goalPosition, startPosition, 0);
            solvingNodes.Add(endnode);
            closestInd = solvingNodes.Count - 1;
            rertManager.FoundGoal(this);
            Solved = true;
        }
    }
}