#include "DaisyDuino.h"
#include "math.h"

#define PIN_A4  11
#define PIN_AS4 14
#define PIN_B4  10
#define PIN_C5   9
#define PIN_CS5 13
#define PIN_D5   8
#define PIN_DS5 12
#define PIN_E5   7
#define PIN_F5   3
#define PIN_FS5  6
#define PIN_G5   2
#define PIN_GS5  5
#define PIN_A5   1
#define PIN_AS5  4
#define PIN_B5   0

#define PIN_MUX_ADSR_A 16
#define PIN_MUX_ADSR_B 17
#define PIN_MUX_ADSR_C 18
#define PIN_MUX_ADSR_ADC A8

#define PIN_MUX_ADSR_VOL_ATTACK  3
#define PIN_MUX_ADSR_VOL_DECAY   0
#define PIN_MUX_ADSR_VOL_SUSTAIN 1
#define PIN_MUX_ADSR_VOL_RELEASE 2
#define PIN_MUX_ADSR_FLT_ATTACK  5
#define PIN_MUX_ADSR_FLT_DECAY   7
#define PIN_MUX_ADSR_FLT_SUSTAIN 6
#define PIN_MUX_ADSR_FLT_RELEASE 4

#define PIN_MUX_FM_A 20
#define PIN_MUX_FM_B 21
#define PIN_MUX_FM_C 22
#define PIN_MUX_FM_ADC A4

#define PIN_MUX_FM_CAR_SIN 7
#define PIN_MUX_FM_CAR_SQ  5
#define PIN_MUX_FM_CAR_SAW 0
#define PIN_MUX_FM_MOD_SIN 3
#define PIN_MUX_FM_MOD_SQ  1
#define PIN_MUX_FM_MOD_SAW 2
#define PIN_MUX_FM_WET     4
#define PIN_MUX_FM_DEV     6

#define PIN_VOL A9
#define PIN_PITCH A11

#define PIN_FLT_Q A0
#define PIN_FLT_SWITCH D25

#define ANALOG_MAX 1023.0
#define FM_FREQ_MAX 120.0

DaisyHardware hw;

size_t num_channels;
size_t sample_rate = 44000;

Adsr volEnv;
Oscillator osc;

float vol = 0.0;

size_t key_count = 15;
int keys_pins[] = {PIN_A4, PIN_AS4, PIN_B4, PIN_C5, PIN_CS5, PIN_D5, PIN_DS5, PIN_E5, PIN_F5, PIN_FS5, PIN_G5, PIN_GS5, PIN_A5, PIN_AS5, PIN_B5};
float keys_pitches[] = {440.0, 466.1638, 493.8833, 523.2511, 554.3653, 587.3295, 622.254, 659.2551, 698.4565, 739.9888, 783.9909, 830.6094, 880.0, 932.3275, 987.7666};
bool keys_pressed[] = {false, false, false, false, false, false, false, false, false, false, false, false, false, false, false};
Adsr keys_adsr_vol[15];
Adsr keys_adsr_flt[15];
Oscillator keys_osc[15];
Oscillator keys_osc_mod[15];
Svf filters[15];
Flanger flanger;
SampleRateReducer smp;
Overdrive dist;

bool use_high_pass;

float vol_adsr_attack;
float vol_adsr_decay;
float vol_adsr_sustain;
float vol_adsr_release;

float mod_wet;
float mod_dev;
float dist_val;

float pitch_dev;
float key_pitch;

Limiter limiter;

float adsr_max_seconds = 8.0;

void Callback(float **in, float **out, size_t size) {
  for (size_t i = 0; i < size; i += 1) {
    float osc_out = 0.0;
    float osc_result = 0.0;
    float env_result = 0.0;
    float flt_result = 0.0;
    for (int k = 0; k < key_count; k += 1) {
      // filters[k].SetFreq(1000);

      env_result = keys_adsr_vol[k].Process(keys_pressed[k]);
      if (env_result == 0.0) {
        // skip processing the oscillator if it wouldn't play a sound anyway to save on processing power
        continue;
      }

      keys_osc[k].SetAmp(env_result);
      // osc_result = dist.Process(smp.Process(keys_osc[k].Process()));
      osc_result = keys_osc[k].Process();
      // filters[k].Process(osc_result);

      if (mod_wet > 0.04) {
        osc_result = (osc_result * (1 - mod_wet)) + ((osc_result * keys_osc_mod[k].Process()) * mod_wet);
      }

      if (use_high_pass) {
        osc_result = flanger.Process(osc_result);
      }

      osc_result = smp.Process(osc_result);

      osc_result = ((osc_result * (dist_val - 1)) * -1) + dist.Process(osc_result);
      
      osc_out += osc_result;
    }

    osc_out *= vol;

    limiter.ProcessBlock(&osc_out, 1, 1.0);
    for (size_t chn = 0; chn < num_channels; chn++) {
      out[chn][i] = osc_out;
    }
  }
}

void setup() {
  hw = DAISY.init(DAISY_SEED, AUDIO_SR_48K);
  num_channels = hw.num_channels;
  sample_rate = DAISY.get_samplerate();

  float sample_rate = DAISY.get_samplerate();
  Serial.begin(9600);
  analogReadResolution(10);

  limiter.Init();
  
  pinMode(PIN_MUX_ADSR_A, OUTPUT);
  pinMode(PIN_MUX_ADSR_B, OUTPUT);
  pinMode(PIN_MUX_ADSR_C, OUTPUT);
  pinMode(PIN_MUX_FM_A, OUTPUT);
  pinMode(PIN_MUX_FM_B, OUTPUT);
  pinMode(PIN_MUX_FM_C, OUTPUT);
  
  pinMode(PIN_FLT_SWITCH, INPUT_PULLUP);

  for (int k = 0; k < key_count; k += 1) {
    pinMode(keys_pins[k], INPUT_PULLUP);
    keys_osc[k].Init(sample_rate);
    keys_osc[k].SetWaveform(Oscillator::WAVE_SIN);
    // keys_osc[k].SetFreq(keys_pitches[k]);
    keys_osc[k].SetAmp(0.25);

    keys_osc_mod[k].Init(sample_rate);
    keys_osc_mod[k].SetWaveform(Oscillator::WAVE_SIN);
    // keys_osc_mod[k].SetFreq(keys_pitches[k]);
    keys_osc_mod[k].SetAmp(0.25);

    keys_adsr_vol[k].Init(sample_rate);
    keys_adsr_flt[k].Init(sample_rate);

    filters[k].Init(sample_rate);
    // filters[k].SetFilterMode(FilterMode::LOW_PASS);
  }

  flanger.Init(sample_rate);
  smp.Init();
  dist.Init();

  DAISY.begin(Callback);
}

void loop() {

  vol_adsr_attack = (get_mux_adsr_value(PIN_MUX_ADSR_VOL_ATTACK) / ANALOG_MAX) * adsr_max_seconds;
  vol_adsr_decay = (get_mux_adsr_value(PIN_MUX_ADSR_VOL_DECAY) / ANALOG_MAX) * adsr_max_seconds;
  vol_adsr_sustain = get_mux_adsr_value(PIN_MUX_ADSR_VOL_SUSTAIN) / ANALOG_MAX;
  vol_adsr_release = (get_mux_adsr_value(PIN_MUX_ADSR_VOL_RELEASE) / ANALOG_MAX) * adsr_max_seconds;

  // Modulator deviation
  mod_dev = FM_FREQ_MAX * (((get_mux_fm_value(PIN_MUX_FM_DEV) / ANALOG_MAX) * 2.0) - 1.0);

  // Pitch control
  pitch_dev = (((analogRead(PIN_PITCH) / ANALOG_MAX) * 24.0) - 12.0);
  if (pitch_dev < 0.15 && pitch_dev > -0.15) {
    pitch_dev = 0.0;
  }

  for (int k = 0; k < key_count; k += 1) {
    key_pitch = pow(pow(2.0, 1.0/12.0), (49.0 + k + pitch_dev) - (49.0)) * 440.0;

    // Played keys
    keys_pressed[k] = digitalRead(keys_pins[k]) == 0;

    keys_osc[k].SetFreq(key_pitch);
    keys_osc_mod[k].SetFreq(key_pitch);

    // Volume ADSR
    keys_adsr_vol[k].SetTime(ADSR_SEG_ATTACK, vol_adsr_attack);
    keys_adsr_vol[k].SetTime(ADSR_SEG_DECAY, vol_adsr_decay);
    keys_adsr_vol[k].SetSustainLevel(vol_adsr_sustain);
    keys_adsr_vol[k].SetTime(ADSR_SEG_RELEASE, vol_adsr_release);

    keys_osc_mod[k].SetFreq(keys_pitches[k] + mod_dev);
  }
  
  // Volume control
  vol = analogRead(PIN_VOL) / ANALOG_MAX;

  // Flanger
  flanger.SetLfoFreq(get_mux_adsr_value(PIN_MUX_ADSR_FLT_ATTACK) / ANALOG_MAX);
  flanger.SetLfoDepth(get_mux_adsr_value(PIN_MUX_ADSR_FLT_DECAY) / ANALOG_MAX);
  flanger.SetDelay((analogRead(PIN_FLT_Q) / ANALOG_MAX) * 4.0);

  // Distortion
  dist_val = get_mux_adsr_value(PIN_MUX_ADSR_FLT_SUSTAIN) / ANALOG_MAX;
  dist.SetDrive(dist_val);

  // Sample Rate Reducer
  smp.SetFreq(pow(get_mux_adsr_value(PIN_MUX_ADSR_FLT_RELEASE) / ANALOG_MAX, 2.0));
  
  // Carrier waveform
  if (get_mux_fm_button(PIN_MUX_FM_CAR_SIN) == 0) {
    set_car_osc(Oscillator::WAVE_SIN);
  }
  if (get_mux_fm_button(PIN_MUX_FM_CAR_SQ) == 0) {
    set_car_osc(Oscillator::WAVE_POLYBLEP_SQUARE);
  }
  if (get_mux_fm_button(PIN_MUX_FM_CAR_SAW) == 0) {
    set_car_osc(Oscillator::WAVE_POLYBLEP_SAW);
  }
  
  // Modulator waveform
  if (get_mux_fm_button(PIN_MUX_FM_MOD_SIN) == 0) {
    set_mod_osc(Oscillator::WAVE_SIN);
  }
  if (get_mux_fm_button(PIN_MUX_FM_MOD_SQ) == 0) {
    set_mod_osc(Oscillator::WAVE_POLYBLEP_SQUARE);
  }
  if (get_mux_fm_button(PIN_MUX_FM_MOD_SAW) == 0) {
    set_mod_osc(Oscillator::WAVE_POLYBLEP_SAW);
  }

  // Modulator wet
  mod_wet = get_mux_fm_value(PIN_MUX_FM_WET) / ANALOG_MAX;

  // Flanger on/off toggle
  use_high_pass = digitalRead(PIN_FLT_SWITCH) == LOW;
  
  // Refresh 20 times per second
  delay(50);
}

float get_mux_adsr_value(int pin) {
  int a = LOW;
  int b = LOW;
  int c = LOW;
  if (pin % 2 != 0) {
    a = HIGH;
  }
  if (pin == 2 || pin == 3 || pin > 5) {
    b = HIGH;
  }
  if (pin > 3) {
    c = HIGH;
  }
  digitalWrite(PIN_MUX_ADSR_A, a);
  digitalWrite(PIN_MUX_ADSR_B, b);
  digitalWrite(PIN_MUX_ADSR_C, c);
  return analogRead(PIN_MUX_ADSR_ADC);
}

float get_mux_fm_value(int pin) {
  int a = LOW;
  int b = LOW;
  int c = LOW;
  if (pin % 2 != 0) {
    a = HIGH;
  }
  if (pin == 2 || pin == 3 || pin > 5) {
    b = HIGH;
  }
  if (pin > 3) {
    c = HIGH;
  }
  digitalWrite(PIN_MUX_FM_A, a);
  digitalWrite(PIN_MUX_FM_B, b);
  digitalWrite(PIN_MUX_FM_C, c);
  return analogRead(PIN_MUX_FM_ADC);
}

int get_mux_fm_button(int pin) {
  int a = LOW;
  int b = LOW;
  int c = LOW;
  if (pin % 2 != 0) {
    a = HIGH;
  }
  if (pin == 2 || pin == 3 || pin > 5) {
    b = HIGH;
  }
  if (pin > 3) {
    c = HIGH;
  }
  digitalWrite(PIN_MUX_FM_A, a);
  digitalWrite(PIN_MUX_FM_B, b);
  digitalWrite(PIN_MUX_FM_C, c);
  return analogRead(PIN_MUX_FM_ADC) < 0.1 ? 0 : 1;
}

void set_car_osc(uint8_t waveform) {
  for (int k = 0; k < key_count; k += 1) {
    keys_osc[k].SetWaveform(waveform);
  }
}

void set_mod_osc(uint8_t waveform) {
  for (int k = 0; k < key_count; k += 1) {
    keys_osc_mod[k].SetWaveform(waveform);
  }
}