import com.pi4j.io.gpio.GpioPinDigitalInput;
import com.pi4j.io.gpio.GpioPinDigitalOutput;

public class Distance
{
    // instance variables - replace the example below with your own

    
    
    public static final int     DIVISOR               = 58200;
    public static final int     ERROR_NO_MEASUREMENT  = -1;
    public static final int     ERROR_DIST_TO_SHORT   = -2;
    
    private GpioPinDigitalInput     echo        = null;
    private GpioPinDigitalOutput    trigger     = null;
    private long                     timeOut     = 250000000; // in ns/ time for 4m for 20°C ~232
    private long                     triggerTime = 2;   // 10ms for SR04, 2 for US-100
    private long                     initTime    = 2;    // default 2 ms
      
    /**
     * Constructor for objects of class Distance
     */
    public Distance(GpioPinDigitalOutput trigger, GpioPinDigitalInput echo)
    {
        this.echo = echo;
        this.trigger = trigger;

    }
    
    /**
     * get the distance in mm
     */
    public double getDistMM()
    {
        return aquire()*1000;
    }
    
     /**
     * get the distance in cm
     */
    public double getDistCM()
    {
        return aquire();
    }
    
    /** 
     * returns the time in milli xeconds
     */
    public double aquire(){
        long start = 0;
        double diff = 0;

        try {
           
          trigger.low();
          Thread.sleep(initTime);
          trigger.high();
          Thread.sleep(triggerTime);
          trigger.low();
    
          while (echo.isLow()) {
            start = System.nanoTime();
          }
    
          while (echo.isHigh()) {
              //System.out.print((System.nanoTime() - start)+",");
              if((System.nanoTime() - start) > timeOut){
                  return ERROR_NO_MEASUREMENT;
              }
          }
    
          // check if distance is to short and return errror
          diff =  ((System.nanoTime() - start) / DIVISOR );
          return diff;
        } catch (InterruptedException e) {
          e.printStackTrace();
        }
        return -1.0;
    }
}