/ani/mrses

#include <stdio.h>

#include <stdlib.h>

#include <malloc.h>

#include <string.h>

#include <sys/time.h>

#include <blas_s.h>

#include <cblas.h>

#include <lapack.h>

#include <spu_intrinsics.h>

#include <spu_mfcio.h>

    // we should also consider HW_ALIGN (when change this values)

#define SPE_BLOCK 16		// Thats for BLAS/Lapack in items

#define HW_HIDE_DETAILS

#include "mrses_spe.h"

#define spe_scal(N,a,X,incX) sscal_spu(X, a, N)

#define spe_axpy(N, a, X, incX, Y, incY) saxpy_spu(X, Y, a, N)

#define spe_syrk(Order,Uplo,Trans, N, K, a, A, lda, b, C, ldc) \

    ssyrk_spu(A, C, a, N, K, lda, ldc)

#undef rnd

#define rnd(i) ((int)((i) * (rand() / (RAND_MAX + 1.0))))

#ifdef HW_USE_BLOCKED_MULTIPLY

# define calc_ralloc(width, walloc) walloc

#else

//# define calc_ralloc(width, walloc) ((width<13)?width:walloc)

# define calc_ralloc(width, walloc) width

#endif /* HW_USE_BLOCKED_MULTIPLY */

//#include "atlas_potrf.h"

#include "vec_potrf.h"

static inline int mrses_spe_real_multiply(

    MRSESDataType *D,

    short int pack, short int at_once,

    short int d_step, short int width, short int ralloc, short int walloc, short int nA, short int nB,

    MRSESDataType *A, MRSESDataType *B,

    MRSESDataType *Ca, MRSESDataType *Cb

) {

    short int i, k, l;

//    PRINT_MATRIX("% 6.4f ", A, nA, 16, 16)

/*

    int rww = at_once * walloc * width;

    memset(Ca, 0, rww * sizeof(MRSESDataType));

    memset(Cb, 0, rww * sizeof(MRSESDataType));

    spe_syrk(CblasRowMajor, CblasLower, CblasNoTrans, walloc, nA, 1, A, nA, 0, Ca, walloc);

    spe_syrk(CblasRowMajor, CblasLower, CblasNoTrans, walloc, nB, 1, B, nB, 0, Cb, walloc);

*/

    vec_ssyrk_rln_11(ralloc, nA, A, nA, Ca, walloc);

    vec_ssyrk_rln_11(ralloc, nB, B, nB, Cb, walloc);

    short int c_step = width * (walloc + 1);

    for (i = 0; i < at_once; ++i) {

	short int c_offset = i * c_step;

	MRSESDataType *Ca_cur = Ca + c_offset;

	MRSESDataType *Cb_cur = Cb + c_offset;

	MRSESDataType *Da = D + (3 * i + 1) * d_step;

	MRSESDataType *Db = Da + d_step;

	for (k = 0; k < width; ++k) {

	    for (l = 0; l < width; ++l) {

		Da[pack*(k * width + l)] = Ca_cur[k * walloc + l];

		Db[pack*(k * width + l)] = Cb_cur[k * walloc + l];

	    }

	}

    }

    return 0;

}

static vector unsigned int mask1 = {0, 0xFFFFFFFF, 0, 0xFFFFFFFF};

static vector unsigned int mask2 = {0, 0, 0xFFFFFFFF, 0xFFFFFFFF};

static vector unsigned int mask3 = {0xFFFFFFFF, 0, 0, 0xFFFFFFFF};

#define VECTOR_PRINT(var, val) \

    printf("Vector %10s is: {% 6.4e, % 6.4e, % 6.4e, % 6.4e}\n", var, spu_extract(val, 0), spu_extract(val, 1), spu_extract(val, 2), spu_extract(val, 3));

#define iVECTOR_PRINT(var, val) \

    printf("Vector %10s is: {%i, %i, %i, %i}\n", var, spu_extract(val, 0), spu_extract(val, 1), spu_extract(val, 2), spu_extract(val, 3));

#define REPACK(Da, Ca_cur) \

		r1 =              *(vector float*)(Ca_cur +             k * walloc + l); \

		r2 = spu_rlqwbyte(*(vector float*)(Ca_cur +     c_big + k * walloc + l), -4); \

		r3 = spu_rlqwbyte(*(vector float*)(Ca_cur + 2 * c_big + k * walloc + l), -8); \

		r4 = spu_rlqwbyte(*(vector float*)(Ca_cur + 3 * c_big + k * walloc + l), -12); \

		\

		r5 = spu_sel(r1, r2, mask1); \

		r6 = spu_sel(r3, r4, mask1); \

		\

		Da[k*width + l    ] = spu_sel(r5, r6, mask2); \

	        Da[k*width + l + 2] = spu_rlqwbyte(spu_sel(r6, r5, mask2), 8); \

		\

		r5 = spu_sel(r2, r1, mask1); \

		r6 = spu_sel(r4, r3, mask1); \

		\

		Da[k*width + l + 1] = spu_rlqwbyte(spu_sel(r5, r6, mask3), 4); \

		Da[k*width + l + 3] = spu_rlqwbyte(spu_sel(r6, r5, mask3), 12);

static inline int mrses_spe_real_multiply_recover(

    MRSESDataType *D,

    short int pack, short int at_once,

    short int d_step, short int width, short int ralloc, short int walloc,

    MRSESDataType *Ca, MRSESDataType *Cb,

    MRSESIntType *drp_gen, MRSESIntType *rst_gen,

    MRSESDataType *Ea, MRSESDataType *Eb

) {

    short int i, j, k, l, m;

    short int c_step = width * (walloc + 1);

    short int c_big = ralloc * walloc;

    short int e_big = at_once * walloc;

    for (i = 0; i < at_once; ++i) {

	short int e_offset = i * walloc;

	short int c_offset = i * c_step;

	MRSESDataType *Da = D + (3 * i + 1) * d_step;

	MRSESDataType *Db = Da + d_step;

	for (m = 0; m < pack; ++m, c_offset += c_big, e_offset += e_big) {

	    short int gen = drp_gen[m * at_once + i];

	    if (rst_gen[m * at_once + i] > width) {

		for (j = 0; j < width; j++) {

		    if (j < gen) {

			l = j; k = gen;

		    } else {

			l = gen; k = j;

		    }

		    Ca[c_offset + k * walloc + l] = Ea[e_offset + j];

		    Cb[c_offset + k * walloc + l] = Eb[e_offset + j];

		}

	    }

	}

	    // Recovering to current stage

    	for (k = 0; k < width; ++k) {

	    for (l = 0; l < width; l += 4) {

		register vector float r1, r2, r3, r4, r5, r6;

		REPACK(((vector float*)Da), ((float*)Ca));

		REPACK(((vector float*)Db), ((float*)Cb));

	    }

	}

    }

    return 0;

}

static inline int mrses_spe_real_multiply_update(

    MRSESDataType *D,

    short int pack, short int at_once,

    short int d_step, short int width, short int ralloc, short int walloc, short int nA, short int nB,

    MRSESDataType *A, MRSESDataType *B,

    MRSESIntType *drp_gen,

    MRSESDataType *Ea, MRSESDataType *Eb

) {

    short int i, j, k, l;

/*

    if (*rst_gen > width) {

	vec_ssyrk_rln_11(ralloc, nA, A, nA, Ca, walloc);

	vec_ssyrk_rln_11(ralloc, nB, B, nB, Cb, walloc);

    }

*/

    for (i = 0; i < at_once; ++i) {

	short int e_offset = i * walloc;

	short int gen = drp_gen[i];

	MRSESDataType *Ea_cur = Ea + e_offset;

	MRSESDataType *Eb_cur = Eb + e_offset;

	MRSESDataType *Da = D + (3 * i + 1) * d_step;

	MRSESDataType *Db = Da + d_step;

        vec_update_row(gen, ralloc, nA, A, nA, Ea_cur);

        vec_update_row(gen, ralloc, nB, B, nB, Eb_cur);

	for (j = 0; j < width; j++) {

	    if (j < gen) {

		l = j; k = gen;

	    } else {

		l = gen; k = j;

	    }

//    if ((((int)D)%16)==0) {

//	    if (fabs(Da[pack * (k*width + l)] - Ea_cur[j])>0.001) {

//		printf("Updating (%i): %i %i: % 6.4f to % 6.4f\n", gen, k, l, Da[pack * (k*width + l)], Ea_cur[j]);

//	    }

//    }

	    Da[pack*(k * width + l)] = Ea_cur[j];

	    Db[pack*(k * width + l)] = Eb_cur[j];

	}

    }

    return 0;

}

static inline int mrses_spe_real_decompose(

    MRSESDistance dist, MRSESDataType *res,

    short int pack, short int at_once,

    short int step, short int width,

    MRSESDataType *D, MRSESDataType * mean

) {

    short int i, j;

    short int mstep = pack * width;

    vector unsigned int err;

    vector unsigned int error;

    vector float rcorr;

    vector float rmahal;

    vector float result;

    vector float zero = {0, 0, 0, 0};

    for (i = 0; i < at_once; ++i) {

    	MRSESDataType *C = D + 3 * i * step;

	MRSESDataType *Ca = C + step;

    	MRSESDataType *Cb = Ca + step;

	MRSESDataType *m = mean + i * mstep;

	memcpy(C, Ca, step * sizeof(MRSESDataType));

	spe_axpy(step, 1, Cb, 1, C, 1);

	spe_scal(step, 0.5, C, 1);

	error = vec_spotrf_u(width, C, width);

	err = vec_spotrf_u(width, Ca, width);

	error = spu_and(err, error);

	err = vec_spotrf_u(width, Cb, width);

	error = spu_and(err, error);

	rcorr = vec_rcorr(width, C, Ca, Cb);

	rmahal = vec_rmahal(width, C, m);

	switch (dist) {

	    case BHATTACHARYYA:

		result = vec_bhata(rcorr, rmahal);

	    break;

	    case MAHALANOBIS:

		result = rmahal;

	    break;

	    case CORCOR:

		result = rcorr;

	    break;

	    default:

		result = zero;

	}

	for (j = 0; j < pack; j++) {

	    if (*(((unsigned int*)&error)+j)) {

		res[j * at_once + i] = *(((float*)&result)+j);

//		printf("SPU result: %e (mahal: %e, corcor: %e)\n", *(((float*)&result)+j), *(((float*)&rmahal)+j), *(((float*)&rcorr)+j));

	    } else {

		res [j * at_once + i] = 0;

//		printf("SPU result: singular matrix, assuming 0 distance\n");

	    }

	}

    }

    return 0;

}

struct MRSESTemporaryDataT {

    MRSESDataType *A;

    MRSESDataType *B;

    MRSESDataType *Ca;

    MRSESDataType *Cb;

    MRSESDataType *Ea;

    MRSESDataType *Eb;

    MRSESDataType *D;

    MRSESDataType *mean;

    MRSESDataType *mean_copy;

    MRSESIntType *index;

    MRSESIntType *index_storage;

    MRSESDataType *Mfull;

};

typedef struct MRSESTemporaryDataT MRSESTemporaryDataS;

typedef struct MRSESTemporaryDataT *MRSESTemporaryData;

static unsigned char *local_data = NULL;

static inline MRSESTemporaryData mrses_spe_malloc(MRSESContext mrses, unsigned int rdch) {

    unsigned char *allocation;

    short int aA, aB;

    MRSESIntType properties;

    short int at_once, used_ralloc;

    short int pack;

    short int width;

    short int walloc, walloc2, dalloc, ralloc;

    short int index_segment_size; 

    int i, pos = 0, size;

    int extra_rows = 0;

    MRSESTemporaryData data;

    properties = mrses->properties;

//    pos = calc_alloc(properties * sizeof(uint32_t), HW_ALIGN);

    if (local_data) return (MRSESTemporaryData)(local_data + pos);

    width = mrses->width;

    aA = calc_alloc(mrses->nA, SPE_BLOCK);

    aB = calc_alloc(mrses->nB, SPE_BLOCK);

    walloc = calc_alloc(width, SPE_BLOCK);

    ralloc = calc_ralloc(width, walloc);

    walloc2 = walloc * walloc;

    at_once = max(1, ralloc / width);

    pack = SIMD_BLOCK / sizeof(MRSESDataType);

    dalloc = calc_alloc(pack * width * width, 128);//64);

    index_segment_size = (HW_ALIGN / sizeof(MRSESIntType));

    used_ralloc = at_once * width;

    if ((ralloc < walloc)&&(ralloc > 12)) {

	 extra_rows = walloc - ralloc;

    }

    size = (

	//calc_alloc(properties * sizeof(uint32_t), HW_ALIGN) +							// result histogram

	calc_alloc(sizeof(MRSESTemporaryDataS), HW_ALIGN) +							// structure

	(pack * ralloc + extra_rows) * (aA + aB) * sizeof(MRSESDataType) +					// A, B		- source matrices

	2 * (pack * ralloc + extra_rows) * walloc * sizeof(MRSESDataType) +					// Ca, Cb	- variances (temp)

	2 * pack * at_once * walloc * sizeof(MRSESDataType) +							// Ea, Eb	- variance updates

	    3 * at_once * dalloc * sizeof(MRSESDataType) +							// D		- variances (aligned)

	2 * pack * ralloc * sizeof(MRSESDataType) +								// mean, mean_copy

//	calc_alloc(mrses->iterate_size * properties * sizeof(MRSESIntType), HW_ALIGN) +

	(walloc + index_segment_size) * at_once * pack * sizeof(MRSESIntType) +					// index + exchange area

	calc_alloc(properties * sizeof(MRSESDataType), HW_ALIGN)						// Mfull, complete mean

    );

/*    

    printf("Allocating data: %i KB (A+B: %li, C: %li, D: %li, mean: %li, Mfull: %li)\n", size / 1024,

	pack * ralloc * (aA + aB) * sizeof(MRSESDataType) / 1024,

	2 * (pack * ralloc + extra_rows) * walloc * sizeof(MRSESDataType) / 1024,

	3 * at_once * dalloc * sizeof(MRSESDataType) / 1024,

	2 * (pack * ralloc + extra_rows) * sizeof(MRSESDataType) / 1024,

	calc_alloc(properties * sizeof(MRSESDataType), HW_ALIGN) / 1024

    );

*/

    allocation = (unsigned char*)memalign(HW_ALIGN, size);

    if (!allocation) return NULL;

//    memset(allocation, 0, properties * sizeof(uint32_t));

    data = (MRSESTemporaryData)(allocation + pos);

    pos += calc_alloc(sizeof(MRSESTemporaryDataS), HW_ALIGN);

    data->A = (MRSESDataType*)(allocation + pos);

    pos += (pack * ralloc + extra_rows) * aA * sizeof(MRSESDataType);

    data->B = (MRSESDataType*)(allocation + pos);

    pos += (pack * ralloc + extra_rows) * aB * sizeof(MRSESDataType);

    data->Ca = (MRSESDataType*)(allocation + pos);

    pos += (pack * ralloc + extra_rows) * walloc * sizeof(MRSESDataType);

    data->Cb = (MRSESDataType*)(allocation + pos);

    pos += (pack * ralloc + extra_rows) * walloc * sizeof(MRSESDataType);

    data->Ea = (MRSESDataType*)(allocation + pos);

    pos += pack * at_once * walloc * sizeof(MRSESDataType);

    data->Eb = (MRSESDataType*)(allocation + pos);

    pos += pack * at_once * walloc * sizeof(MRSESDataType);

    data->D = (MRSESDataType*)(allocation + pos);

    pos += 3 * at_once * dalloc * sizeof(MRSESDataType);

    data->mean = (MRSESDataType*)(allocation + pos);

    pos += pack * ralloc * sizeof(MRSESDataType);

    data->mean_copy = (MRSESDataType*)(allocation + pos);

    pos += pack * ralloc * sizeof(MRSESDataType);

    data->index = (MRSESIntType*)(allocation + pos);

    pos += walloc * at_once * pack * sizeof(MRSESIntType);

    data->index_storage = (MRSESIntType*)(allocation + pos);

    pos += index_segment_size * at_once * pack * sizeof(MRSESIntType);

    data->Mfull = (MRSESDataType*)(allocation + pos);

    size = calc_alloc(properties * sizeof(MRSESDataType), HW_ALIGN);

    for (i = 0; i < size; i += SPE_MAX_TRANSFER_SIZE) {

        spu_mfcdma32((void*)(((char *)(data->Mfull)) + i), (unsigned int)(((char*)mrses->mean) + i), min(size - i, SPE_MAX_TRANSFER_SIZE), rdch, MFC_GET_CMD);

    }

    local_data = (void*)allocation;

    if (used_ralloc < ralloc) {

	    // we need to do it, to clean the end parts for non-even case

	memset(data->A, 0, pack * (ralloc) * aA * sizeof(MRSESDataType));

	memset(data->B, 0, pack * (ralloc) * aB * sizeof(MRSESDataType));

	memset(data->mean, 0, pack * ralloc * sizeof(MRSESDataType));

	memset(data->mean_copy, 0, pack * ralloc * sizeof(MRSESDataType));

    }

    return data;

}

int mrses_spe_iterate(int block_group, MRSESContext mrses, unsigned int rdch) {

    int idx;

    short int j, k, l, m;

    short int width = mrses->width;

    short int walloc = calc_alloc(width, SPE_BLOCK);

    short int alloc = mrses->alloc;

    short int ralloc = calc_ralloc(width, walloc);

    short int nA = mrses->nA;

    short int cA = calc_alloc(nA * sizeof(MRSESDataType), HW_ALIGN);

    short int aA = calc_alloc(nA, SPE_BLOCK);	// aligned size

    short int nB = mrses->nB;

    short int cB = calc_alloc(nB * sizeof(MRSESDataType), HW_ALIGN);

    short int aB = calc_alloc(nB, SPE_BLOCK);

    MRSESDistance dist = mrses->dist;

    MRSESIntType properties = mrses->properties;

    MRSESIntType palloc = mrses->palloc;

    int i, iterations = mrses->iterations;

    short int iterate_size = mrses->iterate_size;

    short int at_once = max(1, ralloc / width);

    short int pack = SIMD_BLOCK / sizeof(MRSESDataType);

    short int dalloc = calc_alloc(pack * width * width, 128);

    short int index_segment_size = (HW_ALIGN / sizeof(MRSESIntType));

    int spe_real_block = pack * at_once;

    MRSESIntType rpl_gen_k, rpl_gen_segment;

    MRSESIntType drp_gen[spe_real_block], rpl_gen[spe_real_block], rst_gen[spe_real_block];

    MRSESDataType result[spe_real_block], cur_result[spe_real_block];

//    MRSESDataType *result = mrses->result + block;

    volatile MRSESDataType *Afull = mrses->A;

    volatile MRSESDataType *Bfull = mrses->B;

    MRSESIntType *mrses_index = mrses->index;

    MRSESTemporaryData data;

    MRSESDataType *A;

    MRSESDataType *B;

    MRSESDataType *Ca;

    MRSESDataType *Cb;

    MRSESDataType *Ea;

    MRSESDataType *Eb;

    MRSESDataType *D;

    MRSESDataType *mean;

    MRSESDataType *mean_copy;

    volatile MRSESIntType *index_storage, *index_storage_k;

    volatile MRSESIntType *index, *index_k;

    volatile MRSESDataType *Mfull;

#ifdef USE_FAST_RANDOM

    unsigned int g_seed;

    struct timeval tvseed;

#endif /* USE_FAST_RANDOM */

#ifdef USE_FAST_RANDOM

    gettimeofday(&tvseed, NULL);

    g_seed = tvseed.tv_usec;

#endif /* USE_FAST_RANDOM */

    data = mrses_spe_malloc(mrses, rdch);

    if (!data) {

	printf("Memory allocation failed\n");

	exit(1);

    }

    A = data->A;

    B = data->B;

    Ca = data->Ca;

    Cb = data->Cb;

    Ea = data->Ea;

    Eb = data->Eb;

    D = data->D;

    mean = data->mean;

    mean_copy = data->mean_copy;

    index = data->index;

    index_storage = data->index_storage;

    Mfull = data->Mfull;

//    printf("Iterate %i, pack %i, at_once %i ralloc %i walloc %i\n", iterate_size, pack, at_once, ralloc, walloc);

    for (l = 0; l < iterate_size; l += pack * at_once) {

	for (j = 0; j < spe_real_block; j++) {

	    spu_mfcdma32((void *)(index + j * walloc), (unsigned int)(mrses_index + (j + l + block_group * iterate_size) * palloc), walloc * sizeof(MRSESIntType), rdch, MFC_GET_CMD);

	}

	(void)spu_mfcstat(MFC_TAG_UPDATE_ALL);

	for (m = 0; m < pack; m++) {

	    for (j = 0, k = 0; j < at_once; ++j) {

		//printf("SPE: %i = %i %i %i\n", l + m * at_once + j, l, m, j);

		index_k = index + (m * at_once + j) * walloc;

		for (i = 0; i < width; ++i, ++k) {

		    idx = index_k[i];

		    //printf(" * %i\n", idx);

		    spu_mfcdma32((void *)(A + ((int)(m * ralloc + k)) * aA), (unsigned int)(Afull + idx * alloc), cA, rdch, MFC_GET_CMD);

		    spu_mfcdma32((void *)(B + ((int)(m * ralloc + k)) * aB), (unsigned int)(Bfull + idx * alloc), cB, rdch, MFC_GET_CMD);

		    mean[k * pack + m] = Mfull[idx];

		}

	    }

	    (void)spu_mfcstat(MFC_TAG_UPDATE_ALL);

	    mrses_spe_real_multiply(D + m, pack, at_once, dalloc, width, ralloc, walloc, aA, aB, A + m * ralloc * aA, B + m * ralloc * aB, Ca + m * ralloc * walloc, Cb + m * ralloc * walloc);

	}

	memcpy(mean_copy, mean, pack * ralloc * sizeof(MRSESDataType));

	for (k = 0; k < spe_real_block; k++) {

	    rst_gen[k] = width;

	}

	mrses_spe_real_decompose(dist, cur_result, pack, at_once, dalloc, width, D, mean_copy);

//    }

//    return 0;

//    {

	for (i = 0; i < iterations; ++i) 

	{

	    memcpy(mean_copy, mean, pack * ralloc * sizeof(MRSESDataType));

#ifndef HW_USE_BLOCKED_MULTIPLY

	    mrses_spe_real_multiply_recover(D, pack, at_once, dalloc, width, ralloc, walloc, Ca, Cb, drp_gen, rst_gen, Ea, Eb);

#endif /* HW_USE_BLOCKED_MULTIPLY */

	    for (m = 0, k = 0; m < pack; ++m) {

		for (j = 0; j < at_once; ++j, ++k) {

		    index_k = index + k * walloc;

	    	    drp_gen[k] = rnd(width);

		    rpl_gen[k] = rnd(properties - width) + width;

		    if ((rst_gen[k] < width)&&(rst_gen[k] != drp_gen[k])) {

			idx = index_k[rst_gen[k]];

			//printf("%i, Restoring: %i to index %i\n", k, rst_gen[k], idx);

	    		spu_mfcdma32((void *)(A + ((int)(m * ralloc + j * width + rst_gen[k])) * aA), (unsigned int)(Afull + idx * alloc), cA, rdch, MFC_GET_CMD);

			spu_mfcdma32((void *)(B + ((int)(m * ralloc + j * width + rst_gen[k])) * aB), (unsigned int)(Bfull + idx * alloc), cB, rdch, MFC_GET_CMD);

		    }

		    rpl_gen_k = rpl_gen[k];

		    if (rpl_gen_k < walloc) {

			idx = index_k[rpl_gen[k]];

		    } else {

			index_storage_k = index_storage + k * index_segment_size;

			rpl_gen_segment = (rpl_gen_k / index_segment_size) * index_segment_size;

//			printf("%i, getting part of index %i, index segment %i\n", k + l, rpl_gen_k, rpl_gen_segment);

			spu_mfcdma32((void*)(index_storage_k), (unsigned int)(mrses_index + (l + k + block_group * iterate_size) * palloc + rpl_gen_segment), HW_ALIGN, rdch, MFC_GET_CMD);

			(void)spu_mfcstat(MFC_TAG_UPDATE_ALL);

			idx = index_storage_k[rpl_gen_k - rpl_gen_segment];

			//printf("done, index: %i\n", idx);

		    }

//		    printf("%i, receiving data for %i, index %i (pack %i)\n", k + l, drp_gen[k], idx, m);

		    spu_mfcdma32((void *)(A + ((int)(m * ralloc + j * width + drp_gen[k])) * aA), (unsigned int)(Afull + idx * alloc), cA, rdch, MFC_GET_CMD);

		    spu_mfcdma32((void *)(B + ((int)(m * ralloc + j * width + drp_gen[k])) * aB), (unsigned int)(Bfull + idx * alloc), cB, rdch, MFC_GET_CMD);

		    //puts("done");

		    mean_copy[(j * width + drp_gen[k]) * pack + m] = Mfull[idx];

		}

	        (void)spu_mfcstat(MFC_TAG_UPDATE_ALL);

#ifdef HW_USE_BLOCKED_MULTIPLY

		mrses_spe_real_multiply(D + m, pack, at_once, dalloc, width, ralloc, walloc, aA, aB, A + m * ralloc * aA, B + m * ralloc * aB, Ca, Cb);

#else  /* HW_USE_BLOCKED_MULTIPLY */

		mrses_spe_real_multiply_update(

		    D + m, 

		    pack, at_once, dalloc, width, ralloc, walloc, aA, aB, 

		    A + m * ralloc * aA, B + m * ralloc * aB, 

		    drp_gen + m * at_once,

		    Ea + m * at_once * walloc, Eb + m * at_once * walloc

		);

#endif /* HW_USE_BLOCKED_MULTIPLY */

	    }

	    mrses_spe_real_decompose(dist, result, pack, at_once, dalloc, width, D, mean_copy);

	    for (m = 0, k = 0; m < pack; ++m) {

		for (j = 0; j < at_once; ++j, ++k) {

		    if (result[k] < cur_result[k]) {

			rst_gen[k] = drp_gen[k];

		    } else {

			rst_gen[k] = width + 1;

		    	cur_result[k] = result[k];

			index_k = index + k * walloc;

			rpl_gen_k = rpl_gen[k];

			if (rpl_gen_k < walloc) {

			    idx = index_k[rpl_gen_k];

			    SWAPij(index_k, drp_gen[k], rpl_gen_k);

			} else {

			    index_storage_k = index_storage + k * index_segment_size;

			    rpl_gen_segment = (rpl_gen_k / index_segment_size) * index_segment_size;

			    idx = index_storage_k[rpl_gen_k - rpl_gen_segment];

			    index_storage_k[rpl_gen_k - rpl_gen_segment] = index_k[drp_gen[k]];

			    //printf("%i, write back\n", k);

				// write back segment

			    spu_mfcdma32((void*)(index_storage_k), (unsigned int)(mrses_index + (l + k + block_group * iterate_size) * palloc + rpl_gen_segment), HW_ALIGN, rdch, MFC_PUT_CMD);

		    	    //(void)spu_mfcstat(MFC_TAG_UPDATE_ALL);

			    //puts("done");

			    index_k[drp_gen[k]] = idx;

			}

			mean[(j * width + drp_gen[k]) * pack + m] = Mfull[idx];

		    }

		}

	    }

	}

	    // We can write back initial part of index, but we don't really need to

	for (j = 0; j < spe_real_block; j++) {

	    spu_mfcdma32((void *)(index + j * walloc), (unsigned int)(mrses_index + (j + l + block_group * iterate_size) * palloc), walloc * sizeof(MRSESIntType), rdch, MFC_PUT_CMD);

	}

	(void)spu_mfcstat(MFC_TAG_UPDATE_ALL);

/*

	for (k = 0; k < spe_real_block; k++) {

	    printf("SPU result, block %i: %e\n", l + k, cur_result[k]);

	}

*/

    }

    return 0;

}