#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include "ds18b20.h"
#include "main.h"
#include "io.h"

#ifdef DEBUG
#include "debug.h"
#endif /* DEBUG */

void init_pwm(void)
{
    DDRB |= 1 << SERVO_L_PIN | 1 << SERVO_R_PIN;
    /* Phase Correct PWM; Set/clear on Compare Match when down/up-counting */
    TCCR1A |= 1 << WGM11 | 1 << COM1A1 | 1 << COM1B1;
    /*  "Clear Timer on Compare match" mode; Prescaler = 1 */
    TCCR1B |= 1 << WGM12 | 1 << WGM13 | 1 << CS10;
    ICR1 = F_CPU / 50 - 1; // 50Hz required by servos
}

void init_led(void)
{
    DDRD |= 1 << LED_1;
}

void init_adc(void)
{
    ADCSRA |= 1 << ADPS0 | 1 << ADPS1; // Prescaler = 8 => ADC clock = 125Hz
    ADMUX |= 1 << REFS0; // AVcc with external cap as reference voltage
    ADCSRA |= 1 << ADEN | 1 << ADSC;
}

void read_temp(void)
{
    double d;

    sei();
    for (;;) {
        d = ds18b20_gettemp();
        if (d >= 21)
            SET(PORTD, LED_1);
        else
            CLR(PORTD, LED_1);

        _delay_ms(500);
    }
}

void run_servos(void)
{
    /* Stop */
    SERVO_L = 1500;
    SERVO_R = 1500;
    _delay_ms(1500);

    /* Reverse */
    SERVO_L = 2000;
    SERVO_R = 2000;
    _delay_ms(1500);

    /* Forwards */
    SERVO_L = 1000;
    SERVO_R = 1000;
    _delay_ms(1500);
}

uint16_t read_adc(uint8_t channel)
{
    ADMUX &= 0xF0;
    ADMUX |= channel;
    SET(ADCSRA, ADSC); // Start conversion
    while (CHK(ADCSRA, ADSC)); // Wait for conversion to finish

    return ADCW;
}

int main(void)
{
    uint16_t adc_val;

    init_led();
    init_pwm();
    init_adc();

    while (1) {
        adc_val = read_adc(0);
        if (adc_val <= 512) {
            SERVO_R = 2000;
            SERVO_L = 2000;
        } else {
            SERVO_R = 1000;
            SERVO_L = 1000;
        }
    }

    return 0;
}