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

// ================== USB MIDI (PICO) ==================
Adafruit_USBD_MIDI usb_midi;

// ================== 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;

// ================== ENCODER =================
const int encCLK = 14;
const int encDT  = 15;
const int encSW  = 16;   // opzionale, non usato per ora

int lastEncCLK = HIGH;

// ================== EEPROM =================
const int EEPROM_SIZE = 64;
const int EEPROM_ADDR_GAIN = 0;

// sensibilità regolabile
float gain = 1.00f;
float lastSavedGain = 1.00f;

const float gainStep = 0.01f;
const float gainMin  = 0.80f;
const float gainMax  = 1.60f;

// timer per mostrare la schermata sensibilità
unsigned long gainDisplayUntil = 0;
unsigned long gainChangedAt = 0;
bool gainDirty = false;

// ================== MIDI ===================
const byte MIDI_CHANNEL = 1;
const byte MIDI_CC = 2;

// ================== FILTER =================
float smoothValue = 0.0;
const float alphaUpBase   = 0.05;
const float alphaDown     = 0.16;

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

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

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

const int tongueDeltaThreshold = 45;
const int tongueCooldownMs     = 70;
const int tongueBoost = 18;
const int tongueMinMidiForTrig = 4;
const float tongueBaseLatch = 0.95f;

// ================== TONGUE ENVELOPE ==================
static unsigned long tongueEnvHoldUntil = 0;
static unsigned long tongueEnvEnd = 0;

const int tongueHoldSustainMs = 45;
const int tongueReleaseMs     = 90;

// ================== PP STABILIZER ==================
static int ppHold = 0;
const int ppLimit = 35;
const int ppBand  = 2;

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

// ================== MIDI SEND ==================
static inline void sendCC_Pico(uint8_t cc, uint8_t val, uint8_t ch) {
  uint8_t msg[3] = {
    uint8_t(0xB0 | ((ch - 1) & 0x0F)),
    cc,
    val
  };
  usb_midi.write(msg, 3);
  usb_midi.flush();
}

// ================== DISPLAY ==================
void drawMainDisplay(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();
}

void drawGainDisplay() {
  display.clearDisplay();

  display.setTextSize(1);
  display.setCursor(24, 8);
  display.print("Sensitivity");

  char buf[8];
  dtostrf(gain, 4, 2, buf);

  display.setTextSize(2);
  display.setCursor(36, 28);
  display.print(buf);

  display.display();
}

// ================== EEPROM HELPERS ==================
void loadGainFromEEPROM() {
  EEPROM.get(EEPROM_ADDR_GAIN, gain);

  // protezione valori assurdi / memoria non inizializzata
  if (isnan(gain) || gain < gainMin || gain > gainMax) {
    gain = 1.00f;
  }

  lastSavedGain = gain;
}

void saveGainToEEPROM() {
  EEPROM.put(EEPROM_ADDR_GAIN, gain);
  EEPROM.commit();
  lastSavedGain = gain;
  gainDirty = false;
}

// ================== ENCODER ==================
void readEncoder() {
  int currentCLK = digitalRead(encCLK);

  if (currentCLK != lastEncCLK && currentCLK == HIGH) {
    if (digitalRead(encDT) != currentCLK) {
      gain += gainStep;
    } else {
      gain -= gainStep;
    }

    if (gain < gainMin) gain = gainMin;
    if (gain > gainMax) gain = gainMax;

    gainDisplayUntil = millis() + 1500;
    gainChangedAt = millis();
    gainDirty = true;

    drawGainDisplay();
  }

  lastEncCLK = currentCLK;
}

void setup() {
  Wire.begin();

  pinMode(encCLK, INPUT_PULLUP);
  pinMode(encDT, INPUT_PULLUP);
  pinMode(encSW, INPUT_PULLUP);

  lastEncCLK = digitalRead(encCLK);

  EEPROM.begin(EEPROM_SIZE);
  loadGainFromEEPROM();

  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();
  ppHold = 0;

  sendCC_Pico(MIDI_CC, 0, MIDI_CHANNEL);
  lastSentValue = 0;
  drawMainDisplay(0);
}

void loop() {
  TinyUSBDevice.task();
  usb_midi.read();

  readEncoder();

  // salva solo dopo 1.5 s senza toccare più l'encoder
  if (gainDirty && (millis() - gainChangedAt > 1500)) {
    if (fabs(gain - lastSavedGain) > 0.0001f) {
      saveGainToEEPROM();
    }
  }

  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 (lastSentValue > ppLimit) {
    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;
  }

  // ===== SENSITIVITY GAIN =====
  float scaled = rawMin + ((filtered - rawMin) * gain);

  int midiValue = map((int)scaled, rawMin, rawMax, 0, 127);
  midiValue = constrain(midiValue, 0, 127);

  // ------ PP stabilizer ------
  if (midiValue <= ppLimit) {
    if (abs(midiValue - ppHold) <= ppBand) midiValue = ppHold;
    else ppHold = midiValue;
  } else {
    ppHold = midiValue;
  }

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

    lastTongueTrig = now;

    float target = (float)raw * tongueBaseLatch;
    if (target > smoothValue) smoothValue = target;

    tongueEnvHoldUntil = now + (unsigned long)tongueHoldSustainMs;
    tongueEnvEnd = tongueEnvHoldUntil + (unsigned long)tongueReleaseMs;
  }

  // ------ OUTPUT ------
  int outValue = midiValue;

  if (now < tongueEnvEnd) {
    if (now <= tongueEnvHoldUntil) {
      outValue = max(outValue, constrain(midiValue + tongueBoost, 0, 127));
    } else {
      unsigned long relT = now - tongueEnvHoldUntil;
      if (relT > (unsigned long)tongueReleaseMs) relT = tongueReleaseMs;

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

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

  if (outValue != lastSentValue) {
    sendCC_Pico(MIDI_CC, (uint8_t)outValue, MIDI_CHANNEL);
    lastSentValue = outValue;

    if (now > gainDisplayUntil) {
      drawMainDisplay(outValue);
    }
  }

  // se la schermata gain è scaduta, torna a quella principale
  if (gainDisplayUntil != 0 && now > gainDisplayUntil) {
    gainDisplayUntil = 0;
    drawMainDisplay(lastSentValue < 0 ? 0 : lastSentValue);
  }

  delay(5);
}