Поиск медианы списка с использованием быстрого выбора и медианы медиан

Предположим, A = [1, 2, 3, 4, 5, 6, 7, 8, 9]. Я должен найти здесь медиану, которая равна 5, и мне нужно использовать концепцию быстрого выбора и медианы медиан. Я сделал следующий код, но по какой-то причине он выводит 4, что неверно. Где я мог ошибиться?

Ниже приведены лишь некоторые вспомогательные функции, необходимые для последних функций.

int quick_select(int A[], int p, int r, int k);

void swapElements(int* i, int* j)
{
    int temp;
    temp = *i;
    *i = *j;
    *j = temp;
}
void insertion_sort(int A[], int from, int to)
{
    for(int i = from; i < to; i++) {
        for(int j = i + 1; j > from && A[j] < A[j - 1]; j--) {
            int temp = A[j - 1];
            A[j - 1] = A[j];
            A[j] = temp;
        }
    }
}

Для следующего я сделал код для медианы медиан. Это означает, что он разбивает весь массив на группы из 5 элементов и не более 1 группы, содержащей менее 5 элементов.

int median_of_medians(int A[], int p, int r){
    int N = (r-p)+1;
    int numberOfGroups = ceil((double)N/(double)5);
    int groupsOf5 = floor((double)N/(double)5);
    int lessThan5 = numberOfGroups - groupsOf5;
    int *arrayofMedians = (int*)malloc(numberOfGroups*sizeof(int));
    int rank = floor(((double)N+(double)1)/(double)2); //floor((N+1)/2)

    //sort each groups
    if(N>=5)
    {
        int ctrLeft = 0, ctrRight = 4;
        for(int j=1; j<=numberOfGroups;j++)
        {
            insertion_sort(A,ctrLeft,ctrRight);
            if(j<groupsOf5)
            {
                ctrLeft = ctrLeft + 5;
                ctrRight = ctrRight + 5;
            }
            else if(lessThan5>0)
            {
                ctrLeft = ctrRight + 1;
                ctrRight = N-1; //ctrRight+1+((N-1)%5);
            }

        }
    }
    else if(lessThan5!=0)
    {
        int ctrLeft = 0, ctrRight = N-1;
        insertion_sort(A, ctrLeft, ctrRight);

    }

    //find median from each group then put each median to arrayofMedians
    int ctr = 0;
    for(int j=0; j<numberOfGroups; j++)
    {
        if(j<groupsOf5)
        {
            arrayofMedians[ctr] = A[2+(j*5)];
            ctr++;
        }
        else
        {
            int rem = N % 5;
            if((rem % 2)==0) //if even
            {
                arrayofMedians[ctr] = A[(5*groupsOf5) + ((rem/2) - 1)];
            }
            else //if odd
            {
                arrayofMedians[ctr] = A[(5*groupsOf5) + (((rem+1)/2)-1)];
            }
            ctr++;
        }

    }

    //for(int i=0; i<=numberOfGroups-1; i++)
        //printf("%d ", arrayofMedians[i]);
    //Find median from arrayofMedians

    int finalMedian = quick_select(arrayofMedians, 0, numberOfGroups-1,rank);

    return finalMedian;
}

Теперь это та часть, где он разбивает массив, используя медиану, найденную в median-of-medians.

int median_partition(int A[], int p, int r){
    int median = median_of_medians(A, p, r);
    //find index ind of median
    int ind;
    for(int j=p; j<=r; j++)
    {
        if(A[j]==median)
            ind = j; //we found the index
    }

    swapElements(A+ind, A+r); //then swap A[ind] with A[r]


    int x = A[r];
    int i = p-1;
    for(int j=p; j<=r-1; j++)
    {
        if(A[j]<=x)
        {
            i++;
            swapElements(A+i, A+j);
        }
    }
    swapElements(A+(i+1), A+r);
    return(i+1);
}

Это функция для quick_select

int quick_select(int A[], int p, int r, int rank){
    if(p==r)
        return A[p];
    int q = median_partition(A, p, r); //median_partition(A, p, r)
    int k = q-p+1;
    if(rank==k)
        return A[q];
    else if(rank<k)
        return quick_select(A, p, q-1, rank);
    else
        return quick_select(A, q+1, r, rank-k);
}

это функция для main()

int main(){
    int T, M, *arr;
    scanf("%d", &T);
    while(T > 0){
        scanf("%d", &M);
        arr = (int*)malloc(M*sizeof(int));

        //read the elements of the input array
        for(int i=0; i<M; i++){
            scanf("%d",&arr[i]);
        }

        int median_index = ((M+1)/2);
        printf("Median: %d\n", quick_select(arr, 0, M-1, median_index));
        T--;
    }
}