/ani/mrses

#include <stdio.h>
#include <string.h>
#include <sys/time.h>
#include <assert.h>

#include <cblas.h>
#include <lapack.h>

#include "mrses_impl.h"
#include "mrses_ppu.h"

#ifdef USE_FAST_RANDOM
#define FASTRND(var, max, add) \
    g_seed = (214013*g_seed+2531011); \
    var = ((int)((max)*((g_seed>>16)&0x7FFF) / (0x7FFF + 1.0))) + add;
#endif

#undef rnd
//#define rnd(i) ((int)((i) * ((random_r(&data->seed, &rndval)?0:rndval) / (RAND_MAX + 1.0))))
#define rnd(i) ((int)((i) * (rand_r(&data->seed) / (RAND_MAX + 1.0))))


static inline int mrses_ppu_real_run(
    MRSESContext mrses, MRSESDataType *result,
    int width, int width2, int nA, int nB,
    MRSESDataType *A, MRSESDataType *B, MRSESDataType *mean,
    MRSESDataType *C, MRSESDataType *Ca, MRSESDataType *Cb
) {
    int i, err;
    
    char hmode = 'L';
    
    MRSESDataType detAB, detC;
    MRSESDataType rmahal, rcorr;

/*
	// We can save twice by not computing symmetric parts
    //blas_gemm(CblasRowMajor, CblasNoTrans, CblasTrans, width, width, nA, 1, A, nA, A, nA, 0, Ca, width);
    //blas_gemm(CblasRowMajor, CblasNoTrans, CblasTrans, width, width, nB, 1, B, nB, B, nB, 0, Cb, width);
    //printf("%f %f\n", Ca[4], Ca[20]);

    //blas_syrk(CblasRowMajor, CblasUpper, CblasTrans, nA, width, 1.0, A, nA, 0, Ca, width);
    //blas_syrk(CblasRowMajor, CblasUpper, CblasTrans, nB, width, 1.0, B, nB, 0, Cb, width);
*/

    blas_syrk(CblasRowMajor, CblasUpper, CblasNoTrans, width, nA, 1.0, A, nA, 0, Ca, width);
    blas_syrk(CblasRowMajor, CblasUpper, CblasNoTrans, width, nB, 1.0, B, nB, 0, Cb, width);

    memcpy(C, Ca, width2 * sizeof(MRSESDataType));
    blas_axpy(width2, 1, Cb, 1, C, 1);
    blas_scal(width2, 0.5, C, 1);

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

    lapack_potrf(&hmode, &width, C, &width, &err);
    if (err) return 1;

    lapack_potrf(&hmode, &width, Ca, &width, &err);
    if (err) return 1;

    lapack_potrf(&hmode, &width, Cb, &width, &err);
    if (err) return 1;
    

    detC = C[0]; detAB = Ca[0] * Cb[0];
    for (i = width + 1; i < width2; i+= (width+1)) {
	detAB *= (Ca[i] * Cb[i]);
	detC *= C[i];
    }

	/* we just computing sqrt(detX) actually */
    rcorr = 2 * logf(detC * detC / detAB);

/*
    blas_trsm(
	CblasRowMajor, CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit,
	1, width, 1, C, width, mean, width //1?
    );
*/

    blas_trsv(
	CblasRowMajor, CblasUpper, CblasTrans, CblasNonUnit,
	width, C, width, mean, 1
    );


    rmahal = blas_dot(width, mean, 1, mean, 1);

    switch (mrses->dist) {
	case BHATTACHARYYA:
	    *result = rmahal/8 + rcorr/4;
	break;
	case MAHALANOBIS:
	    *result = rmahal;
	break;
	case CORCOR:
	    *result = rcorr;
	break;
	default:
	    *result = 0;
    }

/*    
    if (!block)
    printf("Det: %f = %f %f (%e)\n", rmahal/8 + rcorr/4, rmahal, rcorr, detC*detC);
*/

    return 0;
}

struct MRSESTemporaryDataT {
    MRSESDataType *A;
    MRSESDataType *B;
    MRSESDataType *C;
    MRSESDataType *Ca;
    MRSESDataType *Cb;
    MRSESDataType *mean;
    MRSESDataType *mean_copy;

#ifndef USE_FAST_RANDOM
//    struct random_data seed;
    unsigned int seed;
#endif /* USE_FAST_RANDOM */
};
typedef struct MRSESTemporaryDataT MRSESTemporaryDataS;
typedef struct MRSESTemporaryDataT *MRSESTemporaryData;

static inline MRSESTemporaryData mrses_ppu_malloc(HWThread thr, MRSESContext mrses) {
    unsigned char *alloc;
    MRSESTemporaryData data;
    int nAB;
    int properties = mrses->properties;
    int width, width2;
    int pos, size;

#ifndef USE_FAST_RANDOM
# ifndef FIX_RANDOM
    struct timeval tv;
# endif /* FIX_RANDOM */
#endif /* USE_FAST_RANDOM */
    
    pos = calc_alloc(properties * sizeof(uint32_t), HW_ALIGN);
    if (thr->data) return (MRSESTemporaryData)(thr->data + pos);

    width = mrses->width;    
    width2 = width * width;
    nAB = max(mrses->nA, mrses->nB);

    size = (
	calc_alloc(properties * sizeof(uint32_t), HW_ALIGN) +
	calc_alloc(sizeof(MRSESTemporaryDataS), HW_ALIGN) +
	2 * calc_alloc(width * nAB * sizeof(MRSESDataType), HW_ALIGN) + 
	3 * calc_alloc(width * width * sizeof(MRSESDataType), HW_ALIGN) +
	2 * calc_alloc(width * sizeof(MRSESDataType), HW_ALIGN)
    );

    posix_memalign((void*)&alloc, HW_ALIGN, size);
    if (!alloc) return NULL;

    memset(alloc, 0, properties * sizeof(uint32_t));
    
    data = (MRSESTemporaryData)(alloc + pos);
    pos += calc_alloc(sizeof(MRSESTemporaryDataS), HW_ALIGN);

    data->A = (MRSESDataType*)(alloc + pos);
    pos += calc_alloc(width * nAB * sizeof(MRSESDataType), HW_ALIGN);

    data->B = (MRSESDataType*)(alloc + pos);
    pos += calc_alloc(width * nAB * sizeof(MRSESDataType), HW_ALIGN);

    data->C = (MRSESDataType*)(alloc + pos);
    pos += calc_alloc(width * width * sizeof(MRSESDataType), HW_ALIGN);

    data->Ca = (MRSESDataType*)(alloc + pos);
    pos += calc_alloc(width * width * sizeof(MRSESDataType), HW_ALIGN);

    data->Cb = (MRSESDataType*)(alloc + pos);
    pos += calc_alloc(width * width * sizeof(MRSESDataType), HW_ALIGN);

    data->mean = (MRSESDataType*)(alloc + pos);
    pos += calc_alloc(width * sizeof(MRSESDataType), HW_ALIGN);

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

#ifndef USE_FAST_RANDOM
# ifdef FIX_RANDOM
//    srandom_r(FIX_RANDOM, &data->seed);
    data->seed = FIX_RANDOM;
# else /* FIX_RANDOM */
    gettimeofday(&tv, NULL);
    data->seed = tv.tv_usec;
//    memset(&data->seed, 0, sizeof(struct random_data));
//    srandom_r(tv.tv_usec, &data->seed);
# endif /* FIX_RANDOM */
#endif /* USE_FAST_RANDOM */

    thr->data = (void*)alloc;

    return data;
}


int mrses_ppu_run(HWThread thr, void *hwctx __attribute__ ((unused)), int block, MRSESContext mrses) {
    int err;
    int i, idx;

    int width = mrses->width;
    int width2 = width * width;
    int alloc = mrses->alloc;
    int nA = mrses->nA;
    int nB = mrses->nB;
    
    MRSESIntType *index = mrses->index + block * width;
    MRSESDataType *result = mrses->result + block;
    
    MRSESDataType *Afull = mrses->A;
    MRSESDataType *Bfull = mrses->B;
    MRSESDataType *Mfull = mrses->mean;

    MRSESTemporaryData data;

    MRSESDataType *A;
    MRSESDataType *B;
    MRSESDataType *C;
    MRSESDataType *Ca;
    MRSESDataType *Cb;
    MRSESDataType *mean;

    data = mrses_ppu_malloc(thr, mrses);
    if (!data) return 1;

    A = data->A;
    B = data->B;
    Ca = data->Ca;
    Cb = data->Cb;
    C = data->C;
    mean = data->mean;

    for (i = 0; i < width; i++) {
	idx = index[i] - 1;
	    
	memcpy(A + i * nA, Afull + idx * alloc, nA * sizeof(MRSESDataType));
	memcpy(B + i * nB, Bfull + idx * alloc, nB * sizeof(MRSESDataType));
	mean[i] = Mfull[idx];
    }
    
    err = mrses_ppu_real_run(
	mrses, result,
	width, width2, nA, nB,
	A, B, mean, C, Ca, Cb
    );

    if (err) {
	MRSESDataType *result = mrses->result + block;
	*result = 0;	/* Just do octave computation instead */
    }
    
    return 0;
}



int mrses_ppu_iterate(HWThread thr, void *hwctx __attribute__ ((unused)), int block_group, MRSESContext mrses) {
    int err;
    int i, idx;

    int iterate_size = mrses->iterate_size;
    int block = block_group * iterate_size;
    int block_end = block + iterate_size;
    
    int width = mrses->width;
    int width2 = width * width;
    int alloc = mrses->alloc;
    int nA = mrses->nA;
    int nB = mrses->nB;
    int properties = mrses->properties;
    int palloc = mrses->palloc;
    int iterations = mrses->iterations;
    
    unsigned int *hist;

    MRSESIntType *index;
    MRSESIntType *ires = mrses->ires; 
    MRSESIntType drp_gen, rpl_gen, rst_gen;
    MRSESDataType result = 0, cur_result = 0;
    
    MRSESDataType *Afull = mrses->A;
    MRSESDataType *Bfull = mrses->B;
    MRSESDataType *Mfull = mrses->mean;

    MRSESTemporaryData data;

    MRSESDataType *A;
    MRSESDataType *B;
    MRSESDataType *C;
    MRSESDataType *Ca;
    MRSESDataType *Cb;
    MRSESDataType *mean;
    MRSESDataType *mean_copy;

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

#ifdef USE_FAST_RANDOM
    unsigned int g_seed;
# ifndef TRACE_TIMINGS
    struct timeval tvseed;
# endif /* TRACE_TIMINGS */
#else /* USE_FAST_RANDOM */
//    int32_t rndval;
#endif /* USE_FAST_RANDOM */

#ifdef TRACE_TIMINGS
    gettimeofday(&tv1, NULL);
#endif /* TRACE_TIMINGS */

#ifdef USE_FAST_RANDOM
# ifdef TRACE_TIMINGS
    g_seed = tv1.tv_usec;
# else
    gettimeofday(&tvseed, NULL);
    g_seed = tvseed.tv_usec;
# endif /* TRACE_TIMINGS */
#endif /* USE_FAST_RANDOM */


    data = mrses_ppu_malloc(thr, mrses);
    if (!data) return 1;

    A = data->A;
    B = data->B;
    Ca = data->Ca;
    Cb = data->Cb;
    C = data->C;
    mean = data->mean;
    mean_copy = data->mean_copy;
    
    hist = (uint32_t*)thr->data;


    for (;block < block_end; ++block) { 
	index = mrses->index + block * palloc;
	
	
	for (i = 0; i < width; ++i) {
	    idx = index[i];

	    memcpy(A + i * nA, Afull + idx * alloc, nA * sizeof(MRSESDataType));
	    memcpy(B + i * nB, Bfull + idx * alloc, nB * sizeof(MRSESDataType));
	    mean[i] = Mfull[idx];
	}
    
	memcpy(mean_copy, mean, width * sizeof(MRSESDataType));

	err = mrses_ppu_real_run(
	    mrses, &cur_result,
	    width, width2, nA, nB,
	    A, B, mean_copy, C, Ca, Cb
	);

	rst_gen = width;
	for (i = 0; i < iterations; i++) {
#ifdef USE_FAST_RANDOM
	    FASTRND(drp_gen, width, 0);
	    FASTRND(rpl_gen, properties - width, width);
#else /* USE_FAST_RANDOM */
	    drp_gen = rnd(width);
	    rpl_gen = rnd(properties - width) + width;
#endif /* USE_FAST_RANDOM */
	
	    if ((rst_gen < width)&&(rst_gen != drp_gen)) {
		idx = index[rst_gen];
	        memcpy(A + rst_gen * nA, Afull + idx * alloc, nA * sizeof(MRSESDataType));
		memcpy(B + rst_gen * nB, Bfull + idx * alloc, nB * sizeof(MRSESDataType));
	        rst_gen = width;
	    }	

	    idx = index[rpl_gen];
	    memcpy(A + drp_gen * nA, Afull + idx * alloc, nA * sizeof(MRSESDataType));
	    memcpy(B + drp_gen * nB, Bfull + idx * alloc, nB * sizeof(MRSESDataType));
	
	    memcpy(mean_copy, mean, width * sizeof(MRSESDataType));
	    mean_copy[drp_gen] = Mfull[idx];
	
	    err = mrses_ppu_real_run(
	        mrses, &result,
		width, width2, nA, nB,
		A, B, mean_copy, C, Ca, Cb
	    );
	
	    if (result < cur_result) {
		rst_gen = drp_gen;
	    } else {
//	    printf("%i=%i %i=%i %f %f\n", drp_gen, index[drp_gen], rpl_gen, index[rpl_gen], cur_result, result);
		cur_result = result;
	        SWAPij(index, drp_gen, rpl_gen);

		mean[drp_gen] = Mfull[idx];
	    }
	}
	for (i = 0; i < width; i++) {
	    hist[index[i]]++;
//	printf("%5i ", index[i] + 1);
	}
	if (ires) {
	    memcpy(ires + width * block, index, width * sizeof(MRSESIntType));
	} 
	
//    printf("\n\n\n");
    }
    
#ifdef TRACE_TIMINGS
    gettimeofday(&tv2, NULL);
    printf("Thead %p: %li ms\n", thr, (tv2.tv_sec - tv1.tv_sec)*1000+(tv2.tv_usec - tv1.tv_usec)/1000);
#endif /* TRACE_TIMINGS */

    return 0;
}