#include "search_sort_util.h"

//##########################################
//# PRINT FUNCTIONS
//##########################################

void print_uint8_t(const uint8_t* data, size_t size) {
    s_printf("%s", "");
	for(size_t i=0; i<size; i++) {
		printf("%u ", data[i]);
	}
	printf("\n");
}

void print_uint32_t(const uint32_t* data, size_t size) {
    s_printf("%s", "");
	for(size_t i=0; i<size; i++) {
		printf("%u ", data[i]);
	}
	printf("\n");
}

void print_int32_t(const int32_t* data, size_t size) {
    s_printf("%s", "");
    for(size_t i=0; i<size; i++) {
        printf("%d ", data[i]);
    }
    printf("\n");
}

void print_2d_uint8_t(const uint8_t* data, size_t size1, size_t size2) {
	for(size_t i=0; i<size1; i++) {
        s_printf("%s", "");
		for(size_t j=0; j<size2; j++) {
			printf("%u ", data[i*size2 + j]);
		}
		printf("\n");
	}	
}

void print_2d_uint32_t(const uint32_t* data, size_t size1, size_t size2) {
	for(size_t i=0; i<size1; i++) {
        s_printf("%s", "");
		for(size_t j=0; j<size2; j++) {
			printf("%u ", data[i*size2 + j]);
		}
		printf("\n");
	}	
}

void print_2d_int32_t(const int32_t* data, size_t size1, size_t size2) {
    for(size_t i=0; i<size1; i++) {
        s_printf("%s", "");
        for(size_t j=0; j<size2; j++) {
            printf("%d ", data[i*size2 + j]);
        }
        printf("\n");
    }   
}

void print_2d_int64_t(const int64_t* data, size_t size1, size_t size2) {
    for(size_t i=0; i<size1; i++) {
        s_printf("%s", "");
        for(size_t j=0; j<size2; j++) {
            printf("%" PRId64 " ", data[i*size2 + j]);
        }
        printf("\n");
    }   
}

void print_2d_mpz_t(const mpz_t* data, size_t size1, size_t size2) {
    for(size_t i=0; i<size1; i++) {
        s_printf("%s", "");
        for(size_t j=0; j<size2; j++) {
            gmp_printf("%Zd ", data[i*size2 + j]);
        }
        printf("\n");
    }   
}

void print_2d_compact_uint32_t(const uint32_t* data, size_t size1, size_t size2) {
    for(size_t i=0; i<size1; i++) {
        printf("(");
        for(size_t j=0; j<(size2-1); j++) {
            printf("%u,", data[i*size2 + j]);
        }
        printf("%u)", data[i*size2 + (size2-1)]);
    }   
}

void print_2d_compact_uint8_t(const uint8_t* data, size_t size1, size_t size2) {
    for(size_t i=0; i<size1; i++) {
        printf("(");
        for(size_t j=0; j<(size2-1); j++) {
            printf("%u,", data[i*size2 + j]);
        }
        printf("%u)", data[i*size2 + (size2-1)]);
    }   
}

void print_tableau(struct tableau * t, struct shape_data_c * s_data, int coeff, int topbar, int bottombar, int tableau_display_mode) {
    int pad = 1;
    for(int b = 0; b < s_data[0].num_boxes; b++) {
        if(t[0].entries[b] > 9 && pad < 2) {
            pad = 3;
        }
        if(t[0].entries[b] > 99 && pad < 3) {
            pad = 4;
        }
    }
    if(topbar) { s_printf("--------------------------------------------------\n"); }
    
    if(tableau_display_mode) { 
        char t_string[256];
        tableau_to_string(t, t_string, coeff, s_data, 0);
        s_printf("%s\n", t_string);
    }
    else {
        if(coeff) { s_printf("Coeff: %" PRId64 "\n", t[0].coefficient); }
        for(int r = 0; r < s_data[0].conjugate[0]; r++) {
            s_printf("%s", "");
            for(int c = 0; c < s_data[0].shape[r]; c++) {
                printf("%*u ", pad, t[0].entries[s_data[0].row_col_to_box[(r * s_data[0].shape[0]) + c]]);
            }
            printf("\n");
        }
    }
    if(bottombar) { s_printf("--------------------------------------------------\n"); }
}

void print_packed_tableau(struct packed_tableau * t, struct shape_data_c * s_data, int topbar, int bottombar, int tableau_display_mode) {
    int pad = 1;
    for(int b = 0; b < s_data[0].num_packed_boxes; b++) {
        if(t[0].entries[b] > 9 && pad < 2) {
            pad = 3;
        }
        if(t[0].entries[b] > 99 && pad < 3) {
            pad = 4;
        }
    }
    if(topbar) { s_printf("--------------------------------------------------\n"); }
    if(tableau_display_mode) {
        char t_string[256];
        packed_tableau_to_string(t, t_string, s_data);
        s_printf("%s\n", t_string);
    }
    else {
        for(int r = 0; r < s_data[0].conjugate[0]; r++) {
            s_printf("%s", "");
            for(int c = 0; c < s_data[0].shape[r]; c++) {
                printf("%*u ", pad, get_packed(t[0].entries, s_data[0].row_col_to_box[(r * s_data[0].shape[0]) + c]));
            }
            printf("\n");
        }
    }
    if(bottombar) { s_printf("--------------------------------------------------\n"); }
}

void print_straighten_result_int64_t(int64_t * straighten_result, struct sstd_data_c * sstd_data, struct shape_data_c * s_data, int index_only, int tableau_display_mode) {
    int nonzero = 0;
    int64_t temp = 0;
    for(int sstd_index = 0; sstd_index < sstd_data[0].num_sstd_tableau; sstd_index++) {
        if(straighten_result[sstd_index] != 0) {
            if(index_only) {
                s_printf("[ %d ] => %" PRId64 "\n", sstd_index, straighten_result[sstd_index]);
            }
            else {
                temp = sstd_data[0].all_sstd_tableau[sstd_index].coefficient;
                sstd_data[0].all_sstd_tableau[sstd_index].coefficient = straighten_result[sstd_index];
                print_tableau(sstd_data[0].all_sstd_tableau + sstd_index, s_data, !tableau_display_mode, !tableau_display_mode, !tableau_display_mode, tableau_display_mode);
                sstd_data[0].all_sstd_tableau[sstd_index].coefficient = temp;
            }
            nonzero++;
        }
    }
    straighten_log(STRAIGHTEN_INFO, "Total of %d terms nonzero.", nonzero);
}

/*
void print_straighten_result_bigint(mpz_t * straighten_result, struct sstd_data_c * sstd_data, struct shape_data_c * s_data, int pretty_print) {
    int nonzero = 0;
    for(int sstd_index = 0; sstd_index < sstd_data[0].num_sstd_tableau; sstd_index++) {
        if(mpz_cmp_si(straighten_result[sstd_index], 0) != 0) {
            if(pretty_print == 1) {
                s_printf("--------------------------------------------------\n");
            }
            s_printf("[ %d ] => %s\n", sstd_index, mpz_get_str(NULL, 10, straighten_result[sstd_index]));
            if(pretty_print == 1) {
                print_tableau(sstd_data[0].all_sstd_tableau + sstd_index, s_data, 0, 0, 0, 0);
                s_printf("--------------------------------------------------\n");
            }
            nonzero++;
        }
    }
    s_printf("Total of %d terms nonzero.\n", nonzero);
}
*/

//##########################################
//# TABLEAU MANIPULATION
//##########################################

void* realloc_zero(void* pBuffer, size_t oldSize, size_t newSize) {
    void* pNew = realloc(pBuffer, newSize);
    if ( newSize > oldSize && pNew ) {
        size_t diff = newSize - oldSize;
        void* pStart = ((char*)pNew) + oldSize;
        memset(pStart, 0, diff);
    }
    return pNew;
}

extern inline int8_t get_packed(const uint8_t * d, const uint8_t pos);

extern inline void set_packed(uint8_t * d, const uint8_t pos, const uint8_t val);

extern inline void set_packed_mult(uint8_t * d, const uint8_t * s, const int len);

extern inline void mpz_set_sll(mpz_t n, long long sll);

extern inline unsigned long long mpz_get_ull(mpz_t n);

extern inline long long mpz_get_sll(mpz_t n);

extern inline int factorial(int x);

void straighten_gen_permutations(uint8_t * perm, size_t size, uint8_t * storage) {           
    int p[size+1];
    register int i, j, x, n=0, tmp;
    
    for(i = 0; i < size; i++){
        p[i] = i;
    }

    p[size] = size;
    for(x = 0; x < size; x++)
        storage[n*size + x] = perm[x];
    n++;
    i = 1;
    while(i < size){
        p[i]--;    
        j = i % 2 * p[i];
        tmp = perm[j];
        perm[j] = perm[i];
        perm[i] = tmp;
        
        for(x = 0; x < size; x++)
            storage[n*size + x] = perm[x];
        n++;
        i = 1;
        while (!p[i]){
            p[i] = i;
            i++;
        }
    }
}

//##########################################
//# COMPARISON FUNCTIONS
//##########################################

extern inline int compare_uint8_lex(const uint8_t* x, const uint8_t* y, const size_t len) ;

extern inline int compare_uint8(const void* x, const void* y);

extern inline int compare_uint8_lex_s(void *arg, const void *lhs, const void *rhs);

extern inline int compare_uint8_lex_r(const void *lhs, const void *rhs, void *arg);

extern inline int sstd_tableau_colword_cmp_s(void *arg, const void *lhs, const void *rhs);

extern inline int sstd_tableau_colword_cmp_r(const void *lhs, const void *rhs, void *arg);

extern inline int sstd_tableau_rowword_cmp_rev_s(void *arg, const void *lhs, const void *rhs);

extern inline int sstd_tableau_rowword_cmp_rev_r(const void *lhs, const void *rhs, void *arg);

extern inline int sstd_tableau_rowword_single_cmp_rev_r(const void *arg1, const void *arg2, void *arg);

extern inline void * _straighten_bsearch_r(const void *key, const void *base, size_t nmemb, size_t size,
                 int (*compar) (const void *, const void *, void *), void *arg);

extern inline int straighten_bsearch_r(const void *key, const void *base, size_t nmemb, size_t size,
                  int (*compar) (const void *, const void *, void *), void *arg);



void set_tableau_bulk(struct tableau * tableau_array, size_t size, struct shape_data_c * s_data) {
    uint8_t * tableau_entries = (uint8_t*) calloc(size * s_data[0].num_boxes, sizeof(uint8_t));
    if(tableau_entries == NULL) {
        straighten_log(STRAIGHTEN_FATAL, "Could not create tableau array.");
        exit(EXIT_FAILURE);
    }
    for(int tab = 0; tab < size; tab++) {
        tableau_array[tab].entries = tableau_entries;
        tableau_entries = tableau_entries + s_data[0].num_boxes;
    }
}

void copy_tableau_bulk(struct tableau * dest, struct tableau * src, size_t size, struct shape_data_c * s_data, int contiguous) {
    if(contiguous == 1) {
        memcpy(dest[0].entries, src[0].entries, size * s_data[0].num_boxes * sizeof(uint8_t));
    }
    else {
        for(int tab = 0; tab < size; tab++) {
            memcpy(dest[tab].entries, src[tab].entries, s_data[0].num_boxes * sizeof(uint8_t));
            dest[tab].coefficient = src[tab].coefficient;
        }
    }   
}

void set_packed_tableau_bulk(struct packed_tableau * tableau_array, size_t size, struct shape_data_c * s_data) {
    uint8_t * tableau_entries = (uint8_t*) calloc(size * s_data[0].num_packed_boxes, sizeof(uint8_t));
    if(tableau_entries == NULL) {
        straighten_log(STRAIGHTEN_FATAL, "Could not create packed tableau array.");
        exit(EXIT_FAILURE);
    }
    for(int tab = 0; tab < size; tab++) {
        tableau_array[tab].entries = tableau_entries;
        tableau_entries = tableau_entries + s_data[0].num_packed_boxes;
    }
}

void copy_packed_tableau_bulk(struct packed_tableau * dest, struct packed_tableau * src, size_t size, struct shape_data_c * s_data, int contiguous) {
    if(contiguous == 1) {
        memcpy(dest[0].entries, src[0].entries, size * s_data[0].num_packed_boxes * sizeof(uint8_t));
    }
    else {
        for(int tab = 0; tab < size; tab++) {
            memcpy(dest[tab].entries, src[tab].entries, s_data[0].num_packed_boxes * sizeof(uint8_t));
        }
    }   
}

void realloc_packed_tableau_bulk_contiguous(struct packed_tableau ** tableau_array, size_t old_size, size_t new_size, struct shape_data_c * s_data) {
    tableau_array[0] = (struct packed_tableau *) realloc_zero(tableau_array[0], old_size * sizeof(struct packed_tableau), new_size * sizeof(struct packed_tableau));
    if(tableau_array[0] == NULL && new_size != 0) {
        straighten_log(STRAIGHTEN_FATAL, "Could not realloc packed tableau array.");
        exit(EXIT_FAILURE);
    }
    if(new_size != 0) {
        uint8_t * tableau_entries = (uint8_t*) realloc_zero(tableau_array[0][0].entries, old_size * s_data[0].num_packed_boxes * sizeof(uint8_t), new_size * s_data[0].num_packed_boxes * sizeof(uint8_t));
        if(tableau_entries == NULL) {
            straighten_log(STRAIGHTEN_FATAL, "Could not realloc packed tableau array entries.");
            exit(EXIT_FAILURE);
        }
        for(int tab = 0; tab < new_size; tab++) {
            tableau_array[0][tab].entries = tableau_entries;
            tableau_entries = tableau_entries + s_data[0].num_packed_boxes;
        }        
    }
}

void realloc_tableau_bulk_contiguous(struct tableau ** tableau_array, size_t old_size, size_t new_size, struct shape_data_c * s_data) {
    tableau_array[0] = (struct tableau *) realloc_zero(tableau_array[0], old_size * sizeof(struct tableau), new_size * sizeof(struct tableau));
    if(tableau_array[0] == NULL && new_size != 0) {
        straighten_log(STRAIGHTEN_FATAL, "Could not realloc tableau array.");
        exit(EXIT_FAILURE);
    }
    if(new_size != 0) {
        uint8_t * tableau_entries = (uint8_t*) realloc_zero(tableau_array[0][0].entries, old_size * s_data[0].num_boxes * sizeof(uint8_t), new_size * s_data[0].num_boxes * sizeof(uint8_t));
        if(tableau_entries == NULL) {
            straighten_log(STRAIGHTEN_FATAL, "Could not realloc tableau array entries.");
            exit(EXIT_FAILURE);
        }
        for(int tab = 0; tab < new_size; tab++) {
            tableau_array[0][tab].entries = tableau_entries;
            tableau_entries = tableau_entries + s_data[0].num_boxes;
        }
    }
}


int32_t shape_to_string(uint32_t * shape, uint32_t shape_length, char * shape_string) {
    int32_t index = 0;
    index += sprintf(&shape_string[index], "[");
    for(int entry = 0; entry < shape_length; entry++) {
        index += sprintf(&shape_string[index], "%u,", shape[entry]);
    }
    index--; //removes the trailing comma
    index += sprintf(&shape_string[index], "]");
    return index;
}

int32_t string_to_shape(uint32_t ** shape, uint32_t * shape_length, char * shape_string) {
    uint32_t max_size = 32;
    shape[0] = (uint32_t*) calloc(max_size, sizeof(uint32_t));
    shape_length[0] = 0;
    char * token;
    char * rest = NULL;
    
    uint32_t value = 0;

    token = strtok_r(shape_string,"([,])\n", &rest);
    while (token != NULL) {
        value = atoi(token);

        // this will be zero if token was not a number
        if (value == 0) {
            straighten_log(STRAIGHTEN_ERROR, "The supplied shape is formatted incorrectly.");
            return -1;
        }
        shape[0][shape_length[0]] = value;
        shape_length[0]++;
        token = strtok_r(NULL, "([,])\n", &rest);
        if(shape_length[0] == max_size) {
            straighten_log(STRAIGHTEN_ERROR, "The supplied shape has length greater than 32, this is not currently supported.");
            return -1;
        }
    }
    shape[0] = realloc(shape[0], shape_length[0] * sizeof(uint32_t));
    return ((shape_length[0] == 0) ? -1 : 1);
}

int32_t string_to_content(uint8_t ** content, uint8_t * content_length, char * shape_string) {
    uint32_t max_size = 128;
    content[0] = (uint8_t*) calloc(max_size, sizeof(uint8_t));
    content_length[0] = 0;
    char * token;
    char * rest = NULL;
    
    uint32_t value = 0;

    token = strtok_r(shape_string,"([,])\n", &rest);
    while (token != NULL) {
        value = atoi(token);

        // this will be zero if token was not a number
        if (value == 0) {
            straighten_log(STRAIGHTEN_ERROR, "The supplied content is formatted incorrectly.");
            return -1;
        }
        content[0][content_length[0]] = value;
        content_length[0]++;
        token = strtok_r(NULL, "([,])\n", &rest);
        if(content_length[0] == max_size) {
            straighten_log(STRAIGHTEN_ERROR, "The supplied content has length greater than 128, this is not currently supported.");
            return -1;
        }
    }
    content[0] = realloc(content[0], content_length[0] * sizeof(uint8_t));
    return ((content_length[0] == 0) ? -1 : 1);
}

int32_t get_tableau_content(uint8_t ** content, uint8_t * content_length, struct tableau * tab, struct shape_data_c * s_data) {
    content_length[0] = 0;
    for(int box = 0; box < s_data[0].num_boxes; box++) {
        if(tab[0].entries[box] > content_length[0]) {
            content_length[0] = tab[0].entries[box];
        }
    }
    content[0] = (uint8_t*)calloc(content_length[0], sizeof(uint8_t));
    for(int box = 0; box < s_data[0].num_boxes; box++) {
        content[0][tab[0].entries[box] - 1]++;
    }
    return 1;
}

int32_t tableau_to_string(struct tableau * tableau_arr, char * shape_string, int32_t coeff, struct shape_data_c * s_data, int row_word) {
    int32_t index = 0;
    if(coeff) {
        index += sprintf(&shape_string[index], "(");
    }
    index += sprintf(&shape_string[index], "[");
    for(int entry = 0; entry < s_data[0].num_boxes; entry++) {
        if(row_word) {
            index += sprintf(&shape_string[index], "%u,", tableau_arr[0].entries[s_data[0].roworder_to_box[entry]]);
        }
        else {
            index += sprintf(&shape_string[index], "%u,", tableau_arr[0].entries[entry]);
        }
    }
    index--; //removes the trailing comma
    index += sprintf(&shape_string[index], "]");
    if(coeff) {
        index += sprintf(&shape_string[index], ",%" PRId64 ")", tableau_arr[0].coefficient);
    }
    return index;
}

int32_t packed_tableau_to_string(struct packed_tableau * tableau_arr, char * shape_string, struct shape_data_c * s_data) {
    int32_t index = 0;
    index += sprintf(&shape_string[index], "[");
    for(int entry = 0; entry < s_data[0].num_boxes; entry++) {
        index += sprintf(&shape_string[index], "%u,", get_packed(tableau_arr[0].entries, entry));
    }
    index--; //removes the trailing comma
    index += sprintf(&shape_string[index], "]");
    return index;
}

int32_t string_to_tableau(struct tableau ** tableau_arr, uint32_t * arr_max_length, uint32_t * arr_pos, char * shape_string, struct shape_data_c * s_data) {
    if(tableau_arr[0] == NULL) {
        arr_max_length[0] = 8;
        tableau_arr[0] = (struct tableau*) calloc(arr_max_length[0], sizeof(struct tableau));
        set_tableau_bulk(tableau_arr[0], arr_max_length[0], s_data);
    }
    if(arr_pos[0] > arr_max_length[0]) {
        int old_size = arr_max_length[0];
        while(arr_pos[0] > arr_max_length[0]) {
            arr_max_length[0] = arr_max_length[0] * 2;
        }
        realloc_tableau_bulk_contiguous(tableau_arr, old_size, arr_max_length[0], s_data);
    }

    char * outer_rest = NULL;
    char * inner_rest = NULL;
    char * outer_token;
    char * inner_token;
    
    uint32_t entry = 0;

    outer_token = strtok_r(shape_string, "([])\n", &outer_rest);

    inner_token = strtok_r(outer_token, "([,])\n", &inner_rest);
    while(inner_token != NULL) {
        tableau_arr[0][arr_pos[0]].entries[entry] = atoi(inner_token);
        if(tableau_arr[0][arr_pos[0]].entries[entry] < 1) {
            straighten_log(STRAIGHTEN_ERROR, "The values in the tableau must be between greater than 1");
            return -1;
        }
        entry++;
        inner_token = strtok_r(NULL, "([,])\n", &inner_rest);
        if(entry == s_data[0].num_boxes && inner_token != NULL) {
            straighten_log(STRAIGHTEN_ERROR, "The supplied tableau has more boxes than the shape provided.");
            return -1;
        }
    }

    if(entry != s_data[0].num_boxes) {
        straighten_log(STRAIGHTEN_ERROR, "The supplied tableau has less boxes than the shape provided.");
        return -1;
    }
    outer_token = strtok_r(NULL, "([])\n", &outer_rest);

    if(outer_token != NULL) {
        inner_token = strtok_r(outer_token, "([,])\n", &inner_rest);
        if(inner_token == NULL) {
            tableau_arr[0][arr_pos[0]].coefficient = 1;
        }
        else {
            tableau_arr[0][arr_pos[0]].coefficient = atoi(inner_token);
        }
    }
    else {
        tableau_arr[0][arr_pos[0]].coefficient = 1;
    }

    return 1;
}

int32_t file_to_tableau(struct tableau ** tableau_arr, uint32_t * arr_max_length, uint32_t * arr_pos, char * filename, struct shape_data_c * s_data) {
    FILE *in_file = fopen(filename, "r");
    if(in_file == NULL) {
        straighten_log(STRAIGHTEN_VVINFO, "Could not open file %s.", filename);
        return -1;
    }
    
    int res;
    char line[2048];
    int32_t line_number = 0;
    while (fgets(line, sizeof(line), in_file)) {
        if (line[0] != '\n') {
            res = string_to_tableau(tableau_arr, arr_max_length, arr_pos, line, s_data);
            if(res == -1) {
                straighten_log(STRAIGHTEN_ERROR, "The tableau supplied in %s on line number %d is not in the correct format.", filename, line_number);
                return -1;
            }
            arr_pos[0]++;
        }
        line_number++;
    }

    return 1;
}

//##########################################
//# SORT FUNCTIONS
//##########################################

extern inline int signed_sort2_fast(uint8_t * d);

extern inline int signed_sort3_fast(uint8_t * d);

extern inline int signed_sort4_fast(uint8_t * d);

extern inline int signed_sort5_fast(uint8_t * d);

extern inline int signed_sort6_fast(uint8_t * d);

extern inline int signed_sort7_fast(uint8_t * d);

extern inline int signed_sort_fast(uint8_t * d, size_t start, uint8_t len);

extern inline void sort2_fast(uint8_t * d);

extern inline void sort3_fast(uint8_t * d);

extern inline void sort4_fast(uint8_t * d);

extern inline void sort5_fast(uint8_t * d);

extern inline void sort6_fast(uint8_t * d);

extern inline void sort7_fast(uint8_t * d);

extern inline void sort8_fast(uint8_t * d);

extern inline void sort9_fast(uint8_t * d);

extern inline void sort10_fast(uint8_t * d);

extern inline void sort11_fast(uint8_t * d);

extern inline void sort12_fast(uint8_t * d);

extern inline void sort13_fast(uint8_t * d);

extern inline void sort14_fast(uint8_t * d);

extern inline void sort15_fast(uint8_t * d);

extern inline void sort16_fast(uint8_t * d);

extern inline void sort17_fast(uint8_t * d);

extern inline void sort18_fast(uint8_t * d);

extern inline void sort19_fast(uint8_t * d);

extern inline void sort20_fast(uint8_t * d);

extern inline void sort21_fast(uint8_t * d);

extern inline void sort22_fast(uint8_t * d);

void sort23_fast(uint8_t * d) {
#define min(a,b) ((a<b) ? a : b )
#define max(a,b) ((a<b) ? b : a )
#define SWAP(x,y) { int tmp = min(d[x], d[y]); d[y] = max(d[x], d[y]); d[x] = tmp; }
//#define SWAP(x,y) { int dx = d[x], dy = d[y], tmp; tmp = d[x] = ((dx < dy) ? dx : dy); d[y] ^= dx ^ tmp; }
    SWAP(0, 16);
    SWAP(1, 17);
    SWAP(2, 18);
    SWAP(3, 19);
    SWAP(4, 20);
    SWAP(5, 21);
    SWAP(6, 22);
    SWAP(0, 8);
    SWAP(1, 9);
    SWAP(2, 10);
    SWAP(3, 11);
    SWAP(4, 12);
    SWAP(5, 13);
    SWAP(6, 14);
    SWAP(7, 15);
    SWAP(8, 16);
    SWAP(9, 17);
    SWAP(10, 18);
    SWAP(11, 19);
    SWAP(12, 20);
    SWAP(13, 21);
    SWAP(14, 22);
    SWAP(0, 4);
    SWAP(1, 5);
    SWAP(2, 6);
    SWAP(3, 7);
    SWAP(8, 12);
    SWAP(9, 13);
    SWAP(10, 14);
    SWAP(11, 15);
    SWAP(16, 20);
    SWAP(17, 21);
    SWAP(18, 22);
    SWAP(4, 16);
    SWAP(5, 17);
    SWAP(6, 18);
    SWAP(7, 19);
    SWAP(4, 8);
    SWAP(5, 9);
    SWAP(6, 10);
    SWAP(7, 11);
    SWAP(12, 16);
    SWAP(13, 17);
    SWAP(14, 18);
    SWAP(15, 19);
    SWAP(0, 2);
    SWAP(1, 3);
    SWAP(4, 6);
    SWAP(5, 7);
    SWAP(8, 10);
    SWAP(9, 11);
    SWAP(12, 14);
    SWAP(13, 15);
    SWAP(16, 18);
    SWAP(17, 19);
    SWAP(20, 22);
    SWAP(2, 16);
    SWAP(3, 17);
    SWAP(6, 20);
    SWAP(7, 21);
    SWAP(2, 8);
    SWAP(3, 9);
    SWAP(6, 12);
    SWAP(7, 13);
    SWAP(10, 16);
    SWAP(11, 17);
    SWAP(14, 20);
    SWAP(15, 21);
    SWAP(2, 4);
    SWAP(3, 5);
    SWAP(6, 8);
    SWAP(7, 9);
    SWAP(10, 12);
    SWAP(11, 13);
    SWAP(14, 16);
    SWAP(15, 17);
    SWAP(18, 20);
    SWAP(19, 21);
    SWAP(0, 1);
    SWAP(2, 3);
    SWAP(4, 5);
    SWAP(6, 7);
    SWAP(8, 9);
    SWAP(10, 11);
    SWAP(12, 13);
    SWAP(14, 15);
    SWAP(16, 17);
    SWAP(18, 19);
    SWAP(20, 21);
    SWAP(1, 16);
    SWAP(3, 18);
    SWAP(5, 20);
    SWAP(7, 22);
    SWAP(1, 8);
    SWAP(3, 10);
    SWAP(5, 12);
    SWAP(7, 14);
    SWAP(9, 16);
    SWAP(11, 18);
    SWAP(13, 20);
    SWAP(15, 22);
    SWAP(1, 4);
    SWAP(3, 6);
    SWAP(5, 8);
    SWAP(7, 10);
    SWAP(9, 12);
    SWAP(11, 14);
    SWAP(13, 16);
    SWAP(15, 18);
    SWAP(17, 20);
    SWAP(19, 22);
    SWAP(1, 2);
    SWAP(3, 4);
    SWAP(5, 6);
    SWAP(7, 8);
    SWAP(9, 10);
    SWAP(11, 12);
    SWAP(13, 14);
    SWAP(15, 16);
    SWAP(17, 18);
    SWAP(19, 20);
    SWAP(21, 22);
    
#undef min
#undef max
#undef SWAP
}

void sort24_fast(uint8_t * d) {
#define min(a,b) ((a<b) ? a : b )
#define max(a,b) ((a<b) ? b : a )
#define SWAP(x,y) { int tmp = min(d[x], d[y]); d[y] = max(d[x], d[y]); d[x] = tmp; }
//#define SWAP(x,y) { int dx = d[x], dy = d[y], tmp; tmp = d[x] = ((dx < dy) ? dx : dy); d[y] ^= dx ^ tmp; }
    SWAP(0, 16);
    SWAP(1, 17);
    SWAP(2, 18);
    SWAP(3, 19);
    SWAP(4, 20);
    SWAP(5, 21);
    SWAP(6, 22);
    SWAP(7, 23);
    SWAP(0, 8);
    SWAP(1, 9);
    SWAP(2, 10);
    SWAP(3, 11);
    SWAP(4, 12);
    SWAP(5, 13);
    SWAP(6, 14);
    SWAP(7, 15);
    SWAP(8, 16);
    SWAP(9, 17);
    SWAP(10, 18);
    SWAP(11, 19);
    SWAP(12, 20);
    SWAP(13, 21);
    SWAP(14, 22);
    SWAP(15, 23);
    SWAP(0, 4);
    SWAP(1, 5);
    SWAP(2, 6);
    SWAP(3, 7);
    SWAP(8, 12);
    SWAP(9, 13);
    SWAP(10, 14);
    SWAP(11, 15);
    SWAP(16, 20);
    SWAP(17, 21);
    SWAP(18, 22);
    SWAP(19, 23);
    SWAP(4, 16);
    SWAP(5, 17);
    SWAP(6, 18);
    SWAP(7, 19);
    SWAP(4, 8);
    SWAP(5, 9);
    SWAP(6, 10);
    SWAP(7, 11);
    SWAP(12, 16);
    SWAP(13, 17);
    SWAP(14, 18);
    SWAP(15, 19);
    SWAP(0, 2);
    SWAP(1, 3);
    SWAP(4, 6);
    SWAP(5, 7);
    SWAP(8, 10);
    SWAP(9, 11);
    SWAP(12, 14);
    SWAP(13, 15);
    SWAP(16, 18);
    SWAP(17, 19);
    SWAP(20, 22);
    SWAP(21, 23);
    SWAP(2, 16);
    SWAP(3, 17);
    SWAP(6, 20);
    SWAP(7, 21);
    SWAP(2, 8);
    SWAP(3, 9);
    SWAP(6, 12);
    SWAP(7, 13);
    SWAP(10, 16);
    SWAP(11, 17);
    SWAP(14, 20);
    SWAP(15, 21);
    SWAP(2, 4);
    SWAP(3, 5);
    SWAP(6, 8);
    SWAP(7, 9);
    SWAP(10, 12);
    SWAP(11, 13);
    SWAP(14, 16);
    SWAP(15, 17);
    SWAP(18, 20);
    SWAP(19, 21);
    SWAP(0, 1);
    SWAP(2, 3);
    SWAP(4, 5);
    SWAP(6, 7);
    SWAP(8, 9);
    SWAP(10, 11);
    SWAP(12, 13);
    SWAP(14, 15);
    SWAP(16, 17);
    SWAP(18, 19);
    SWAP(20, 21);
    SWAP(22, 23);
    SWAP(1, 16);
    SWAP(3, 18);
    SWAP(5, 20);
    SWAP(7, 22);
    SWAP(1, 8);
    SWAP(3, 10);
    SWAP(5, 12);
    SWAP(7, 14);
    SWAP(9, 16);
    SWAP(11, 18);
    SWAP(13, 20);
    SWAP(15, 22);
    SWAP(1, 4);
    SWAP(3, 6);
    SWAP(5, 8);
    SWAP(7, 10);
    SWAP(9, 12);
    SWAP(11, 14);
    SWAP(13, 16);
    SWAP(15, 18);
    SWAP(17, 20);
    SWAP(19, 22);
    SWAP(1, 2);
    SWAP(3, 4);
    SWAP(5, 6);
    SWAP(7, 8);
    SWAP(9, 10);
    SWAP(11, 12);
    SWAP(13, 14);
    SWAP(15, 16);
    SWAP(17, 18);
    SWAP(19, 20);
    SWAP(21, 22);
    
#undef min
#undef max
#undef SWAP
}

void sort25_fast(uint8_t * d) {
#define min(a,b) ((a<b) ? a : b )
#define max(a,b) ((a<b) ? b : a )
#define SWAP(x,y) { int tmp = min(d[x], d[y]); d[y] = max(d[x], d[y]); d[x] = tmp; }
//#define SWAP(x,y) { int dx = d[x], dy = d[y], tmp; tmp = d[x] = ((dx < dy) ? dx : dy); d[y] ^= dx ^ tmp; }
    SWAP(0, 16);
    SWAP(1, 17);
    SWAP(2, 18);
    SWAP(3, 19);
    SWAP(4, 20);
    SWAP(5, 21);
    SWAP(6, 22);
    SWAP(7, 23);
    SWAP(8, 24);
    SWAP(0, 8);
    SWAP(1, 9);
    SWAP(2, 10);
    SWAP(3, 11);
    SWAP(4, 12);
    SWAP(5, 13);
    SWAP(6, 14);
    SWAP(7, 15);
    SWAP(16, 24);
    SWAP(8, 16);
    SWAP(9, 17);
    SWAP(10, 18);
    SWAP(11, 19);
    SWAP(12, 20);
    SWAP(13, 21);
    SWAP(14, 22);
    SWAP(15, 23);
    SWAP(0, 4);
    SWAP(1, 5);
    SWAP(2, 6);
    SWAP(3, 7);
    SWAP(8, 12);
    SWAP(9, 13);
    SWAP(10, 14);
    SWAP(11, 15);
    SWAP(16, 20);
    SWAP(17, 21);
    SWAP(18, 22);
    SWAP(19, 23);
    SWAP(4, 16);
    SWAP(5, 17);
    SWAP(6, 18);
    SWAP(7, 19);
    SWAP(12, 24);
    SWAP(4, 8);
    SWAP(5, 9);
    SWAP(6, 10);
    SWAP(7, 11);
    SWAP(12, 16);
    SWAP(13, 17);
    SWAP(14, 18);
    SWAP(15, 19);
    SWAP(20, 24);
    SWAP(0, 2);
    SWAP(1, 3);
    SWAP(4, 6);
    SWAP(5, 7);
    SWAP(8, 10);
    SWAP(9, 11);
    SWAP(12, 14);
    SWAP(13, 15);
    SWAP(16, 18);
    SWAP(17, 19);
    SWAP(20, 22);
    SWAP(21, 23);
    SWAP(2, 16);
    SWAP(3, 17);
    SWAP(6, 20);
    SWAP(7, 21);
    SWAP(10, 24);
    SWAP(2, 8);
    SWAP(3, 9);
    SWAP(6, 12);
    SWAP(7, 13);
    SWAP(10, 16);
    SWAP(11, 17);
    SWAP(14, 20);
    SWAP(15, 21);
    SWAP(18, 24);
    SWAP(2, 4);
    SWAP(3, 5);
    SWAP(6, 8);
    SWAP(7, 9);
    SWAP(10, 12);
    SWAP(11, 13);
    SWAP(14, 16);
    SWAP(15, 17);
    SWAP(18, 20);
    SWAP(19, 21);
    SWAP(22, 24);
    SWAP(0, 1);
    SWAP(2, 3);
    SWAP(4, 5);
    SWAP(6, 7);
    SWAP(8, 9);
    SWAP(10, 11);
    SWAP(12, 13);
    SWAP(14, 15);
    SWAP(16, 17);
    SWAP(18, 19);
    SWAP(20, 21);
    SWAP(22, 23);
    SWAP(1, 16);
    SWAP(3, 18);
    SWAP(5, 20);
    SWAP(7, 22);
    SWAP(9, 24);
    SWAP(1, 8);
    SWAP(3, 10);
    SWAP(5, 12);
    SWAP(7, 14);
    SWAP(9, 16);
    SWAP(11, 18);
    SWAP(13, 20);
    SWAP(15, 22);
    SWAP(17, 24);
    SWAP(1, 4);
    SWAP(3, 6);
    SWAP(5, 8);
    SWAP(7, 10);
    SWAP(9, 12);
    SWAP(11, 14);
    SWAP(13, 16);
    SWAP(15, 18);
    SWAP(17, 20);
    SWAP(19, 22);
    SWAP(21, 24);
    SWAP(1, 2);
    SWAP(3, 4);
    SWAP(5, 6);
    SWAP(7, 8);
    SWAP(9, 10);
    SWAP(11, 12);
    SWAP(13, 14);
    SWAP(15, 16);
    SWAP(17, 18);
    SWAP(19, 20);
    SWAP(21, 22);
    SWAP(23, 24);
    
#undef min
#undef max
#undef SWAP
}

void sort26_fast(uint8_t * d) {
#define min(a,b) ((a<b) ? a : b )
#define max(a,b) ((a<b) ? b : a )
#define SWAP(x,y) { int tmp = min(d[x], d[y]); d[y] = max(d[x], d[y]); d[x] = tmp; }
//#define SWAP(x,y) { int dx = d[x], dy = d[y], tmp; tmp = d[x] = ((dx < dy) ? dx : dy); d[y] ^= dx ^ tmp; }
    SWAP(0, 16);
    SWAP(1, 17);
    SWAP(2, 18);
    SWAP(3, 19);
    SWAP(4, 20);
    SWAP(5, 21);
    SWAP(6, 22);
    SWAP(7, 23);
    SWAP(8, 24);
    SWAP(9, 25);
    SWAP(0, 8);
    SWAP(1, 9);
    SWAP(2, 10);
    SWAP(3, 11);
    SWAP(4, 12);
    SWAP(5, 13);
    SWAP(6, 14);
    SWAP(7, 15);
    SWAP(16, 24);
    SWAP(17, 25);
    SWAP(8, 16);
    SWAP(9, 17);
    SWAP(10, 18);
    SWAP(11, 19);
    SWAP(12, 20);
    SWAP(13, 21);
    SWAP(14, 22);
    SWAP(15, 23);
    SWAP(0, 4);
    SWAP(1, 5);
    SWAP(2, 6);
    SWAP(3, 7);
    SWAP(8, 12);
    SWAP(9, 13);
    SWAP(10, 14);
    SWAP(11, 15);
    SWAP(16, 20);
    SWAP(17, 21);
    SWAP(18, 22);
    SWAP(19, 23);
    SWAP(4, 16);
    SWAP(5, 17);
    SWAP(6, 18);
    SWAP(7, 19);
    SWAP(12, 24);
    SWAP(13, 25);
    SWAP(4, 8);
    SWAP(5, 9);
    SWAP(6, 10);
    SWAP(7, 11);
    SWAP(12, 16);
    SWAP(13, 17);
    SWAP(14, 18);
    SWAP(15, 19);
    SWAP(20, 24);
    SWAP(21, 25);
    SWAP(0, 2);
    SWAP(1, 3);
    SWAP(4, 6);
    SWAP(5, 7);
    SWAP(8, 10);
    SWAP(9, 11);
    SWAP(12, 14);
    SWAP(13, 15);
    SWAP(16, 18);
    SWAP(17, 19);
    SWAP(20, 22);
    SWAP(21, 23);
    SWAP(2, 16);
    SWAP(3, 17);
    SWAP(6, 20);
    SWAP(7, 21);
    SWAP(10, 24);
    SWAP(11, 25);
    SWAP(2, 8);
    SWAP(3, 9);
    SWAP(6, 12);
    SWAP(7, 13);
    SWAP(10, 16);
    SWAP(11, 17);
    SWAP(14, 20);
    SWAP(15, 21);
    SWAP(18, 24);
    SWAP(19, 25);
    SWAP(2, 4);
    SWAP(3, 5);
    SWAP(6, 8);
    SWAP(7, 9);
    SWAP(10, 12);
    SWAP(11, 13);
    SWAP(14, 16);
    SWAP(15, 17);
    SWAP(18, 20);
    SWAP(19, 21);
    SWAP(22, 24);
    SWAP(23, 25);
    SWAP(0, 1);
    SWAP(2, 3);
    SWAP(4, 5);
    SWAP(6, 7);
    SWAP(8, 9);
    SWAP(10, 11);
    SWAP(12, 13);
    SWAP(14, 15);
    SWAP(16, 17);
    SWAP(18, 19);
    SWAP(20, 21);
    SWAP(22, 23);
    SWAP(24, 25);
    SWAP(1, 16);
    SWAP(3, 18);
    SWAP(5, 20);
    SWAP(7, 22);
    SWAP(9, 24);
    SWAP(1, 8);
    SWAP(3, 10);
    SWAP(5, 12);
    SWAP(7, 14);
    SWAP(9, 16);
    SWAP(11, 18);
    SWAP(13, 20);
    SWAP(15, 22);
    SWAP(17, 24);
    SWAP(1, 4);
    SWAP(3, 6);
    SWAP(5, 8);
    SWAP(7, 10);
    SWAP(9, 12);
    SWAP(11, 14);
    SWAP(13, 16);
    SWAP(15, 18);
    SWAP(17, 20);
    SWAP(19, 22);
    SWAP(21, 24);
    SWAP(1, 2);
    SWAP(3, 4);
    SWAP(5, 6);
    SWAP(7, 8);
    SWAP(9, 10);
    SWAP(11, 12);
    SWAP(13, 14);
    SWAP(15, 16);
    SWAP(17, 18);
    SWAP(19, 20);
    SWAP(21, 22);
    SWAP(23, 24);
    
#undef min
#undef max
#undef SWAP
}

void sort27_fast(uint8_t * d) {
#define min(a,b) ((a<b) ? a : b )
#define max(a,b) ((a<b) ? b : a )
#define SWAP(x,y) { int tmp = min(d[x], d[y]); d[y] = max(d[x], d[y]); d[x] = tmp; }
//#define SWAP(x,y) { int dx = d[x], dy = d[y], tmp; tmp = d[x] = ((dx < dy) ? dx : dy); d[y] ^= dx ^ tmp; }
    SWAP(0, 16);
    SWAP(1, 17);
    SWAP(2, 18);
    SWAP(3, 19);
    SWAP(4, 20);
    SWAP(5, 21);
    SWAP(6, 22);
    SWAP(7, 23);
    SWAP(8, 24);
    SWAP(9, 25);
    SWAP(10, 26);
    SWAP(0, 8);
    SWAP(1, 9);
    SWAP(2, 10);
    SWAP(3, 11);
    SWAP(4, 12);
    SWAP(5, 13);
    SWAP(6, 14);
    SWAP(7, 15);
    SWAP(16, 24);
    SWAP(17, 25);
    SWAP(18, 26);
    SWAP(8, 16);
    SWAP(9, 17);
    SWAP(10, 18);
    SWAP(11, 19);
    SWAP(12, 20);
    SWAP(13, 21);
    SWAP(14, 22);
    SWAP(15, 23);
    SWAP(0, 4);
    SWAP(1, 5);
    SWAP(2, 6);
    SWAP(3, 7);
    SWAP(8, 12);
    SWAP(9, 13);
    SWAP(10, 14);
    SWAP(11, 15);
    SWAP(16, 20);
    SWAP(17, 21);
    SWAP(18, 22);
    SWAP(19, 23);
    SWAP(4, 16);
    SWAP(5, 17);
    SWAP(6, 18);
    SWAP(7, 19);
    SWAP(12, 24);
    SWAP(13, 25);
    SWAP(14, 26);
    SWAP(4, 8);
    SWAP(5, 9);
    SWAP(6, 10);
    SWAP(7, 11);
    SWAP(12, 16);
    SWAP(13, 17);
    SWAP(14, 18);
    SWAP(15, 19);
    SWAP(20, 24);
    SWAP(21, 25);
    SWAP(22, 26);
    SWAP(0, 2);
    SWAP(1, 3);
    SWAP(4, 6);
    SWAP(5, 7);
    SWAP(8, 10);
    SWAP(9, 11);
    SWAP(12, 14);
    SWAP(13, 15);
    SWAP(16, 18);
    SWAP(17, 19);
    SWAP(20, 22);
    SWAP(21, 23);
    SWAP(24, 26);
    SWAP(2, 16);
    SWAP(3, 17);
    SWAP(6, 20);
    SWAP(7, 21);
    SWAP(10, 24);
    SWAP(11, 25);
    SWAP(2, 8);
    SWAP(3, 9);
    SWAP(6, 12);
    SWAP(7, 13);
    SWAP(10, 16);
    SWAP(11, 17);
    SWAP(14, 20);
    SWAP(15, 21);
    SWAP(18, 24);
    SWAP(19, 25);
    SWAP(2, 4);
    SWAP(3, 5);
    SWAP(6, 8);
    SWAP(7, 9);
    SWAP(10, 12);
    SWAP(11, 13);
    SWAP(14, 16);
    SWAP(15, 17);
    SWAP(18, 20);
    SWAP(19, 21);
    SWAP(22, 24);
    SWAP(23, 25);
    SWAP(0, 1);
    SWAP(2, 3);
    SWAP(4, 5);
    SWAP(6, 7);
    SWAP(8, 9);
    SWAP(10, 11);
    SWAP(12, 13);
    SWAP(14, 15);
    SWAP(16, 17);
    SWAP(18, 19);
    SWAP(20, 21);
    SWAP(22, 23);
    SWAP(24, 25);
    SWAP(1, 16);
    SWAP(3, 18);
    SWAP(5, 20);
    SWAP(7, 22);
    SWAP(9, 24);
    SWAP(11, 26);
    SWAP(1, 8);
    SWAP(3, 10);
    SWAP(5, 12);
    SWAP(7, 14);
    SWAP(9, 16);
    SWAP(11, 18);
    SWAP(13, 20);
    SWAP(15, 22);
    SWAP(17, 24);
    SWAP(19, 26);
    SWAP(1, 4);
    SWAP(3, 6);
    SWAP(5, 8);
    SWAP(7, 10);
    SWAP(9, 12);
    SWAP(11, 14);
    SWAP(13, 16);
    SWAP(15, 18);
    SWAP(17, 20);
    SWAP(19, 22);
    SWAP(21, 24);
    SWAP(23, 26);
    SWAP(1, 2);
    SWAP(3, 4);
    SWAP(5, 6);
    SWAP(7, 8);
    SWAP(9, 10);
    SWAP(11, 12);
    SWAP(13, 14);
    SWAP(15, 16);
    SWAP(17, 18);
    SWAP(19, 20);
    SWAP(21, 22);
    SWAP(23, 24);
    SWAP(25, 26);
    
#undef min
#undef max
#undef SWAP
}

void sort28_fast(uint8_t * d) {
#define min(a,b) ((a<b) ? a : b )
#define max(a,b) ((a<b) ? b : a )
#define SWAP(x,y) { int tmp = min(d[x], d[y]); d[y] = max(d[x], d[y]); d[x] = tmp; }
//#define SWAP(x,y) { int dx = d[x], dy = d[y], tmp; tmp = d[x] = ((dx < dy) ? dx : dy); d[y] ^= dx ^ tmp; }
    SWAP(0, 16);
    SWAP(1, 17);
    SWAP(2, 18);
    SWAP(3, 19);
    SWAP(4, 20);
    SWAP(5, 21);
    SWAP(6, 22);
    SWAP(7, 23);
    SWAP(8, 24);
    SWAP(9, 25);
    SWAP(10, 26);
    SWAP(11, 27);
    SWAP(0, 8);
    SWAP(1, 9);
    SWAP(2, 10);
    SWAP(3, 11);
    SWAP(4, 12);
    SWAP(5, 13);
    SWAP(6, 14);
    SWAP(7, 15);
    SWAP(16, 24);
    SWAP(17, 25);
    SWAP(18, 26);
    SWAP(19, 27);
    SWAP(8, 16);
    SWAP(9, 17);
    SWAP(10, 18);
    SWAP(11, 19);
    SWAP(12, 20);
    SWAP(13, 21);
    SWAP(14, 22);
    SWAP(15, 23);
    SWAP(0, 4);
    SWAP(1, 5);
    SWAP(2, 6);
    SWAP(3, 7);
    SWAP(8, 12);
    SWAP(9, 13);
    SWAP(10, 14);
    SWAP(11, 15);
    SWAP(16, 20);
    SWAP(17, 21);
    SWAP(18, 22);
    SWAP(19, 23);
    SWAP(4, 16);
    SWAP(5, 17);
    SWAP(6, 18);
    SWAP(7, 19);
    SWAP(12, 24);
    SWAP(13, 25);
    SWAP(14, 26);
    SWAP(15, 27);
    SWAP(4, 8);
    SWAP(5, 9);
    SWAP(6, 10);
    SWAP(7, 11);
    SWAP(12, 16);
    SWAP(13, 17);
    SWAP(14, 18);
    SWAP(15, 19);
    SWAP(20, 24);
    SWAP(21, 25);
    SWAP(22, 26);
    SWAP(23, 27);
    SWAP(0, 2);
    SWAP(1, 3);
    SWAP(4, 6);
    SWAP(5, 7);
    SWAP(8, 10);
    SWAP(9, 11);
    SWAP(12, 14);
    SWAP(13, 15);
    SWAP(16, 18);
    SWAP(17, 19);
    SWAP(20, 22);
    SWAP(21, 23);
    SWAP(24, 26);
    SWAP(25, 27);
    SWAP(2, 16);
    SWAP(3, 17);
    SWAP(6, 20);
    SWAP(7, 21);
    SWAP(10, 24);
    SWAP(11, 25);
    SWAP(2, 8);
    SWAP(3, 9);
    SWAP(6, 12);
    SWAP(7, 13);
    SWAP(10, 16);
    SWAP(11, 17);
    SWAP(14, 20);
    SWAP(15, 21);
    SWAP(18, 24);
    SWAP(19, 25);
    SWAP(2, 4);
    SWAP(3, 5);
    SWAP(6, 8);
    SWAP(7, 9);
    SWAP(10, 12);
    SWAP(11, 13);
    SWAP(14, 16);
    SWAP(15, 17);
    SWAP(18, 20);
    SWAP(19, 21);
    SWAP(22, 24);
    SWAP(23, 25);
    SWAP(0, 1);
    SWAP(2, 3);
    SWAP(4, 5);
    SWAP(6, 7);
    SWAP(8, 9);
    SWAP(10, 11);
    SWAP(12, 13);
    SWAP(14, 15);
    SWAP(16, 17);
    SWAP(18, 19);
    SWAP(20, 21);
    SWAP(22, 23);
    SWAP(24, 25);
    SWAP(26, 27);
    SWAP(1, 16);
    SWAP(3, 18);
    SWAP(5, 20);
    SWAP(7, 22);
    SWAP(9, 24);
    SWAP(11, 26);
    SWAP(1, 8);
    SWAP(3, 10);
    SWAP(5, 12);
    SWAP(7, 14);
    SWAP(9, 16);
    SWAP(11, 18);
    SWAP(13, 20);
    SWAP(15, 22);
    SWAP(17, 24);
    SWAP(19, 26);
    SWAP(1, 4);
    SWAP(3, 6);
    SWAP(5, 8);
    SWAP(7, 10);
    SWAP(9, 12);
    SWAP(11, 14);
    SWAP(13, 16);
    SWAP(15, 18);
    SWAP(17, 20);
    SWAP(19, 22);
    SWAP(21, 24);
    SWAP(23, 26);
    SWAP(1, 2);
    SWAP(3, 4);
    SWAP(5, 6);
    SWAP(7, 8);
    SWAP(9, 10);
    SWAP(11, 12);
    SWAP(13, 14);
    SWAP(15, 16);
    SWAP(17, 18);
    SWAP(19, 20);
    SWAP(21, 22);
    SWAP(23, 24);
    SWAP(25, 26);
    
#undef min
#undef max
#undef SWAP
}

void sort29_fast(uint8_t * d) {
#define min(a,b) ((a<b) ? a : b )
#define max(a,b) ((a<b) ? b : a )
#define SWAP(x,y) { int tmp = min(d[x], d[y]); d[y] = max(d[x], d[y]); d[x] = tmp; }
//#define SWAP(x,y) { int dx = d[x], dy = d[y], tmp; tmp = d[x] = ((dx < dy) ? dx : dy); d[y] ^= dx ^ tmp; }
    SWAP(0, 16);
    SWAP(1, 17);
    SWAP(2, 18);
    SWAP(3, 19);
    SWAP(4, 20);
    SWAP(5, 21);
    SWAP(6, 22);
    SWAP(7, 23);
    SWAP(8, 24);
    SWAP(9, 25);
    SWAP(10, 26);
    SWAP(11, 27);
    SWAP(12, 28);
    SWAP(0, 8);
    SWAP(1, 9);
    SWAP(2, 10);
    SWAP(3, 11);
    SWAP(4, 12);
    SWAP(5, 13);
    SWAP(6, 14);
    SWAP(7, 15);
    SWAP(16, 24);
    SWAP(17, 25);
    SWAP(18, 26);
    SWAP(19, 27);
    SWAP(20, 28);
    SWAP(8, 16);
    SWAP(9, 17);
    SWAP(10, 18);
    SWAP(11, 19);
    SWAP(12, 20);
    SWAP(13, 21);
    SWAP(14, 22);
    SWAP(15, 23);
    SWAP(0, 4);
    SWAP(1, 5);
    SWAP(2, 6);
    SWAP(3, 7);
    SWAP(8, 12);
    SWAP(9, 13);
    SWAP(10, 14);
    SWAP(11, 15);
    SWAP(16, 20);
    SWAP(17, 21);
    SWAP(18, 22);
    SWAP(19, 23);
    SWAP(24, 28);
    SWAP(4, 16);
    SWAP(5, 17);
    SWAP(6, 18);
    SWAP(7, 19);
    SWAP(12, 24);
    SWAP(13, 25);
    SWAP(14, 26);
    SWAP(15, 27);
    SWAP(4, 8);
    SWAP(5, 9);
    SWAP(6, 10);
    SWAP(7, 11);
    SWAP(12, 16);
    SWAP(13, 17);
    SWAP(14, 18);
    SWAP(15, 19);
    SWAP(20, 24);
    SWAP(21, 25);
    SWAP(22, 26);
    SWAP(23, 27);
    SWAP(0, 2);
    SWAP(1, 3);
    SWAP(4, 6);
    SWAP(5, 7);
    SWAP(8, 10);
    SWAP(9, 11);
    SWAP(12, 14);
    SWAP(13, 15);
    SWAP(16, 18);
    SWAP(17, 19);
    SWAP(20, 22);
    SWAP(21, 23);
    SWAP(24, 26);
    SWAP(25, 27);
    SWAP(2, 16);
    SWAP(3, 17);
    SWAP(6, 20);
    SWAP(7, 21);
    SWAP(10, 24);
    SWAP(11, 25);
    SWAP(14, 28);
    SWAP(2, 8);
    SWAP(3, 9);
    SWAP(6, 12);
    SWAP(7, 13);
    SWAP(10, 16);
    SWAP(11, 17);
    SWAP(14, 20);
    SWAP(15, 21);
    SWAP(18, 24);
    SWAP(19, 25);
    SWAP(22, 28);
    SWAP(2, 4);
    SWAP(3, 5);
    SWAP(6, 8);
    SWAP(7, 9);
    SWAP(10, 12);
    SWAP(11, 13);
    SWAP(14, 16);
    SWAP(15, 17);
    SWAP(18, 20);
    SWAP(19, 21);
    SWAP(22, 24);
    SWAP(23, 25);
    SWAP(26, 28);
    SWAP(0, 1);
    SWAP(2, 3);
    SWAP(4, 5);
    SWAP(6, 7);
    SWAP(8, 9);
    SWAP(10, 11);
    SWAP(12, 13);
    SWAP(14, 15);
    SWAP(16, 17);
    SWAP(18, 19);
    SWAP(20, 21);
    SWAP(22, 23);
    SWAP(24, 25);
    SWAP(26, 27);
    SWAP(1, 16);
    SWAP(3, 18);
    SWAP(5, 20);
    SWAP(7, 22);
    SWAP(9, 24);
    SWAP(11, 26);
    SWAP(13, 28);
    SWAP(1, 8);
    SWAP(3, 10);
    SWAP(5, 12);
    SWAP(7, 14);
    SWAP(9, 16);
    SWAP(11, 18);
    SWAP(13, 20);
    SWAP(15, 22);
    SWAP(17, 24);
    SWAP(19, 26);
    SWAP(21, 28);
    SWAP(1, 4);
    SWAP(3, 6);
    SWAP(5, 8);
    SWAP(7, 10);
    SWAP(9, 12);
    SWAP(11, 14);
    SWAP(13, 16);
    SWAP(15, 18);
    SWAP(17, 20);
    SWAP(19, 22);
    SWAP(21, 24);
    SWAP(23, 26);
    SWAP(25, 28);
    SWAP(1, 2);
    SWAP(3, 4);
    SWAP(5, 6);
    SWAP(7, 8);
    SWAP(9, 10);
    SWAP(11, 12);
    SWAP(13, 14);
    SWAP(15, 16);
    SWAP(17, 18);
    SWAP(19, 20);
    SWAP(21, 22);
    SWAP(23, 24);
    SWAP(25, 26);
    SWAP(27, 28);
    
#undef min
#undef max
#undef SWAP
}

void sort30_fast(uint8_t * d) {
#define min(a,b) ((a<b) ? a : b )
#define max(a,b) ((a<b) ? b : a )
#define SWAP(x,y) { int tmp = min(d[x], d[y]); d[y] = max(d[x], d[y]); d[x] = tmp; }
//#define SWAP(x,y) { int dx = d[x], dy = d[y], tmp; tmp = d[x] = ((dx < dy) ? dx : dy); d[y] ^= dx ^ tmp; }
    SWAP(0, 16);
    SWAP(1, 17);
    SWAP(2, 18);
    SWAP(3, 19);
    SWAP(4, 20);
    SWAP(5, 21);
    SWAP(6, 22);
    SWAP(7, 23);
    SWAP(8, 24);
    SWAP(9, 25);
    SWAP(10, 26);
    SWAP(11, 27);
    SWAP(12, 28);
    SWAP(13, 29);
    SWAP(0, 8);
    SWAP(1, 9);
    SWAP(2, 10);
    SWAP(3, 11);
    SWAP(4, 12);
    SWAP(5, 13);
    SWAP(6, 14);
    SWAP(7, 15);
    SWAP(16, 24);
    SWAP(17, 25);
    SWAP(18, 26);
    SWAP(19, 27);
    SWAP(20, 28);
    SWAP(21, 29);
    SWAP(8, 16);
    SWAP(9, 17);
    SWAP(10, 18);
    SWAP(11, 19);
    SWAP(12, 20);
    SWAP(13, 21);
    SWAP(14, 22);
    SWAP(15, 23);
    SWAP(0, 4);
    SWAP(1, 5);
    SWAP(2, 6);
    SWAP(3, 7);
    SWAP(8, 12);
    SWAP(9, 13);
    SWAP(10, 14);
    SWAP(11, 15);
    SWAP(16, 20);
    SWAP(17, 21);
    SWAP(18, 22);
    SWAP(19, 23);
    SWAP(24, 28);
    SWAP(25, 29);
    SWAP(4, 16);
    SWAP(5, 17);
    SWAP(6, 18);
    SWAP(7, 19);
    SWAP(12, 24);
    SWAP(13, 25);
    SWAP(14, 26);
    SWAP(15, 27);
    SWAP(4, 8);
    SWAP(5, 9);
    SWAP(6, 10);
    SWAP(7, 11);
    SWAP(12, 16);
    SWAP(13, 17);
    SWAP(14, 18);
    SWAP(15, 19);
    SWAP(20, 24);
    SWAP(21, 25);
    SWAP(22, 26);
    SWAP(23, 27);
    SWAP(0, 2);
    SWAP(1, 3);
    SWAP(4, 6);
    SWAP(5, 7);
    SWAP(8, 10);
    SWAP(9, 11);
    SWAP(12, 14);
    SWAP(13, 15);
    SWAP(16, 18);
    SWAP(17, 19);
    SWAP(20, 22);
    SWAP(21, 23);
    SWAP(24, 26);
    SWAP(25, 27);
    SWAP(2, 16);
    SWAP(3, 17);
    SWAP(6, 20);
    SWAP(7, 21);
    SWAP(10, 24);
    SWAP(11, 25);
    SWAP(14, 28);
    SWAP(15, 29);
    SWAP(2, 8);
    SWAP(3, 9);
    SWAP(6, 12);
    SWAP(7, 13);
    SWAP(10, 16);
    SWAP(11, 17);
    SWAP(14, 20);
    SWAP(15, 21);
    SWAP(18, 24);
    SWAP(19, 25);
    SWAP(22, 28);
    SWAP(23, 29);
    SWAP(2, 4);
    SWAP(3, 5);
    SWAP(6, 8);
    SWAP(7, 9);
    SWAP(10, 12);
    SWAP(11, 13);
    SWAP(14, 16);
    SWAP(15, 17);
    SWAP(18, 20);
    SWAP(19, 21);
    SWAP(22, 24);
    SWAP(23, 25);
    SWAP(26, 28);
    SWAP(27, 29);
    SWAP(0, 1);
    SWAP(2, 3);
    SWAP(4, 5);
    SWAP(6, 7);
    SWAP(8, 9);
    SWAP(10, 11);
    SWAP(12, 13);
    SWAP(14, 15);
    SWAP(16, 17);
    SWAP(18, 19);
    SWAP(20, 21);
    SWAP(22, 23);
    SWAP(24, 25);
    SWAP(26, 27);
    SWAP(28, 29);
    SWAP(1, 16);
    SWAP(3, 18);
    SWAP(5, 20);
    SWAP(7, 22);
    SWAP(9, 24);
    SWAP(11, 26);
    SWAP(13, 28);
    SWAP(1, 8);
    SWAP(3, 10);
    SWAP(5, 12);
    SWAP(7, 14);
    SWAP(9, 16);
    SWAP(11, 18);
    SWAP(13, 20);
    SWAP(15, 22);
    SWAP(17, 24);
    SWAP(19, 26);
    SWAP(21, 28);
    SWAP(1, 4);
    SWAP(3, 6);
    SWAP(5, 8);
    SWAP(7, 10);
    SWAP(9, 12);
    SWAP(11, 14);
    SWAP(13, 16);
    SWAP(15, 18);
    SWAP(17, 20);
    SWAP(19, 22);
    SWAP(21, 24);
    SWAP(23, 26);
    SWAP(25, 28);
    SWAP(1, 2);
    SWAP(3, 4);
    SWAP(5, 6);
    SWAP(7, 8);
    SWAP(9, 10);
    SWAP(11, 12);
    SWAP(13, 14);
    SWAP(15, 16);
    SWAP(17, 18);
    SWAP(19, 20);
    SWAP(21, 22);
    SWAP(23, 24);
    SWAP(25, 26);
    SWAP(27, 28);
    
#undef min
#undef max
#undef SWAP
}

extern inline void sort_fast(uint8_t * d, int start, int len);

extern inline int comp_fast1(const uint8_t * lhs, const uint8_t * rhs);

extern inline int comp_fast2(const uint8_t * lhs, const uint8_t * rhs);

extern inline int comp_fast3(const uint8_t * lhs, const uint8_t * rhs);

extern inline int comp_fast4(const uint8_t * lhs, const uint8_t * rhs);

extern inline int comp_fast5(const uint8_t * lhs, const uint8_t * rhs);

extern inline int comp_fast6(const uint8_t * lhs, const uint8_t * rhs);

extern inline int comp_fast(const uint8_t * lhs, const uint8_t * rhs, const int len);

extern inline void swap_blocks_with_loop( uint8_t* a, uint8_t* b, const size_t n);

extern inline void sort2_fast_array(uint8_t * d, size_t size_of_element);

extern inline void sort3_fast_array(uint8_t * d, size_t size_of_element);

extern inline void sort4_fast_array(uint8_t * d, size_t size_of_element);

extern inline void sort5_fast_array(uint8_t * d, size_t size_of_element);

extern inline void sort6_fast_array(uint8_t * d, size_t size_of_element);

extern inline void sort7_fast_array(uint8_t * d, size_t size_of_element);

extern inline void sort8_fast_array(uint8_t * d, size_t size_of_element);

extern inline void sort9_fast_array(uint8_t * d, size_t size_of_element);

extern inline void sort10_fast_array(uint8_t * d, size_t size_of_element);

extern inline void sort11_fast_array(uint8_t * d, size_t size_of_element);

extern inline void sort12_fast_array(uint8_t * d, size_t size_of_element);

extern inline void sort13_fast_array(uint8_t * d, size_t size_of_element);

extern inline void sort14_fast_array(uint8_t * d, size_t size_of_element);

extern inline void sort15_fast_array(uint8_t * d, size_t size_of_element);

extern inline void sort16_fast_array(uint8_t * d, size_t size_of_element);

extern inline void sort17_fast_array(uint8_t * d, size_t size_of_element);

extern inline void sort18_fast_array(uint8_t * d, size_t size_of_element);

extern inline void sort_fast_array(uint8_t * d, size_t size_of_element, uint8_t len);
