/normxcorr/trunk

#define _GNU_SOURCE

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#ifdef HW_HAVE_SCHED_HEADERS
# include <sys/types.h>
# include <unistd.h>
# include <sched.h>
#endif /* HW_HAVE_SCHED_HEADERS */

#include "normxcorr_hw_msg.h"
#include "hw_sched.h"

//#include "mrses_ppu.h"


#define MUTEX_INIT(ctx, name) \
    if (!err) { \
	ctx->name##_mutex = g_mutex_new(); \
	if (!ctx->name##_mutex) err = DICT_ERROR_GLIB; \
    }
    
#define MUTEX_FREE(ctx, name) \
    if (ctx->name##_mutex) g_mutex_free(ctx->name##_mutex);

#define COND_INIT(ctx, name) \
    MUTEX_INIT(ctx, name##_cond) \
    if (!err) { \
	ctx->name##_cond = g_cond_new(); \
	if (!ctx->name##_cond) { \
	    err = DICT_ERROR_GLIB; \
	    MUTEX_FREE(ctx, name##_cond) \
	} \
    }

#define COND_FREE(ctx, name) \
    if (ctx->name##_cond) g_cond_free(ctx->name##_cond); \
    MUTEX_FREE(ctx, name##_cond)

HWRunFunction ppu_run[] = {
    (HWRunFunction)NULL
};

static int hw_sched_initialized = 0;


int hw_sched_get_cpu_count() {
#ifdef HW_HAVE_SCHED_HEADERS
    int err;

    int cpu_count;
    cpu_set_t mask;

    err = sched_getaffinity(getpid(), sizeof(mask), &mask);
    if (err) return 1;

# ifdef CPU_COUNT
    cpu_count = CPU_COUNT(&mask);
# else
#  ifndef CPU_SETSIZE
#    define CPU_SET_SIZE 64
#  endif /* CPU_SETSIZE */
    for (cpu_count = 0; cpu_count < CPU_SETSIZE; cpu_count++) {
	if (!CPU_ISSET(cpu_count, &mask)) break;
    }
# endif

    if (!cpu_count) cpu_count = 1;
    return cpu_count;    
#else /* HW_HAVE_SCHED_HEADERS */
    return 1;
#endif /* HW_HAVE_SCHED_HEADERS */
}

int hw_sched_init() {
    if (!hw_sched_initialized) {
	g_thread_init(NULL);

#ifdef HAVE_OPENMP
	int cpus = hw_sched_get_cpu_count() / 2;
	omp_set_num_threads(cpus?cpus:1);
#endif /* HAVE_OPENMP */

	hw_sched_initialized = 1;
    }

    return 0;
}


HWSched hw_sched_create(int ppu_count) {
    int i;
    int err = 0;

    HWSched ctx;

    hw_sched_init();
    
    ctx = (HWSched)malloc(sizeof(HWSchedS));
    if (!ctx) return NULL;

    memset(ctx, 0, sizeof(HWSchedS));

    ctx->status = 1;

    MUTEX_INIT(ctx, data);
    COND_INIT(ctx, compl);
    COND_INIT(ctx, job);
    
    if (err) {
	reportError("Error initializing conditions and mutexes");
	hw_sched_destroy(ctx);
	return NULL;
    }

    ctx->n_threads = 0;
    for (i = 0; i < ppu_count; i++) {
	ctx->thread[ctx->n_threads] = hw_thread_create(ctx, ctx->n_threads, NULL, ppu_run, NULL);
	if (ctx->thread[ctx->n_threads]) ++ctx->n_threads;
    }
    
    return ctx;
}

static int hw_sched_wait_threads(HWSched ctx) {
    int i = 0;
    
    hw_sched_lock(ctx, compl_cond);
    while (i < ctx->n_threads) {
        for (; i < ctx->n_threads; i++) {
	    if (ctx->thread[i]->status == HW_THREAD_STATUS_INIT) {
		hw_sched_wait(ctx, compl);
		break;
	    }
	}
	
    }
    hw_sched_unlock(ctx, compl_cond);
    
    ctx->started = 1;

    return 0;
}

void hw_sched_destroy(HWSched ctx) {
    int i;

    if (ctx->n_threads > 0) {
	if (!ctx->started) {
	    hw_sched_wait_threads(ctx);
	}

	ctx->status = 0;
	hw_sched_lock(ctx, job_cond);
	hw_sched_broadcast(ctx, job);
	hw_sched_unlock(ctx, job_cond);
    
	for (i = 0; i < ctx->n_threads; i++) {
	    hw_thread_destroy(ctx->thread[i]);
	}
    }

    COND_FREE(ctx, job);
    COND_FREE(ctx, compl);
    MUTEX_FREE(ctx, data);

    free(ctx);
}

int hw_sched_limit_num_threads(HWSched ctx, int count) {
    ctx->n_threads_actual = count;
    return 0;
}


int hw_sched_set_sequential_mode(HWSched ctx, int *n_blocks, int *cur_block) {
    ctx->mode = HW_SCHED_MODE_SEQUENTIAL;
    ctx->n_blocks = n_blocks;
    ctx->cur_block = cur_block;
    
    return 0;
}

int hw_sched_get_chunk(HWSched ctx, int thread_id) {
    int block;

    if ((ctx->n_threads_actual)&&(thread_id >= ctx->n_threads_actual)) return -1;

    switch (ctx->mode) {
	case HW_SCHED_MODE_PREALLOCATED:
	    if (ctx->thread[thread_id]->status == HW_THREAD_STATUS_IDLE) {
		return thread_id;
	    } else {
		return -1;
	    }
	case HW_SCHED_MODE_SEQUENTIAL:
	    hw_sched_lock(ctx, data);
	    block = *ctx->cur_block;
	    if (block < *ctx->n_blocks) {
		*ctx->cur_block = *ctx->cur_block + 1;
	    } else {
		block = -1;
	    }
	    hw_sched_unlock(ctx, data);
	    return block;
	default:
	    return -1;
    }

    return -1;
}

    
int hw_sched_schedule_task(HWSched ctx, void *appctx, HWEntry entry) {
    if (!ctx->started) {
	hw_sched_wait_threads(ctx);
    }
    
    ctx->ctx = appctx;
    ctx->entry = entry;
    
    hw_sched_lock(ctx, compl_cond);

    hw_sched_lock(ctx, job_cond);
    hw_sched_broadcast(ctx, job);
    hw_sched_unlock(ctx, job_cond);

    return 0;
}

int hw_sched_wait_task(HWSched ctx) {
    int i = 0;

    while (i < ctx->n_threads) {
        for (; i < ctx->n_threads; i++) {
	    if (ctx->thread[i]->status == HW_THREAD_STATUS_DONE) {
		ctx->thread[i]->status = HW_THREAD_STATUS_IDLE;
	    } else {
		hw_sched_wait(ctx, compl);
		break;
	    }
	}
	
    }

    hw_sched_unlock(ctx, compl_cond);

    return 0;
}