public void CalcUnitPath()
    {
        foreach (PathNode node in navMesh.PathNodes)
        {
            node.A_StarPath = false;
        }

        RaycastHit hit;
        int        fromIndex = -1, toIndex = -1;

        if (Physics.Raycast(UnitAI.position, Vector3.down, out hit, Mathf.Infinity, NavMeshMask))
        {
            fromIndex = hit.triangleIndex;
        }
        //else                                               // *********************    IF UNIT IS OFF NAVMESH FIND CLOSEST PATHNODE, OPTIMIZE PATH GETS GLITCHY WITH THIS     ***************
        //{
        //    int nearestIndex = 0;
        //    Vector3 localAIPosition = UnitAI.position - transform.position;
        //    for (int i = 0; i < navMesh.PathNodes.Count; i++)
        //    {
        //        if((navMesh.PathNodes[i].Position - localAIPosition).sqrMagnitude < (navMesh.PathNodes[nearestIndex].Position - localAIPosition).sqrMagnitude)
        //        {
        //            nearestIndex = i;
        //        }
        //    }
        //    fromIndex = nearestIndex;
        //}
        if (Physics.Raycast(TargetAI.position, Vector3.down, out hit, Mathf.Infinity, NavMeshMask))
        {
            toIndex = hit.triangleIndex;
        }
        if (fromIndex != -1 && toIndex != -1 && fromIndex != toIndex)
        {
            path = HelperFunctions.A_Star(navMesh.PathNodes[fromIndex], navMesh.PathNodes[toIndex], navMesh.PathNodes);
        }
        optimizedPath = HelperFunctions.optimizePath(path, navMesh.Edges, navMesh.PointPositions, UnitAI.position, TargetAI.position, NavMeshMask);

        Vector3[] wayPoints = optimizedPath.ToArray();
        for (int i = 0; i < wayPoints.Length; i++)
        {
            wayPoints[i] += transform.position;
        }
        aiScript.setPath(wayPoints);
    }