#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>

#define NB_TICK 104 //1563
#define CPU_FREQ 16000000L
#define QUANTUM_ms 10

#define SAVE_CONTEXT()  \
asm volatile ( \
"push	r0				\n\t" \
"in		r0, __SREG__		\n\t" \
"cli					\n\t" \
"push	r0				\n\t" \
"push	r1				\n\t" \
"clr	r1				\n\t" \
"push	r2				\n\t" \
"push	r3				\n\t" \
"push	r4				\n\t" \
"push	r5				\n\t" \
"push	r6				\n\t" \
"push	r7				\n\t" \
"push	r8				\n\t" \
"push	r9				\n\t" \
"push	r10				\n\t" \
"push	r11				\n\t" \
"push	r12				\n\t" \
"push	r13				\n\t" \
"push	r14				\n\t" \
"push	r15				\n\t" \
"push	r16				\n\t" \
"push	r17				\n\t" \
"push	r18				\n\t" \
"push	r19				\n\t" \
"push	r20				\n\t" \
"push	r21				\n\t" \
"push	r22				\n\t" \
"push	r23				\n\t" \
"push	r24				\n\t" \
"push	r25				\n\t" \
"push	r26				\n\t" \
"push	r27				\n\t" \
"push	r28				\n\t" \
"push	r29				\n\t" \
"push	r30				\n\t" \
"push	r31				\n\t" \
);

#define RESTORE_CONTEXT() \
asm volatile ( \
"pop	r31				\n\t" \
"pop	r30				\n\t" \
"pop	r29				\n\t" \
"pop	r28				\n\t" \
"pop	r27				\n\t" \
"pop	r26				\n\t" \
"pop	r25				\n\t" \
"pop	r24				\n\t" \
"pop	r23				\n\t" \
"pop	r22				\n\t" \
"pop	r21				\n\t" \
"pop	r20				\n\t" \
"pop	r19				\n\t" \
"pop	r18				\n\t" \
"pop	r17				\n\t" \
"pop	r16				\n\t" \
"pop	r15				\n\t" \
"pop	r14				\n\t" \
"pop	r13				\n\t" \
"pop	r12				\n\t" \
"pop	r11				\n\t" \
"pop	r10				\n\t" \
"pop	r9				\n\t" \
"pop	r8				\n\t" \
"pop	r7				\n\t" \
"pop	r6				\n\t" \
"pop	r5				\n\t" \
"pop	r4				\n\t" \
"pop	r3				\n\t" \
"pop	r2				\n\t" \
"pop	r1				\n\t" \
"pop	r0				\n\t" \
"out 		__SREG__, r0		\n\t" \
"pop	r0				\n\t" \
);

/** Configure les ports */
void init_ports(){
	//TODO DDRXs
}

/** ----- Liaison serie ----- */
void init_serial(int speed){
	UBRR0 = CPU_FREQ/(((unsigned long int)speed)<<4)-1; //Set baud rate
	UCSR0B = (1<<TXEN0 | 1<<RXEN0); //Enable transmitter & receiver
	UCSR0C = (1<<UCSZ01 | 1<<UCSZ00); //Set 8 bits character and 1 stop bit
	UCSR0A &= ~(1 << U2X0); //Set off UART baud doubler
}
void send_serial(unsigned char c){
	loop_until_bit_is_set(UCSR0A, UDRE0);
	UDR0 = c;
}
unsigned char get_serial(void){
	loop_until_bit_is_set(UCSR0A, RXC0);
	return UDR0;
}

/** ----- Taches ----- */
void task_print_a(void){
	while(1){
		send_serial('a');
		_delay_ms(1000);
	}
}
void task_print_b(void){
	while(1){
		send_serial('b');
		_delay_ms(700);
	}
}

/** ----- Ordonnancement ----- */
struct task{
	uint16_t sp_vise;
};
uint8_t cpt = 0;
uint8_t premier_lancement = 0;
struct task print_a = {0x0300};
struct task print_b = {0x0500};

/** Démarre le timer pour l'ordonnancement */
void init_timer(){
	TCCR1B |= _BV(WGM12); // CTC mode with value in OCR1A 
	TCCR1B |= _BV(CS12); // CS12 = 1; CS11 = 1; CS10 =1 => CLK/1024 prescaler
	TCCR1B |= _BV(CS10);
	OCR1A = NB_TICK;
	TIMSK1 |= _BV(OCIE1A);
}
/** Changement de contexte */
ISR(TIMER1_COMPA_vect){
	if(premier_lancement == 0){
		premier_lancement++;
		sei();
		SP = print_a.sp_vise;
		task_print_a();
	}else if(premier_lancement == 1){
		SAVE_CONTEXT();
		print_a.sp_vise = SP;
		premier_lancement++;
		sei();
		SP = print_b.sp_vise;
		task_print_b();
	}else{
		if(cpt==0){
			SAVE_CONTEXT();
			print_b.sp_vise = SP;
			SP = print_a.sp_vise;
			RESTORE_CONTEXT();
			cpt++;
		}else if(cpt==1){
			SAVE_CONTEXT();
			print_a.sp_vise = SP;
			SP = print_b.sp_vise;
			RESTORE_CONTEXT();
			cpt = 0;
		}
	}
	sei();
}

int main(void){
	// Initialisation
	init_timer();
	init_ports();
	init_serial(9600);
	sei();

	while(1){}
	return 0;
}