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

/* =========================================================
   CONFIG
   ========================================================= */

// ---- OLED ----
#define OLED_ADDR 0x3C
#define OLED_W 128
#define OLED_H 64

//Tracking Functions + Menues
bool funcKey = false;

float VOL = 0.5f;
float ABS_VOL = 0.5f;
//float decay = 1.0f;
float decay = 0.99998f;
float decay_off = 0.9995f;
Adafruit_SSD1306 display(OLED_W, OLED_H, &Wire);
// ----- Menu Functions
int sound = 0;  //Default voice
String sounds[6] = { "Saw+Sine", "Sine", "Saw", "Triangle", "Square", "Noise" };
int sound_count = sizeof(sounds) / sizeof(sounds[0]);

// Modes
int mode = 0;
String mode_names[7] = { "Volume", "Sound", "Octave", "Envel.", "ArpTyp", "ArpSpd", "ArpDir"};
int mode_count = sizeof(mode_names) / sizeof(mode_names[0]);

// Octave
int octave = 2;

// Arpeggio
//int arp_pattern[5] = {1 ,3 ,5 ,3 ,1 };
int arp_pattern[5] = {0 ,4 ,7 ,3 ,1 };
int arpeggio = 1;  // 0 = Off, 1 =
int arp_notes = 3; // Now many notes to arpeggiate
int arp_delay = 150; // 300 ms


// ---- Audio ----
//constexpr uint32_t SAMPLE_RATE = 96000;
constexpr uint32_t SAMPLE_RATE = 44100;
constexpr uint8_t CHANNELS = 1;
constexpr uint8_t BITS = 16;

// ----- Reverb -----
constexpr int reverb_time = 200;  // 200 ms.
constexpr int reverb_samples = (int)(reverb_time / 1000 * SAMPLE_RATE);
float reverbqueue[reverb_samples] = {};
float reverb_fade = 0.3f;

// ---- Matrix ----
const int X_PINS[4] = { 8, 9, 10, 7 };
const int Y_PINS[4] = { 19, 20, 21, 22 };

// ---- Synth ----
constexpr int MAX_VOICES = 200;

/* =========================================================
   NOTES
   ========================================================= */

const char *noteNames[12] = {
  "C", "C#", "D", "D#",
  "E", "F", "F#", "G",
  "G#", "A", "A#", "B"
};

float noteFreq[12];

void initNotes() {
  int octMult = powf(2, octave);
  for (int i = 0; i < 12; i++) {
    noteFreq[i] = 55.0f * octMult * powf(2.0f, i / 12.0f);
  }
}

/* =========================================================
   VOICES
   ========================================================= */

struct Voice {
  bool active = false;
  float freq = 0;
  float startFreq = 0;
  float phase1 = 0;
  float phase2 = 0;
  float decay = decay;
  float env = 0;
  float amp = 0;
  long  arp_time = 0;
  int   arp_position = 0;
};

Voice voices[MAX_VOICES];

void noteOn(float freq) {
  for (auto &v : voices) {
    if (!v.active) {
      v.active = true;
      v.freq = freq;
      v.startFreq = freq;
      v.env = VOL;
      v.arp_time = millis();
      v.arp_position = 0;
      v.phase1 = 0.5f;
      v.phase2 = 0.5f;
      v.decay = decay;
      return;
    }
  }
}

void noteOff(float freq) {
  for (auto &v : voices) {
    if (v.active && fabs(v.startFreq - freq) < 0.01f) {
      //v.active = false;
      v.decay = decay_off;
    }
  }
}

float sineWave(float phase) {
  // Convert phase (0-1) to radians (0-2π)
  float radians = phase * 2.0f * 3.14156f;

  // Generate sine value (-1 to 1)
  float sineValue = sin(radians);

  // Map from (-1 to 1) to (0 to 1)
  float output = (sineValue + 1.0f) / 2.0f;

  return output;
}

float handleReverb(float val) {
  for (int i = 0; i < reverb_samples - 1; i++) {
    reverbqueue[i] = reverbqueue[i + 1];
  }
  reverbqueue[reverb_samples - 1] = val;
  return reverbqueue[0] * reverb_fade;
}


float wavetable[16] = { 0.2f, 0.4f, 0.9f, 0.2f, 0.4f, 0.9f, 0.2f, 0.4f, 0.9f, 0.2f, 0.4f, 0.9f, 0.2f, 0.4f, 0.9f, 0.2f };
float wavetableInt[1024] = {};


int genWaveTable() {
  int size = 16;
  int newSize = 1024;
  int div = newSize / size;
  for (int s = 0; s < size - 1; s++) {
    for (int i = 0; i < div; i++) {
      wavetableInt[(s * div) + i] = wavetable[s] + ((wavetable[s + 1] - wavetable[s]) * (i / div));
    }
  }
  return 0;
}


float transposeSemitones(float freq, int semitones) {
    return freq * powf(2.0f, semitones / 12.0f);
}
/* =========================================================
   SYNTH STREAM
   ========================================================= */

class SynthStream : public Stream {
public:
  uint8_t frame[4];
  uint8_t index = 4;

  float lp = 0.0f;
  float cutoff = 1.0f * VOL;
  float detune = 0.004f;
  //float detune = 0.006f;

  static constexpr int DELAY_SAMPLES = 960;
  float delayL[DELAY_SAMPLES]{};
  int delayIndex = 0;

  float scope[128]{};
  int scopeIndex = 0;

  int available() override {
    return 4;
  }

  int read() override {
    if (index >= 4) {
      generate();
      index = 0;
    }
    return frame[index++];
  }

  int peek() override {
    return -1;
  }
  size_t write(uint8_t) override {
    return 0;
  }

private:
  void generate() {
    float mix = 0.0f;

    for (auto &v : voices) {
      if (!v.active) continue;
      float osc = 0.0f;


      if (arpeggio>0 && (millis()-v.arp_time)>arp_delay){
        v.arp_time = millis();
        v.arp_position++;
        v.arp_position = v.arp_position % arp_notes;
        v.freq = transposeSemitones(v.startFreq, arp_pattern[v.arp_position]);
      }
      // Defined voices
      switch (sound) {
        // Saw+Sine
        case 0:
          osc = (sineWave(v.phase1) + sineWave(v.phase2) + wavetableInt[(int)floor(1024.0f * v.phase2)]) / 3.0f;
          break;
        // Sine Wave
        case 1:
          osc = sineWave(v.phase1);
          break;
        // Saw Wave
        case 2:
          osc = v.phase1;
          break;
        // Triangle Wave
        case 3:
          osc = abs((v.phase1 * 2) - 1.0f);
          break;
        // Square Wave
        case 4:
          osc = (v.phase1 > 0.5f);
          break;
        // Noise
        case 5:
          osc = sineWave(v.phase1) + (random(-1000, 1000) / 2000.0f);
          break;
      }

      v.phase1 += v.freq / SAMPLE_RATE;
      v.phase2 += (v.freq * 1.0f * (1.25f + detune)) / SAMPLE_RATE;
      //v.phase2 += (v.freq *1.25f * (1.0f + detune)) / SAMPLE_RATE;
      if (v.phase1 >= 1) v.phase1 -= 1;
      if (v.phase2 >= 1) v.phase2 -= 1;

      v.amp = v.env;
      v.env *= v.decay;
      if (v.env < 0.0005f) v.active = false;

      mix += osc * v.amp;
    }
    // Handle reverb
    mix = (handleReverb(mix) + mix) / 2.0f;


    lp += cutoff * (mix - lp);
    float out = lp * 0.4f;


    int r = (delayIndex - 480 + DELAY_SAMPLES) % DELAY_SAMPLES;
    float ch = out + delayL[r] * 0.6f;

    delayL[delayIndex] = out;
    delayIndex = (delayIndex + 1) % DELAY_SAMPLES;

    scope[scopeIndex++] = ch;
    scopeIndex &= 127;

    int16_t pcm = (int16_t)(ch * 14000);

    frame[0] = pcm & 0xFF;
    frame[1] = pcm >> 8;
    frame[2] = frame[0];
    frame[3] = frame[1];
  }
};

/* =========================================================
   I2S
   ========================================================= */

I2SStream i2s;
SynthStream synth;
StreamCopy copier(i2s, synth);

/* =========================================================
   MATRIX SCAN
   ========================================================= 
 Scan the key matrix and update the 16 bits representing them
 */
uint16_t scanMatrix() {
  uint16_t state = 0;

  for (int x = 0; x < 4; x++) {
    digitalWrite(X_PINS[x], HIGH);
    delayMicroseconds(10);

    for (int y = 0; y < 4; y++) {
      if (digitalRead(Y_PINS[y])) {
        state |= (1 << (x + y * 4));
      }
    }

    digitalWrite(X_PINS[x], LOW);
  }
  return state;
}

static uint16_t lastKeys = 0;
static uint32_t lastUI = 0;
uint16_t keys = 0;
uint16_t changed = keys ^ lastKeys;

// Handle key scanning and menus
void handleInputs() {
  lastKeys = keys;
  keys = scanMatrix();
  changed = keys ^ lastKeys;
  for (int i = 0; i < 16; i++) {
    if (changed & (1 << i)) {
      if (i < 12) {
        if ((keys & (1 << i))) {
          noteOn(noteFreq[i]);
        } else {
          noteOff(noteFreq[i]);
        }
      } else {
        funcKey = (keys & (1 << 12));
        // If Enter key pressed
        if (keys & (1 << 14)) {
          mode = mode + 1;
          mode = mode % mode_count;
        }
        // If Left key is pressed
        if (keys & (1 << 13)) {
          switch (mode) {
            case 0:  // Volume
              if (VOL > 0.1f) { VOL -= 0.1f; }
              break;
            case 1:  // Sound
              sound = sound - 1;
              if (sound < 0) { sound = sound_count - 1; }
              break;
            case 2:  // Octave
              if (octave > 1) { octave = octave - 1; }
              initNotes();
              break;
            case 3:  // Envelope
              // TODO
              break;
          }

          // If Right key is pressed
        } else if ((keys & (1 << 15))) {
          switch (mode) {
            case 0:  // Volume
              if (VOL < 1.0f) { VOL += 0.1f; }
              break;
            case 1:  // Sound
              sound = sound + 1;
              sound = sound % sound_count;
              break;
            case 2:  // Octave
              if (octave < 8) { octave = octave + 1; }
              initNotes();
              break;
            case 3:  // Envelope
              break;
          }
        }
        //menuButton(i-12);
      }
    }
  }
  return;
}


/* =========================================================
   OLED DRAWING
   ========================================================= */

void drawPads(uint16_t keys) {
  const int w = 30, h = 14;

  for (int r = 0; r < 3; r++) {
    for (int c = 0; c < 4; c++) {
      int i = c + r * 4;
      int x = c * w + 2;
      int y = r * h + 2;
      bool on = keys & (1 << i);

      display.drawRect(x, y, w - 2, h - 2, SSD1306_WHITE);
      if (on) display.fillRect(x + 1, y + 1, w - 4, h - 4, SSD1306_WHITE);

      display.setTextColor(on ? SSD1306_BLACK : SSD1306_WHITE);
      display.setCursor(x + 4, y + 2);
      display.print(noteNames[i]);
    }
  }
}

void drawScope() {
  int mid = 45;  // middle of scope on Y position
  int height = 20;
  float maxVal = 0.0f;
  float threshold = 0.002f;
  float scale = 1.0f;
  for (int x = 0; x < 127; x++) {
    int i1 = (synth.scopeIndex + x) & 127;
    if (synth.scope[i1] > threshold && synth.scope[i1] > maxVal) {
      maxVal = synth.scope[i1];
    }
  }
  scale = 1.0f / maxVal;
  for (int x = 0; x < 127; x++) {
    int i1 = (synth.scopeIndex + x) & 127;
    int i2 = (i1 + 1) & 127;
    display.drawLine(
      x,
      mid - synth.scope[i1] * scale * height,
      x + 1,
      mid - synth.scope[i2] * scale * height,
      SSD1306_WHITE);
  }
}

void drawVoices() {
  int y = 62;
  for (int i = 0; i < MAX_VOICES; i++) {
    int x = i * 20;
    if (voices[i].active) {
      display.fillRect(x, y - 6, 16, 6, SSD1306_WHITE);
    } else {
      display.drawRect(x, y - 6, 16, 6, SSD1306_WHITE);
    }
  }
}

void drawVolumebar() {
  display.fillRect(0, 60, (128 * VOL), 64, SSD1306_WHITE);
}
// Draws the current Synthesizer sound preset
void drawInfo() {

  // Show current Sound preset
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 6);
  display.print(sounds[sound]);

  // Show current menu mode
  display.setCursor(64, 6);
  display.print("Set:" + mode_names[mode]);
  display.setCursor(90, 18);
  display.print("Oct:" + String(octave));
  display.setCursor(40, 18);
  display.print("Arp: "+ String((arpeggio ? "On" : "Off")));
}

void updateOLED(uint16_t keys) {
  display.clearDisplay();
  //drawPads(keys);
  drawScope();
  drawInfo();
  //drawVoices();
  drawVolumebar();
  display.display();
}


/* =========================================================
   SETUP
   ========================================================= */
void setup() {
  genWaveTable();
  for (int i = 0; i < 4; i++) {
    pinMode(X_PINS[i], OUTPUT);
    digitalWrite(X_PINS[i], LOW);
    pinMode(Y_PINS[i], INPUT);  // external pulldowns
  }

  // Set up Display
  Wire.setSDA(4);  // GP4 (pin 6)
  Wire.setSCL(5);  // GP5 (pin 7)
  Wire.begin();
  display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR);
  display.clearDisplay();

  initNotes();

  auto cfg = i2s.defaultConfig(TX_MODE);
  cfg.sample_rate = SAMPLE_RATE;
  cfg.bits_per_sample = BITS;
  cfg.channels = CHANNELS;
  cfg.pin_bck = 16;
  cfg.pin_ws = 17;
  cfg.pin_data = 18;
  i2s.begin(cfg);
}

/* =========================================================
   LOOP
   ========================================================= */

void loop() {
  // Handle the realtime synthesizer stream
  copier.copy();
}

// Seocnd core does inputs + display + leds
void loop1() {

  handleInputs();
  if (millis() - lastUI > 30) {
    updateOLED(keys);
    lastUI = millis();
  }
}