#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/sleep.h>
#include <string.h>
#define F_CPU 8000000UL  // 8 MHz

#define T1MS  (F_CPU/1000) 
//((unsigned int)(65536UL - (3*T1MS)))
#define T1BOT 41536 
#define T1INC 31

#define MAX_SERVOS 15

unsigned char servoPos[MAX_SERVOS];
unsigned int whichServo = 0;

void configureTimer() {
  // TIMER 0
  // total cycle: 1/30s
  TCCR0A = 0x00;
  TCCR0B = 0x05;
  TCNT0 = 0x00;
  TIMSK0 = 0x01;  // interrupt on overflow

  
  // TIMER 1
  // total cycle: 2ms
  // set compare match at servo posn
  // no prescaling
  // Set prescaler to 1/16
  // 100 cycles/sec
  // no output compare match pin
  // output compare register A triggers interrupt
  TCCR1A = 0x00; // compare
  TCCR1B = 0x01; // clock on, no prescaler
  TCNT1 = T1BOT; // initial cycle at 0
  OCR1A = T1BOT + ((3*T1MS)/2);
  OCR1B = T1BOT + ((3*T1MS)/2);
  TIMSK1 = 0x07; // interrupt on match A, match B, and overflow
}

/*
 * 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 pulseStart(int servo);
void pulseEnd();

void writeLights( unsigned int bits );

void configureSerial() {
  // Setting to 9600 baud, 8N1, @ 8Mhz
  // 9600 baud: U2X = 1, UBRR = 103
  UBRR0H = 0;
  UBRR0L = 103;
  UCSR0A = 0x02;
  // 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 fromHex( unsigned char hc ) {
  if ( hc >= '0'&& hc <= '9' ) {
    return hc - '0';
  } else if ( hc >= 'a' && hc <= 'f' ) {
    return 10 + hc - 'a';
  } else if ( hc >= 'A' && hc <= 'F' ) {
    return 10 + hc - 'A';
  }
  return 0;
}

#define BAD "Bad command\n"

/// Commands are of the form "[LTS][0-4]XX", where XX is the hex position
void processCommand()
{
  flags &= ~FLAG_PENDING_COMMAND;
  rxBuf[RX_BUF_SIZE-1] = '\0';
  unsigned char all = 0;
  if ( rxBuf[1] == 'a' || rxBuf[1] == 'A' ) {
    all = 1;
  }
  else if (rxBuf[1] > '4' || rxBuf[1]<'0') {
    putString( BAD );
    return;
  }
  unsigned int servo = 3*(rxBuf[1]-'0');
  if ( all ) { servo = 0; }
  switch (rxBuf[0]) {
    case 'S':
      servo++;
    case 'T':
      servo++;
    case 'L':
      break;
    default:
      putString( BAD );
      return;
  }
  unsigned char pos;
  pos = (fromHex(rxBuf[2])<<4) | fromHex(rxBuf[3]);
  if ( all ) {
    unsigned char i;
    for ( i=0; i < 5; i++ ) {
      servoPos[servo] = pos;
      servo += 3;
    }
  } else {
    servoPos[servo] = pos;
  }
  putString( "Sorry, no talk.\n" );
}

/*
 * 00  PD2  Red leg
 * 01  PD3  Red thigh
 * 02  PD4  Red shoulder
 * 03  PD5  Green leg
 * 04  PD6  Green thigh
 * 05  PD7  Green shoulder
 * 06  PB0  Blue leg
 * 07  PB1  Blue thigh
 * 08  PB2  Blue shoulder
 * 09  PB3  Black leg 
 * 10  PB4  Black thigh
 * 11  PB5  Black shoulder
 * 12  PC0  White leg
 * 13  PC1  White thigh
 * 14  PC2  White shoulder
 */
void configurePins()
{
  DDRD = 
    _BV(2) | _BV(3) | 
    _BV(4) | _BV(5) | _BV(6) | _BV(7);
  DDRB =
    _BV(0) | _BV(1) | _BV(2) | _BV(3) |
    _BV(4) | _BV(5);
  DDRC=
    _BV(0) | _BV(1) | _BV(2);
}

void pulseStart(int servo) {
  if        (servo < 6) {
    PORTD |= _BV(2+servo);
  } else if (servo < 12) {
    PORTB |= _BV(servo-6);
  } else if (servo < 15) {
    PORTC |= _BV(servo-12);
  }
}

void pulseEnd(int servo) {
  if        (servo < 6) {
    PORTD &= ~_BV(2+servo);
  } else if (servo < 12) {
    PORTB &= ~_BV(servo-6);
  } else if (servo < 15) {
    PORTC &= ~_BV(servo-12);
  }
}

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( TIMER1_COMPA_vect ) {
  pulseEnd(whichServo);
}

ISR( TIMER1_COMPB_vect ) {
  pulseEnd(whichServo+1);
}

ISR( TIMER1_OVF_vect ) {
  TCNT1  = T1BOT; 
  whichServo = (whichServo+2)%MAX_SERVOS;
  pulseStart(whichServo); // on channel A
  pulseStart(whichServo+1); // on channel B
  unsigned int len = T1BOT + T1MS + (T1INC*servoPos[whichServo]);
  OCR1A  = len;
  len = T1BOT + T1MS + (T1INC*servoPos[whichServo+1]);
  OCR1B  = len;
}

int cycle = 0;

void setPos(char leg, char joint, unsigned char pos) {
  servoPos[(3*leg)+joint] = pos;
}

void park() {
  char leg;
  for (leg=0;leg<5;leg++) {
    setPos(leg,0,64);
    setPos(leg,1,0);
    setPos(leg,2,128);
  }
}

ISR( TIMER0_OVF_vect ) {
}

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