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

// för ultraljudssensorn
uint16_t echo_recive();
void beep();

//för pwm3
void timer3_init();
void set_pulse3(uint16_t);
void set_period3(uint16_t);

int main(void)
{

    DDRD |= 0b11111111; // lampan/zoomer
    DDRC |= (1<<DDC1);  // trig
    timer3_init();
    set_period3(260); // 33 ms
    set_pulse3(20); // 16 ms

    while(1){
        //för zommer och ultraljudssensorn

        uint16_t counter = echo_recive();
        set_pulse3(20);

        if (TCNT1 < 35 && TCNT1 > 4){
            beep();

        } else if ((TCNT1 <= 4)){
        PORTD = (1<< PORTD7);
        set_pulse3(0); // stopppar motorn

        }else{
        PORTD = (0<<PORTD7);
        }

    }
}
// För ultraljudssensorn och zommer
uint16_t echo_recive(){
    _delay_ms(60);
    TCNT1 = 0;

    PORTC |= (1<<PORTC1); // c1  trig                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                             trig
    _delay_us(10);
    PORTC &= ~(1<<PORTC1);

    while(!(PINC & (1<<PINC0))){ // c0 echo

    }
    TCCR1B |= (1<<CS12)|(1<<CS10);


    while(PINC & (1<<PINC0)){

    }
    TCCR1B = 0;

    return (TCNT1);

}
void beep(){ //Dubbekollad, funkar, 100ms

        PORTD = (1 << PORTD7); // zoomer
        _delay_ms(100);
        PORTD = (0 << PORTD7);
        _delay_ms(TCNT1*10);
}

// För PWM timer 3
void timer3_init(){
    TCCR3A |= (1<< COM3A1) | (1<< WGM31);
    TCCR3B |= (1<<WGM33) | (1<<WGM32) | (1<<CS30) | (1<<CS32);
    DDRB |= (1<<DDRB6);
}
void set_pulse3(uint16_t ocr){
    OCR3A = ocr;
}
void set_period3(uint16_t icr){
    ICR3 = icr;
}