/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 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 * properties;

	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;

}