Rotation issue when selecting a new waypoint.

Hello all,

I am editing some scripts I found for snake like enemies to fit the nature of my enemy. This enemy has a set of waypoints that he moves towards, once a waypoint is reached the enemy's head rotates towards the next waypoint. When the enemy hits a waypoint the tail (body objects) flick or rotate(?) too fast and the snakes tail ends up in an incorrect position.

There are 2 scripts that effect the snake. One for the head (leads the body to the waypoints) and the other for the body (Controls how the body follows and sticks to eachother). I'm pretty sure that the issue is originating in the body script, but I could be wrong. Any help with this is greatly appreciated.

Here's the head script

 using UnityEngine;
 //Head Script
 public class FollowThePath : MonoBehaviour
 {
 
     // Array of waypoints to walk from one to the next one
     [SerializeField]
     private Transform[] patterns;
     [SerializeField]
     private Transform[] waypoints;
 
     // Walk speed that can be set in Inspector
     [SerializeField]
     private float moveSpeed = 5f;
 
     [SerializeField]
     private float turnSpeed = 5f;
 
     // Index of current waypoint from which Enemy walks
     // to the next one
     private int waypointIndex = 0;
 
     // Use this for initialization
     private void Start()
     {
 
         // Set position of Enemy as position of the first waypoint
         transform.position = waypoints[waypointIndex].transform.position;
     }
 
     // Update is called once per frame
     private void Update()
     {
 
         // Move Enemy
         Move();
     }
 
     // Method that actually make Enemy walk
     private void Move()
     {
         // If Enemy didn't reach last waypoint it can move
         // If enemy reached last waypoint then it stops
         if (waypointIndex <= waypoints.Length - 1)
         {
 
             // Move Enemy from current waypoint to the next one
             // using MoveTowards method
             transform.position = Vector2.MoveTowards(transform.position,
                waypoints[waypointIndex].transform.position,
                moveSpeed * Time.deltaTime);
 
             Vector3 vectorToTarget = waypoints[waypointIndex].position - transform.position;
             float angle = Mathf.Atan2(vectorToTarget.y, vectorToTarget.x) * Mathf.Rad2Deg;
             Quaternion q = Quaternion.AngleAxis(angle, Vector3.forward);
             transform.rotation = Quaternion.Slerp(transform.rotation, q, Time.deltaTime * turnSpeed);
 
 
             // If Enemy reaches position of waypoint he walked towards
             // then waypointIndex is increased by 1
             // and Enemy starts to walk to the next waypoint
             if (transform.position == waypoints[waypointIndex].transform.position)
             {
                 waypointIndex += 1;
             }
         }
     }
 }

And here's the body script:

 using System.Collections;
 using System.Collections.Generic;
 using UnityEngine;
 
 public class SnakeBody : MonoBehaviour
 {
     //head object.
     public GameObject goLeader = null;
     //Object for the body to follow.
     public GameObject headFollowPoint;
     //All body objects.
     public GameObject[] body;
     //Objects for next body object to follow.
     public GameObject[] bodyFollowPoints;
     //Move speed.
     public float speed = 2f;
     //Rigid body of each body object.
     public Rigidbody2D[] rb;
     //Vector to move the object along the x axis.
     Vector2 vec = new Vector2(1, 0);
 
     void Update()
     {
         //If unable to find the head return null
         if (goLeader == null) return;
 
 
         for (int i = 0; i < body.Length; i++)
         {
             //body[i].transform.up = bodyFollowPoints[i].transform.position;
             rb[i] = body[i].GetComponent<Rigidbody2D>();
             if (i == 0)
             {
                 body[0].transform.right = headFollowPoint.transform.position - body[0].transform.position;
                 follow(i);
             }
             else
             {
                 body[i].transform.right = bodyFollowPoints[i].transform.position - body[i].transform.position;
                 follow(i);
             }
             rb[i].velocity = body[i].transform.TransformDirection(vec * speed);
 
 
         }
     }
 
     //Follow method is used to space the objects a specific distance while rotating and following the previous object.
     void follow(int i)
     {
         // First body object follows the head.
         if (i == 0)
         {
             Vector3 v3FromLeader = body[i].transform.position - headFollowPoint.transform.position;
             v3FromLeader = v3FromLeader.normalized * .5f;
             body[i].transform.position = v3FromLeader + headFollowPoint.transform.position;
         }
         // All other body objects follow eachother.
         else
         {
             Vector3 v3FromLeader = body[i].transform.position - bodyFollowPoints[i].transform.position;
             v3FromLeader = v3FromLeader.normalized * .5f; //Changing this creates gaps in the boss but smooths the end body.
             body[i].transform.position = v3FromLeader + bodyFollowPoints[i].transform.position;
         }
 
     }
 }

Here's a visual representation of what's happening.