#include <avr/io.h>
#include <util/delay.h>
#define F_CPU 16000000

void adc_init(void)
{
  ADMUX |= (1 << REFS0);
  ADCSRA |= (1 << ADEN) | (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0);
}

void start_conversion(void)
{
  ADCSRA |= (1 << ADSC);
}

void timer_init(void)
{
  DDRB = (1 << PB1);
  TCCR1A |= (1 << COM1A1) | (1 << WGM11);
  TCCR1B |= (1 << WGM12) | (1 << WGM13) | (1 << CS11) | (1 << CS10);
  ICR1 = 4999;
}

uint16_t adc_read(uint8_t channel)
{
  //channel &= 0x07;
  ADMUX |= ((ADMUX & 0xF0) | (channel & 0x0F));
  start_conversion();
  while(ADCSRA & (1 << ADSC));
  return ADCW;
}

void set_servo_position(uint16_t position)
{
  if(position < 1000) position = 1000;
  if(position > 2000) position = 2000;
  OCR1A = position / 4;
}

int main(void)
{
  uint16_t adc_value;
  uint16_t servo_position;
  adc_init();
  timer_init();
  while(1)
  {
    adc_value = adc_read(0);
    servo_position = (adc_value * 1000 / 1023) + 1000;
    set_servo_position(servo_position);
    _delay_ms(20);
  }
  return 0;
}