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

// ================== OLED ==================
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1
#define OLED_ADDR     0x3C
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// ================== SENSOR =================
const int breathPin = A0;

// ================== MIDI ===================
const byte MIDI_CHANNEL = 1;   // CH1
const byte MIDI_CC = 2;        // CC2 Breath

// ================== FILTER =================
float smoothValue = 0.0;
const float alphaUpBase   = 0.05;  // salita base (pp controllabili)
const float alphaDown     = 0.16;  // discesa più veloce

// ================== RANGE ==================
int rawMin = 20;
int rawMax = 850;
const int closeThreshold = 8;

// ================== RETURN TO ZERO (smart) ==================
static unsigned long lastActivityTime = 0;
const int zeroTimeoutMs   = 40;
const int activityMargin  = 5;

// ================== TONGUE DETECT ==================
static int lastRaw = 0;
static unsigned long lastTongueTrig = 0;

// Soglie tongue
const int tongueDeltaThreshold = 45;
const int tongueCooldownMs     = 70;
const int tongueBoost = 18;   // 12..25

// Evita tongue a fiato quasi zero
const int tongueMinMidiForTrig = 4;

// Aggancio base quando tongue (elimina "Ta TA TA")
const float tongueBaseLatch = 0.95f;  // 0.85..1.00

// ================== TONGUE SUSTAIN ENVELOPE ==================
// Mantiene un sustain dopo tongue e poi rilascia dolcemente
static unsigned long tongueEnvStart = 0;
static unsigned long tongueEnvHoldUntil = 0;
static unsigned long tongueEnvEnd = 0;

const int tongueHoldSustainMs = 45;   // 20..60  (tenuta)
const int tongueReleaseMs     = 90;   // 60..180 (rilascio morbido)
const int tongueSustainLevel  = 127;  // 115..127 (quanto alto)

// ================== STATE ==================
int lastSentValue = -1;

void setup() {
  Wire.begin();

  display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR);
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);

  // Splash
  display.setTextSize(2);
  display.setCursor(18, 1);
  display.println(" BREATH");
  display.setTextSize(1);
  display.setCursor(35, 25);
  display.println("CONTROLLER");
  display.setCursor(37, 55);
  display.println("Lanfranco");
  display.display();
  delay(2200);

  display.clearDisplay();
  display.display();

  int r0 = analogRead(breathPin);
  smoothValue = r0;
  lastRaw = r0;
  lastActivityTime = millis();
}

void loop() {
  while (usbMIDI.read()) {}

  unsigned long now = millis();
  int raw = analogRead(breathPin);

  // ------ activity timer ------
  if (raw > rawMin + activityMargin) lastActivityTime = now;

  // ------ delta ------
  int delta = raw - lastRaw;
  lastRaw = raw;

  // ------ filtro asimmetrico con salita dinamica ------
  float alphaUp = alphaUpBase;
  if (delta > 20) alphaUp = 0.12f;
  if (delta > 60) alphaUp = 0.25f;

  if (raw > smoothValue) smoothValue += alphaUp * (raw - smoothValue);
  else                   smoothValue += alphaDown * (raw - smoothValue);

  int filtered = (int)smoothValue;

  // close to zero
  if (filtered < rawMin + closeThreshold) {
    filtered = rawMin;
    smoothValue = (float)rawMin;
  }

  // force to zero if inactive
  if (now - lastActivityTime > (unsigned long)zeroTimeoutMs) {
    filtered = rawMin;
    smoothValue = (float)rawMin;
  }

  // ------ mapping ------
  int midiValue = map(filtered, rawMin, rawMax, 0, 127);
  midiValue = constrain(midiValue, 0, 127);

  // ------ TONGUE detect ------
  if (delta > tongueDeltaThreshold &&
      (now - lastTongueTrig) > (unsigned long)tongueCooldownMs &&
      midiValue >= tongueMinMidiForTrig) {

    lastTongueTrig = now;

    // aggancio base (niente Ta TA TA)
    float target = (float)raw * tongueBaseLatch;
    if (target > smoothValue) smoothValue = target;

    // avvia inviluppo sustain/decay
    tongueEnvStart = now;
    tongueEnvHoldUntil = now + (unsigned long)tongueHoldSustainMs;
    tongueEnvEnd = tongueEnvHoldUntil + (unsigned long)tongueReleaseMs;
  }

  // ------ OUTPUT ------
  int outValue = midiValue;
// Applica inviluppo solo se attivo (RELATIVO al livello corrente)
if (now < tongueEnvEnd) {
  if (now <= tongueEnvHoldUntil) {
    // fase HOLD: boost relativo
    outValue = max(outValue, constrain(midiValue + tongueBoost, 0, 127));
  } else {
    // fase RELEASE: boost che decade dolcemente
    unsigned long relT = now - tongueEnvHoldUntil; // 0..tongueReleaseMs
    if (relT > (unsigned long)tongueReleaseMs) relT = tongueReleaseMs;

    float k = 1.0f - (float)relT / (float)tongueReleaseMs; // 1 -> 0
    int envBoost = (int)(tongueBoost * k);

    outValue = max(outValue, constrain(midiValue + envBoost, 0, 127));
  }
}


  // send only on change
  if (outValue != lastSentValue) {
    usbMIDI.sendControlChange(MIDI_CC, outValue, MIDI_CHANNEL);
    lastSentValue = outValue;
    drawDisplay(outValue);
  }

  delay(5);
}

// ================== DISPLAY =================
void drawDisplay(int value) {
  display.clearDisplay();

  display.setTextSize(1);
  display.setCursor(38, 0);
  display.print("Expression");

  display.setTextSize(1);
  display.setCursor(50, 16);
  display.print("CC 02");

  char buf[4];
  sprintf(buf, "%03d", value);
  display.setTextSize(2);
  display.setCursor(48, 30);
  display.print(buf);

  int barWidth = map(value, 0, 127, 0, 120);
  display.drawRect(4, 50, 120, 10, SSD1306_WHITE);
  display.fillRect(4, 50, barWidth, 10, SSD1306_WHITE);

  display.display();
}