#ifndef __AMSMATHUTIL25_AMSARRAY_SORTIMPL_HPP__
#define __AMSMATHUTIL25_AMSARRAY_SORTIMPL_HPP__

namespace ams
{

void amsarray_permutation_swap(amsarray<amsarray_size_t> *permutation, amsarray_size_t I, amsarray_size_t J);


template<typename T> int amsarray_quicksort_round(
    amsarray<T> *array,                   //size N - array to sort
    amsarray<amsarray_size_t> *permarray, //size N - permutation of sorting
    ams::pair<amsarray_size_t,amsarray_size_t> range, //range over which to quicksort
    ams::pair<amsarray_size_t,amsarray_size_t> *leftrange,
    ams::pair<amsarray_size_t,amsarray_size_t> *rightrange
)
{
    int ret = 0;
    bool b1,b2;
    amsarray_size_t I,J,P;
    T v1,v2;
    amsarray_size_t tmp;
    


    b1 = range.a < 0 || range.b < 0;
    b2 = (range.b - range.a) < 2;
    if(b1 || b2)
    {
        //there is no more work to be done within this range
        *leftrange = ams::pair<amsarray_size_t,amsarray_size_t>(-1,-1);
        *rightrange = ams::pair<amsarray_size_t,amsarray_size_t>(-1,-1);
        ret = -1;
        return ret;
    }
    if((range.b - range.a) == 2)
    {
        //two element range - sort directly
        v1 = array->data[permarray->data[range.a]];
        v2 = array->data[permarray->data[range.b-1]];
        if(v2<v1)
        {
            //swap permutation indices
            amsarray_permutation_swap(permarray,range.a,range.b-1);
            // tmp = permarray->data[range.a];
            // permarray->data[range.a] = permarray->data[range.b-1];
            // permarray->data[range.b-1] = tmp;
        }
        //there is no more work to be done within this range
        *leftrange = ams::pair<amsarray_size_t,amsarray_size_t>(-1,-1);
        *rightrange = ams::pair<amsarray_size_t,amsarray_size_t>(-1,-1);
        ret = -1;
        return ret;
    }
    else
    {
        //perform quicksort round

        //choose midpoint pivot
        P = (range.a + range.b)/2;
        P = (P<range.a) ? range.a : P;
        P = (P>=range.b) ? range.b-1 : P;
        //swap pivot to end of range
        amsarray_permutation_swap(permarray,P,range.b-1);
        P = range.b-1;
        
        // amsarray_permutation_swap(permarray,P,range.a);
        // P = range.a;
        

        J = range.a;
        for(I=range.a;I<range.b-1;I++)
        {
            
            //printf("debug: I=%ld, J=%ld, P=%ld, a=%1.3f, b=%1.3f\n",I,J,P,
            //    (double)array->data[permarray->data[I]],(double)array->data[permarray->data[P]]);

            if(array->data[permarray->data[I]]<array->data[permarray->data[P]])
            {
                if(J!=I)
                {
                    //printf("debug: swap\n");
                    amsarray_permutation_swap(permarray,I,J);
                    J++;
                }
                else
                {
                    //printf("debug: skip\n");
                    J++;
                }
            }
        }

        if(array->data[permarray->data[J]]<array->data[permarray->data[P]])
        {
            J++;  
            amsarray_permutation_swap(permarray,P,J);

        }
        else
        {
            amsarray_permutation_swap(permarray,P,J);
        }
        P = J;

        if(P-range.a<=0)
        {
            leftrange->a = -1;
            leftrange->b = -1;
        }
        else
        {
            leftrange->a = range.a;
            leftrange->b = P;
        }
        if(range.b-(P+1)<=0)
        {
            rightrange->a = -1;
            rightrange->b = -1;
        }
        else
        {
            rightrange->a = P+1;
            rightrange->b = range.b;
        }

        ret = 1; //there is more work to do
    }

    return ret;
}

template<typename T> int amsarray_quicksort_subrange(
    amsarray<T> *array,                   //size N - array to sort
    amsarray<amsarray_size_t> *permarray, //size N - permutation of sorting
    ams::pair<amsarray_size_t,amsarray_size_t> _range
)
{
    int ret = amsarray_success;
    int res;

    amsarray<ams::pair<amsarray_size_t,amsarray_size_t>> ranges;
    amsarray_size_t rangeptr = 0;
    ams::pair<amsarray_size_t,amsarray_size_t> range,rangeleft,rangeright;

    ranges.append(_range);
    rangeptr = 0;
    while(rangeptr<ranges.length)
    {
        //printf("debug2:"); _debug_amsarray_print(permarray);
        range = ranges[rangeptr];
        rangeptr++;
        //printf("debug3: range=(%d,%d)\n",(int)range.a,(int)range.b);
        amsarray_quicksort_round(array,permarray,range,&rangeleft,&rangeright);
        if(rangeleft.a>=0 && rangeleft.b>rangeleft.a)
        {
            ranges.append(rangeleft);
        }
        if(rangeright.a>=0 && rangeright.b>rangeright.a)
        {
            ranges.append(rangeright);
        }
    }


    return ret;
}

template<typename T> int amsarray_quicksort_unthreaded(
    amsarray<T> *array,                   //size N - array to sort
    amsarray<amsarray_size_t> *permarray //size N - permutation of sorting
)
{
    int ret = amsarray_success;
    int res;

    amsarray<ams::pair<amsarray_size_t,amsarray_size_t>> ranges;
    amsarray_size_t rangeptr = 0;
    ams::pair<amsarray_size_t,amsarray_size_t> range,rangeleft,rangeright;

    if(permarray->length!=array->length)
    {
        *permarray = permutation_identity(array->length);
        if(permarray->length!=array->length)
        {
            ret = amsarray_failure;
            return ret;
        }
    }

    ranges.append(ams::pair<amsarray_size_t,amsarray_size_t>(0,array->length));
    rangeptr = 0;
    while(rangeptr<ranges.length)
    {
        //printf("debug2:"); _debug_amsarray_print(permarray);
        range = ranges[rangeptr];
        rangeptr++;
        //printf("debug3: range=(%d,%d)\n",(int)range.a,(int)range.b);
        amsarray_quicksort_round(array,permarray,range,&rangeleft,&rangeright);
        if(rangeleft.a>=0 && rangeleft.b>rangeleft.a)
        {
            ranges.append(rangeleft);
        }
        if(rangeright.a>=0 && rangeright.b>rangeright.a)
        {
            ranges.append(rangeright);
        }
    }

    return ret;
}

template<typename T> void amsarray_quicksort_tf(
    amsarray<T> *array,                   //size N - array to sort
    amsarray<amsarray_size_t> *permarray, //size N - permutation of sorting
    ams::pair<amsarray_size_t,amsarray_size_t> range,
    amsarray<ams::pair<amsarray_size_t,amsarray_size_t>> *ranges,
    std::mutex* threadlock
)
{
    int res;

    ams::pair<amsarray_size_t,amsarray_size_t> rangeleft,rangeright;

    if(range.b-range.a < amsarray_sortthreadpsize)
    {
        res = amsarray_quicksort_subrange(
            array,
            permarray,
            range
        );
        //there should be no work to be done after this
    }
    else
    {
        //range is too big, quicksort the pivot and supply subranges
        res = amsarray_quicksort_round(
            array,
            permarray,
            range,
            &rangeleft,
            &rangeright
        );
    
        { //scope wrapper for std::lock_guard
            std::lock_guard<std::mutex> lock(*threadlock);
            //critical section
            if(rangeleft.a>=0 && rangeleft.b>rangeleft.a)
            {
                ranges->append(rangeleft);
            }
            if(rangeright.a>=0 && rangeright.b>rangeright.a)
            {
                ranges->append(rangeright);
            }
        }
        //end critical section (end of function)
    }

    return;
}

//TODO - if the range falls below a specified size, I want to be able to run through
// quicksorting the entire range within a thread before returning


template<typename T> int amsarray_quicksort_threaded(
    amsarray<T> *array,                   //size N - array to sort
    amsarray<amsarray_size_t> *permarray //size N - permutation of sorting
)
{
    int ret = amsarray_success;
    int res;

    amsarray<ams::pair<amsarray_size_t,amsarray_size_t>> ranges;
    amsarray_size_t rangeptr = 0;
    amsarray_size_t I = 0;
    ams::pair<amsarray_size_t,amsarray_size_t> range;
    amsarray<std::thread*> threads;
    std::mutex threadlock;

    int maxthreads = std::thread::hardware_concurrency();
    maxthreads = (maxthreads < 1) ? 1 : maxthreads;
    maxthreads = (maxthreads>amsmathutil25_maxthreads) ? amsmathutil25_maxthreads : maxthreads;

    int nthreads;

    if(permarray->length!=array->length)
    {
        *permarray = permutation_identity(array->length);
        if(permarray->length!=array->length)
        {
            ret = amsarray_failure;
            return ret;
        }
    }

    threads.resize(maxthreads);
    threads.setall(NULL);
    rangeptr = 0;
    ranges.append(ams::pair<amsarray_size_t,amsarray_size_t>(0,array->length));

    while(rangeptr<ranges.length)
    {
        //spawn up to the maximum number of threads
        nthreads = ranges.length-rangeptr;
        nthreads = (nthreads>maxthreads) ? maxthreads : nthreads;

        //printf("debug: %d %d %ld %ld\n",nthreads,maxthreads,rangeptr,ranges.length);
        for(I=0;I<nthreads;I++)
        {
            threadlock.lock();
            range = ranges[rangeptr]; rangeptr++;
            threadlock.unlock();
            //printf("debug: thread %ld exec with range(%ld,%ld), rptr=%ld rlen=%ld\n",I,range.a,range.b,rangeptr,ranges.length);
            threads[I] = new(std::nothrow) std::thread(
                amsarray_quicksort_tf<T>,
                array,permarray,
                range,
                &ranges,
                &threadlock
            );
        }
        for(I=0;I<nthreads;I++)
        {
            if(threads[I]==NULL)
            {
                printf("amsarray_quicksort_threaded: error: thread %ld failed to spawn\n",I);
                ret = amsarray_failure;
            }
        }
        //printf("debug3\n");
        for(I=0;I<nthreads;I++)
        {
            if(threads[I]!=NULL)
            {
                threads[I]->join();
                delete threads[I];
                threads[I] = NULL;
            }
            //rangeptr++;
        }
    }

    return ret;
}

template<typename T> int amsarray_quicksort(
    amsarray<T> *array,                   //size N - array to sort
    amsarray<amsarray_size_t> *permarray //size N - permutation of sorting
)
{
    int ret = amsarray_success;
    int res;

    if(permarray->length!=array->length)
    {
        *permarray = permutation_identity(array->length);
        if(permarray->length!=array->length)
        {
            ret = amsarray_failure;
            return ret;
        }
    }

    if(array->length<amsarray_sortthreadpsize)
    {
        //perform unthreaded quicksort
        ret = amsarray_quicksort_unthreaded(
            array,
            permarray
        );
    }
    else
    {
        //perform threaded quicksort
        ret = amsarray_quicksort_threaded(
            array,
            permarray
        );
    }

    return ret;
}


//returns an array of indices that is a permutation which will sort
//this array in ascending order
template<typename T> amsarray<amsarray_size_t> amsarray<T>::sort_permutation()
{
    int res;
    amsarray<amsarray_size_t> ret;

    ret = permutation_identity(this->length);
    if(ret.length!=this->length)
    {
        printf("sort_permutation: error - permutation array failed to allocate.\n");
    }

    res = amsarray_quicksort(
        this,&ret
    );

    return ret;
}

//returns an array of indices that is a permutation which will sort
//this array in ascending order
template<typename T> int amsarray<T>::sort()
{
    int ret = amsarray_success;
    amsarray<amsarray_size_t> perm;
    perm = sort_permutation();
    if(perm.length==this->length)
    {
        *this = this->select(perm);
    }
    else
    {
        ret = amsarray_failure;
    }

    return ret;
}




}; //end namespace ams

#endif

