#include <cyg/kernel/kapi.h>
#include <cyg/hal/hal_arch.h>

#include <assert.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <stdint.h>

#include "ezs_fb.h"

#define STACKSIZE (CYGNUM_HAL_STACK_SIZE_MINIMUM+4096)

// Thread user
static cyg_uint8     user_thread_stack[STACKSIZE];
static const uint8_t user_prio=10;
static cyg_handle_t  user_thread_handle;
static cyg_thread    user_thread;

// Thread user2
static cyg_uint8     user2_thread_stack[STACKSIZE];
static const uint8_t user2_prio=11;
static cyg_handle_t  user2_thread_handle;
static cyg_thread    user2_thread;

// Alarm
#define ROUND_PERIOD 1000
static cyg_handle_t real_time_counter;
static cyg_handle_t alarm_alarmhdl;
static cyg_alarm alarm;

static void user_thread_entry(cyg_addrword_t data) {
	int counter = 1;
	while (1) {
		diag_printf("Thread 1 - Data: %d, Counter: %d\n", (int) data, counter++);
		cyg_thread_suspend(cyg_thread_self());
	}
}

static void user2_thread_entry(cyg_addrword_t data) {
	int counter = 1;
	while (1) {
		diag_printf("Thread 2 - Data: %d, Counter: %d\n", (int) data, counter++);
		cyg_thread_resume(user_thread_handle);			
		cyg_thread_suspend(cyg_thread_self());		
	}
}

static cyg_tick_count_t ms_to_ticks(cyg_uint32 ms) {
	cyg_resolution_t resolution = cyg_clock_get_resolution(cyg_real_time_clock());
	const cyg_uint64 delay_ns = ms * 1000000;
	const cyg_tick_count_t ticks = (delay_ns * resolution.divisor)/resolution.dividend; //ticks

	return ticks;
}

static void alarmfn(cyg_handle_t alarmH, cyg_addrword_t data) {
	cyg_thread_resume(user2_thread_handle);
}

// Initialisierung
void cyg_user_start(void) {
	
	cyg_thread_create(user_prio,       		 /* priority 0-31, 0 represents highest priority */
					  &user_thread_entry,    /* thread entry point */
					  42,                    /* data-parameter for thread_entry */
					  "user thread",         /* thread name (for debugging) */
					  user_thread_stack,     /* stack address */
					  STACKSIZE,             /* stack size */
					  &user_thread_handle,   /* thread handle */
					  &user_thread);         /* thread data (internally used by kernel) */
					  
	cyg_thread_create(user2_prio,       		 /* priority 0-31, 0 represents highest priority */
					  &user2_thread_entry,    /* thread entry point */
					  21,                    /* data-parameter for thread_entry */
					  "user 2 thread",         /* thread name (for debugging) */
					  user2_thread_stack,     /* stack address */
					  STACKSIZE,             /* stack size */
					  &user2_thread_handle,   /* thread handle */
					  &user2_thread);         /* thread data (internally used by kernel) */					  
	
// 	cyg_thread_resume(user_thread_handle);	
// 	cyg_thread_resume(user2_thread_handle);	
	
	cyg_clock_to_counter(cyg_real_time_clock(), &real_time_counter);

	cyg_alarm_create(real_time_counter,   /* counter, representing the source of activating events */
	                 alarmfn,             /* alarmfn, handler that is called when the alarm is triggered */
	                 0,                   /* data, passed to setup_round_alarmfn as a parameter when triggered */
	                 &alarm_alarmhdl,     /* alarm handle: needed to reconfigure this alarm, etc */
	                 &alarm);             /* alarm data (internally used by kernel */	
	
	cyg_alarm_initialize(alarm_alarmhdl,
						 1, // Offset
						 ms_to_ticks(1000));
	
}