#include <Wire.h> //OLED
#include <Adafruit_GFX.h> //OLED
#include <Adafruit_SSD1306.h> //OLED

//VARIABLES************************************

#define SCREEN_WIDTH 128 // OLED display width,  in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

//Ultrasonic Sensor Settings
const int ultrasonicPinTX = 17; //Hardware Level Serial Port 2 Defaults
const int ultrasonicPinRX = 16;
unsigned char data[4] = {0}; //Data Buffer
float distance;
// OLED SCL defaults to 22
// OLED SDA defaults to 21

const int dataSetSize = 10;
int averageRate[dataSetSize] = {0,0,0,0,0,0,0,0,0,0}; // Keep up to 10 data sets
int counter = 0; // for loop
int refreshDelay = 500 - (2+10); // Refresh Rate of Reading, Minus the sensor init timings
int tankCapacity = 300; //in cm
int tankGallons = 150;

// 'cropped-cropped-weblogo-300x100[1]', 128x64px
const unsigned char logoGRPC [1024] PROGMEM = {
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf8, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf8, 0x7f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf8, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xc0, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xc0, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x80, 0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x80, 0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x80, 0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xc3, 0xc3, 0xf1, 0xe3, 0xdc, 0x3f, 0x00, 0x07, 0xfc, 0x3f, 0x8f, 0x0f, 0xb8, 0x7e, 0x1f, 
	0xff, 0x81, 0x81, 0xf1, 0xe3, 0x9c, 0x0f, 0x00, 0x0f, 0xfc, 0x0f, 0x0f, 0x03, 0x18, 0x1c, 0x1f, 
	0xff, 0x01, 0x80, 0xe1, 0xe3, 0x98, 0x0f, 0x00, 0x0f, 0xfc, 0x0f, 0x0e, 0x03, 0x18, 0x18, 0x0f, 
	0xff, 0x11, 0x80, 0xe1, 0xe3, 0x98, 0x0f, 0x00, 0x09, 0xfc, 0x0f, 0x0e, 0x03, 0x38, 0x18, 0x8f, 
	0xff, 0x11, 0x98, 0xe1, 0xe1, 0x98, 0x8f, 0x00, 0x10, 0xfc, 0xcf, 0x0e, 0x23, 0x39, 0x99, 0x8f, 
	0xff, 0x31, 0x98, 0xe1, 0xe1, 0x98, 0x8f, 0x00, 0x10, 0xf8, 0xce, 0x4e, 0x23, 0x39, 0x99, 0x8f, 
	0xff, 0x3f, 0x98, 0xc1, 0xc1, 0x18, 0x8f, 0x00, 0x10, 0xf8, 0xce, 0x4e, 0x23, 0x31, 0x99, 0xff, 
	0xff, 0x3f, 0x99, 0xc9, 0xc1, 0x19, 0x8f, 0x00, 0x00, 0xf8, 0xce, 0x4e, 0x62, 0x31, 0x98, 0xff, 
	0xfe, 0x3f, 0x81, 0xc9, 0xc1, 0x19, 0x8f, 0x08, 0x00, 0xf8, 0x8e, 0x4e, 0x62, 0x31, 0x98, 0xff, 
	0xfe, 0x23, 0x03, 0xc9, 0xc9, 0x39, 0x8f, 0x30, 0x00, 0xf8, 0x1c, 0x4e, 0x66, 0x31, 0x18, 0x7f, 
	0xfe, 0x23, 0x03, 0xc9, 0xc9, 0x39, 0x9f, 0x30, 0x00, 0xf8, 0x1c, 0xce, 0x06, 0x33, 0x1c, 0x3f, 
	0xfe, 0x23, 0x01, 0x89, 0xc9, 0x39, 0x9f, 0xa0, 0x00, 0xf8, 0x1c, 0xcc, 0x06, 0x33, 0x1e, 0x3f, 
	0xfe, 0x33, 0x11, 0x99, 0xcd, 0x31, 0x9f, 0xa0, 0x00, 0xf9, 0x8c, 0xcc, 0x06, 0x73, 0x1e, 0x1f, 
	0xfe, 0x63, 0x19, 0x89, 0xcd, 0x31, 0x9f, 0x80, 0x00, 0xf9, 0x8c, 0x8c, 0x7e, 0x73, 0x3f, 0x1f, 
	0xfe, 0x63, 0x31, 0x81, 0xcc, 0x31, 0x1f, 0x80, 0x00, 0xf9, 0x98, 0x0c, 0x7e, 0x73, 0x3f, 0x1f, 
	0xfe, 0x63, 0x31, 0x01, 0x8c, 0x31, 0x1f, 0x80, 0x00, 0xf1, 0x98, 0x0c, 0x7e, 0x73, 0x33, 0x1f, 
	0xfe, 0x63, 0x31, 0x01, 0x8c, 0x33, 0x1f, 0x80, 0x01, 0xf1, 0x98, 0x0c, 0xfe, 0x63, 0x33, 0x1f, 
	0xfe, 0x63, 0x31, 0x19, 0x8c, 0x30, 0x1f, 0x80, 0x01, 0xf1, 0x99, 0x8c, 0xfc, 0x60, 0x31, 0x3f, 
	0xfe, 0x06, 0x33, 0x39, 0x9c, 0x70, 0x3f, 0x00, 0x01, 0xf1, 0x99, 0x8c, 0xfc, 0x60, 0x30, 0x3f, 
	0xfe, 0x06, 0x33, 0x39, 0x9c, 0x70, 0x3f, 0x00, 0x01, 0xf3, 0x11, 0x8c, 0xfc, 0x60, 0x70, 0x3f, 
	0xfe, 0x36, 0x33, 0x39, 0x9c, 0x70, 0x7e, 0x00, 0x03, 0xf3, 0x11, 0x8c, 0xfc, 0x60, 0xf0, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xe0, 0x1e, 0x03, 0xc0, 0xe0, 0x1f, 0xf0, 0x3f, 0x03, 0xc1, 0x8c, 0x03, 0x00, 0xf8, 0x1e, 0x1f, 
	0xe0, 0x0e, 0x03, 0x80, 0xe0, 0x1f, 0xe0, 0x1e, 0x03, 0xc1, 0x8c, 0x03, 0x00, 0x70, 0x0e, 0x1f, 
	0xe0, 0x0e, 0x03, 0x80, 0x60, 0x1f, 0xe0, 0x1c, 0x01, 0xc1, 0x8c, 0x03, 0x00, 0x70, 0x0e, 0x1f, 
	0xe0, 0x0e, 0x03, 0x08, 0x60, 0x1f, 0xe1, 0x0c, 0x21, 0xc1, 0x8c, 0x03, 0x08, 0x70, 0x86, 0x1f, 
	0xe1, 0x86, 0x3f, 0x0c, 0x78, 0x7f, 0xe3, 0x0c, 0x31, 0xc1, 0x8f, 0x0f, 0x08, 0x71, 0x86, 0x1f, 
	0xe1, 0x86, 0x3f, 0x0f, 0xf8, 0x7f, 0xc3, 0x0c, 0x31, 0xc0, 0x8f, 0x0f, 0x0c, 0x71, 0x86, 0x1f, 
	0xe1, 0x86, 0x3f, 0x0f, 0xf8, 0x7f, 0xc3, 0x0c, 0x31, 0xc0, 0x8f, 0x0f, 0x0c, 0x61, 0x86, 0x1f, 
	0xe1, 0x86, 0x3f, 0x03, 0xf8, 0x7f, 0xc3, 0xfc, 0x31, 0xc0, 0x8f, 0x0f, 0x08, 0x61, 0x86, 0x1f, 
	0xe1, 0x86, 0x03, 0x01, 0xf8, 0x7f, 0xc3, 0xfc, 0x31, 0xc0, 0x8f, 0x0f, 0x00, 0x61, 0x86, 0x1f, 
	0xe1, 0x0e, 0x03, 0x80, 0xf8, 0x7f, 0xc3, 0xfc, 0x31, 0xc0, 0x8f, 0x0f, 0x00, 0xe1, 0x86, 0x1f, 
	0xe0, 0x0e, 0x03, 0xc0, 0x78, 0x7f, 0xc3, 0xfc, 0x31, 0xc4, 0x0f, 0x0f, 0x00, 0xe1, 0x86, 0x1f, 
	0xe0, 0x0e, 0x03, 0xe0, 0x78, 0x7f, 0xc3, 0xfc, 0x31, 0xc4, 0x0f, 0x0f, 0x08, 0x61, 0x86, 0x1f, 
	0xe0, 0x1e, 0x3f, 0xf8, 0x78, 0x7f, 0xc3, 0xfc, 0x31, 0xc4, 0x0f, 0x0f, 0x08, 0x61, 0x86, 0x1f, 
	0xe1, 0xfe, 0x3f, 0xf8, 0x78, 0x7f, 0xc3, 0x0c, 0x31, 0xc4, 0x0f, 0x0f, 0x0c, 0x61, 0x86, 0x1f, 
	0xe1, 0xfe, 0x3f, 0x0c, 0x78, 0x7f, 0xe3, 0x0c, 0x31, 0xc4, 0x0f, 0x0f, 0x0c, 0x71, 0x86, 0x1f, 
	0xe1, 0xfe, 0x03, 0x0c, 0x78, 0x7f, 0xe3, 0x0c, 0x21, 0xc6, 0x0f, 0x0f, 0x0c, 0x71, 0x86, 0x01, 
	0xe1, 0xfe, 0x03, 0x08, 0x78, 0x7f, 0xe0, 0x1c, 0x01, 0xc6, 0x0f, 0x0f, 0x0c, 0x70, 0x0e, 0x01, 
	0xe1, 0xfe, 0x03, 0x00, 0x78, 0x7f, 0xe0, 0x1e, 0x01, 0xc6, 0x0f, 0x0f, 0x0c, 0x70, 0x0e, 0x01, 
	0xe1, 0xfe, 0x03, 0x80, 0xf8, 0x7f, 0xf0, 0x1e, 0x03, 0xc6, 0x0f, 0x0f, 0x0c, 0x78, 0x0e, 0x01, 
	0xff, 0xff, 0xff, 0xc1, 0xff, 0xff, 0xf8, 0x7f, 0x07, 0xff, 0xff, 0xff, 0xff, 0xfc, 0x3f, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff
};



//OBJECTS*********************************

// declare an SSD1306 display object connected to I2C
Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

//Randy's Functions ***********************************

int getDistance(){
  
   // Run if data available
  if (Serial1.available() > 0) {
 
    delay(4);
 
    // Check for packet header character 0xff
    if (Serial1.read() == 0xff) {
      // Insert header into array
      data_buffer[0] = 0xff;
      // Read remaining 3 characters of data and insert into array
      for (int i = 1; i < 4; i++) {
        data_buffer[i] = Serial1.read();
      }
 
      //Compute checksum
      CS = data_buffer[0] + data_buffer[1] + data_buffer[2];
      // If checksum is valid compose distance from data
      if (data_buffer[3] == CS) {
        int distance = (data_buffer[1] << 8) + data_buffer[2];
        // Print to serial monitor
        Serial.print("distance: ");
        Serial.print(distance);
        Serial.println(" mm");
        return distance/10; //mm to cm
      }else{ 
        return -1;
      }
    }
  }

  
  /*
  digitalWrite(ultrasonicPinTX, LOW);
  delayMicroseconds(2);
  digitalWrite(ultrasonicPinTX, HIGH);
  delayMicroseconds(10);
  digitalWrite(ultrasonicPinTX, LOW);
  
  int duration = pulseIn(ultrasonicPinRX, HIGH);
  //Serial.print(duration);
  if ((duration * 0.034 / 2) > tankCapacity)
    {return tankCapacity;} //Getting blips out of the system
  if ((duration * 0.034 / 2) < 0)
    {return 0;}
  
  return duration * 0.034 / 2;
  */
}

int getGals(int distance){
  return (-0.5 * distance) + 150;
}

void updateRate(int counterNum, int distance){
  averageRate[counterNum] = distance;
}

void emptyRateData(){
  for (int i = 0; i < dataSetSize; i++) {
    averageRate[i] = 0;
  }
}

double getRate() {
    int sum = 0;
    for (int i = 0; i < dataSetSize; i++) {
        sum += (abs(averageRate[i]-averageRate[i+1])) / (0.5);//(i+1);
        Serial.print("i,sum,[i],[i+1],(i+1)");
        Serial.print(i);Serial.print(",");
        Serial.print(sum);Serial.print(",");
        Serial.print(averageRate[i]);Serial.print(",");
        Serial.print(averageRate[i+1]);Serial.print(",");
        Serial.print(i+1);Serial.println(">>");
        
    }
    
    return abs(static_cast<double>(sum) / dataSetSize); //in cm/s
    //return abs(static_cast<double>(getGals(sum)) / dataSetSize); //in gal/s
}

double getRateGals() {
    int sum = 0;
    for (int i = 0; i < dataSetSize; i++) {
        sum += (averageRate[i]-averageRate[i+1]) / (i+1);
        /*Serial.print("i,sum,[i],[i+1],(i+1)");
        Serial.print(i);Serial.print(",");
        Serial.print(sum);Serial.print(",");
        Serial.print(averageRate[i]);Serial.print(",");
        Serial.print(averageRate[i+1]);Serial.print(",");
        Serial.print(i+1);Serial.println(">>");
        */
    }

    sum = getGals(sum);
    
    return abs(static_cast<double>(sum) / dataSetSize); //in cm/s
    //return abs(static_cast<double>(getGals(sum)) / dataSetSize); //in gal/s
}


double getETA(double fillRateCm, double currentDistance) {
  // Check if the fill rate is zero or negative
  if (fillRateCm <= 0) {
    return 0;  // Return -1 to indicate an invalid fill rate
  }
  
  // Calculate the remaining distance to fill
  double remainingDistance = currentDistance;
  
  // Calculate the ETA in seconds
  double eta = remainingDistance / fillRateCm;
  
  return eta;
}

double getAverageRateOfChange(int distances[], int size = dataSetSize) {
  //in seconds
  int firstDistance = distances[0];
  int lastDistance = distances[size - 1];
  
  // Calculate the change in distance and time
  int distanceDiff = lastDistance - firstDistance;
  double timeDiff = refreshDelay/1000;
  
  // Calculate the average rate of change
  double averageRateOfChange = static_cast<double>(distanceDiff) / timeDiff;
  
  return averageRateOfChange;
}

void setup() {
 
  // initialize OLED display with address 0x3C for 128x64
  if (!oled.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    while (true);
  }

  // put your setup code here, to run once:
  Serial1.begin(9600, SERIAL_8N1, ultrasonicPinRX, ultrasonicPinTX);
  //Serial1.begin(9600);
  Serial.begin(115200);
  Serial.println("Serial Txd is on pin: "+String(ultrasonicPinTX));
  Serial.println("Serial Rxd is on pin: "+String(ultrasonicPinRX));
  

  // Loading Screen
  delay(1000);         // wait two seconds for initializing
  oled.clearDisplay(); // clear display
  oled.drawBitmap(0, 0, logoGRPC, 128, 64, WHITE);
  oled.display();              // display on OLED

  delay(5000);
  oled.clearDisplay(); // clear display
  oled.setTextSize(2);         // set text size
  oled.setTextColor(WHITE);    // set text color


  // initialize ultrasonic Sensor
  //pinMode(ultrasonicPinTX, OUTPUT);
  //pinMode(ultrasonicPinRX, INPUT);

  emptyRateData();
}

void loop() {
  delay(refreshDelay/2); // Refresh Rate I want to use.  alf here, Half at end.

  // Test Area **********
  int newDistance = getDistance();
  int gals = getGals(newDistance);
  int rate = getRate();
  int eta = getETA(rate,newDistance);

  //SERIAL************************************
  Serial.print("Rate in cm/s:");
  Serial.println(rate);
  Serial.print("ETA: ");
  Serial.println(eta);

  /*Serial.print(" cm | Rate: ");
  //Serial.print(getAverageRateOfChange(averageRate));
  Serial.print(getRate());
  Serial.print(" cm/s | Gals: ");
  Serial.println(gals);
  */

  //OLED*******************************
  oled.clearDisplay();
  oled.setCursor(0, 0);       // set position to display
  oled.print(newDistance); oled.print(" cm");
  oled.setCursor(0, 16);
  oled.print(gals); oled.print(" gals");
  oled.setCursor(0, 32);
  //oled.print(getAverageRateOfChange(averageRate)); oled.print(" m/s");
  oled.print(getRate()); oled.print(" g/s");
  oled.setCursor(0, 47);
  oled.print(eta); oled.print("s ETA");
  oled.display();              // display on OLED
  
  delay(refreshDelay/2);

  //Loop 10 times, the array size, then reset
  if (counter > dataSetSize) {counter = 0;}
  //if (newDistance == oldDistance){
  //  emptyRateData();
  //}

  updateRate(counter, newDistance);
  int oldDistance = newDistance;
  counter++;

}