/*
This code will cause a TekBot connected to the AVR board to
move forward and when it touches an obstacle, it will reverse
and turn away from the obstacle and resume forward motion.

PORT MAP
Port B, Pin 4 -> Output -> Right Motor Enable
Port B, Pin 5 -> Output -> Right Motor Direction
Port B, Pin 7 -> Output -> Left Motor Enable
Port B, Pin 6 -> Output -> Left Motor Direction
Port D, Pin 1 -> Input -> Left Whisker
Port D, Pin 0 -> Input -> Right Whisker
*/

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

int main(void)
{
    DDRB = 0b11110000;
    PORTB= 0b11110000;
    DDRD = 0b11111100;
    PORTD= 0b11111111;

    void leftHit(){
        //Move forward for 2 seconds, attempting to push the object out of the way
        PORTB = 0b01100000;
        _delay_ms(2000);
        //Move backwards for one second
        PORTB = 0b00000000;
        _delay_ms(1000);
        //Turn left, towards the object, left motor is powered, right motor is not
        PORTB = 0b00100000;
        _delay_ms(1000);
    }

    void rightHit(){
        //Move forward for 2 seconds, attempting to push the object out of the way
        PORTB = 0b01100000;
        _delay_ms(2000);
        //Move backwards for one second
        PORTB = 0b00000000;
        _delay_ms(1000);
        //Turn right, towards the object, right motor is powered, left motor is not
        PORTB =0b01000000;
        _delay_ms(1000);
    }

    void bothHit(){
        //Move forward for 2 seconds, attempting to push the object out of the way
        PORTB = 0b01100000;
        _delay_ms(2000);
        //Move backwards for one second
        PORTB = 0b00000000;
        _delay_ms(1000);
        //Turns left, away from the object for 2 seconds
        //PORTB = 0b00100000;
        //_delay_ms(2000);
    }


    while (1) // loop forever
    {
        //make bot move forward
        PORTB = 0b01100000;

        //if both are hit
        //For challenge, BumpBot is already facing the object, so it will repeatedly attempt to push it out of the way
        if(PIND == 0b11111100){
            bothHit();
        }

        //if left whisker is hit
        else if(PIND == 0b11111101){
            _delay_ms(70);
            if(PIND == 0b11111100){
                bothHit();
            }
            else{
                leftHit();
            }
        }

        //if the right whisker is hit
        else if(PIND == 0b11111110){
            _delay_ms(70);
            if(PIND == 0b11111100){
                bothHit();
            }
            else{
                rightHit();
            }
        }
    }
}