#include <avr/interrupt.h>
#include <avr/sleep.h>
#include <string.h>
#define F_CPU 12000000UL  // 12 MHz

// Don't trust the compiler; mark everything updated in an
// ISR as volatile.
volatile int turnsRemaining;
volatile char switchState; // switch last time we decided
volatile char switchLast;  // switch last time we checked

// We want to scan ~100 times/sec.
void configureTimer() {
  // TIMER 0
  // Set prescaler to 1/1024
  // 100 cycles/sec
  // no output compare match pin
  // output compare register A triggers interrupt
  TCCR0A = 0x00; // compare
  TCCR0B = 0x03; // prescaler at 1/1024
  TCNT0  = 0x00; // initial cycle at 0
  OCR0A  = 117;  // 117 * 1024 * 100/sec =~ 12MHz
  TIMSK0 = 0x02; // interrupt on match A
}

/*
 * Serial setup
 */
#define FLAG_PENDING_COMMAND 0x01
volatile char flags = 0;

#define RX_BUF_SIZE 64
char rxBuf[RX_BUF_SIZE];
int rxOffset;

#define TX_BUF_SIZE 448
char txBuf[TX_BUF_SIZE];

char* pNextTx = NULL;

void writeLights( unsigned int bits );

void configureSerial() {
  // Setting to 9600 baud, 8N1.
  // 9600 baud: U2X = 0, UBRR = 71
  UBRR0H = 0;
  UBRR0L = 77;
  UCSR0A = 0;
  // enable TX/RX
  UCSR0B = (1<<RXCIE0) | (1<<RXEN0) | (1<<TXEN0);
  // 8 bits, no parity, 1 stop bit
  UCSR0C = (1<<UCSZ01) | (1<<UCSZ00);
  rxOffset = 0;
}

void putString( char* pStr ) {
  pNextTx = pStr;
  UCSR0B |= (1 << UDRIE0);
}

void turn( int turns );

void processCommand()
{
  flags &= ~FLAG_PENDING_COMMAND;
  rxBuf[RX_BUF_SIZE-1] = '\0';
  char* pR = rxBuf;
  if ( *(pR++) != '+' ) return;
  if (strncmp("turn", pR, 4) == 0) {
    int turns = 0;
    pR += 4;
    while ( *pR >= '0' && *pR <= '9' ) {
      turns *= 10;
      turns += *pR - '0';
      pR++;
    }
    if ( *pR == '-' ) {
      turn( turns );
    }
    else
    {
      putString( "No terminator" );
    }
  }
}


#define BUTTON_MASK (1<<5)
#define MOTOR_MASK  (1<<6)

/*
 * PD5    IN   Button pin
 * PD6    OUT  Motor drive
 */
void configurePins()
{
  PORTD  = 0x00;
  DDRD   = MOTOR_MASK;  // Set D6 as output
}

// Maxes out at 3MHz, so we need ~4 nops per cycle to be on the
// safe side.  That, plus possible call overhead, should ensure
// it works.
void nops4() {
  asm volatile("nop\n\t"
	       "nop\n\t"
	       "nop\n\t"
	       "nop\n\t"
	       ::);
}

void nops() {
  nops4();
  nops4();
  nops4();
  nops4();
}

void motorOn() {
  PORTD |= MOTOR_MASK;
}

void motorOff() {
  PORTD &= ~(MOTOR_MASK);
}
  
char readButton() {
  if ( (PIND & BUTTON_MASK) != 0 ) {
    return 0;
  }
  return 1;
}

void turn( int turns ) {
  turnsRemaining = turns;
  while ( turnsRemaining ) {
    motorOn();
    // wait for switch to turn off to indicate we're going
    while ( switchState != 0 ) {}
    // wait for switch to turn back on, indicating a full trip
    while ( switchState == 0 ) {}
    turnsRemaining--;
  }
  motorOff();
}

void init()
{
  cli();
  configurePins();
  configureSerial();
  set_sleep_mode( SLEEP_MODE_IDLE );
  configureTimer();
  switchLast = switchState = readButton();
  sei();
}

ISR(USART_RX_vect)
{  
  rxBuf[ rxOffset ] = UDR0;
  if ( rxBuf[ rxOffset ] == '\n' ) {
    rxBuf[ rxOffset ] = '\0';
    flags |= FLAG_PENDING_COMMAND;
    rxOffset = 0;
  } else {
    rxOffset = (rxOffset+1) % RX_BUF_SIZE;
  }
}

ISR(USART_UDRE_vect)
{
  if (pNextTx == 0 || *pNextTx == '\0') {
    UCSR0B &= ~(1 << UDRIE0);
  } else {
    UDR0=*pNextTx;
    pNextTx++;
  }
}

ISR( TIMER0_COMPA_vect ) {
  // Scan inputs
  char sw = readButton();
  if ( sw == switchLast ) {
    switchState = sw;
  }
  switchLast = sw;
}

int main( void )
{
  init();
  putString( "v1\nInitialized.\n" );
  while (1) {
    if ( (flags & FLAG_PENDING_COMMAND) != 0 ) {
      processCommand();
    }
  }
  return 0;
}