remove_trajectory Subroutine

    subroutine remove_trajectory(self, traj)
        class(TrajectoryFollower), intent(inout) :: self
        type(Trajectory), pointer, intent(in) :: traj       !! the trajectory to stop following
        integer :: k
        do k = 1, self%n
            if (associated(self%trajs(k)%p, traj)) then
                self%trajs(k:self%n-1) = self%trajs(k+1:self%n)
                self%trajs(self%n)%p => null()
                self%n = self%n - 1
                if (self%current >= k) self%current = max(0, self%current - 1)
        self%stamp_ = next_motion_stamp()
                exit
            end if
        end do
    end subroutine remove_trajectory