#include <ESP_I2S.h>
#include <U8g2lib.h>
#include <MPU9250.h>
#include <FastTrig.h>

#define ARRAY_LEN(arr) (sizeof(arr)/sizeof *(arr))

const int button_pin = 19;
const int sample_rate = 16000; // sample rate in Hz

// frequency values for notes
const float E2  = 82.41;
const float F2  = 87.31;
const float G2  = 98;
const float A2  = 110;

const float AS4 = 466.16;
const float B4  = 493.88;
const float C5  = 523.25;
const float CS5 = 554.37;
const float D5  = 587.33;
const float FS5 = 739.99;

struct input_buffer {
  static const int value_count = 200;
  int write_p;
  float values[value_count];
};

// structure that stores the infos about the notes being played
struct mixer {
  struct note {
    float frequency;
    float start_time;
    float duration;
    float amplitude;
  };

  static constexpr float attack   = 0.1f;
  static constexpr float release  = 0.2f;
  // the amount of the note's amplitude is being ramped up every sample during attack
  static constexpr float up_val   = 1.0f / attack / sample_rate;
  // the amount of the note's amplitude is being turned down every sample during release
  static constexpr float down_val = 1.0f / release / sample_rate;

  static const int note_count = 10;
  note notes[note_count];

  uint64_t sample_count = 0;
  float dt;
};

inline float fraction(float f) { return f - (int)f; }

// sin function in turns
// isin() is the optimized version of sin() from the FastTrig library
inline float tsin(float t) { return isin(360*t); }

inline void push_value(input_buffer *input, float val) {
  input->values[input->write_p++] = val;
  if (input->write_p >= input->value_count)
    input->write_p = 0;
}

inline float get_sample_value(input_buffer *input, float a /* 0..1 */) {
  return input->values[(size_t)(fraction(a) * input->value_count)];
}

inline float get_semetrical_value(input_buffer *input, float a /* 0..1 */) {
  return input->values[(size_t)((1.5f - a) * input->value_count + input->write_p) % input->value_count];
}

inline float get_circular_value(input_buffer *input, float a /* 0..1 */) {
  return input->values[(size_t)(a * input->value_count + input->write_p) % input->value_count];
}

void mixer_add_note(mixer* m, float frequency, float duration) {
  for (mixer::note& n: m->notes) {
    float played = m->dt - n.start_time;
    if (played > n.duration + m->release) {
      n.frequency = frequency;
      n.duration = duration;
      n.amplitude = 0;
      n.start_time = (float)m->sample_count/sample_rate;
      return;
    }
  }
  Serial.println("No free note avaliable in the mixer.");
}

float mix_one_sample(mixer* m, input_buffer buffers[], int buffer_count) {
  m->dt = (float)m->sample_count++ / sample_rate;

  int buffer_idx = 0;
  float value = 0;
  for (mixer::note& n: m->notes) {
    float played = m->dt - n.start_time;
    if (played > n.duration + m->release) continue;

    if (played > n.duration)
      n.amplitude -= m->down_val;
    else if (m->dt > n.start_time && played < m->attack)
      n.amplitude += m->up_val;

    n.amplitude = std::clamp(n.amplitude, 0.0f, 1.0f);
    float val = get_sample_value(&buffers[buffer_idx], n.frequency * m->dt) * n.amplitude;
    ++buffer_idx %= buffer_count;
    value += val;
  }

  return value;
}

// Global variables
I2SClass i2s;
U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0);
MPU9250 mpu; // ESP32 I2C pins: SDA - 21, SCL - 22

bool mute = false;
float volumn = 0.1;

mixer m = {};
input_buffer buffers[6] = {};

void task_i2s_audio(void *param) {
  const int i2s_bclk_pin = 4;
  const int i2s_ws_pin   = 5;
  const int i2s_dout_pin = 18;
  i2s.setPins(i2s_bclk_pin, i2s_ws_pin, i2s_dout_pin);
  i2s_mode_t mode = I2S_MODE_STD;
  i2s_data_bit_width_t bits_per_sample = I2S_DATA_BIT_WIDTH_32BIT;
  i2s_slot_mode_t slot_mode = I2S_SLOT_MODE_STEREO;
  if (!i2s.begin(mode, sample_rate, bits_per_sample, slot_mode)) {
    Serial.println("Failed to initialize I2S!");
    while (1) ; // do nothing
  }

  for (;;) {
    float amplitude = mute ? 0 : INT32_MAX * volumn;
    float value = mix_one_sample(&m, buffers, ARRAY_LEN(buffers));
    value *= amplitude;
    int32_t sample[2] = {(int32_t)value, (int32_t)value};
    i2s.write((uint8_t *)sample, sizeof(sample));
  }
}

void setup() {
  Serial.begin(115200);
  Serial.println();
  Serial.printf("setup() running on core %d\n", xPortGetCoreID());

  pinMode(button_pin, INPUT_PULLUP);

  u8g2.begin();
  u8g2.setFont(u8g2_font_ncenB08_tr);	// choose a suitable font

  if (!mpu.setup(0x68)) {
    Serial.println("MPU connection failed.");
    while (1);
  }

  // generate and output audio samples on another core
  TaskHandle_t i2s_task;
  xTaskCreatePinnedToCore(task_i2s_audio, /* Task function. */
                          "I2S Audio",    /* name of task. */
                          10000,          /* Stack size of task */
                          0,              /* p=arameter of the task */
                          0,              /* priority of the task */
                          &i2s_task,      /* Task handle to keep track of created task */
                          0);             /* pin task to core 0 */
}

void loop() {
#define INTERVAL(ms) for (static unsigned long last_time; last_time == 0 || millis() > last_time + (ms); last_time = millis())

  int button_state = digitalRead(button_pin); 

  // read accelerometer every 10 milliseconds if the button is pressed
  INTERVAL(10)
  if (button_state == 0 && mpu.update()) {
    push_value(&buffers[0], mpu.getYaw() / 180.0f);
    push_value(&buffers[1], mpu.getPitch() / 90.0f);
    push_value(&buffers[2], mpu.getRoll() / 180.0f);
    push_value(&buffers[3], mpu.getAccX() / 3.0f);
    push_value(&buffers[4], mpu.getAccY() / 3.0f);
    push_value(&buffers[5], mpu.getAccZ() / 3.0f);
  }

  // push the next note to the mixer every 250 seconds
  INTERVAL(250) {
    static const float sequence[] = {AS4, C5, CS5, FS5};
    // static const float sequence[] = {B4, CS5, D5, FS5};
    static int idx = 0;
    mixer_add_note(&m, sequence[++idx%4], 0.2);
  }

  // update screen every 100 milliseconds
  INTERVAL(100) {
    u8g2.clearBuffer();
    if (button_state == 0) u8g2.drawStr(5, 20, "Sampling Inputs");
    for (int x = 0; x < u8g2.getWidth(); ++x) {
      for (int i = 0; i < 3; ++i) {
        int idx = (float)x / u8g2.getWidth() * buffers[i].value_count;
        int y = (1 - (buffers[i].values[idx] + 1) / 2) * (u8g2.getHeight() - 1);
        u8g2.drawPixel(x, y);
      }
    }
    u8g2.sendBuffer();
  }

}