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

#define SERIAL_BAUDE_RATE 9600
#define DELAI_STEP 1000
#define PIN_MOTEUR_GAUCHE 0x40 // pin 12
#define PIN_MOTEUR_DROIT 0x20 // pin 11

#define CPU_FREQ 16000000L

//gestion des moteurs
void init_pins(void){
    // pin 12 et 11
    DDRB = PIN_MOTEUR_GAUCHE | PIN_MOTEUR_DROIT;
}

void pas_moteur_gauche(void){
    // pin 12
    PORTB |= PIN_MOTEUR_GAUCHE;
    _delay_us(DELAI_STEP);
    PORTB &= ~PIN_MOTEUR_GAUCHE;
    _delay_us(DELAI_STEP);
}

void pas_moteur_droit(void){
    // pin 11
    PORTB |= PIN_MOTEUR_DROIT;
    _delay_us(DELAI_STEP);
    PORTB &= ~PIN_MOTEUR_DROIT;
    _delay_us(DELAI_STEP);
}

//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
        int deltaL = (deltaL1 << 8) + deltaL2;
        int deltaR = (deltaR1 << 8) + deltaR2;
        
        int deltaMax = deltaL > deltaR ? deltaL : deltaR;
        for(int i=0; i<deltaMax; i++){
            if(deltaL > i)
                pas_moteur_gauche();
            if(deltaR > i)
                pas_moteur_droit();
        }
	
	send_serial(0x01);
    }
    return 0;
}