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

#include "serial.h"
#include "main.h"

#define NB_TICK 104 //1563

/** ----- Taches ----- */
void task_deplacement(void){
    
}
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 deplacement = {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 = deplacement.sp_vise;
		task_deplacement();
	}else if(premier_lancement == 1){
		SAVE_CONTEXT();
		deplacement.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 = deplacement.sp_vise;
			RESTORE_CONTEXT();
			cpt++;
		}else if(cpt==1){
			SAVE_CONTEXT();
			deplacement.sp_vise = SP;
			SP = print_b.sp_vise;
			RESTORE_CONTEXT();
			cpt = 0;
		}
	}
	sei();
}

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

	while(1){}
	return 0;
}

