int TRIG_PIN = 12; // Trigger
int ECHO_PIN = 13; // Echo
long duration, cm;
const unsigned int TIMEOUT_MICROSECONDS = 25000;


void setup() {
//Serial Port begin
Serial.begin (9600);
//Define inputs and outputs
pinMode(TRIG_PIN, OUTPUT);
pinMode(ECHO_PIN, INPUT);
}
void loop() {
int dist = ULTRASONIC_MeasureDistance();
Serial.println(dist);
delay(50);
}

int ULTRASONIC_MeasureDistance(void){
    unsigned long duration, start_time, end_time, current_time = 0;

    ULTRASONIC_SendPulse();
    start_time = micros();
    ULTRASONIC_WaitForPulseReturn(start_time);
    start_time = micros();
    end_time = ULTRASONIC_Measure_Pulse_Duration(start_time);
    
    Serial.print("Start: ");
    Serial.print(start_time);

    Serial.print("End: ");
    Serial.print(end_time);
    
    Serial.print("Duration: ");
    Serial.print(end_time - start_time);

    long duration_pulse = end_time - start_time;

    return ULTRASONIC_CalculateDistance(duration_pulse);
};

void ULTRASONIC_SendPulse(void){
    PORTB &= ~(1 << TRIG_PIN);
    _delay_us(5);
    PORTB |= (1 << TRIG_PIN);
    _delay_us(10);
    PORTB &= ~(1 << TRIG_PIN);
}

void ULTRASONIC_WaitForPulseReturn(long start){
    long current_time, duration = 0;
    bool ECHO_PIN_LOW = !(PINB & (1 << ECHO_PIN));

    while(ECHO_PIN_LOW){
        current_time = micros();
        duration = current_time - start;

        // TIMEOUT, NO ECHO RECEIVED
        if(duration > TIMEOUT_MICROSECONDS){
            return -1;
        }
    };

}

long ULTRASONIC_Measure_Pulse_Duration(long start){
    long current_time, duration = 0;
    bool ECHO_PIN_HIGH = (PINB & (1 << ECHO_PIN));

    while(ECHO_PIN_HIGH){
        current_time = micros();
        duration = current_time - start;

        // TIMEOUT, ECHO TOO LONG
        if(duration > TIMEOUT_MICROSECONDS){
            return -1;
        }
    }
    return micros();
}

int ULTRASONIC_CalculateDistance(long duration){
    unsigned long distance_cm;
    const unsigned long MAX_DURATION = 200000;

    if(duration < 0 || duration > MAX_DURATION) return -1;

    distance_cm = (duration / 2) / 29.1;
    return distance_cm;
}