/* based_mt_task.c -- A basic multi-threaded real-time task skeleton. 
 *
 * This (by itself useless) task demos how to setup a multi-threaded LITMUS^RT
 * real-time task. Familiarity with the single threaded example (base_task.c)
 * is assumed.
 *
 * Currently, liblitmus still lacks automated support for real-time
 * tasks, but internaly it is thread-safe, and thus can be used together
 * with pthreads.
 */

#include <stdio.h>
#include <stdlib.h>
/* Extras */
#include <stdint.h>
#include <unistd.h>
#include <assert.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>

/* Include gettid() */
#include <sys/types.h>

/* Include threading support. */
#include <pthread.h>

/* Include the LITMUS^RT API.*/
#include "litmus.h"

/* Let's create 4 threads in the example,
 */
#define NUM_THREADS      4
#define MAX_PHASES	10

/* The information passed to each thread. Could be anything. */
struct thread_context {
	int id;
	int fd;
	int semaphore;
	lt_t exec;
	lt_t period;
	int split;
	int phases;
	double initial_phase;
	double locked[MAX_PHASES];
	double unlocked[MAX_PHASES];
};

/* The real-time thread program. Doesn't have to be the same for
 * all threads. Here, we only have one that will invoke job().
 */
void* rt_thread(void *tcontext);

/* Declare the periodically invoked job. 
 * Returns 1 -> task should exit.
 *         0 -> task should continue.
 */
int job(struct thread_context* ctx);


/* Catch errors.
 */
#if 0
#define CALL( exp ) do { \
		int ret; \
		ret = exp; \
		if (ret != 0) \
			fprintf(stderr, "%s failed: %m\n", #exp);\
		else \
			fprintf(stderr, "%s ok.\n", #exp); \
	} while (0)
#endif
#define CALL( exp ) exp

/* Basic setup is the same as in the single-threaded example. However, 
 * we do some thread initiliazation first before invoking the job.
 */
int main(int argc, char** argv)
{
	int i;
	struct thread_context ctx[NUM_THREADS];
	pthread_t             task[NUM_THREADS];
	int fd;

	/* The task is in background mode upon startup. */		


	/*****
	 * 1) Command line paramter parsing would be done here.
	 */


       
	/*****
	 * 2) Work environment (e.g., global data structures, file data, etc.) would
	 *    be setup here.
	 */



	/*****
	 * 3) Initialize LITMUS^RT.
	 *    Task parameters will be specified per thread.
	 */
	fd = open("semaphores", O_RDONLY | O_CREAT, S_IRUSR | S_IWUSR);
	CALL( init_litmus() );


	/***** 
	 * 4) Launch threads.
	 */

	ctx[0].id = 0;
	ctx[0].fd = fd;
	ctx[0].exec = 10500000;
	ctx[0].period = 20000000;
	ctx[0].split = 5;
	ctx[0].phases = 1;
	ctx[0].initial_phase = .0025;
	ctx[0].locked[0] = .003;
	ctx[0].unlocked[0] = .0045;

	ctx[1].id = 1;
	ctx[1].fd = fd;
	ctx[1].exec = 10500000;
	ctx[1].period = 20000000;
	ctx[1].split = 5;
	ctx[1].phases = 1;
	ctx[1].initial_phase = .003;
	ctx[1].locked[0] = .002;
	ctx[1].unlocked[0] = .005;

	ctx[2].id = 2;
	ctx[2].fd = fd;
	ctx[2].exec = 4050000;
	ctx[2].period = 10000000;
	ctx[2].split = 1;
	ctx[2].phases = 0;
	ctx[2].initial_phase = .004;

	ctx[3].id = 3;
	ctx[3].fd = fd;
	ctx[3].exec = 4050000;
	ctx[3].period = 10000000;
	ctx[3].split = 1;
	ctx[3].phases = 0;
	ctx[3].initial_phase = .004;

	for (i = 0; i < NUM_THREADS; i++){
		pthread_create(task + i, NULL, rt_thread, (void *) (ctx + i));
	}

	
	/*****
	 * 5) Wait for RT threads to terminate.
	 */
	for (i = 0; i < NUM_THREADS; i++)
		pthread_join(task[i], NULL);
	

	/***** 
	 * 6) Clean up, maybe print results and stats, and exit.
	 */
	return 0;
}



/* A real-time thread is very similar to the main function of a single-threaded
 * real-time app. Notice, that init_rt_thread() is called to initialized per-thread
 * data structures of the LITMUS^RT user space libary.
 */
void* rt_thread(void *tcontext)
{
	int do_exit = 0;
	struct thread_context *ctx = (struct thread_context *) tcontext;

	/* Make presence visible. */
	printf("RT Thread %d active.\n", ctx->id);

	/*****
	 * 1) Initialize real-time settings.
	 */
	CALL( init_rt_thread() );
	
	ctx->semaphore = open_fmlp_sem(ctx->fd, 0);
        CALL( sporadic_task_ns(ctx->exec, ctx->period, 0, ctx->split, 1, 0,
                                   RT_CLASS_HARD, PRECISE_ENFORCEMENT, 1));


	/*****
	 * 2) Transition to real-time mode.
	 */
	CALL( task_mode(LITMUS_RT_TASK) );

	/* The task is now executing as a real-time task if the call didn't fail. 
	 */

        printf("[%d] Waiting for TS release.\n ", ctx->id);
	wait_for_ts_release();


	/*****
	 * 3) Invoke real-time jobs.
	 */
	do {
		/* Wait until the next job is released. */
		sleep_next_period();
		/* Invoke job. */
		do_exit = job(ctx);		
	} while (!do_exit);


	
	/*****
	 * 4) Transition to background mode.
	 */
	CALL( task_mode(BACKGROUND_TASK) );


	return NULL;
}

#define NUMS 4096
static int num[NUMS];

static int loop_once(void)
{
	int i, j = 0;
	for (i = 0; i < NUMS; i++)
		j += num[i]++;
	return j;
}

static int loop_for(double exec_time, double emergency_exit)
{
	double last_loop = 0, loop_start;
	int tmp = 0;

	double start = cputime();
	double now = cputime();
	while (now + last_loop < start + exec_time) {
		loop_start = now;
		tmp += loop_once();
		now = cputime();
		last_loop = now - loop_start;
		if (emergency_exit && wctime() > emergency_exit) {
			/* Oops --- this should only be possible if the execution time tracking
			 * is broken in the LITMUS^RT kernel. */
			fprintf(stderr, "!!! fmlp_test_task/%d emergency exit!\n", getpid());
			fprintf(stderr, "Something is seriously wrong! Do not ignore this.\n");
			break;
		}
	}

	return tmp;
}

int job(struct thread_context* ctx) 
{
	int i;
	/* Do real-time calculation. */
	double emergency_exit = ctx->initial_phase;
	for (i = 0; i < ctx->phases; i++) {
		emergency_exit += ctx->locked[i];
		emergency_exit += ctx->unlocked[i];
	}
	emergency_exit *= 200;
	emergency_exit += wctime();
	loop_for(ctx->initial_phase, emergency_exit);
	for (i = 0; i < ctx->phases; i++) {
		CALL( litmus_lock( 0 ));
		loop_for(ctx->locked[i], emergency_exit);
		CALL( litmus_unlock( 0 ));
		loop_for(ctx->unlocked[i], emergency_exit);
	}
	/* Don't exit. */
	return 0;
}