/* ---------------------------------------------------------------- //
   Ultrasonic Ruler Using HC-SR04 Ultrasonic Sensor Module
   Author: PM Wiegand, May 2025
*/

#include "QuickStats.h"
#include <Wire.h>
#include <U8g2lib.h>  // Include file for the U8g2 library.



#define NUMREADINGS 10
constexpr uint8_t echoPin = 12;  // attach pin D12 Arduino to pin Echo of HC-SR04
constexpr uint8_t trigPin = 13;    //attach pin D13 Arduino to pin Trig of HC-SR04

QuickStats stats;  //initialize an instance of this class
//oled:next line uses _2_ meaning paged buffer and use of firstpage/nextpage.  Pins are SCK=A5 and SDA=A4 as defined in include file
U8G2_SSD1306_128X64_NONAME_2_HW_I2C oled(U8G2_R0, /* reset=*/U8X8_PIN_NONE);  //this matches the display being used see include file.  we'll call the display 'oled'
#include "OLEDHelpers.h"
float readings[NUMREADINGS];
float times[NUMREADINGS];
int idx = NUMREADINGS;
int count = 0;
bool metricUnits = false;
float curReading;

void setup() {
  pinMode(trigPin, OUTPUT);            // Sets the trigPin as an OUTPUT
  pinMode(echoPin, INPUT);             // Sets the echoPin as an INPUT
    //start the oled display
  oled.begin();
  oled.setFont(u8g2_font_5x7_tn);
  oled.setFontPosTop();
  Serial.begin(9600);                // // Serial Communication is starting with 9600 of baudrate speed
  Serial.println("Ultrasonic Ruler");  // print some text in Serial Monitor
}

void drawStats(float reading, float statAvg, float statLowest, float statHighest, float statMedian, float statGmean, float statGmdn) {
  oled.firstPage();
  unsigned int yPos;
  do {
    yPos = 0; //start at top of display
    oled.setCursor(0, 0);  //top left is 0,0 see setup
    if (!metricUnits){
      int dummyX = drawCenteredString(0, "Units: Inches", true, false, false);
    } else {
      int dummyX = drawCenteredString(0, "Units: cm", true, false, false);
    }
    yPos = newLine(0);
    drawOneStat("Reading:", reading, yPos);   //write message and stat
    yPos = newLine(yPos);     //this advances the y position one line, based on the current font height
    drawOneStat("Avg:", statAvg, yPos);  //etc...
    yPos = newLine(yPos);
    drawOneStat("Median:", statMedian, yPos);
     yPos = newLine(yPos);
    drawOneStat("Gmean:", statGmean, yPos);
    yPos = newLine(yPos);
    drawOneStat("Gmdn:", statGmdn, yPos);
    yPos = newLine(yPos);
    drawOneStat("Low:", statLowest, yPos);
    yPos = newLine(yPos);
    drawOneStat("High:", statHighest, yPos);
  } while (oled.nextPage());
}

void drawOneStat(char* msg, float stat, unsigned int yPos) {
  oled.setCursor(0, yPos);  //set the starting print position
  oled.print(msg);          //print the message part.  Print keeps track of next x position it should print to
  oled.print(stat);         //so we can just print and it will automatically be to the right of the message.  No need to set cursor again.
}

void updateOLED() {
  //print the current bpm on the oled display
  float statAvg = stats.average(readings, NUMREADINGS);
  float statLowest = stats.minimum(readings, NUMREADINGS);
  float statHighest = stats.maximum(readings, NUMREADINGS);
  float statMedian = stats.median(readings, NUMREADINGS);
  float statGmean = stats.g_average(readings, NUMREADINGS);
  float statGmdn = stats.gmdn(readings, NUMREADINGS, 0.0001);
  oled.firstPage();  //using multipl page approach to update display to save memory
  do {
    drawStats(curReading, statAvg, statLowest, statHighest, statMedian, statGmean, statGmdn);
  } while (oled.nextPage());  //will continue to loop until entire display is written
}

long getDuration() {
  // triggers the ultrasonic pulse and returns the echo time
  // Clears the trigPin condition
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  // Sets the trigPin HIGH (ACTIVE) for 10 microseconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  long echoDuration = pulseIn(echoPin, HIGH);
  return echoDuration;
}

float cmCalc(long duration) {
  //convert duration of echo in microseconds to distance in cm
  float distance_cm = duration * 0.034 / 2;  // Speed of sound wave divided by 2 (go and back) units of cm/microsecond
  return distance_cm;
}

float inCalc(long duration) {
  //convert duration of echo in microseconds to distance in in
  float distance_inch = duration * 0.0133 / 2;  // Speed of sound wave divided by 2 (go and back) units of in/microsecond
  return distance_inch;
}

void storeReading() {
  count++;
  idx++;
  if (idx >= NUMREADINGS) {
    idx = 0;
  }
  times[idx] = millis();
  readings[idx] = curReading;
}

void loop() {
  long duration = getDuration();
  if (metricUnits) {
    curReading = cmCalc(duration);
  } else {
    curReading = inCalc(duration);
  }
  storeReading();
  if (count >= NUMREADINGS){
    updateOLED();
  }
  delay(100);
}