#include <Wire.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1 // No reset pin
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define NUM_REF_RESISTORS 8
#define NUM_SELECT_PINS   3
#define MAX_ANALOG_VALUE  1023
#define SWITCH_RESISTANCE 4.5

// Reference Resistor values for the multiplexer
float rRef[NUM_REF_RESISTORS] = {47, 100, 1000, 10000, 100000, 1000000, 5000000, 10000000};

// Multiplexer select pins S0, S1, S2, S3
const byte rSelPins[NUM_SELECT_PINS] = {5, 4, 3};  // Update sesuai pin Arduino
const byte enableMux = 6; // Enable pin for the 74HC4067

void setup() {
  pinMode(enableMux, OUTPUT);
  digitalWrite(enableMux, HIGH);  // Disable all switches initially
  
  // Set selector pins as output
  for (int i = 0; i < NUM_SELECT_PINS; i++) {
    pinMode(rSelPins[i], OUTPUT);
    digitalWrite(rSelPins[i], LOW);  // Set to LOW initially
  }

  // Initialize display with I2C
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3C for SSD1306
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }
  display.clearDisplay();
  display.display();
  
  // Print intro message on the screen
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0, 0);
  display.println("Starting ArduinOhmmeter...");
  display.display();

  Serial.begin(9600);
  Serial.println("\nStarting ArduinOhmmeter...");
}

void loop() {
  int cOut;
  float delta, deltaBest1 = MAX_ANALOG_VALUE, deltaBest2 = MAX_ANALOG_VALUE;
  float rBest1 = -1, rBest2 = -1, rR, rX;
  char unit = 0, fStr[16];

  for (byte count = 0; count < NUM_REF_RESISTORS; count++) {
    // Set the Mux select pins to switch in one Rref at a time.
    // Use the count to control S0-S3 of the 74HC4067
    for (int i = 0; i < NUM_SELECT_PINS; i++) {
      digitalWrite(rSelPins[i], (count >> i) & 1); // Set the appropriate selector pins
    }

    digitalWrite(enableMux, LOW);  // Enable the selected channel
    delay(count + 1);  // Small delay for stabilization
    cOut = analogRead(A0);  // Read analog value from the selected channel
    digitalWrite(enableMux, HIGH);  // Disable the mux channel
    delay(NUM_REF_RESISTORS - count);
    
    // Process valid analog values
    if (cOut < MAX_ANALOG_VALUE) {
      rR = rRef[count] + SWITCH_RESISTANCE;
      rX = (rR * cOut) / (MAX_ANALOG_VALUE - cOut);
      delta = (MAX_ANALOG_VALUE / 2.0 - cOut);
      
      if (fabs(delta) < fabs(deltaBest1)) {
        deltaBest2 = deltaBest1;
        rBest2 = rBest1;
        deltaBest1 = delta;
        rBest1 = rX;
      } else if (fabs(deltaBest2) > fabs(delta)) {
        deltaBest2 = delta;
        rBest2 = rX;
      }
    }
  }

  // Make sure there are valid readings
  if (rBest1 >= 0 && rBest2 >= 0) {
    if (deltaBest1 * deltaBest2 < 0) {
      rX = rBest1 - deltaBest1 * (rBest2 - rBest1) / (deltaBest2 - deltaBest1);
    } else {
      rX = rBest1;
    }
    unit = ScaleToMetricUnits(&rX, fStr);
    // Tambahkan Serial print untuk menampilkan hasil
    Serial.print("Resistance: ");
    Serial.print(fStr);
    Serial.print(" ");
    Serial.print(unit);
    Serial.println("Ohms");
  } else {
    Serial.println("No valid readings");
  }

  // Tampilkan nilai resistansi potensiometer dan hasil multiplexer
  DisplayResultsOnLEDScreen(unit, fStr);
  delay(250);
}

// Convert resistance value to metric units and format for display
char ScaleToMetricUnits(float *prVal, char fStr[]) {
  char unit;

  if (*prVal < 1000) {
    unit = ' ';
  } else if (*prVal < 1000000) {
    *prVal /= 1000;
    unit = 'K';
  } else if (*prVal < 1000000000) {
    *prVal /= 1000000;
    unit = 'M';
  } else {
    *prVal /= 1000000000;
    unit = 'G';
  }

  // // Format float value to string with 3-digit precision
  // dtostrf(*prVal, 6, 2, fStr);
   for (int k=2, s=10; k >= 0; k--, s*=10)
  {
    if ((int)(*prVal) / s == 0)
    {
      dtostrf(*prVal, 6, k, fStr); // convert the float result to a string
      break;
    }
  }
  return unit;
}

// Display results on the SSD1306 OLED screen, including potensiometer value
void DisplayResultsOnLEDScreen(char unit, char fStr[]) {
  display.clearDisplay();
  display.setTextSize(1);
  display.setCursor(0, 0);
  
  if (unit != 0) {
    display.print(fStr);
    display.print(" ");
    display.print(unit);
    display.println("Ohms");
  } else {
    display.println("No reading");
  }
  display.display();
}