// Remove slice from the middle of the sequence// !!! TODO !!! Implement more efficient algorithmCV_IMPL voidcvSeqRemoveSlice( CvSeq* seq, CvSlice slice ){    CV_FUNCNAME("cvSeqRemoveSlice");    __BEGIN__;    int total, length;    if( !CV_IS_SEQ(seq) )        CV_ERROR( CV_StsBadArg, "Invalid sequence header" );    length = cvSliceLength( slice, seq );    total = seq->total;    if( slice.start_index < 0 )        slice.start_index += total;    else if( slice.start_index >= total )        slice.start_index -= total;    if( (unsigned)slice.start_index >= (unsigned)total )        CV_ERROR( CV_StsOutOfRange, "start slice index is out of range" );    slice.end_index = slice.start_index + length;    if( slice.end_index < total )    {        CvSeqReader reader_to, reader_from;        int elem_size = seq->elem_size;        cvStartReadSeq( seq, &reader_to );        cvStartReadSeq( seq, &reader_from );        if( slice.start_index > total - slice.end_index )        {            int i, count = seq->total - slice.end_index;            cvSetSeqReaderPos( &reader_to, slice.start_index );            cvSetSeqReaderPos( &reader_from, slice.end_index );            for( i = 0; i < count; i++ )            {                CV_MEMCPY_AUTO( reader_to.ptr, reader_from.ptr, elem_size );                CV_NEXT_SEQ_ELEM( elem_size, reader_to );                CV_NEXT_SEQ_ELEM( elem_size, reader_from );            }            cvSeqPopMulti( seq, 0, slice.end_index - slice.start_index );        }        else        {            int i, count = slice.start_index;            cvSetSeqReaderPos( &reader_to, slice.end_index );            cvSetSeqReaderPos( &reader_from, slice.start_index );            for( i = 0; i < count; i++ )            {                CV_PREV_SEQ_ELEM( elem_size, reader_to );                CV_PREV_SEQ_ELEM( elem_size, reader_from );                CV_MEMCPY_AUTO( reader_to.ptr, reader_from.ptr, elem_size );            }            cvSeqPopMulti( seq, 0, slice.end_index - slice.start_index, 1 );        }    }    else    {        cvSeqPopMulti( seq, 0, total - slice.start_index );        cvSeqPopMulti( seq, 0, slice.end_index - total, 1 );    }    __END__;}// Inserts a new sequence into the middle of another sequence// !!! TODO !!! Implement more efficient algorithmCV_IMPL voidcvSeqInsertSlice( CvSeq* seq, int index, const CvArr* from_arr ){    CvSeqReader reader_to, reader_from;    int i, elem_size, total, from_total;        CV_FUNCNAME("cvSeqInsertSlice");    __BEGIN__;    CvSeq from_header, *from = (CvSeq*)from_arr;    CvSeqBlock block;    if( !CV_IS_SEQ(seq) )        CV_ERROR( CV_StsBadArg, "Invalid destination sequence header" );    if( !CV_IS_SEQ(from))    {        CvMat* mat = (CvMat*)from;        if( !CV_IS_MAT(mat))            CV_ERROR( CV_StsBadArg, "Source is not a sequence nor matrix" );        if( !CV_IS_MAT_CONT(mat->type) || (mat->rows != 1 && mat->cols != 1) )            CV_ERROR( CV_StsBadArg, "The source array must be 1d coninuous vector" );        CV_CALL( from = cvMakeSeqHeaderForArray( CV_SEQ_KIND_GENERIC, sizeof(from_header),                                                 CV_ELEM_SIZE(mat->type),                                                 mat->data.ptr, mat->cols + mat->rows - 1,                                                 &from_header, &block ));    }    if( seq->elem_size != from->elem_size )        CV_ERROR( CV_StsUnmatchedSizes,        "Sizes of source and destination sequences' elements are different" );    from_total = from->total;    if( from_total == 0 )        EXIT;    total = seq->total;    index += index < 0 ? total : 0;    index -= index > total ? total : 0;    if( (unsigned)index > (unsigned)total )        CV_ERROR( CV_StsOutOfRange, "" );    elem_size = seq->elem_size;    if( index < (total >> 1) )    {        cvSeqPushMulti( seq, 0, from_total, 1 );        cvStartReadSeq( seq, &reader_to );        cvStartReadSeq( seq, &reader_from );        cvSetSeqReaderPos( &reader_from, from_total );        for( i = 0; i < index; i++ )        {            CV_MEMCPY_AUTO( reader_to.ptr, reader_from.ptr, elem_size );            CV_NEXT_SEQ_ELEM( elem_size, reader_to );            CV_NEXT_SEQ_ELEM( elem_size, reader_from );        }    }    else    {        cvSeqPushMulti( seq, 0, from_total );        cvStartReadSeq( seq, &reader_to );        cvStartReadSeq( seq, &reader_from );        cvSetSeqReaderPos( &reader_from, total );        cvSetSeqReaderPos( &reader_to, seq->total );        for( i = 0; i < total - index; i++ )        {            CV_PREV_SEQ_ELEM( elem_size, reader_to );            CV_PREV_SEQ_ELEM( elem_size, reader_from );            CV_MEMCPY_AUTO( reader_to.ptr, reader_from.ptr, elem_size );        }    }    cvStartReadSeq( from, &reader_from );    cvSetSeqReaderPos( &reader_to, index );    for( i = 0; i < from_total; i++ )    {        CV_MEMCPY_AUTO( reader_to.ptr, reader_from.ptr, elem_size );        CV_NEXT_SEQ_ELEM( elem_size, reader_to );        CV_NEXT_SEQ_ELEM( elem_size, reader_from );    }    __END__;}// Sort the sequence using user-specified comparison function.// The semantics is similar to qsort() function. The code is based// on BSD system qsort()://    * Copyright (c) 1992, 1993//    *  The Regents of the University of California.  All rights reserved.//    *//    * Redistribution and use in source and binary forms, with or without//    * modification, are permitted provided that the following conditions//    * are met://    * 1. Redistributions of source code must retain the above copyright//    *    notice, this list of conditions and the following disclaimer.//    * 2. Redistributions in binary form must reproduce the above copyright//    *    notice, this list of conditions and the following disclaimer in the//    *    documentation and/or other materials provided with the distribution.//    * 3. All advertising materials mentioning features or use of this software//    *    must display the following acknowledgement://    *  This product includes software developed by the University of//    *  California, Berkeley and its contributors.//    * 4. Neither the name of the University nor the names of its contributors//    *    may be used to endorse or promote products derived from this software//    *    without specific prior written permission.//    *//    * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND//    * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE//    * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE//    * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE//    * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL//    * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS//    * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)//    * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT//    * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY//    * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF//    * SUCH DAMAGE.typedef struct CvSeqReaderPos{    CvSeqBlock* block;    char* ptr;    char* block_min;    char* block_max;}CvSeqReaderPos;#define CV_SAVE_READER_POS( reader, pos )   \{                                           \    (pos).block = (reader).block;           \    (pos).ptr = (reader).ptr;               \    (pos).block_min = (reader).block_min;   \    (pos).block_max = (reader).block_max;   \}#define CV_RESTORE_READER_POS( reader, pos )\{                                           \    (reader).block = (pos).block;           \    (reader).ptr = (pos).ptr;               \    (reader).block_min = (pos).block_min;   \    (reader).block_max = (pos).block_max;   \}inline char*icvMed3( char* a, char* b, char* c, CvCmpFunc cmp_func, void* aux ){    return cmp_func(a, b, aux) < 0 ?      (cmp_func(b, c, aux) < 0 ? b : cmp_func(a, c, aux) < 0 ? c : a)     :(cmp_func(b, c, aux) > 0 ? b : cmp_func(a, c, aux) < 0 ? a : c);}CV_IMPL voidcvSeqSort( CvSeq* seq, CvCmpFunc cmp_func, void* aux ){             int elem_size;    int isort_thresh = 7;    CvSeqReader left, right;    int sp = 0;    struct    {        CvSeqReaderPos lb;        CvSeqReaderPos ub;    }    stack[48];    CV_FUNCNAME( "cvSeqSort" );    __BEGIN__;        if( !CV_IS_SEQ(seq) )        CV_ERROR( !seq ? CV_StsNullPtr : CV_StsBadArg, "Bad input sequence" );    if( !cmp_func )        CV_ERROR( CV_StsNullPtr, "Null compare function" );    if( seq->total <= 1 )        EXIT;    elem_size = seq->elem_size;    isort_thresh *= elem_size;    cvStartReadSeq( seq, &left, 0 );    right = left;    CV_SAVE_READER_POS( left, stack[0].lb );    CV_PREV_SEQ_ELEM( elem_size, right );    CV_SAVE_READER_POS( right, stack[0].ub );    while( sp >= 0 )    {        CV_RESTORE_READER_POS( left, stack[sp].lb );        CV_RESTORE_READER_POS( right, stack[sp].ub );        sp--;        for(;;)        {            int i, n, m;            CvSeqReader ptr, ptr2;            if( left.block == right.block )                n = (int)(right.ptr - left.ptr) + elem_size;            else            {                n = cvGetSeqReaderPos( &right );                n = (n - cvGetSeqReaderPos( &left ) + 1)*elem_size;            }            if( n <= isort_thresh )            {            insert_sort:                ptr = ptr2 = left;                CV_NEXT_SEQ_ELEM( elem_size, ptr );                CV_NEXT_SEQ_ELEM( elem_size, right );                while( ptr.ptr != right.ptr )                {                    ptr2.ptr = ptr.ptr;                    if( ptr2.block != ptr.block )                    {                        ptr2.block = ptr.block;                        ptr2.block_min = ptr.block_min;                        ptr2.block_max = ptr.block_max;                    }                    while( ptr2.ptr != left.ptr )                    {                        char* cur = ptr2.ptr;                        CV_PREV_SEQ_ELEM( elem_size, ptr2 );                        if( cmp_func( ptr2.ptr, cur, aux ) <= 0 )                            break;                        CV_SWAP_ELEMS( ptr2.ptr, cur, elem_size );                    }                    CV_NEXT_SEQ_ELEM( elem_size, ptr );                }                break;            }            else            {                CvSeqReader left0, left1, right0, right1;                CvSeqReader tmp0, tmp1;                char *m1, *m2, *m3, *pivot;                int swap_cnt = 0;                int l, l0, l1, r, r0, r1;                left0 = tmp0 = left;                right0 = right1 = right;                n /= elem_size;                                if( n > 40 )                {                    int d = n / 8;                    char *p1, *p2, *p3;                    p1 = tmp0.ptr;                    cvSetSeqReaderPos( &tmp0, d, 1 );                    p2 = tmp0.ptr;                    cvSetSeqReaderPos( &tmp0, d, 1 );                    p3 = tmp0.ptr;                    m1 = icvMed3( p1, p2, p3, cmp_func, aux );                    cvSetSeqReaderPos( &tmp0, (n/2) - d*3, 1 );                    p1 = tmp0.ptr;                    cvSetSeqReaderPos( &tmp0, d, 1 );                    p2 = tmp0.ptr;                    cvSetSeqReaderPos( &tmp0, d, 1 );                    p3 = tmp0.ptr;                    m2 = icvMed3( p1, p2, p3, cmp_func, aux );                    cvSetSeqReaderPos( &tmp0, n - 1 - d*3 - n/2, 1 );                    p1 = tmp0.ptr;                    cvSetSeqReaderPos( &tmp0, d, 1 );                    p2 = tmp0.ptr;                    cvSetSeqReaderPos( &tmp0, d, 1 );                    p3 = tmp0.ptr;                    m3 = icvMed3( p1, p2, p3, cmp_func, aux );                }                else                {                    m1 = tmp0.ptr;                    cvSetSeqReaderPos( &tmp0, n/2, 1 );                    m2 = tmp0.ptr;                    cvSetSeqReaderPos( &tmp0, n - 1 - n/2, 1 );                    m3 = tmp0.ptr;                }                pivot = icvMed3( m1, m2, m3, cmp_func, aux );                left = left0;                if( pivot != left.ptr )                {                    CV_SWAP_ELEMS( pivot, left.ptr, elem_size );                    pivot = left.ptr;                }                CV_NEXT_SEQ_ELEM( elem_size, left );                left1 = left;                for(;;)                {                    while( left.ptr != right.ptr && (r = cmp_func(left.ptr, pivot, aux)) <= 0 )                    {                        if( r == 0 )                        {                            if( left1.ptr != left.ptr )                                CV_SWAP_ELEMS( left1.ptr, left.ptr, elem_size );                            swap_cnt = 1;                            CV_NEXT_SEQ_ELEM( elem_size, left1 );                        }                        CV_NEXT_SEQ_ELEM( elem_size, left );                    }                    while( left.ptr != right.ptr && (r = cmp_func(right.ptr,pivot, aux)) >= 0 )                    {                        if( r == 0 )                        {                            if( right1.ptr != right.ptr )                                CV_SWAP_ELEMS( right1.ptr, right.ptr, elem_size );                            swap_cnt = 1;                            CV_PREV_SEQ_ELEM( elem_size, right1 );                        }                        CV_PREV_SEQ_ELEM( elem_size, right );                    }                    if( left.ptr == right.ptr )                    {                        r = cmp_func(left.ptr, pivot, aux);                        if( r == 0 )                        {                            if( left1.ptr != left.ptr )                                CV_SWAP_ELEMS( left1.ptr, left.ptr, elem_size );                            swap_cnt = 1;                            CV_NEXT_SEQ_ELEM( elem_size, left1 );                        }                        if( r <= 0 )                        {                            CV_NEXT_SEQ_ELEM( elem_size, left );                        }                        else                        {                            CV_PREV_SEQ_ELEM( elem_size, right );                        }                        break;                    }                    CV_SWAP_ELEMS( left.ptr, right.ptr, elem_size );                    CV_NEXT_SEQ_ELEM( elem_size, left );                    r = left.ptr == right.ptr;                    CV_PREV_SEQ_ELEM( elem_size, right );                    swap_cnt = 1;                    if( r )                        break;                }                if( swap_cnt == 0 )                {                    left = left0, right = right0;                    goto insert_sort;                }                l = cvGetSeqReaderPos( &left );                if( l == 0 )                    l = seq->total;                l0 = cvGetSeqReaderPos( &left0 );                l1 = cvGetSeqReaderPos( &left1 );                if( l1 == 0 )                    l1 = seq->total;                                n = MIN( l - l1, l1 - l0 );                if( n > 0 )                {                    tmp0 = left0;                    tmp1 = left;                    cvSetSeqReaderPos( &tmp1, 0-n, 1 );                    for( i = 0; i < n; i++ )                    {                        CV_SWAP_ELEMS( tmp0.ptr, tmp1.ptr, elem_size );                        CV_NEXT_SEQ_ELEM( elem_size, tmp0 );                        CV_NEXT_SEQ_ELEM( elem_size, tmp1 );                    }                }                r = cvGetSeqReaderPos( &right );                r0 = cvGetSeqReaderPos( &right0 );                r1 = cvGetSeqReaderPos( &right1 );                m = MIN( r0 - r1, r1 - r );                if( m > 0 )                {                    tmp0 = left;                    tmp1 = right0;                    cvSetSeqReaderPos( &tmp1, 1