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

// --- FONTS ---
// These are included with the Adafruit GFX library
#include <Fonts/FreeSansBold18pt7b.h> // Large font for numbers
#include <Fonts/FreeSans9pt7b.h>      // Small font for labels

// --- HARDWARE CONFIGURATION ---
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1 
#define SCREEN_ADDRESS 0x3C // Check if your OLED is 0x3C or 0x3D

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

// --- PINS ---
const int PIN_ENCODER_A = 2; // Interrupt Pin
const int PIN_ENCODER_B = 3; // Interrupt Pin
const int PIN_BTN_RESET = 4;
const int PIN_BTN_UNIT  = 5;

// --- CALIBRATION (CRITICAL) ---
// You must calculate this based on your wheel diameter.
// Formula: Pulses per Revolution / (Wheel Diameter (m) * PI)
// Example: 60mm wheel, 20 pulses per rev logic -> Adjust strictly by trial
// STARTING VALUE: Set to 1000.0 pulses per meter for testing, then calibrate.
float PULSES_PER_METER = 2400.0; 

// --- VARIABLES ---
volatile long encoderCount = 0;
long lastEncoderCount = 0;

// Unit State: 0 = Meters, 1 = Feet
bool isFeet = false; 

// Debounce handling
unsigned long lastDebounceTime = 0;
const unsigned long debounceDelay = 300;

void setup() {
  // 1. Initialize Serial for debugging
  Serial.begin(9600);

  // 2. Initialize Pins
  pinMode(PIN_ENCODER_A, INPUT_PULLUP);
  pinMode(PIN_ENCODER_B, INPUT_PULLUP);
  pinMode(PIN_BTN_RESET, INPUT_PULLUP);
  pinMode(PIN_BTN_UNIT, INPUT_PULLUP);

  // 3. Attach Interrupts for high-speed counting
  attachInterrupt(digitalPinToInterrupt(PIN_ENCODER_A), readEncoder, RISING);
  //attachInterrupt(digitalPinToInterrupt(PIN_ENCODER_B), readEncoder, CHANGE);

  // 4. Initialize OLED
  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }

  // Initial Display
  updateDisplay();
}

void loop() {
  bool needsUpdate = false;
  unsigned long currentTime = millis();

  // --- BUTTON: RESET ---
  if (digitalRead(PIN_BTN_RESET) == LOW) {
    if ((currentTime - lastDebounceTime) > debounceDelay) {
      encoderCount = 0;
      lastEncoderCount = 0; // Force update
      needsUpdate = true;
      lastDebounceTime = currentTime;
    }
  }

  // --- BUTTON: UNIT TOGGLE ---
  if (digitalRead(PIN_BTN_UNIT) == LOW) {
    if ((currentTime - lastDebounceTime) > debounceDelay) {
      isFeet = !isFeet;
      needsUpdate = true;
      lastDebounceTime = currentTime;
    }
  }

  // --- CHECK FOR MOVEMENT ---
  // We disable interrupts briefly to read the volatile variable safely
  noInterrupts();
  long currentCount = encoderCount;
  interrupts();

  if (currentCount != lastEncoderCount) {
    needsUpdate = true;
    lastEncoderCount = currentCount;
  }

  // --- REFRESH DISPLAY ---
  if (needsUpdate) {
    updateDisplay();
  }
}

// --- INTERRUPT SERVICE ROUTINE ---
void readEncoder() {

  int bValue = digitalRead(PIN_ENCODER_B);
  if (bValue == HIGH) {
    encoderCount++; // Clockwise
  }
  if (bValue == LOW) {
    encoderCount--; // Counterclockwise
  }
}

// --- DISPLAY FUNCTION ---
void updateDisplay() {
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);

  // Calculate Distance
  float distanceMeters = encoderCount / PULSES_PER_METER;
  float displayValue = 0.0;
  
  if (isFeet) {
    displayValue = distanceMeters * 3.28084; // Convert Meters to Feet
  } else {
    displayValue = distanceMeters;
  }

  // 1. Draw Header (Unit Mode)
  display.setFont(&FreeSans9pt7b);
  display.setCursor(0, 15);
  if (isFeet) {
    display.print("Length (ft)");
  } else {
    display.print("Length (m)");
  }

  // 2. Draw Measurement Value (Large Font)
  display.setFont(&FreeSansBold18pt7b);
  
  // Center alignment logic (approximate)
  int xPos = 0;
  if (abs(displayValue) < 10) xPos = 20;
  else if (abs(displayValue) < 100) xPos = 10;
  
  display.setCursor(xPos, 50); 
  display.print(displayValue, 3); // 2 decimal places

  display.display();
}