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

int buttons;
int leds;

/*
 * Timer setup
 */

// 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);
}

unsigned char hexChar( int v ) {
  if ( v < 10 ) return '0' + v;
  return 'a' + (v - 10);
}

unsigned char fromHex( unsigned char h ) {
  if ( h >='0' && h <='9' ) { return h-'0'; }
  if ( h >='a' && h <='f' ) { return 10+(h-'a'); }
  if ( h >='A' && h <='F' ) { return 10+(h-'A'); }
  return 0;
}

void processCommand()
{
  flags &= ~FLAG_PENDING_COMMAND;
  rxBuf[RX_BUF_SIZE-1] = '\0';
  char* pR = rxBuf;
  if ( *(pR++) != '+' ) return;
  if (strncmp("read", pR, 4) == 0) {
    pR += 4;
    if ( *pR == '-' ) {
      txBuf[0] = '0';
      txBuf[1] = 'x';
      txBuf[2] = hexChar( buttons >> 4 );
      txBuf[3] = hexChar( buttons & 0xf );
      txBuf[4] = '\n';
      txBuf[5] = '\0';
      putString( txBuf );
    }
    else
    {
      putString( "No terminator" );
    }
  } else if (strncmp("show", pR, 4) == 0) {
    unsigned int v = 0;
    pR += 4;
    while ( *pR != '-' ) {
      v = v << 4;
      v = v | fromHex( *(pR++) );
    }
    if ( *pR == '-' ) {
      writeLights( v );
    }
    else
    {
      putString( "No terminator" );
    }
  }
}


#define B_LATCH_MASK (1<<6)
#define B_CLOCK_MASK (1<<7)
#define B_IN_MASK    (1<<5)

#define L_OUT_MASK   (1<<0)
#define L_CLOCK_MASK (1<<1)
#define L_LATCH_MASK (1<<2)

/*
 * PD6    OUT  Configure as button latch
 * PD7    OUT  Configure as button clock
 * PD5    IN   Configure as button data in
 * PC0    OUT  Configure as LED data out
 * PC1    OUT  Configure as LED clock
 * PC2    OUT  Configure as LED latch
 */
void configurePins()
{
  PORTC  = 0x00;
  PORTD  = 0x00;
  DDRC   = L_OUT_MASK | L_LATCH_MASK | L_CLOCK_MASK;  // Set C0-2 as outputs
  DDRD   = B_LATCH_MASK | B_CLOCK_MASK;  // Set D5-6 as outputs
}

// 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();
}

inline void buttonClock() {
  nops();
  // clock rise
  PORTD |= B_CLOCK_MASK;
  nops();
  // clock fall
  PORTD &= ~(B_CLOCK_MASK);
}

inline void ledClock() {
  nops();
  // clock rise
  PORTC |= L_CLOCK_MASK;
  nops();
  // clock fall
  PORTC &= ~(L_CLOCK_MASK);
}
  
unsigned char readButtons() {
  int i = 8;
  unsigned char v = 0;
  // first, get parallel inputs.  Latch high, cycle clock.
  PORTD |= B_LATCH_MASK;
  buttonClock();
  PORTD &= ~(B_LATCH_MASK);
  nops();
  while (i--) {
    v = v << 1;
    if ( (PIND & B_IN_MASK) != 0 ) {
      v = v | 0x01;
    }
    buttonClock();
  }
  return v;
}

void writeLights( unsigned int bits ) {
  int i = 16;
  while ( i-- ) {
    nops();
    if ( (bits & 0x01) == 0 ) {
      PORTC &= ~(L_OUT_MASK);
    } else {
      PORTC |= L_OUT_MASK;
    }
    ledClock();
    bits = bits >> 1;
  }
  // toggle latch
  nops();
  PORTC |= L_LATCH_MASK;
  nops();
  PORTC &= ~(L_LATCH_MASK);
}

void init()
{
  cli();
  configurePins();
  configureSerial();
  set_sleep_mode( SLEEP_MODE_IDLE );
  configureTimer();
  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
  buttons = readButtons();
  // Refresh outputs
}

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