#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 initIO()
{
    /* Servos */
    DDRB |= (1 << SERVO_L);
    /* Phase Correct PWM, 9-bit; Inverting mode */
    TCCR1A |= 1 << WGM11 | 1 << COM1A1 | 1 << COM1A0;
    /*  "Clear Timer on Compare match" mode; Prescaler = 1 */
    TCCR1B |= 1 << WGM13 | 1 << WGM12 | 1 << CS10;
    ICR1 = 19999; // F_CPU / 50Hz - 1

    /* LED */
    DDRD |= (1 << LED_1);
}

void readTemp()
{
    double d;

    sei();
    for (;;) {
        d = ds18b20_gettemp();
        if (d >= 21)
            PORTD |= (1 << LED_1);
        else
            PORTD &= ~(1 << LED_1);

        _delay_ms(500);
    }
}

void runServos()
{
    /* Stop */
    OCR1A = ICR1 - 1500;
    _delay_ms(1500);

    /* Reverse */
    OCR1A = ICR1 - 2000;
    _delay_ms(1500);

    /* Forwards */
    OCR1A = ICR1 - 1000;
    _delay_ms(1500);
}

int main(void)
{
    initIO();
    runServos();
    readTemp();


    return 0;
}