#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 initPWM(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 initLED(void)
{
    SET(DDRD, LED_1);
}

void readTemp(void)
{
    double d;

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

        _delay_ms(500);
    }
}

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

int main(void)
{
    initLED();
    initPWM();

    runServos();
    readTemp();

    return 0;
}