// ---------------------------------------------------------------- //
// Arduino Ultrasoninc Sensor HC-SR04
// ---------------------------------------------------------------- //

#include "TM1637Display.h"





constexpr int echoPin = 2;  // attach pin D2 Arduino to pin Echo of HC-SR04
constexpr int trigPin = 3;  //attach pin D3 Arduino to pin Trig of HC-SR04
constexpr int clkPin = 4;
constexpr int dioPin = 5;



volatile uint64_t ultrasonicEchoStart = 0;  //used for interrupt communication
volatile double ultrasonicDistance = 0.0;   //needs to be volatile
volatile bool measured = false;

TM1637Display      display(clkPin, dioPin);

volatile uint16_t last_measure=0;
void useMesurment(double measurment) {
  last_measure=measurment;
  Serial.print("Distance[mm]:");
  Serial.println(measurment);
  if(0<last_measure&&10000>last_measure)
    display.showNumberDec(last_measure);
  else
   display.showNumberHexEx(0xABBA);
  delay(50);
}

void ultrasonicPulseStart() {
  noInterrupts();  //atomic
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);  // Sets the trigger on HIGH state for 10 micro seconds to send a series of pulses
  delayMicroseconds(10);        // blocks 10 microseconds from the interrupt, I think we'll live :)
  digitalWrite(trigPin, LOW);   // disable the sending again so we can wait for a response
  attachInterrupt(digitalPinToInterrupt(echoPin), ultrasonicEchoInterrupt, RISING);
  interrupts();
}


void ultrasonicEchoInterrupt() {
  constexpr double speedOfSound0 = 0.3313;                                          //[mm/us]
  constexpr double sOs_tempCoef = 0.000606;                                         //[mm/us/C]
  constexpr double airTemperature = 20;                                             //[C]
  constexpr double speedOfSound = (speedOfSound0 + airTemperature * sOs_tempCoef);  //[mm/us]  if temperature is variable cannot be constexpr and should be calibrated with some sensor, otherwise 343.42e-3
  if (0 == ultrasonicEchoStart) {
    // record the send time
    ultrasonicEchoStart = micros();
    attachInterrupt(digitalPinToInterrupt(echoPin), ultrasonicEchoInterrupt, FALLING);
    return;
  }
  if (ultrasonicEchoStart != 0) {
    uint64_t measureTime = micros();
    uint64_t diff = measureTime - ultrasonicEchoStart;
    if (diff < 10) return;  //debounce, cannot measure below 3.5mm
    // calculate the distance by measuring how long it took to return the sound
    ultrasonicDistance = diff * speedOfSound * 0.5;
    measured = true;
    ultrasonicEchoStart = 0;
    detachInterrupt(digitalPinToInterrupt(echoPin));
  }
}



double getAvargeOfMeanElements(double newReading) {
  constexpr size_t bufferSize = 14;       //30
  constexpr size_t lengthOfAvarage = 10;  //10
  constexpr size_t avarageStart = (bufferSize - lengthOfAvarage) / 2;
  static double valueCircBuff[bufferSize];
  static unsigned int oldestIndex = 0;        //oldest element in circular buffer
  static unsigned int sortOrder[bufferSize];  //to keep track of sorted array
  static bool init = false;                   //flag for function initialization
  constexpr double badReadingLimit = 4100.0; //HC-SR04 has effective range from 20mm to 4000mm 
  //if not initialized
  if (!init) {
    for (unsigned int i = 0; i < bufferSize; i++) {
      sortOrder[i] = i;
      valueCircBuff[i]=newReading;
    }
    init = true;
  }

  valueCircBuff[oldestIndex] = newReading;
  oldestIndex = (oldestIndex + 1) % bufferSize;

  //start of insertion sort
  for (unsigned int i = 1; i < bufferSize; i++) {
    unsigned int temp = sortOrder[i];
    int j = i - 1;
    for (; j >= 0 && valueCircBuff[sortOrder[j]] > valueCircBuff[temp]; j--)
      sortOrder[j + 1] = sortOrder[j];
    sortOrder[j + 1] = temp;
  }
  //end of insertion sort

  //find average
  double sum = 0;
  for (unsigned int i = 0; i < lengthOfAvarage; i++) {
    sum += valueCircBuff[sortOrder[avarageStart + i]];
  }
  double result = sum / lengthOfAvarage;
  if(result>badReadingLimit)init=false; //special case for HC-SR04
  return result;
}


double measureDistance(){
    ultrasonicPulseStart();
    while (!measured)
         ;
    return getAvargeOfMeanElements(ultrasonicDistance);
    measured = false;
}

void setup() {
  pinMode(trigPin, OUTPUT);                                    // Sets the trigPin as an OUTPUT
  pinMode(echoPin, INPUT);                                     // Sets the echoPin as an INPUT
  Serial.begin(115200);                                        // // Serial Communication is starting with 115200 of baudrate speed
  Serial.println("Ultrasonic Sensor interrupt HC-SR04 Test");  // print some text in Serial Monitor
  Serial.println("with Arduino UNO R3");
  display.setBrightness(7);
}
void loop() {
  useMesurment(measureDistance());
}