/*****************************************************************************

    source::worx libPiGPio
    Copyright © 2022 c.holzheuer
    chris@sourceworx.org

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    Uses:

    pigpiod_if2 by joan2937, pigpio @ abyz.me.uk
    https://abyz.me.uk/rpi/pigpio

    sigslot  by 2017 Pierre-Antoine Lacaze
    https://github.com/palacaze/sigslot

    pigSonar based on: https://github.com/OmarAflak/HC-SR04-Raspberry-Pi-C-

***************************************************************************/


#include <pigsonar.h>
#include <iostream>

/**
 * @brief pigSonar::pigSonar
 *
 *
 * Beschreibung HC-SR04:
 * Das Ultraschall Modul HC-SR04 eignet sich zur
 * Entfernungsmessung im Bereich zwischen 2cm und ca.
 * 3m mit einer Auflösung von 3mm. Es benötigt nur eine
 * einfache Versorgungsspannung von 5V bei einer
 * Stromaufnahme von <2mA. Nach Triggerung mit einer
 * fallenden Flanke (TTL - Pegel) misst das Modul
 * selbstständig die Entfernung und wandelt diese in ein
 * PWM Signal welches am Ausgang zur Verfügung steht.
 * Ein Messintervall hat eine Dauer von 20ms. Es können
 * also 50 Messungen pro Sekunde durchgeführt werden
 *
 * https://www.mikrocontroller.net/attachment/218122/HC-SR04_ultraschallmodul_beschreibung_3.pdf
 */

pigSonar::pigSonar()
{

}


pigSonar::pigSonar( pigpio::bcm_t trigger, pigpio::bcm_t echo )
{
    init( trigger, echo );
}


pigSonar::~pigSonar()
{
    //detach_pin( ) //?? doch wg .callback
}


void pigSonar::init( pigpio::bcm_t trigger, pigpio::bcm_t echo )
{

    _pinEcho.init( echo );
    //_pinEcho.set_callback( this, rising );

    _pinTrig.init( trigger );
    _pinTrig.set_level( low );

    delay_millis( 500 );

}

void pigSonar::stop()
{
    _active = false;
}

void pigSonar::start( double thresh )
{
    _active = true;
    std::thread thrd(
    [=]()
    {
        while( _active  )
        {
            double dist = distance();
            if( dist < thresh )
                sigValue( dist );
            delay_millis( 50 );
        }

    } );
    thrd.detach();

}


double pigSonar::distance( int timeout )
{

    // erholungspause
    delay_millis( 10 );

    // ein 10 ms pulse löst die messung aus
    _pinTrig.set_level( high );
    delay_micros( 10 );
    _pinTrig.set_level( low );

    //_timer.start();
    _now = get_current_tick();
    // busy waiting ??
    while ( _pinEcho.get_level() == low && _timer.elapsed_millis() < timeout )
    //while ( _pinEcho.get_level() == low )
        ;

    //_timer.start();
    _now = get_current_tick();
    while( _pinEcho.get_level() == high )
        ;

    uint32_t tof = get_current_tick() - _now;
    //double tof = _timer.elapsed_micros();
    _dist = 100 * ( ( tof /1000000.0 ) * 340.29 ) / 2;

    return _dist;
}

/*
void pigSonar::trigger( pigpio::bcm_t bcm, uint32_t level )
{
    double duration = _timer.elapsed_micros();
    // Convert the time into a distance
    _dist = (duration/2) / 29.1;     // Divide by 29.1 or multiply by 0.0343
    //double inches = (duration/2) / 74;   // Divide by 74 or multiply by 0.0135

}
*/

/**

  Die Variante mit WiringPi ist erheblich genauer. Warum? Timer?
  Code: https://github.com/OmarAflak/HC-SR04-Raspberry-Pi-C-

  dort:

    Distance is 27.4614 cm.
    Distance is 27.4614 cm.
    Distance is 27.4614 cm.
    Distance is 27.4614 cm.
    Distance is 27.4784 cm.
    Distance is 27.4784 cm.
    Distance is 27.4614 cm.
    Distance is 27.4784 cm.
    Distance is 27.4614 cm.
    Distance is 27.4784 cm.
    Distance is 27.4614 cm.
    Distance is 27.4784 cm.
    Distance is 27.4614 cm.

  hier (busy wait):

     distance: 28.21
     distance: 25.9811
     distance: 28.3121
     distance: 28.2441
     distance: 28.4482
     distance: 27.9889
     distance: 28.0569
     distance: 28.0059
     distance: 28.2441
     distance: 25.3346
     distance: 28.0059
     distance: 29.0608
     distance: 27.9718
     distance: 28.0399
     distance: 27.9889


Die eigene Variante _ohne_ busy wating mit callback ist logischerweise
auch nicht besser.

double Sonar::distance(int timeout)
{
    delay(10);

    digitalWrite(trigger, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigger, LOW);

    now=micros();

    while (digitalRead(echo) == LOW && micros()-now<timeout)
        ;
    recordPulseLength();

    travelTimeUsec = endTimeUsec - startTimeUsec;
    distanceMeters = 100*((travelTimeUsec/1000000.0)*340.29)/2;

    return distanceMeters;
}

void Sonar::recordPulseLength()
{
    startTimeUsec = micros();
    while ( digitalRead(echo) == HIGH )
        ;
    endTimeUsec = micros();
}

*/
