/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 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);

    spu_mfcdma32((void *)(data->Mfull), (unsigned int)(mrses->mean), calc_alloc(properties * sizeof(MRSESDataType), HW_ALIGN), 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;
    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) * properties), 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) * properties + 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) * properties + 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) * properties), 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;
}

#include <simdmath.h>
int main(unsigned long long id __attribute__ ((unused)), unsigned long long argp, unsigned long long envp __attribute__ ((unused))) {
    short int size;
    volatile SPUParameters param;
    volatile MRSESContext mrses;
    unsigned int rdch; //, wrch;

#ifndef FIX_RANDOM
    struct timeval tv;
#endif /* FIX_RANDOM */

#ifdef TRACE_TIMINGS
    struct timeval tv1, tv2;
#endif /* TRACE_TIMINGS */

#ifdef TRACE_TIMINGS
    gettimeofday(&tv1, NULL);
#endif /* TRACE_TIMINGS */
    
    rdch = mfc_tag_reserve();
    if (rdch == MFC_TAG_INVALID) {
	printf("Unable to reserve DMA tag, returning");
	exit(1);
    }

    spu_writech(MFC_WrTagMask, 1 << rdch);

    spu_mfcdma32(&param, argp, sizeof(SPUParameters), rdch, MFC_GET_CMD);
    (void)spu_mfcstat(MFC_TAG_UPDATE_ALL);


    size = calc_alloc(sizeof(MRSESContextS), HW_ALIGN);
    mrses = (MRSESContext)malloc(size);
    spu_mfcdma32(mrses, (unsigned int)(param.mrses), size, rdch, MFC_GET_CMD);
    (void)spu_mfcstat(MFC_TAG_UPDATE_ALL);

#ifdef FIX_RANDOM
    srand(FIX_RANDOM);
#else /* FIX_RANDOM */
    gettimeofday(&tv, NULL);
    srand(tv.tv_usec);
    //srand(block);
#endif /* FIX_RANDOM */


    mrses_spe_iterate(param.block_group, mrses, rdch);
    
	// Optimize?
//    free(local_data);
//    local_data = NULL;

#ifdef TRACE_TIMINGS
    gettimeofday(&tv2, NULL);
    printf("SPU 0x%llx: %li ms\n", id, (tv2.tv_sec - tv1.tv_sec)*1000+(tv2.tv_usec - tv1.tv_usec)/1000);
#endif /* TRACE_TIMINGS */

    return 0;
}