#include <driver/i2s.h>
#include <math.h>

#define SAMPLE_RATE 16000
#define BUFFER_LEN 512

// SPEAKER VARIABLES
#define I2S_SPEAKER_PORT I2S_NUM_0

#define SPEAKER_BCLK 7
#define SPEAKER_WS 8
#define SPEAKER_DATA 9

// Control Variables
#define POT_PIN 1  // adc pin
#define POT_POWER 2
#define GAIN_BUTTON 10  // Adjust gain value

// AUDIO
#define MIN_FREQ 0.0f
#define MAX_FREQ 1000.0f
#define TONE_AMPLITUDE 16000

#define NO_GAIN 0.0f
#define MAX_GAIN 4.0f

// Global variables
int16_t audioBuffer[BUFFER_LEN];

enum ButtonState { BTN_IDLE,
                   BTN_DEBOUNCE,
                   BTN_PRESSED };
ButtonState btnState = BTN_IDLE;
unsigned long btnTimer = 0;

float currentGain = 0.0f;

float phase = 0.0f;
float smoothedFreq = 1000.0f;

// I2S SETUP
void setupSpeaker() {
  i2s_config_t cfg = {
    .mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_TX),
    .sample_rate = SAMPLE_RATE,
    .bits_per_sample = I2S_BITS_PER_SAMPLE_16BIT,
    .channel_format = I2S_CHANNEL_FMT_ONLY_LEFT,
    .communication_format = I2S_COMM_FORMAT_I2S,
    .intr_alloc_flags = ESP_INTR_FLAG_LEVEL1,
    .dma_buf_count = 4,
    .dma_buf_len = BUFFER_LEN,
    .use_apll = false
  };

  i2s_pin_config_t pins = {
    .bck_io_num = SPEAKER_BCLK,
    .ws_io_num = SPEAKER_WS,
    .data_out_num = SPEAKER_DATA,
    .data_in_num = I2S_PIN_NO_CHANGE
  };

  i2s_driver_install(I2S_SPEAKER_PORT, &cfg, 0, NULL);
  i2s_set_pin(I2S_SPEAKER_PORT, &pins);
}

float readPotFrequency() {
  int potVal = analogRead(POT_PIN);
  float target = MIN_FREQ + (MAX_FREQ - MIN_FREQ) * potVal / 4095.0f;

  smoothedFreq = smoothedFreq * 0.95f + target * 0.05f;
  return smoothedFreq;
}

int16_t generateTone(float freq) {
  phase += 6.2831853f * freq / SAMPLE_RATE;
  if (phase > 6.2831853f) phase -= 6.2831853f;

  return (int16_t)(sinf(phase) * TONE_AMPLITUDE);
}

void setup() {
  Serial.begin(115200);
  setupSpeaker();

  pinMode(GAIN_BUTTON, INPUT_PULLUP);
  pinMode(POT_POWER, OUTPUT);
  pinMode(POT_PIN, INPUT);
}

void loop() {
  digitalWrite(POT_POWER, HIGH);
  switch (btnState) {
    case BTN_IDLE:
      if (digitalRead(GAIN_BUTTON) == LOW) {
        btnTimer = millis();
        btnState = BTN_DEBOUNCE;
      }
      break;

    case BTN_DEBOUNCE:
      if (millis() - btnTimer > 30) {
        if (digitalRead(GAIN_BUTTON) == LOW) {
          currentGain = (currentGain == MAX_GAIN) ? NO_GAIN : MAX_GAIN;
          btnState = BTN_PRESSED;
        } else {
          btnState = BTN_IDLE;
        }
      }
      break;

    case BTN_PRESSED:
      if (digitalRead(GAIN_BUTTON) == HIGH) {
        btnState = BTN_IDLE;
      }
      break;
  }

  size_t bytesRead = 0;
  size_t bytesWritten = 0;

  float freq = readPotFrequency();
  //Serial.println(freq);
  for (int i = 0; i < BUFFER_LEN; i++) {
    audioBuffer[i] = generateTone(freq);
  }
  bytesRead = sizeof(audioBuffer);

  for (int i = 0; i < BUFFER_LEN; i++) {
    int32_t s = audioBuffer[i] * currentGain;
    audioBuffer[i] = constrain(s, -32768, 32767);
  }

  i2s_write(I2S_SPEAKER_PORT, audioBuffer, bytesRead, &bytesWritten, portMAX_DELAY);
}