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

#include "serial.h"
#include "main.h"
#include "deplacement.h"
#include "tlc5947.h"

#define NB_TICK 104 //1563

/** ----- Taches ----- */
void task_deplacement_gauche(void){
	deplacement(GAUCHE);
}
void task_deplacement_droit(void){
	deplacement(DROIT);
}
void task_com_bluetooth(void){
	// TODO
}
void task_leds(void){
	// TODO
}

/** ----- Ordonnancement ----- */
struct task{
	uint16_t sp_vise;
};
uint8_t cpt = 0;
uint8_t premier_lancement = 0;
struct task deplacement_gauche = {0x0300};
struct task deplacement_droit = {0x0500};
struct task com_bluetooth = {0x0700};
struct task leds = {0x0900};

// 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_gauche.sp_vise;
		task_deplacement_gauche();
	}else if(premier_lancement == 1){
		SAVE_CONTEXT();
		deplacement_gauche.sp_vise = SP;
		premier_lancement++;
		sei();
		SP = deplacement_droit.sp_vise;
		task_deplacement_droit();
	}else if(premier_lancement == 2){
		SAVE_CONTEXT();
		deplacement_droit.sp_vise = SP;
		premier_lancement++;
		sei();
		SP = com_bluetooth.sp_vise;
		task_com_bluetooth();
	}else if(premier_lancement == 3){
		SAVE_CONTEXT();
		com_bluetooth.sp_vise = SP;
		premier_lancement++;
		sei();
		SP = leds.sp_vise;
		task_leds();
	}else{
		if(cpt==0){
			SAVE_CONTEXT();
			com_bluetooth.sp_vise = SP;
			SP = deplacement_gauche.sp_vise;
			RESTORE_CONTEXT();
			cpt++;
		}else if(cpt==1){
			SAVE_CONTEXT();
			deplacement_gauche.sp_vise = SP;
			SP = deplacement_droit.sp_vise;
			RESTORE_CONTEXT();
			cpt++;
		}else if(cpt==2){
			SAVE_CONTEXT();
			deplacement_droit.sp_vise = SP;
			SP = com_bluetooth.sp_vise;
			RESTORE_CONTEXT();
			cpt++;
		}else if(cpt==3){
			SAVE_CONTEXT();
			com_bluetooth.sp_vise = SP;
			SP = leds.sp_vise;
			RESTORE_CONTEXT();
			cpt = 0;
		}
	}
	sei();
}

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

	while(1){}
	return 0;
}