#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/version.h>

#include <rtl_sched.h>

#define EXEC 11
#define ACTIV 22
#define OFFSET 750000
#define NTASKS 4

#define DELAY           10000

#define AUX_FIFO            1
#define FIFO_SIZE_IN     4096

pthread_t tasks[NTASKS];
pthread_t cbs_task;

pthread_attr_t attrib;
struct {
    int id;
    int compute;
    int period;
    hrtime_t deadline;
    hrtime_t budget;
    struct sched_param params;
} sched_attrib[6];

hrtime_t now;

void *fun(void *arg)
{
    int id = (int) arg;
    int compute = sched_attrib[id].compute;
    int dummy_loop, x;
    int end = 100;

    x = pthread_make_periodic_np(pthread_self(), now + OFFSET,
				 sched_attrib[id].period);

    while (end != 0) {

	pthread_wait_np();

	for (dummy_loop = 0; dummy_loop < compute; dummy_loop++) {
	    rtl_delay(DELAY);
	}
	end--;
    }

    pthread_exit(0);
    return (void *) 0;
}

void *fun_cbs(void *arg)
{
    int id = (int) arg;
    int compute = sched_attrib[id].compute;
    int dummy_loop;
    int end = 5;

    pthread_initcbs_np(pthread_self(), sched_attrib[id].period);
    while (end != 0) {
	pthread_suspend_np(pthread_self());
	for (dummy_loop = 0; dummy_loop < compute; dummy_loop++) {
	    rtl_delay(DELAY);
	}
    }
    end--;

    pthread_exit(0);
    return (void *) 0;
}

int my_handler(unsigned int fifo)
{
    int err, event;

    while ((err =
	    rtf_get(AUX_FIFO, &event, sizeof(event))) == sizeof(event)) {

	pthread_wakeup_np(cbs_task);
    }
    if (err != 0) {
	return -EINVAL;
    }
    return 0;
}

int init_module(void)
{
    int x;
    rtf_destroy(AUX_FIFO);
    rtf_create(AUX_FIFO, FIFO_SIZE_IN);

    now = gethrtime();

    sched_attrib[0].compute = 80;
    sched_attrib[0].period = 6 * 1000 * 1000;
    sched_attrib[0].params.sched_priority = 10;
    sched_attrib[0].deadline = 6 * 1000 * 1000;

    sched_attrib[1].compute = 240;
    sched_attrib[1].period = 10 * 1000 * 1000;
    sched_attrib[1].params.sched_priority = 10;
    sched_attrib[1].deadline = 10 * 1000 * 1000;

    sched_attrib[2].compute = 300;
    sched_attrib[2].period = 11 * 1000 * 1000;
    sched_attrib[2].params.sched_priority = 10;
    sched_attrib[2].deadline = 11 * 1000 * 1000;

    sched_attrib[3].compute = 350;
    sched_attrib[3].period = 19 * 1000 * 1000;
    sched_attrib[3].params.sched_priority = 10;
    sched_attrib[3].deadline = 19 * 1000 * 1000;

/// CBS_TASKS
    sched_attrib[4].compute = 100;
    sched_attrib[4].period = 1.2 * 1000 * 1000;
    sched_attrib[4].params.sched_priority = 10;
    sched_attrib[4].deadline = 1.2 * 1000 * 1000;
    sched_attrib[4].budget = 200 * 1000;


    for (x = 0; x < NTASKS; x++) {
	pthread_attr_init(&attrib);

	pthread_attr_setschedparam(&attrib, &sched_attrib[x].params);
	pthread_attr_setdeadline_np(&attrib, sched_attrib[x].deadline);
	pthread_attr_setschedpolicy(&attrib, SCHED_EDF_NP);

	pthread_create(&(tasks[x]), &attrib, fun, (void *) x);

	pthread_attr_destroy(&attrib);
    }

    pthread_attr_init(&attrib);

    pthread_attr_setschedparam(&attrib,   &sched_attrib[x].params);
    pthread_attr_setinitbudget_np(&attrib, sched_attrib[x].budget);
    pthread_attr_setschedpolicy(&attrib, SCHED_CBS_NP);

    pthread_create(&cbs_task, &attrib, fun_cbs, (void *) x);

    pthread_attr_destroy(&attrib);


    rtf_create_handler(AUX_FIFO, &my_handler);
    return 0;
}

void cleanup_module(void)
{
    int x;
    for (x = 0; x < NTASKS; x++) {
	pthread_cancel(tasks[x]);
	pthread_join(tasks[x], NULL);
    }

    pthread_cancel(cbs_task);
    pthread_join(cbs_task, NULL);

    rtf_destroy(AUX_FIFO);
}