#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;
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[4] = {"Vol", "Sou", "Oct", "Env"};
  int mode_count = sizeof(mode_names)/sizeof(mode_names[0]);

  // Octave
  int octave = 2;


// ---- 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 phase1 = 0;
  float phase2 = 0;
  float env = 0;
  float amp = 0;
};

Voice voices[MAX_VOICES];

void noteOn(float freq) {
  for (auto &v : voices) {
    if (!v.active) {
      v.active = true;
      v.freq = freq*(funcKey ? 1.25f : 1.0f);
      v.env = VOL;
      v.phase1 = 0.5f;
      v.phase2 = 0.5f;
      return;
    }
  }
}

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

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;
}
/* =========================================================
   SYNTH STREAM
   ========================================================= */

class JunoStream : 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;

      // 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 *= decay;
      if (v.env < 0.0005f) v.active = false;

      mix += osc * v.amp;
    }

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

    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;
JunoStream 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(5);

    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:");
  display.setCursor(100,6);
  display.print(mode_names[mode]);
  display.setCursor(90,18);
  display.print("Oct:"+String(octave));


}

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