Переставить элементы массива

Последовательность чисел от 0 до N-1 (N >= 2 - целое) случайным образом перемешали, получив массив A длины N. Необходимо изменить массив так, чтобы по окончании работы новый элемент A[i] содержал значение, равное A[A[i]] в старом массиве, для всех i от 0 до N-1, используя O(1) дополнительной памяти.

Например, на входе A = {1, 2, 7, 0, 9, 3, 6, 8, 5, 4}.

Тогда на выходе A = {2, 7, 8, 1, 4, 0, 6, 5, 3, 9}.

Я пробовал вот так:

size_t pos_1 = 0, pos_2 = A[0], assigned = 0;
while (assigned != A.size() - 1 && pos_2 != 0) {
    std::swap(A[pos_1], A[pos_2]);
    std::swap(pos_1, pos_2);
    pos_2 = A[pos_2];
    ++assigned;
}

Но, если мы приходим снова к 0-му элементу, а ещё не всё обменялось, то ответ неверный. Также пробовал квадратное решение:

size_t pos = 0, assigned = 0;
int tmp = A[A[pos]];
while (assigned != A.size()) {
    std::swap(A[pos], tmp);
    pos = std::find(A.cbegin(), A.cend(), pos) - A.cbegin();
    ++assigned;
}

Но тогда на каком-то этапе возникает 2 одинаковых числа в массиве и std::find находит первое из них, что, вообще говоря, неверно.