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

#define M1 0
#define M2 1
#define MOTOR_PORT PORTB
#define ENAB(i) ((i) ? PB2 : PB0)
#define DIR_A(i) ((i) ? PB3 : PB1)
#define DIR_B(i) ((i) ? PB5 : PB4)

unsigned char velocity[2];
unsigned char cycleCounter = 0;

int main(void) {
  // Set USART,
  // enable "databyte received" interrupt
  UBRRH=0;
  UBRRL=51;
  UCSRB=(1<<RXCIE)|(1<<RXEN)|(1<<TXEN);
  UCSRC=(1<<URSEL)|(1<<USBS)|(3<<UCSZ0);

  // Set IO
  DDRB=DDRC=0xFF;

  // Set motor direction
  MOTOR_PORT=(1<<DIR_A(M1)|(1<<DIR_A(M2));

  // Set initial velocity
  velocity[M1] = 0;
  velocity[M2] = 0;

  // Enable interrupt system	
  sei();

  // PWM generation loop
  while(1) {
    // rotate from 0 to 255
    cycleCounter++;

    if(cycleCounter < velocity[M1])
      MOTOR_PORT |= (1<<ENAB(M1));
    else
      MOTOR_PORT &= ~(1<<ENAB(M1));

    if(cycleCounter < velocity[M2])
      MOTOR_PORT |= (1<<ENAB(M2));
    else	
      MOTOR_PORT &= ~(1<<ENAB(M2));
  }

  return 0;
}

// Interrupt handler - databyte received
SIGNAL(SIG_UART_RECV) {
  // Wait for receive completion
  while ( !(UCSRA & (1<<RXC)) );
  unsigned char data = UDR;

  // Set velocity
  velocity[M1] = ((data &0x07) << 5);
  velocity[M2] = ((data &0x70) << 1);

  // wipe previous configuration
  // of driving direction
  MOTOR_PORT &= ~((1<<DIR_B(M1) |
    (1<<DIR_A(M1) | (1<<DIR_B(M2)) |
    (1<<DIR_A(M2));

  // Set driving direction	
  MOTOR_PORT |= ((data & 0x08) > 0) ?
    (1 << DIR_A(M1)) : (1 << DIR_B(M1));
  MOTOR_PORT |= ((data & 0x80) > 0) ?
    (1 << DIR_A(M2)) : (1 << DIR_B(M2));
}