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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define SCREEN_ADDRESS 0x3C

// --- MUX CONTROL ---
const int S0 = 0; const int S1 = 1; const int S2 = 2; const int S3 = 3;
const int SIG_PIN = 26; 

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// --- SMOOTHING VARIABLES ---
float smoothedValue = 0;
float alpha = 0.15; // Smoothing factor (0.01 = very slow/smooth, 0.9 = jerky/fast)
int history[SCREEN_WIDTH];
unsigned long lastPlotTime = 0;
const int plotInterval = 39; // 128 pixels * 39ms ≈ 5 seconds of total screen width

void setup() {
  Wire.setSDA(6); Wire.setSCL(7); Wire.begin();
  pinMode(S0, OUTPUT); pinMode(S1, OUTPUT);
  pinMode(S2, OUTPUT); pinMode(S3, OUTPUT);
  
  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) for(;;);
  
  display.clearDisplay();
  for(int i=0; i<SCREEN_WIDTH; i++) history[i] = SCREEN_HEIGHT - 1;
}

void loop() {
  // 1. Select J1
  digitalWrite(S0, LOW); digitalWrite(S1, LOW);
  digitalWrite(S2, LOW); digitalWrite(S3, LOW);
  
  // 2. Read and Smooth (The "Mario" physics)
  int raw = analogRead(SIG_PIN);
  // Exponential Moving Average Formula
  smoothedValue = (alpha * raw) + ((1.0 - alpha) * smoothedValue);

  // 3. Timing Logic (Plotting at the speed you requested)
  if (millis() - lastPlotTime >= plotInterval) {
    lastPlotTime = millis();

    // Shift history left
    for (int i = 0; i < SCREEN_WIDTH - 1; i++) {
      history[i] = history[i + 1];
    }
    // Add new smoothed point to the end
    history[SCREEN_WIDTH - 1] = map((int)smoothedValue, 0, 1023, SCREEN_HEIGHT - 1, 15);
  }

  // 4. Draw
  display.clearDisplay();
  
  // HUD
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0,0);
  display.print("SMOOTHED V: "); 
  display.print((smoothedValue / 1023.0) * 3.3, 2);

  // Draw the smooth curve
  for (int i = 0; i < SCREEN_WIDTH - 1; i++) {
    display.drawLine(i, history[i], i+1, history[i+1], SSD1306_WHITE);
  }

  display.display();
}