算法导论，找出无序数组中最中间k个数

#include <iostream>
#define swap(a,b) (a)^=(b);(b)^=(a);(a)^=(b);
using namespace std;

void output(int * input, int p, int size){
    for(int i = p ; i < size; i++){
        cout<<" "<<input[i];
    }
    cout<<endl;

}
int sort(int* A, int p, int r) {
    int j = p;
    //output(A, 10);
    for(int i = p+1; i < r; i++) {
        int insert = A[i];
        int k = j;
        for(; k >= p; k--) {
            if(A[k] > insert) {
                A[k+1] = A[k];
            } else {
                break;
            }
        }
        
        A[k+1] = insert;
        j++;
    }
    output(A, p, r);
    int mid = (r-1-p)/2;
    return A[p+mid];
}

int partition(int* A, int p, int r, int kv) {
    int main = r-1;
    
    for(int i = p; i < r; i++) {
        if(A[i]==kv) {
            main = i;
        }
    }
    cout << "main: "<<main<<endl;
    swap(A[main], A[r-1]);
    output(A, p, r);
    int i = p-1, j = p;
    
    while(j < r-1) {
        if(A[j] < A[r-1]) {
            i++;
            if(i!=j) {    
                swap(A[i], A[j]);
            }
        }
        j++;

    }
    swap(A[i+1], A[r-1]);
    output(A, 0, r);
    
    return i+1;
}

int kselect(int* A, int p, int r, int k) {
    if(r - p < 5) {
        sort(A, p, r);
        return p+k-1;
    }
    
    int groups = (r-p)/5, i = 0;
    int mid[groups+1] = {0}, midValue[groups+1] = {0};
    
    for(; i < groups; i++) {
        //cout<<p+i*5<<" "<< p+i*5+5<<endl;
        midValue[i] = sort(A, p+i*5, p+i*5+5);    //    各group中位数
    }

    if((r-p)%5 > 0) {    //    处理余数部分
        midValue[i] = sort(A, p+i*5, r);    
    }
    output(A, p, r);
    output(midValue, 0, groups+1);
    
    int middestValue = sort(midValue, 0, sizeof(midValue)/sizeof(int));
    cout << "middestValue: " << middestValue<< endl;
    int q = partition(A, p, r, middestValue);
    int num = q - p + 1;
    cout<<"q:"<<q<<" num: "<<num <<" k: "<<k<<endl;
    if(num == k) {
        return q;
    } else if(num > k) {
        return kselect(A, p, q-1, k);
    } else {
        return kselect(A, q+1, r, k-num);
    }
}

void SelectKthNearTheMiddle(int A[],int B[],int p,int r,int k) {
    int width = k/2, mid = (r-p)/2;
    int low = mid - width, high = mid + width;
    if(k%2 > 0) {
        (r-p)%2 == 0 ? high++ : low--;
    }
    cout << "low: "<<low<< " high: "<< high<< endl;
    int midIdx = kselect(A, p, r, mid);
    int lowIdx = kselect(A, p, r, low);
    int heighIdx =  kselect(A, p, r, high);
    
    output(A, 0, 10);
    for(int i = lowIdx; i < heighIdx; i++) {
        //if(i < midIdx) {
            B[i-lowIdx] = A[i];
        //} else if(i > midIdx) {
        //    B[i-lowIdx -1] =  A[i];
        //}
    }
}

int main()
{
   cout << "Hello World"<< endl;
    int input[] = {1,3,2,10,5,11, 12, 8 ,6, 7};
    int B[10] = {-1};
    //sort(input, 0, 10);
    //int mid = kselect(input, 0, sizeof(input)/sizeof(int), 8);
    //cout <<"ret:"<< mid;
    SelectKthNearTheMiddle(input, B, 0, sizeof(input)/sizeof(int), 3);
    output(B, 0, 10);
   return 0;
}