#include <Arduino.h>
#include <Wire.h>
#include <math.h>
#include "ESP_I2S.h"
#include "esp_check.h"
#include "Arduino_GFX_Library.h"
#include "pin_config.h"
#include "TouchDrvCSTXXX.hpp"
#include "SensorQMI8658.hpp"
#include "es8311.h"
#include "es7210.h"

static const int DISP_W = 480;
static const int DISP_H = 480;
static const int SAFE = 20;
static const uint32_t SAMPLE_RATE = 16000;
static const size_t REC_SECONDS = 2;
static const size_t AUDIO_BYTES = SAMPLE_RATE * REC_SECONDS * 2 * 2;
static const int I2C_NUM = 0;

I2SClass i2s;
TouchDrvCST92xx touch;
SensorQMI8658 qmi;
IMUdata acc;

Arduino_DataBus *bus = new Arduino_ESP32QSPI(LCD_CS, LCD_SCLK, LCD_SDIO0, LCD_SDIO1, LCD_SDIO2, LCD_SDIO3);
Arduino_CO5300 *gfx = new Arduino_CO5300(bus, LCD_RESET, 0, DISP_W, DISP_H, 0, 0, 0, 0);

uint8_t *audioBuf = nullptr;
size_t audioLen = 0;
bool touchIrq = false;
String statusLine = "Ready";
String tiltLine = "Tilt: FLAT";
String audioLine = "Audio: empty";
uint32_t lastTiltMs = 0;
bool micReady = false;

void IRAM_ATTR onTouchInt() { touchIrq = true; }

esp_err_t codecInit() {
  es8311_handle_t esHandle = es8311_create(I2C_NUM, ES8311_ADDRRES_0);
  ESP_RETURN_ON_FALSE(esHandle, ESP_FAIL, "AV", "es8311 create failed");
  const es8311_clock_config_t clk = {
    .mclk_inverted = false,
    .sclk_inverted = false,
    .mclk_from_mclk_pin = true,
    .mclk_frequency = SAMPLE_RATE * 256,
    .sample_frequency = SAMPLE_RATE,
  };
  ESP_ERROR_CHECK(es8311_init(esHandle, &clk, ES8311_RESOLUTION_16, ES8311_RESOLUTION_16));
  ESP_RETURN_ON_ERROR(es8311_sample_frequency_config(esHandle, clk.mclk_frequency, clk.sample_frequency), "AV", "clock cfg failed");
  ESP_RETURN_ON_ERROR(es8311_microphone_config(esHandle, false), "AV", "mic cfg failed");
  ESP_RETURN_ON_ERROR(es8311_voice_volume_set(esHandle, 90, NULL), "AV", "volume failed");
  ESP_RETURN_ON_ERROR(es8311_microphone_gain_set(esHandle, (es8311_mic_gain_t)6), "AV", "mic gain failed");
  return ESP_OK;
}

esp_err_t micInit() {
  audio_hal_codec_config_t cfg = {
    .adc_input = AUDIO_HAL_ADC_INPUT_ALL,
    .codec_mode = AUDIO_HAL_CODEC_MODE_ENCODE,
    .i2s_iface = {
      .mode = AUDIO_HAL_MODE_SLAVE,
      .fmt = AUDIO_HAL_I2S_NORMAL,
      .samples = AUDIO_HAL_16K_SAMPLES,
      .bits = AUDIO_HAL_BIT_LENGTH_16BITS,
    },
  };
  esp_err_t ret = ESP_OK;
  ret |= es7210_adc_init(&Wire, &cfg);
  ret |= es7210_adc_config_i2s(cfg.codec_mode, &cfg.i2s_iface);
  ret |= es7210_adc_set_gain(
    (es7210_input_mics_t)(ES7210_INPUT_MIC1 | ES7210_INPUT_MIC2),
    (es7210_gain_value_t)GAIN_24DB);
  ret |= es7210_adc_set_gain(
    (es7210_input_mics_t)(ES7210_INPUT_MIC3 | ES7210_INPUT_MIC4),
    (es7210_gain_value_t)GAIN_24DB);
  ret |= es7210_adc_ctrl_state(cfg.codec_mode, AUDIO_HAL_CTRL_START);
  return ret;
}

void drawStaticUi() {
  gfx->fillScreen(RGB565_BLACK);
  gfx->setTextSize(2);
  gfx->setTextColor(RGB565_WHITE);
  gfx->setCursor(SAFE + 16, SAFE + 10);
  gfx->println("Simple AV + Tilt Test");

  gfx->drawRect(24, 76, 132, 64, RGB565_GREEN);
  gfx->setCursor(44, 100);
  gfx->println("RECORD");

  gfx->drawRect(174, 76, 132, 64, RGB565_YELLOW);
  gfx->setCursor(212, 100);
  gfx->println("PLAY");

  gfx->drawRect(324, 76, 132, 64, RGB565_CYAN);
  gfx->setCursor(360, 100);
  gfx->println("BEEP");
}

void drawDynamic() {
  gfx->fillRect(20, 166, 440, 140, RGB565_BLACK);
  gfx->setTextSize(2);
  gfx->setTextColor(RGB565_WHITE);
  gfx->setCursor(28, 178);
  gfx->println(statusLine);
  gfx->setTextColor(RGB565_CYAN);
  gfx->setCursor(28, 212);
  gfx->println(tiltLine);
  gfx->setTextColor(RGB565_ORANGE);
  gfx->setCursor(28, 246);
  gfx->println(audioLine);
  gfx->drawRect(28, 278, 300, 20, RGB565_DARKGREY);
}

void updateTilt() {
  if (!qmi.getDataReady()) return;
  if (!qmi.getAccelerometer(acc.x, acc.y, acc.z)) return;
  String next = "Tilt: FLAT";
  if (acc.x > 0.8f) next = "Tilt: RIGHT";
  else if (acc.x < -0.8f) next = "Tilt: LEFT";
  else if (acc.y > 0.8f) next = "Tilt: UP";
  else if (acc.y < -0.8f) next = "Tilt: DOWN";
  if (next != tiltLine) {
    tiltLine = next;
    drawDynamic();
  }
}

void playBeep() {
  static int16_t tone[16000];
  for (int i = 0; i < 8000; ++i) {
    float s = sinf(2.0f * 3.1415926f * 440.0f * ((float)i / SAMPLE_RATE));
    int16_t v = (int16_t)(s * 6000.0f);
    tone[i * 2] = v;
    tone[i * 2 + 1] = v;
  }
  statusLine = "Playing test beep...";
  drawDynamic();
  i2s.write((uint8_t *)tone, 8000 * 2 * sizeof(int16_t));
  statusLine = "Beep done";
  drawDynamic();
}

void recordAudio() {
  if (!audioBuf || !micReady) {
    statusLine = "No audio buffer";
    drawDynamic();
    return;
  }
  statusLine = "Recording 2 sec...";
  drawDynamic();
  size_t total = 0;
  uint16_t peakAll = 0;
  while (total < AUDIO_BYTES) {
    size_t chunk = min((size_t)4096, AUDIO_BYTES - total);
    size_t got = i2s.readBytes((char *)(audioBuf + total), chunk);
    if (!got) break;
    total += got;

    int16_t *s = (int16_t *)(audioBuf + total - got);
    size_t n = got / sizeof(int16_t);
    uint16_t peak = 0;
    for (size_t i = 0; i < n; i++) {
      uint16_t a = (uint16_t)abs((int)s[i]);
      if (a > peak) peak = a;
    }
    if (peak > peakAll) peakAll = peak;

    int bar = map((int)peak, 0, 20000, 0, 296);
    if (bar < 0) bar = 0;
    if (bar > 296) bar = 296;
    gfx->fillRect(30, 280, 296, 16, RGB565_BLACK);
    uint16_t c = (peak > 12000) ? RGB565_RED : ((peak > 6000) ? RGB565_YELLOW : RGB565_GREEN);
    gfx->fillRect(30, 280, bar, 16, c);
  }
  audioLen = total;
  audioLine = (total > 0) ? "Audio: recorded" : "Audio: empty";
  if (total > 0) {
    statusLine = String("Record done, peak=") + String((int)peakAll);
  } else {
    statusLine = "Record failed";
  }
  drawDynamic();
}

void playAudio() {
  if (!audioLen) {
    statusLine = "No recording. Try BEEP.";
    drawDynamic();
    return;
  }
  statusLine = "Playing recording...";
  drawDynamic();
  size_t wrote = i2s.write(audioBuf, audioLen);
  statusLine = (wrote > 0) ? "Play done" : "Play failed";
  drawDynamic();
}

void handleTouch() {
  if (!touchIrq) return;
  touchIrq = false;
  int16_t x[1];
  int16_t y[1];
  uint8_t touched = touch.getPoint(x, y, 1);
  if (!touched) return;

  int px = x[0];
  int py = y[0];
  if (py < 76 || py > 140) return;
  if (px >= 24 && px <= 156) recordAudio();
  else if (px >= 174 && px <= 306) playAudio();
  else if (px >= 324 && px <= 456) playBeep();
}

void setup() {
  Serial.begin(115200);
  Wire.begin(IIC_SDA, IIC_SCL);

  pinMode(PA, OUTPUT);
  digitalWrite(PA, HIGH);

  if (!gfx->begin()) Serial.println("gfx begin failed");
  bus->writeC8D8(0x36, 0xA0);
  gfx->setBrightness(200);

  pinMode(TP_RST, OUTPUT);
  digitalWrite(TP_RST, LOW);
  delay(30);
  digitalWrite(TP_RST, HIGH);
  delay(50);
  touch.setPins(TP_RST, TP_INT);
  touch.begin(Wire, 0x5A, IIC_SDA, IIC_SCL);
  touch.setMaxCoordinates(480, 480);
  touch.setSwapXY(true);
  touch.setMirrorXY(true, false);
  attachInterrupt(TP_INT, onTouchInt, FALLING);

  if (qmi.begin(Wire, QMI8658_L_SLAVE_ADDRESS, IIC_SDA, IIC_SCL)) {
    qmi.configAccelerometer(SensorQMI8658::ACC_RANGE_4G, SensorQMI8658::ACC_ODR_1000Hz, SensorQMI8658::LPF_MODE_0);
    qmi.enableAccelerometer();
  } else {
    tiltLine = "Tilt: IMU not found";
  }

  i2s.setPins(9, 45, 8, 10, 42);
  if (!i2s.begin(I2S_MODE_STD, SAMPLE_RATE, I2S_DATA_BIT_WIDTH_16BIT, I2S_SLOT_MODE_STEREO, I2S_STD_SLOT_BOTH)) {
    statusLine = "I2S init failed";
  } else if (codecInit() != ESP_OK) {
    statusLine = "ES8311 init failed";
  } else if (micInit() != ESP_OK) {
    statusLine = "ES7210 init failed";
  } else {
    micReady = true;
  }

  audioBuf = (uint8_t *)ps_malloc(AUDIO_BYTES);
  if (!audioBuf) statusLine = "No PSRAM buffer";

  drawStaticUi();
  drawDynamic();
}

void loop() {
  handleTouch();
  if (millis() - lastTiltMs > 120) {
    lastTiltMs = millis();
    updateTilt();
  }
  delay(5);
}
