如何将具有给定索引的新元素添加到链表中？

我正在学习 Java 中的链表，并尝试创建一个函数来将新点添加到具有给定索引的链表中。请帮我检查我的代码然后告诉我我在这里做错了什么。非常感谢您提前！！！！

我的程序有 2 个类名 Waypoint 和 TourElement。我也有一些测试用例。

航点

public class Waypoint {
    int x;
    int y;

    public int getX() {
        return this.x;
    }

    public int getY() {
        return this.y;
    }

    public void setXY(int x, int y) {
        this.x = x;
        this.y = y;
    }

    int[] toArray() {
        int array[] = new int[2];
        array[0] = getX();
        array[1] = getY();
        return array;
    }

    @Override
    public String toString() {
        String convertToString = "(" + getX() + "/" + getY() + ")";
        return convertToString;
    }

塔元素

public class TourElement {
    private Waypoint points;
    private TourElement next;

    public void setWaypoint( Waypoint points) {
        this.points = points; 
    }

    public void setNext(TourElement next) {
        this.next = next;
    }

    Waypoint getWaypoint() {
        return this.points;
    }

    TourElement getNext() {
        return this.next;
    }

    boolean hasNext() {
        if(this.next != null) {
            return true;
        }
        return false;
    }

    int getNoOfWaypoints() {//  return the number of waypoints in the list
    int count = 1;
    TourElement current = this;
    while(current.next != null) {
        count++;
        current = current.next;
        System.out.println(count);
    }
    return count;
}

这是插入具有给定索引的新点的函数：

TourElement insertAt(int index, Waypoint waypoint) {
    int lengthLinkList = getNoOfWaypoints();
    TourElement current = this;
    int count = 0;
    if(waypoint == null || index < 0 || index > lengthLinkList) {
        return this;
    } else {
        if(index == 0) {
            TourElement newElement = new TourElement();
            newElement.setWaypoint(waypoint);
            newElement.setNext(this);
            return newElement;
        } else {
            while(current.next != null) { //I think I'm doing wrong here when trying to add new points.
                if(index == count) {
                    TourElement newElement = new TourElement();
                    current.setNext(current);
                    newElement.setWaypoint(waypoint);
                    newElement.setNext(current.next);
                    return newElement;
                }
                count++;
                current = current.next;
           }
           if(current.next == null) {
            TourElement newElement = new TourElement();
            current.setNext(newElement);
            newElement.setWaypoint(waypoint);
            newElement.setNext(null);
           }
        }
        return this;
    }
}

这是我的测试用例：//创建元素列表：

private Waypoint createWaypoint(int x, int y) {
        Waypoint wp = new Waypoint();
        wp.setXY(x, y);
        return wp;
    }

    /**
     * Creates a ElementList with the given waypoints.
     * @param waypoints array of waypoints to use, the coordinates of the waypoints are also in an array
     * @return List of elements with the given waypoints
     * @pre at least one waypoint has to be in array
     */
    private TourElement createElementList(int[][] waypoints){
        assert waypoints.length > 0;
        TourElement elem = new TourElement();
        int lastIndex = waypoints.length-1;
        Waypoint wp = createWaypoint(waypoints[lastIndex][0], waypoints[lastIndex][1]);
        elem.setWaypoint(wp);
        for (int i = lastIndex-1; i >= 0 ; i--) {
            wp = createWaypoint(waypoints[i][0], waypoints[i][1]);
            elem = elem.addStart(wp);
        }
        return elem;
    }

测试用例 1：通过

@Test
public void testInsertAt_BeforeFirst() {
    TourElement elem = createElementList(new int[][] {{1, 1}, {3, 3}});
    Waypoint wp = createWaypoint(0, 0);
    elem = elem.insertAt(0, wp);
    assertArrayEquals(new int[] {0, 0}, elem.getWaypoint().toArray());
    assertArrayEquals(new int[] {1, 1}, elem.getNext().getWaypoint().toArray());
    assertArrayEquals(new int[] {3, 3}, elem.getNext().getNext().getWaypoint().toArray());
    assertNull(elem.getNext().getNext().getNext());
}

测试用例 2：失败

错误是：数组首先在元素[0]处不同；期望 <2> 但是是：<3>

  @Test
    public void testInsertAt_Second() {
        TourElement elem = createElementList(new int[][] {{1, 1}, {3, 3}});
        Waypoint wp = createWaypoint(2, 2);
        elem = elem.insertAt(1, wp);
        assertArrayEquals(new int[] {1, 1}, elem.getWaypoint().toArray());
        assertArrayEquals(new int[] {2, 2}, elem.getNext().getWaypoint().toArray());
        assertArrayEquals(new int[] {3, 3}, elem.getNext().getNext().getWaypoint().toArray());
        assertNull(elem.getNext().getNext().getNext());
    }

测试案例3：通过

@Test
public void testInsertAt_Last() {
    TourElement elem = createElementList(new int[][] {{1, 1}, {3, 3}});
    Waypoint wp = createWaypoint(4, 4);
    elem = elem.insertAt(2, wp);
    assertArrayEquals(new int[] {1, 1}, elem.getWaypoint().toArray());
    assertArrayEquals(new int[] {3, 3}, elem.getNext().getWaypoint().toArray());
    assertArrayEquals(new int[] {4, 4}, elem.getNext().getNext().getWaypoint().toArray());
    assertNull(elem.getNext().getNext().getNext());
}

在函数中insertAt(index, waypoint)，该函数在index = 0 或index = last 时通过。但我不明白为什么第二个测试用例没有通过。请帮我！！