#include "es8311.h"

#include "drivers/i2s.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

#include "driver/i2s_std.h"

#include "i2c.h"
#include "pins.h"
#include <math.h>

i2c_master_dev_handle_t es8311_i2c_dev;

int es8311_write(uint8_t reg, uint8_t val) {
  esp_err_t ret = i2c_write_reg(es8311_i2c_dev, reg, val);
  if (ret != ESP_OK) {
    printf("ES8311 write failed: reg=0x%02X, ret=%d\n", reg, ret);
    return -1;
  }
  return 0;
}


void es8311_set_dac_volume(uint8_t vol) {
  es8311_write(ES8311_DAC_VOL_L, vol);
  es8311_write(ES8311_DAC_VOL_R, vol);
}

void es8311_set_adc_volume(uint8_t vol) { es8311_write(ES8311_ADC_VOL, vol); }


uint8_t es8311_read(uint8_t reg) {
  uint8_t val;
  esp_err_t ret = i2c_read_reg(es8311_i2c_dev, reg, &val);
  if (ret != ESP_OK) {
    printf("ES8311 read failed: reg=0x%02X, ret=%d\n", reg, ret);
    return 0xFF;
  }
  return val;
}




void es8311_verify_init(void) {
  printf("ES8311 Register Dump:\n");
  printf("PWR_UP_DOWN (0x0C): 0x%02X\n", es8311_read(0x0C));
  printf("PWR_ANALOG (0x0D): 0x%02X\n", es8311_read(0x0D));
  printf("SER_FMT (0x10): 0x%02X\n", es8311_read(0x10));
  printf("DAC_VOL_L (0x33): 0x%02X\n", es8311_read(0x33));
  printf("DAC_VOL_R (0x34): 0x%02X\n", es8311_read(0x34));
}

void es8311_init(void) {
  printf("ES8311: Starting initialization...\n");

  i2c_device_config_t dev_cfg = {
      .device_address = ES8311_I2C_ADDR,
      .scl_speed_hz = 400000,
  };

  esp_err_t ret = i2c_master_bus_add_device(bus, &dev_cfg, &es8311_i2c_dev);
  if (ret != ESP_OK) {
    printf("ES8311: Failed to add I2C device: %d\n", ret);
    return;
  }

  // Reset chip
  printf("ES8311: Resetting...\n");
  es8311_write(ES8311_RESET, ES8311_RESET_CMD);
  vTaskDelay(pdMS_TO_TICKS(ES8311_RESET_DELAY_MS));

  // Configure clock
  printf("ES8311: Configuring clocks...\n");
  es8311_write(ES8311_CLK_MANAGE, ES8311_CLK_ALL_EN);
  es8311_write(ES8311_CLK_DIV, ES8311_CLK_DIV_DEFAULT);

  // Power up sequence
  printf("ES8311: Powering up...\n");
  es8311_write(ES8311_PWR_ANALOG, ES8311_PWR_ANALOG_OFF);
  vTaskDelay(pdMS_TO_TICKS(ES8311_POWER_DELAY_MS));

  es8311_write(ES8311_PWR_ANALOG, ES8311_PWR_ANALOG_ON);
  vTaskDelay(pdMS_TO_TICKS(ES8311_POWER_DELAY_MS));

  // I2S format
  printf("ES8311: Configuring I2S format...\n");
  es8311_write(ES8311_SER_FMT, ES8311_FMT_I2S | ES8311_WORD_LEN_16);

  // ADC configuration
  printf("ES8311: Configuring ADC...\n");
  es8311_write(ES8311_ADC_CTRL1, ES8311_ADC_CTRL1_DEFAULT);
  es8311_write(ES8311_ADC_CTRL2, ES8311_ADC_CTRL2_DEFAULT);
  es8311_set_adc_volume(ES8311_ADC_VOL_DEFAULT);

  // DAC configuration
  printf("ES8311: Configuring DAC...\n");
  es8311_write(ES8311_DAC_CTRL1, ES8311_DAC_CTRL1_DEFAULT);
  es8311_write(ES8311_DAC_CTRL2, ES8311_DAC_CTRL2_DEFAULT);
  es8311_set_dac_volume(ES8311_DAC_VOL_DEFAULT);

  // Output configuration
  printf("ES8311: Configuring outputs...\n");
  es8311_write(ES8311_HP_CTRL, ES8311_HP_CTRL_DEFAULT);
  es8311_write(ES8311_SPK_CTRL, ES8311_SPK_CTRL_MAX);

  // Enable ADC and DAC
  printf("ES8311: Enabling ADC/DAC...\n");
  es8311_write(ES8311_PWR_UP_DOWN, ES8311_PWR_DAC_EN | ES8311_PWR_ADC_EN);
  vTaskDelay(pdMS_TO_TICKS(ES8311_STARTUP_DELAY_MS));

  printf("ES8311: Initializing I2S...\n");
  audio_i2s_init();

  printf("ES8311: Initialization complete\n");
  es8311_verify_init();
}

int audio_write(const int16_t *samples, size_t count) {
  size_t written;

  esp_err_t r = i2s_channel_write(i2s_tx, samples, count * sizeof(int16_t),
                                  &written, portMAX_DELAY);

  if (r != ESP_OK)
    return -1;

  return written / sizeof(int16_t);
}

int audio_read(int16_t *samples, size_t count) {
  size_t read;

  esp_err_t r = i2s_channel_read(i2s_rx, samples, count * sizeof(int16_t),
                                 &read, portMAX_DELAY);

  if (r != ESP_OK)
    return -1;

  return read / sizeof(int16_t);
}

void audio_beep(void) {
  // ES8311 expects stereo I2S (left + right channels)
  static int16_t buf[ES8311_BEEP_DURATION_SAMPLES * 2]; // *2 for stereo

  for (int i = 0; i < ES8311_BEEP_DURATION_SAMPLES; i++) {
    int phase = (i / ES8311_BEEP_HALF_PERIOD) & 1;
    int16_t sample = phase ? ES8311_BEEP_AMPLITUDE : -ES8311_BEEP_AMPLITUDE;

    buf[i * 2] = sample;     // Left channel
    buf[i * 2 + 1] = sample; // Right channel (same as left for mono)
  }

  int written = audio_write(buf, ES8311_BEEP_DURATION_SAMPLES * 2);
  printf("Beep: wrote %d samples\n", written);
}

void audio_test_tone(void) {
  const int sample_rate = 44100;
  const int duration_ms = 1000;
  const int freq_hz = 440; // A4 note
  const int num_samples = (sample_rate * duration_ms) / 1000;

  int16_t *buf = malloc(num_samples * 2 * sizeof(int16_t));
  if (!buf) {
    printf("Failed to allocate tone buffer\n");
    return;
  }

  for (int i = 0; i < num_samples; i++) {
    float t = (float)i / sample_rate;
    int16_t sample = (int16_t)(sin(2.0f * M_PI * freq_hz * t) * 8000.0f);
    buf[i * 2] = sample;     // Left
    buf[i * 2 + 1] = sample; // Right
  }

  printf("Playing 440Hz tone for 1 second...\n");
  audio_write(buf, num_samples * 2);
  free(buf);
}