#include <avr/io.h>
  #include <util/delay.h>
  
  #define SERIAL_BAUDE_RATE 9600

  #define DELAI_STEP 2

  
  #define DDR_GAUCHE DDRB
  #define PORT_GAUCHE PORTB
  #define STEP_MOTEUR_GAUCHE 0x40 // pin 12
  #define DIR_MOTEUR_GAUCHE 0x20 // pin 11
  
  #define DDR_DROIT DDRH
  #define PORT_DROIT PORTH
  #define STEP_MOTEUR_DROIT 0x10 // pin 7
  #define DIR_MOTEUR_DROIT 0x08 // pin 6

  #define CPU_FREQ 16000000L
  

  //gestion des moteurs
  void init_pins(void){

      DDR_GAUCHE = STEP_MOTEUR_GAUCHE | DIR_MOTEUR_GAUCHE;
  	DDR_DROIT = STEP_MOTEUR_DROIT | DIR_MOTEUR_DROIT;

  }
  

  void pas_moteur(int g, int d){

      if (g != 0)

          PORT_GAUCHE |= STEP_MOTEUR_GAUCHE;

      if (d != 0)

          PORT_DROIT |= STEP_MOTEUR_DROIT;

      _delay_ms(DELAI_STEP);

      if (g != 0)

          PORT_GAUCHE &= ~STEP_MOTEUR_GAUCHE;

      if (d != 0)

          PORT_DROIT &= ~STEP_MOTEUR_DROIT;

      _delay_ms(DELAI_STEP);

  }
  

  void moteur_set_dir(int g, int d){
  	if(g != 0)

          PORT_GAUCHE |= DIR_MOTEUR_GAUCHE;

  	else

          PORT_DROIT &= ~DIR_MOTEUR_GAUCHE;

  	if(d != 0)

          PORT_GAUCHE |= DIR_MOTEUR_DROIT;

  	else

          PORT_DROIT &= ~DIR_MOTEUR_DROIT;

  }
  

  //gestion de la 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;
  }
  
  int main(void){
      init_pins();
      init_serial(SERIAL_BAUDE_RATE);
  
      while(1){
          unsigned char deltaL1 = get_serial();
          unsigned char deltaL2 = get_serial();
          unsigned char deltaR1 = get_serial();
          unsigned char deltaR2 = get_serial();

          // conversion 2x8 bits => 16 bits

          uint16_t deltaL = (deltaL1 << 8) + deltaL2;
          uint16_t deltaR = (deltaR1 << 8) + deltaR2;
  		
  		// sens rotation

  		moteur_set_dir((deltaL & 0x80) == 0x80 ? 1 : 0, (deltaR & 0x80) == 0x80 ? 1 : 0);

  		deltaL &= 0x7F;
  		deltaR &= 0x7F;

          uint16_t deltaMax = deltaL > deltaR ? deltaL : deltaR;

  		uint16_t i;
          for(i=0; i<deltaMax; i++){

              if(deltaL > i && deltaR > i)
                  pas_moteur(1, 1);
              else if(deltaL > i)
                  pas_moteur(1, 0);
              else if(deltaR > i)
                  pas_moteur(0, 1);

          }

  		
  		send_serial(0x01);

      }
      return 0;
  }