#include <ctype.h>
#include <stdint.h>
#include <avr/io.h>
#define F_CPU 9600000UL  // 9.6 MHz
#include <util/delay.h>

#define PWM PB0

#define TX PB2


/*
 * Do all the startup-time peripheral initializations.
 */
static void
init(void)
{
  //IO
  DDRB = _BV(PWM)|_BV(TX); //PWM output and serial TX
  PORTB |= _BV(TX);

  //Timer in Fast PWM mode
  TCCR0A = _BV(COM0A1) | _BV(WGM01) | _BV(WGM00);
  TCCR0B = _BV(CS01); //PWM base frequency is 4.7KHz

  //ADC
  ADMUX = _BV(MUX1);
  ADCSRA = _BV(ADEN) | _BV(ADPS2) | _BV(ADPS1);
}

/*
 * Software UART half bit delay routine
 */
void uart_delay()
{
  _delay_us(52); //52us 19200 baud
}

void putchar(uint8_t c)
{
  //start bit
  PORTB &= ~_BV(TX);
  uart_delay();

  //clock out data
  uint8_t i;
  for (i=0; i<8; i++)
    {
      if (c & 0x1)
	PORTB |= _BV(TX);
      else
	PORTB &= ~_BV(TX);

      c = c>>1;
      uart_delay();
    }

  //stop
  PORTB |= _BV(TX);
  uart_delay();
}

uint16_t sample()
{
  ADCSRA |= _BV(ADSC); //start conversion
  loop_until_bit_is_set(ADCSRA, ADSC);

  uint16_t result = ADC;
  return result;
}

void
main(void)
{
  init();

  uint8_t setpoint = 20;

  //initial duty cycle
  OCR0A = 0;

  while (1)
    {
      //run the motor
      _delay_ms(10);
      
      //turn off the PWM
      TCCR0A &= ~_BV(COM0A1);
      PORTB &= ~_BV(PWM);
      
      //wait for things to settle
      _delay_ms(1);

      //take a sample average
      uint8_t value = (sample()>>2);
      value += (sample()>>2);
      _delay_us(10);
      value += (sample()>>2);
      _delay_us(10);
      value += (sample()>>2);
      _delay_us(10);
      value += (sample()>>2);
      value = value>>2;
	
      putchar(value);

      int8_t error = value-setpoint;
      
      OCR0A += (error>>4);
      
      //turn on the PWM
      TCCR0A |= _BV(COM0A1);
    }
}