#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;
  }