#define F_CPU 8000000UL

#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include <compat/deprecated.h>

void initialize(void);

//functions for left wheel
void init_pwm_right(void);
void start_pwm_right(void);
void stop_pwm_right(void);

//functions for right wheel
void init_pwm_left(void);
void start_pwm_left(void);
void stop_pwm_left(void);


int main(void)
{
    volatile int i = 0, j = 0, k = 0;
    initialize();  //call initialize routine, rest would be done by interrupts.
    
    while(1)       //main loop for LFR
    {
        
        i = bit_is_set(PINB, PB0);
        if(!i)
        {
            //Steer to left
            stop_pwm_left();
            stop_pwm_right();
            start_pwm_right();
        }
        else
        {
            //Steer to right
            stop_pwm_left();
            stop_pwm_right();
            start_pwm_left();
        }  

        // Some delay
        for(j=0;j<255;j++)
        {
            for(k=0;k<255;k++)
            {
            }
        }
    }  
}

void initialize()
{
    init_pwm_left();
    init_pwm_right();

    start_pwm_left();
    start_pwm_right();
    
    DDRB |= (0 << DDB0);   //setting PORTB.0 as input
}

void init_pwm_right(void)
{
    TCCR2 |= 1 << CS22;   //pre-scaler 256
    TCCR2 |= 1 << CS21;   //pre-scaler 256
    TCCR2 |= 0 << CS20;   //pre-scaler 256
    
    TCCR2 |= 1 << WGM20;  //fast pwm
    TCCR2 |= 1 << WGM21;  //fast pwm
    
    TCCR2 |= 0 << COM20;  //non-inverting pwm
    TCCR2 |= 1 << COM21;  //non-inverting pwm
    OCR2 = 255;  //Initial value into OCR2
    DDRD |= (1 << DDD7);  //setting PORTD.7 as output
}

void init_pwm_left(void)
{
    TCCR0 |= 1 << CS02;   //pre-scaler 256
    TCCR0 |= 0 << CS01;   //pre-scaler 256
    TCCR0 |= 0 << CS00;   //pre-scaler 256
    
    TCCR0 |= 1 << WGM00;  //fast pwm
    TCCR0 |= 1 << WGM01;  //fast pwm
    
    TCCR0 |= 0 << COM00;  //non-inverting pwm
    TCCR0 |= 1 << COM01;  //non-inverting pwm
    OCR0 = 255;  //Initial value into OCR0
    DDRB |= (1 << DDB3);  //setting PORTB.3 as output
}



void start_pwm_right(void)
{
    TCCR2 |= (1 << CS22);   //pre-scaler, 256
    TCCR2 |= (1 << CS21);   //pre-scaler, 256 
    TCCR2 |= (0 << CS20);   //pre-scaler, 256
}


void start_pwm_left(void)
{
    TCCR0 |= (1 << CS02);   //pre-scaler 256
    TCCR0 |= (0 << CS01);   //pre-scaler 256
    TCCR0 |= (0 << CS00);   //pre-scaler 256
}


void stop_pwm_right(void)
{
    TCCR2 &= ~(1 << CS22);   //NO pre-scaler, timer stopped
    TCCR2 &= ~(1 << CS21);   //NO pre-scaler, timer stopped
    TCCR2 &= ~(1 << CS20);   //NO pre-scaler, timer stopped
}


void stop_pwm_left(void)
{
    TCCR0 &= ~(1 << CS02);   //NO pre-scaler, timer stopped
    TCCR0 &= ~(1 << CS01);   //NO pre-scaler, timer stopped
    TCCR0 &= ~(1 << CS00);   //NO pre-scaler, timer stopped
}