#include <Arduino.h>
#include "driver/i2s_std.h"
#include <Wire.h>

#define DEBUG_INTERVAL 1000
#define SAMPLE_RATE 48000  

#define SDA              17
#define SCL              18
#define I2S_DOUT_PIN     4
#define I2S_WS_PIN       7  // LRCLK
#define I2S_BCLK_PIN     5
#define I2S_MCLK_PIN     6
#define I2S_SD_PIN       15
#define I2S_FAULT        16

i2s_chan_handle_t tx_handle = NULL;

bool initializeI2S();
void playTestTone(uint32_t frequency_hz, uint32_t duration_ms);
void printRegisterDump();

void setup() {
  Serial.begin(115200);
  delay(3000);
  Serial.println("\n\n=== Audio Player Starting ===");

  Wire.begin(SDA, SCL);
  Wire.setClock(100000);  // 100kHz
  Serial.println("I2C initialized");

  pinMode(I2S_SD_PIN, OUTPUT);
  digitalWrite(I2S_SD_PIN, LOW);
  Serial.println("---> SDZ LOW");
  delay(10);

  printRegisterDump();

  if (!initializeI2S()) {
    Serial.println("I2S initialization failed!");
    return;
  }

  printRegisterDump();

  digitalWrite(I2S_SD_PIN, HIGH);
  Serial.println("---> SDZ HIGH");
  
  printRegisterDump();

  delay(1000);

  Serial.println("\nstarting tone playback...");
  playTestTone(440, 10000);  
}


void loop() {

}

bool initializeI2S() {
  Serial.println("initializing I2S...");

  i2s_chan_config_t chan_cfg = I2S_CHANNEL_DEFAULT_CONFIG(I2S_NUM_0, I2S_ROLE_MASTER);
  esp_err_t err = i2s_new_channel(&chan_cfg, &tx_handle, NULL);
  if (err != ESP_OK) {
    Serial.printf("failed to create I2S channel: %d\n", err);
    return false;
  }
  Serial.println("  I2S channel created");

  i2s_std_config_t std_cfg = {
    .clk_cfg = {
      .sample_rate_hz = SAMPLE_RATE,
      .clk_src = I2S_CLK_SRC_XTAL,
      .mclk_multiple = I2S_MCLK_MULTIPLE_256,  
    },
    // .clk_cfg = I2S_STD_CLK_DEFAULT_CONFIG(48000),
    .slot_cfg = I2S_STD_PHILIPS_SLOT_DEFAULT_CONFIG(I2S_DATA_BIT_WIDTH_32BIT, I2S_SLOT_MODE_STEREO),
    .gpio_cfg = {
      .mclk = I2S_GPIO_UNUSED, 
      .bclk = (gpio_num_t)I2S_BCLK_PIN,
      .ws = (gpio_num_t)I2S_WS_PIN,
      .dout = (gpio_num_t)I2S_DOUT_PIN,
      .din = I2S_GPIO_UNUSED,
      .invert_flags = {
        .mclk_inv = false,
        .bclk_inv = false,
        .ws_inv = false,
      },
    },
  };

  err = i2s_channel_init_std_mode(tx_handle, &std_cfg);
  if (err != ESP_OK) {
    Serial.printf("Failed to initialize I2S std mode: %d\n", err);
    return false;
  }

  err = i2s_channel_enable(tx_handle);
  if (err != ESP_OK) {
    Serial.printf("Failed to enable I2S channel: %d\n", err);
    return false;
  }
  Serial.println("  I2S channel enabled");

  return true;
}

void playTestTone(uint32_t frequency_hz, uint32_t duration_ms) {
  const uint32_t sample_rate = SAMPLE_RATE;  
  const uint32_t total_samples = (sample_rate * duration_ms) / 1000;
  const float amplitude = 0x7FFFFFFF * 0.5;  

  Serial.printf("generating %dHz tone for %dms\n", frequency_hz, duration_ms);

  const size_t bufferSize = 1024;
  int32_t* buffer = (int32_t*)malloc(bufferSize * sizeof(int32_t));

  if (!buffer) {
    Serial.println("failed to allocate buffer!");
    return;
  }

  uint32_t samplesGenerated = 0;
  uint32_t bufferIndex = 0;

  for (uint32_t i = 0; i < total_samples; i++) {
    float phase = (float)i * 2.0 * PI * frequency_hz / sample_rate;
    int32_t sample = (int32_t)(sin(phase) * amplitude);

    buffer[bufferIndex++] = sample;

    if (bufferIndex == bufferSize || i == total_samples - 1) {
      size_t bytes_written;

      esp_err_t err = i2s_channel_write(
        tx_handle,
        buffer,
        bufferIndex * sizeof(int32_t),
        &bytes_written,
        pdMS_TO_TICKS(1000)
      );

      if (i == 0) { 
        Serial.printf("first I2S write: err=%d, wrote %d bytes (requested %d)\n",
                     err, bytes_written, bufferIndex * sizeof(int32_t));
      }

      if (err != ESP_OK) {
        Serial.printf("I2S write error at sample %d: %d\n", i, err);
        break;
      }

      samplesGenerated += bufferIndex;
      bufferIndex = 0;

      if (samplesGenerated % sample_rate == 0) {
        Serial.printf("playing... %d seconds\n", samplesGenerated / sample_rate);
      }
    }
  }

  free(buffer);
  Serial.printf("tone playback complete. generated %d samples\n", samplesGenerated);
}

void printRegisterDump() {
  Serial.println("\n=== TAS5720 register dump ===");
  for (uint8_t reg = 0; reg <= 0x11; reg++) {
    if (reg == 0x07 || (reg >= 0x09 && reg <= 0x0F)) continue;
    Wire.beginTransmission(0x6C);
    Wire.write(reg);
    Wire.endTransmission(false);
    Wire.requestFrom(0x6C, 1);
    if (Wire.available()) {
      uint8_t value = Wire.read();
      Serial.printf("reg 0x%02X = 0x%02X\n", reg, value);
    }
  }
  Serial.println("===========================\n");
}