#include <sys/threads.h>
#include <sys/systick.h>
#include <sys/atomic.h>

#include <stdlib.h>

#include <avr/sleep.h>

int		__main_stack_size__ = 0x80;
extern int __main_stack_size __attribute__((weak, alias("__main_stack_size__")));


extern uint8_t  *pSystickSP;

/* _threading_threads:	Global Thread List
 *
 * listhead: thread->list
 *
 * */
list_t		_threading_threads;
/* _threading_queues:	Thread Queues by Priority (all threads are running)
 *
 * listhead: thread->list_queue
 *
 * */
list_t		_threading_queues[4];

/* _threading_periodics: List of all periodically scheduled Threads
 *
 * listhead: thread->list_periodic
 *
 * */
list_t		_threading_periodics;

/* _threading_wait:		List of all sleeping Threads
 *
 * listhead: thread->list_queue
 *
 * */
list_t		_threading_waiting;


/* _threading_current:	Currently active/scheduled Thread */
THREAD*				_threading_current;

/* Predefined System Threads... */
THREAD* _thread_idle;
THREAD* _thread_main;

void main(void *arg);
void idle(void*);

void __ctx_init(void) __attribute__((naked)) __attribute__((section (".init7")));
void __ctx_init(void)
{
	uint8_t n;

	list_init( &_threading_threads );
	list_init( &_threading_waiting );
	list_init( &_threading_periodics );

	for (n=0;n<4;n++) {
		list_init( &_threading_queues[n] );
	};

	_thread_main = thread_alloc( main, NULL, __main_stack_size );
	_thread_idle = thread_alloc( idle, NULL, 60 );
	thread_set_priority( _thread_idle, TP_IDLE );

};

uint8_t* scheduler(uint8_t* oldstack)
{
	uint8_t	n;
	int f;

	list_t *next = NULL;
	THREAD	*next_thread = NULL;

	if (_threading_current){
		_threading_current->stack.stackpointer = oldstack;

		f = (int)oldstack - (int)_threading_current->stack.base;
		if ( f < _threading_current->stack.min_free)
			_threading_current->stack.min_free = f;

		if (_threading_current->remove) {
			unschedule_thread( _threading_current );
		};

		if (list_next(&_threading_queues[ _threading_current->priority ]) == &(_threading_current->list_queue)){
			list_remove( &(_threading_current->list_queue) );
			list_append( &(_threading_current->list_queue), &_threading_queues[ _threading_current->priority ] );
		};
	};

	for (n=0;n<4;n++) {
		if (!list_is_empty(&_threading_queues[n])) {
			next = _threading_queues[n].next;
			break;
		};
	};

	if (next){
		next_thread = list_entry(next,thread_t,list_queue);
	} else {
		next_thread = _thread_idle;
	};

	next_thread->statistic.scheduled++;

	_threading_current = next_thread;

	return next_thread->stack.stackpointer;
};


void idle(void*arg)
{
	while (1) {
/*		cli();
		set_sleep_mode(0);
		sleep_enable();
		sei();
		sleep_cpu();
		sleep_disable();
*/
	};
};

void schedule_thread(THREAD *thread){
	ATOMIC

	if (thread){
		if (!list_is_empty(&thread->list_queue)) {
			unschedule_thread( thread );
		};

		if (thread->periodic){
			list_append( &thread->list_periodic, &_threading_periodics );
		};

		if (thread->wait){
			list_append( &thread->list_queue, &_threading_waiting );
		} else {
			list_append( &thread->list_queue, &_threading_queues[ thread->priority ]);
		};

	};
};

void unschedule_thread(THREAD *thread){
	ATOMIC

	if (thread) {
		list_remove( &thread->list_queue );
		list_remove( &thread->list_periodic );
	};
};

/* st_schedule(): Aktualisiert die periodische Threadplanung */
void st_schedule(void){
	list_t *lh,*tmp;
	THREAD *t;

	for_each_list_entry_save(lh,tmp,&_threading_waiting) {
		t = list_entry(lh,thread_t,list_queue);

		if (t->timeout && (t->timeout <= _systick_ticks))
		{
			t->wait = 0;
			t->timeout = 0;

			schedule_thread( t );
		} else {
		};
	};

	for_each_list_entry(lh,&_threading_periodics) {
		t = list_entry(lh,thread_t,list_periodic);

		t->periodic -= _systick_us;
		if (t->periodic <= 0) {
			t->periodic += t->periode;
			if (!t->wait) {
				t->periodic_miss = 1;
				t->statistic.periodic_misses++;
			} else {
				t->wait = 0;
				lh = lh->next;

				schedule_thread( t );
			}
		};
	};

};