#include <Arduino.h>
#include <U8g2lib.h>
#include <SPI.h>
#include <Wire.h>
U8G2_SSD1306_128X32_UNIVISION_F_HW_I2C u8g2(U8G2_R0);  // set up the SSD1306 OLED display

const int OUT_PIN = A2;
const int IN_PIN = A0;
const float IN_STRAY_CAP_TO_GND = 24.48;
const float IN_CAP_TO_GND = IN_STRAY_CAP_TO_GND;
const float R_PULLUP = 34.8;
const int MAX_ADC_VALUE = 1023;

void setup() {
  pinMode(OUT_PIN, OUTPUT);
  pinMode(IN_PIN, OUTPUT);
  Serial.begin(9600);
  u8g2.begin();
}

void loop() {
  pinMode(IN_PIN, INPUT);
  digitalWrite(OUT_PIN, HIGH);
  int val = analogRead(IN_PIN);
  digitalWrite(OUT_PIN, LOW);

  if (val < 1000) {
    pinMode(IN_PIN, OUTPUT);

    float capacitance = (float)val * IN_CAP_TO_GND / (float)(MAX_ADC_VALUE - val);

    Serial.print(F("Capacitance Value = "));
    Serial.print(capacitance, 3);
    Serial.print(F(" pF ("));
    Serial.print(val);
    Serial.println(F(") "));

    u8g2.setFont(u8g2_font_6x12_tf);  // set font
    // cursor position (L to R, Row)
    u8g2.setCursor(3, 30);
    u8g2.print("~ CAPACITOR READING ~");  // write to the internal memory
  //  u8g2.setFont(u8g2_font_ncenB10_tr);   // set font
   // u8g2.setCursor(0, 15);                // set cursor position
 //   u8g2.print("Insert Capacitor ");
    
   u8g2.drawFrame(1,2,126,10);
   u8g2.drawBox(1,2,14,10);
   u8g2.drawBox(17,2,14,10);
   u8g2.drawBox(33,2,14,10);
    u8g2.drawVLine(53,2,10);
  u8g2.drawVLine(54,2,10);
   u8g2.drawVLine(82,2,10);
  u8g2.drawVLine(83,2,10);
   u8g2.drawVLine(98,2,10);
  u8g2.drawVLine(99,2,10);
     u8g2.drawVLine(114,2,10);
  u8g2.drawVLine(115,2,10);

    u8g2.sendBuffer();  // transfer internal memory to the display

  }

  else {
    pinMode(IN_PIN, OUTPUT);
    delay(1);
    pinMode(OUT_PIN, INPUT_PULLUP);
    unsigned long u1 = micros();
    unsigned long t;
    int digVal;

    do {
      digVal = digitalRead(OUT_PIN);
      unsigned long u2 = micros();
      t = u2 > u1 ? u2 - u1 : u1 - u2;
    } while ((digVal < 1) && (t < 400000L));

    pinMode(OUT_PIN, INPUT);
    val = analogRead(OUT_PIN);
    digitalWrite(IN_PIN, HIGH);
    int dischargeTime = (int)(t / 1000L) * 5;
    delay(dischargeTime);
    pinMode(OUT_PIN, OUTPUT);
    digitalWrite(OUT_PIN, LOW);
    digitalWrite(IN_PIN, LOW);

    float capacitance = -(float)t / R_PULLUP / log(1.0 - (float)val / (float)MAX_ADC_VALUE);

    Serial.print(F("Capacitance Value = "));
    u8g2.clearBuffer();               // clear the internal memory
    u8g2.sendBuffer(); // transfer internal memory to the display
    u8g2.setFont(u8g2_font_6x12_tf);  // set font
    u8g2.setCursor(3, 30);
    u8g2.print("~ CAPACITOR READING ~");  // write to the internal memory
    u8g2.sendBuffer();                    // transfer internal memory to the display
    if (capacitance > 1000.0) {
      Serial.print(capacitance / 1000.0, 2);
      Serial.print(F(" uF"));
      u8g2.setFont(u8g2_font_ncenB10_tr);  // set font
      u8g2.setCursor(30, 15);              // set cursor position
      u8g2.print(capacitance);             // print capacitance value
      u8g2.print(" uF");
    } else {
      Serial.print(capacitance, 2);
      Serial.print(F(" nF"));
      u8g2.setFont(u8g2_font_ncenB10_tr);  // set font
      // cursor position (L to R, Row)
      u8g2.setCursor(30, 15);    // set cursor position
      u8g2.print(capacitance);  // print value with decimal places, as capacitance variable is a float, not an int.
      u8g2.print(" nF");
    }
    u8g2.sendBuffer();  // transfer internal memory to the display
    delay(1000);
    u8g2.clearBuffer();  // clear the internal memory

    Serial.print(F(" ("));
    Serial.print(digVal == 1 ? F("Normal") : F("HighVal"));
    Serial.print(F(", t= "));
    Serial.print(t);
    Serial.print(F(" us, ADC= "));
    Serial.print(val);
    Serial.println(F(")"));
  }
  while (millis() % 1000 != 0)
    ;
}