AI faces Weird Direction when following a waypoint UNITY

Question

My problem is that whenever it tries to move towards the current waypoint, it faces weird directions and does not follow the path completely.

IEnumerator FollowPath() {
    Vector3 currentWaypoint = path[0];

    while (true) {
        //transform.LookAt (currentWaypoint);
        if (transform.position == currentWaypoint) {

            PathRequestManager.RequestPath(transform.position,target.position,OnPathFound);
            targetIndex=0;
            targetIndex ++;
            //Debug.Log(currentWaypoint);

            if (targetIndex >= path.Length) {
                targetIndex =0;
                path = new Vector3[0];
                //yield break;
            }
            currentWaypoint = path[targetIndex];
        }

        transform.LookAt (currentWaypoint);
        transform.position = Vector3.MoveTowards(transform.position,currentWaypoint,speed * Time.deltaTime);

        yield return null;
    }
}  

Currently using a* pathfinding. Complete source code is found here: https://github.com/SebLague/Pathfinding

When it reaches the current waypoint, it randomly changes its direction or randomly faces direction and does not go through the next waypoint. (Screenshot)

Answer 1

At this part in the code, targetIndex will always evaluate to 1

targetIndex=0;
targetIndex ++;

So basically, it will never go further than the first waypoint.

Also, i would recommend that instead of

if (transform.position == currentWaypoint)

You would do this instead:

float threshold = 0.1f;                // Adjust to your preference
if ((currentWaypoint - transform.position).sqrMagnitude < threshold)

This is because i think MoveTowards can possibly overshoot or not reach exactly the target vector and will hand around and change direction drastically when hovering over the target vector.