#include <Wire.h>
#include <SPI.h>
#include <Adafruit_Sensor.h>
#include "Adafruit_BMP3XX.h"
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <afstandssensor.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

//Define Left Sensor
AfstandsSensor afstandssensor(0, 1); 

//Define Right Sensor
AfstandsSensor vRightSensor(2, 3); 

//Connect Vcc-3.3V, Gnd-Gnd, SCL-D22, SDA-D21 (0.96 inch OLED - ESP32 DEV KIT V1)
//CODE_NO: 02

#define BMP_SCK 13
#define BMP_MISO 12
#define BMP_MOSI 11
#define BMP_CS 10

#define SEALEVELPRESSURE_HPA (1020.0)

Adafruit_BMP3XX bmp;
float vDistance = 0;
float vRightDistance = 0;

void getDistance() {
  Serial.println(afstandssensor.afstandCM());
  vDistance = afstandssensor.afstandCM();

  Serial.println(vRightSensor.afstandCM());
  vRightDistance = vRightSensor.afstandCM();
}

void setup() {
  Serial.begin(115200);
  
 
Wire.begin(5, 4); 
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  
  display.display();
  while (!Serial);
  Serial.println("Adafruit BMP388 / BMP390 test");

  if (!bmp.begin_I2C()) {   // hardware I2C mode, can pass in address & alt Wire
    Serial.println("Could not find a valid BMP3 sensor, check wiring!");
    while (1);
    if (! bmp.performReading()) {
    Serial.println("Failed to perform reading :(");
    return;
    }
  }

  // Set up oversampling and filter initialization
  bmp.setTemperatureOversampling(BMP3_OVERSAMPLING_8X);
  bmp.setPressureOversampling(BMP3_OVERSAMPLING_4X);
  bmp.setIIRFilterCoeff(BMP3_IIR_FILTER_COEFF_3);
  bmp.setOutputDataRate(BMP3_ODR_50_HZ);

  display.clearDisplay();
}



void loop() {
  // Clear display for redraw
  display.clearDisplay();
  display.setTextSize(1);             // Draw 2X-scale text
  display.setTextColor(WHITE);
  display.setCursor(0,0);
  display.print(String(bmp.temperature));
  display.print(" c");
  display.display();

//   Serial.print(bmp.temperature);
//   Serial.println(" *C");

//   Serial.print("Pressure = ");
//   Serial.print(bmp.pressure / 100.0);
//   Serial.println(" hPa");

//   Serial.print("Approx. Altitude = ");
//   Serial.print(bmp.readAltitude(SEALEVELPRESSURE_HPA));
//   Serial.println(" m");

//   Serial.println();

  Serial.println("Displaying Ultrasonic Sensor");
  getDistance();
  if (vDistance < 150)
  {
    display.setTextSize(2);             // Draw 2X-scale text
    display.setTextColor(WHITE);
    display.setCursor(0,30);
    display.print("L:" + String(vDistance));
    display.print(" m");
    display.display();
  }else
  {
    display.setTextSize(1);             // Draw 2X-scale text
    display.setTextColor(WHITE);
    display.setCursor(0,30);
    display.print("");
    
    display.display();
  }

  if (vRightDistance < 150)
  {
    display.setTextSize(2);             // Draw 2X-scale text
    display.setTextColor(WHITE);
    display.setCursor(0,50);
    display.print("R:" + String(vRightDistance));
    display.print(" m");
    display.display();
  }else
  {
    display.setTextSize(1);             // Draw 2X-scale text
    display.setTextColor(WHITE);
    display.setCursor(0,30);
    display.print("");
    
    display.display();
  }

  Serial.println("Displaying Right Ultrasonic Sensor");
  Serial.println("Right Sensor:" + String(vRightDistance));

  delay(2000);
}