//
// Distance Direction Sensor.c
//
// HC-SR04 sonar distance detection and direction
// 9600 baud FTDI interface
//
// Neil Gershenfeld & Ravi Tejwani
// (c) Massachusetts Institute of Technology 2015
//
// This work may be reproduced, modified, distributed,
// performed, and displayed for any purpose. Copyright is
// retained and must be preserved. The work is provided
// as is; no warranty is provided, and users accept all 
// liability.
//

#include <avr/io.h>
#include <util/delay.h>
#include <string.h>
#include <stdio.h>

#define output(directions,pin) (directions |= pin) // set port direction for output
#define set(port,pin) (port |= pin) // set port pin
#define clear(port,pin) (port &= (~pin)) // clear port pin
#define pin_test(pins,pin) (pins & pin) // test for port pin
#define bit_test(byte,bit) (byte & (1 << bit)) // test for bit set
#define bit_delay_time 102 // bit delay for 9600 with overhead
#define bit_delay() _delay_us(bit_delay_time) // RS232 bit delay
#define half_bit_delay() _delay_us(bit_delay_time/2) // RS232 half bit delay
#define char_delay() _delay_ms(10) // char delay

#define PWM_delay() _delay_ms(1000) // PWM delay

#define serial_port PORTA
#define serial_direction DDRA
#define serial_pin_out (1 << PA1)

#define en_port PORTB // Enable pin
#define en_direction DDRB
#define en_pin (1 << PB2)

#define dir_port PORTA // Direction pin
#define dir_direction DDRA
#define dir_pin (1 << PA7)

#define trigger_port PORTA
#define trigger_direction DDRA
#define trigger_pin (1 << PA3)

#define echo_pins PINA
#define echo_direction DDRA
#define echo_pin (1 << PA2)

#define timeout 255

void put_char(volatile unsigned char *port, unsigned char pin, char txchar) {
    //
    // send character in txchar on port pin
    //    assumes line driver (inverts bits)
    //
    // start bit
    //
    clear(*port,pin);
    bit_delay();
    //
    // unrolled loop to write data bits
    //
    if bit_test(txchar,0)
        set(*port,pin);
    else
        clear(*port,pin);
    bit_delay();
    if bit_test(txchar,1)
        set(*port,pin);
    else
        clear(*port,pin);
    bit_delay();
    if bit_test(txchar,2)
        set(*port,pin);
    else
        clear(*port,pin);
    bit_delay();
    if bit_test(txchar,3)
        set(*port,pin);
    else
        clear(*port,pin);
    bit_delay();
    if bit_test(txchar,4)
        set(*port,pin);
    else
        clear(*port,pin);
    bit_delay();
    if bit_test(txchar,5)
        set(*port,pin);
    else
        clear(*port,pin);
    bit_delay();
    if bit_test(txchar,6)
        set(*port,pin);
    else
        clear(*port,pin);
    bit_delay();
    if bit_test(txchar,7)
        set(*port,pin);
    else
        clear(*port,pin);
    bit_delay();
    //
    // stop bit
    //
    set(*port,pin);
    bit_delay();
    //
    // char delay
    //
    bit_delay();
}

int main(void) {
    //
    // main
    //
    static unsigned char high,low,pwm;
    uint16_t value,us,cm;
    uint16_t filt = 0;

    uint16_t cm_pre;
    int16_t velocity;

    //
    // set clock divider to /1
    //
    CLKPR = (1 << CLKPCE);
    CLKPR = (0 << CLKPS3) | (0 << CLKPS2) | (0 << CLKPS1) | (0 << CLKPS0);
    //
    // initialize output pins
    //
    set(serial_port,serial_pin_out);
    output(serial_direction,serial_pin_out);

    clear(trigger_port,trigger_pin);
    output(trigger_direction,trigger_pin);

    clear(en_port, en_pin);
    output(en_direction, en_pin);

    clear(dir_port, dir_pin);
    output(dir_direction, dir_pin);
    //
    // start counter
    //
    TCCR0B |= (1 << CS00); // prescale /1
    //
    // main loop
    //
    while (1) {
        //
        // trigger pulse
        //
        set(trigger_port,trigger_pin);
        _delay_us(10);
        clear(trigger_port,trigger_pin);  
        //
        // wait for echo rising edge
        //
        high = 0;
        TCNT0 = 0;
        TIFR0 |= (1 << TOV0);
        while (1) {
            if ((echo_pins & echo_pin) != 0) // check for rising edge
                break;
            if ((TIFR0 & (1 << TOV0)) != 0) { // check for counter overflow
                high += 1;
                if (high == timeout)
                    break;
                TIFR0 |= (1 << TOV0);
            }
        }
        //
        // rising edge found, wait for falling edge
        //
        high = 0;
        TCNT0 = 0;
        TIFR0 |= (1 << TOV0);
        while (1) {
            if ((echo_pins & echo_pin) == 0) { // check for falling edge
                low = TCNT0;
                break;
            }
            if ((TIFR0 & (1 << TOV0)) != 0) { // check for counter overflow
                high += 1;
                if (high == timeout)
                    break;
                TIFR0 |= (1 << TOV0);
            }
        }
        //
        // send count with framing
        //    
        put_char(&serial_port,serial_pin_out,1);
        put_char(&serial_port,serial_pin_out,2);
        put_char(&serial_port,serial_pin_out,3);
        put_char(&serial_port,serial_pin_out,4);
        put_char(&serial_port,serial_pin_out,low);
        put_char(&serial_port,serial_pin_out,high);

        //
        // compute the distance in cm and velocity
        //   
        value = ((high<<8) + low);
        filt = ((uint32_t)filt * 7UL + value) / 8;
        cm = filt/464;      
        velocity = (cm-cm_pre)*10;

        _delay_ms(100);
        cm_pre = cm;

    }
}