#include <Arduino.h>
#include <SPI.h>
#include "driver/i2s_std.h"
#include <math.h>
#include <WiFi.h>

#define TFT_CS 5
#define TFT_DC 4
#define TFT_RST -1
#define TFT_MOSI 12
#define TFT_MISO 13
#define TFT_SCLK 11

#define TFT_WIDTH 480
#define TFT_HEIGHT 320

#define I2S_XSMT 15
#define I2S_DOUT 16
#define I2S_BCLK 17
#define I2S_LRCLK 18

#define SAMPLE_RATE 44100
#define TONE_FREQ 440.0f
#define AUDIO_AMPLITUDE 12000

#define BLACK 0x0000
#define WHITE 0xFFFF

i2s_chan_handle_t txHandle;

static const uint8_t font5x7[][5] = {
  {0x00, 0x00, 0x00, 0x00, 0x00},
  {0x00, 0x00, 0x5F, 0x00, 0x00},
  {0x00, 0x07, 0x00, 0x07, 0x00},
  {0x14, 0x7F, 0x14, 0x7F, 0x14},
  {0x24, 0x2A, 0x7F, 0x2A, 0x12},
  {0x23, 0x13, 0x08, 0x64, 0x62},
  {0x36, 0x49, 0x55, 0x22, 0x50},
  {0x00, 0x05, 0x03, 0x00, 0x00},
  {0x00, 0x1C, 0x22, 0x41, 0x00},
  {0x00, 0x41, 0x22, 0x1C, 0x00},
  {0x14, 0x08, 0x3E, 0x08, 0x14},
  {0x08, 0x08, 0x3E, 0x08, 0x08},
  {0x00, 0x50, 0x30, 0x00, 0x00},
  {0x08, 0x08, 0x08, 0x08, 0x08},
  {0x00, 0x60, 0x60, 0x00, 0x00},
  {0x20, 0x10, 0x08, 0x04, 0x02},
  {0x3E, 0x51, 0x49, 0x45, 0x3E},
  {0x00, 0x42, 0x7F, 0x40, 0x00},
  {0x42, 0x61, 0x51, 0x49, 0x46},
  {0x21, 0x41, 0x45, 0x4B, 0x31},
  {0x18, 0x14, 0x12, 0x7F, 0x10},
  {0x27, 0x45, 0x45, 0x45, 0x39},
  {0x3C, 0x4A, 0x49, 0x49, 0x30},
  {0x01, 0x71, 0x09, 0x05, 0x03},
  {0x36, 0x49, 0x49, 0x49, 0x36},
  {0x06, 0x49, 0x49, 0x29, 0x1E},
  {0x00, 0x36, 0x36, 0x00, 0x00},
  {0x00, 0x56, 0x36, 0x00, 0x00},
  {0x08, 0x14, 0x22, 0x41, 0x00},
  {0x14, 0x14, 0x14, 0x14, 0x14},
  {0x00, 0x41, 0x22, 0x14, 0x08},
  {0x02, 0x01, 0x51, 0x09, 0x06},
  {0x32, 0x49, 0x79, 0x41, 0x3E},
  {0x7E, 0x11, 0x11, 0x11, 0x7E},
  {0x7F, 0x49, 0x49, 0x49, 0x36},
  {0x3E, 0x41, 0x41, 0x41, 0x22},
  {0x7F, 0x41, 0x41, 0x22, 0x1C},
  {0x7F, 0x49, 0x49, 0x49, 0x41},
  {0x7F, 0x09, 0x09, 0x09, 0x01},
  {0x3E, 0x41, 0x49, 0x49, 0x7A},
  {0x7F, 0x08, 0x08, 0x08, 0x7F},
  {0x00, 0x41, 0x7F, 0x41, 0x00},
  {0x20, 0x40, 0x41, 0x3F, 0x01},
  {0x7F, 0x08, 0x14, 0x22, 0x41},
  {0x7F, 0x40, 0x40, 0x40, 0x40},
  {0x7F, 0x02, 0x0C, 0x02, 0x7F},
  {0x7F, 0x04, 0x08, 0x10, 0x7F},
  {0x3E, 0x41, 0x41, 0x41, 0x3E},
  {0x7F, 0x09, 0x09, 0x09, 0x06},
  {0x3E, 0x41, 0x51, 0x21, 0x5E},
  {0x7F, 0x09, 0x19, 0x29, 0x46},
  {0x46, 0x49, 0x49, 0x49, 0x31},
  {0x01, 0x01, 0x7F, 0x01, 0x01},
  {0x3F, 0x40, 0x40, 0x40, 0x3F},
  {0x1F, 0x20, 0x40, 0x20, 0x1F},
  {0x3F, 0x40, 0x38, 0x40, 0x3F},
  {0x63, 0x14, 0x08, 0x14, 0x63},
  {0x07, 0x08, 0x70, 0x08, 0x07},
  {0x61, 0x51, 0x49, 0x45, 0x43},
  {0x1E, 0x21, 0x21, 0x21, 0x7F},   // [
  {0x02, 0x04, 0x08, 0x10, 0x20},   // "\"
  {0x7F, 0x21, 0x21, 0x21, 0x1E},   // ]
  {0x04, 0x02, 0x01, 0x02, 0x04},   // ^
  {0x40, 0x40, 0x40, 0x40, 0x40},   // _
  {0x00, 0x01, 0x02, 0x04, 0x00},   // `
  {0x20, 0x54, 0x54, 0x54, 0x78},   // a
  {0x7F, 0x48, 0x44, 0x44, 0x38},   // b
  {0x38, 0x44, 0x44, 0x44, 0x20},   // c
  {0x38, 0x44, 0x44, 0x48, 0x7F},   // d
  {0x38, 0x54, 0x54, 0x54, 0x18},   // e
  {0x08, 0x7E, 0x09, 0x01, 0x02},   // f
  {0x0C, 0x52, 0x52, 0x52, 0x3E},   // g
  {0x7F, 0x08, 0x04, 0x04, 0x78},   // h
  {0x00, 0x44, 0x7D, 0x40, 0x00},   // i
  {0x20, 0x40, 0x44, 0x3D, 0x00},   // j
  {0x7F, 0x10, 0x28, 0x44, 0x00},   // k
  {0x00, 0x41, 0x7F, 0x40, 0x00},   // l
  {0x7C, 0x04, 0x18, 0x04, 0x78},   // m
  {0x7C, 0x08, 0x04, 0x04, 0x78},   // n
  {0x38, 0x44, 0x44, 0x44, 0x38},   // o
  {0x7C, 0x14, 0x14, 0x14, 0x08},   // p
  {0x08, 0x14, 0x14, 0x18, 0x7C},   // q
  {0x7C, 0x08, 0x04, 0x04, 0x08},   // r
  {0x48, 0x54, 0x54, 0x54, 0x20},   // s
  {0x04, 0x3F, 0x44, 0x40, 0x20},   // t
  {0x3C, 0x40, 0x40, 0x20, 0x7C},   // u
  {0x1C, 0x20, 0x40, 0x20, 0x1C},   // v
  {0x3C, 0x40, 0x30, 0x40, 0x3C},   // w
  {0x44, 0x28, 0x10, 0x28, 0x44},   // x
  {0x0C, 0x50, 0x50, 0x50, 0x3C},   // y
  {0x44, 0x64, 0x54, 0x4C, 0x44},   // z
  {0x00, 0x08, 0x36, 0x41, 0x00},   // {
  {0x00, 0x00, 0x7F, 0x00, 0x00},   // |
  {0x00, 0x41, 0x36, 0x08, 0x00},   // }
  {0x08, 0x08, 0x2A, 0x1C, 0x08},   // ~
};

void tftSelect() {
  digitalWrite(TFT_CS, LOW);
}

void tftDeselect() {
  digitalWrite(TFT_CS, HIGH);
}

void tftWriteCommand(uint8_t cmd) {
  tftSelect();
  digitalWrite(TFT_DC, LOW);
  SPI.transfer(cmd);
  tftDeselect();
}

void tftWriteData(uint8_t data) {
  tftSelect();
  digitalWrite(TFT_DC, HIGH);
  SPI.transfer(data);
  tftDeselect();
}

void tftInit() {
  tftWriteCommand(0x01);
  delay(120);

  tftWriteCommand(0x11);
  delay(120);

  tftWriteCommand(0x3A);
  tftWriteData(0x55);

  tftWriteCommand(0x36);
  tftWriteData(0x48);

  tftWriteCommand(0x29);
  delay(20);
}

void tftSetWindow(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1) {
  tftSelect();
  digitalWrite(TFT_DC, LOW);
  SPI.transfer(0x2A);
  digitalWrite(TFT_DC, HIGH);
  SPI.transfer(x0 >> 8);
  SPI.transfer(x0 & 0xFF);
  SPI.transfer(x1 >> 8);
  SPI.transfer(x1 & 0xFF);
  tftDeselect();

  tftSelect();
  digitalWrite(TFT_DC, LOW);
  SPI.transfer(0x2B);
  digitalWrite(TFT_DC, HIGH);
  SPI.transfer(y0 >> 8);
  SPI.transfer(y0 & 0xFF);
  SPI.transfer(y1 >> 8);
  SPI.transfer(y1 & 0xFF);
  tftDeselect();

  tftSelect();
  digitalWrite(TFT_DC, LOW);
  SPI.transfer(0x2C);
  digitalWrite(TFT_DC, HIGH);
}

void tftFillScreen(uint16_t color) {
  tftSetWindow(0, 0, TFT_WIDTH - 1, TFT_HEIGHT - 1);
  uint8_t hi = color >> 8;
  uint8_t lo = color & 0xFF;
  for (uint32_t i = 0; i < (uint32_t)TFT_WIDTH * TFT_HEIGHT; i++) {
    SPI.transfer(hi);
    SPI.transfer(lo);
  }
  tftDeselect();
}

void tftDrawPixel(uint16_t x, uint16_t y, uint16_t color) {
  if (x >= TFT_WIDTH || y >= TFT_HEIGHT)
    return;
  tftSetWindow(x, y, x, y);
  SPI.transfer(color >> 8);
  SPI.transfer(color & 0xFF);
  tftDeselect();
}

void tftDrawChar(uint16_t x, uint16_t y, char c, uint16_t color, uint8_t scale) {
  if (c < 32 || c > 126)
    c = '?';
  const uint8_t *glyph = font5x7[c - 32];
  for (uint8_t col = 0; col < 5; col++) {
    uint8_t bits = glyph[col];
    for (uint8_t row = 0; row < 7; row++) {
      if (bits & (1 << row)) {
        for (uint8_t dx = 0; dx < scale; dx++) {
          for (uint8_t dy = 0; dy < scale; dy++) {
            tftDrawPixel(x + col * scale + dx, y + row * scale + dy, color);
          }
        }
      }
    }
  }
}

void tftDrawText(uint16_t x, uint16_t y, const char *text, uint16_t color, uint8_t scale) {
  while (*text) {
    if (*text == '\n') {
      y += 8 * scale;
      x = 0;
    } else {
      tftDrawChar(x, y, *text, color, scale);
      x += 6 * scale;
    }
    text++;
  }
}
/*
void initI2S() {
  i2s_chan_config_t chanCfg = I2S_CHANNEL_DEFAULT_CONFIG(I2S_NUM_0, I2S_ROLE_MASTER);
  ESP_ERROR_CHECK(i2s_new_channel(&chanCfg, &txHandle, NULL));
  i2s_std_config_t stdCfg = {
    .clk_cfg = I2S_STD_CLK_DEFAULT_CONFIG(SAMPLE_RATE),
    .slot_cfg = I2S_STD_PHILIPS_SLOT_DEFAULT_CONFIG(I2S_DATA_BIT_WIDTH_16BIT, I2S_SLOT_MODE_STEREO),
    .gpio_cfg = {
      .mclk = I2S_GPIO_UNUSED,
      .bclk = (gpio_num_t)I2S_BCLK,
      .ws = (gpio_num_t)I2S_LRCLK,
      .dout = (gpio_num_t)I2S_DOUT,
      .din = I2S_GPIO_UNUSED
    }
  };
  ESP_ERROR_CHECK(i2s_channel_init_std_mode(txHandle, &stdCfg));
  ESP_ERROR_CHECK(i2s_channel_enable(txHandle));
  pinMode(I2S_XSMT, OUTPUT);
  digitalWrite(I2S_XSMT, HIGH);
}*/
// initI2S()
#define BCK_HIGH()  (*(volatile uint32_t *)(0x60004008) = (1 << I2S_BCLK))
#define BCK_LOW()   (*(volatile uint32_t *)(0x6000400C) = (1 << I2S_BCLK))
#define LRCK_HIGH() (*(volatile uint32_t *)(0x60004008) = (1 << I2S_LRCLK))
#define LRCK_LOW()  (*(volatile uint32_t *)(0x6000400C) = (1 << I2S_LRCLK))
#define DOUT_HIGH() (*(volatile uint32_t *)(0x60004008) = (1 << I2S_DOUT))
#define DOUT_LOW()  (*(volatile uint32_t *)(0x6000400C) = (1 << I2S_DOUT))

void initI2S() {
  pinMode(I2S_BCLK, OUTPUT);
  pinMode(I2S_LRCLK, OUTPUT);
  pinMode(I2S_DOUT, OUTPUT);
  pinMode(I2S_XSMT, OUTPUT);
  digitalWrite(I2S_BCLK, LOW);
  digitalWrite(I2S_LRCLK, HIGH);
  digitalWrite(I2S_DOUT, LOW);
  digitalWrite(I2S_XSMT, LOW);
  delay(250);
  digitalWrite(I2S_XSMT, HIGH);
}

void sendI2SSampleFast(int16_t left, int16_t right) {
  LRCK_LOW(); DOUT_LOW();
  BCK_HIGH(); BCK_LOW();
  for (int b = 15; b >= 0; b--) {
    if ((left >> b) & 1) DOUT_HIGH(); else DOUT_LOW();
    BCK_HIGH(); BCK_LOW();
  }
  DOUT_LOW();
  for (int b = 0; b < 15; b++) { BCK_HIGH(); BCK_LOW(); }

  LRCK_HIGH(); DOUT_LOW();
  BCK_HIGH(); BCK_LOW();
  for (int b = 15; b >= 0; b--) {
    if ((right >> b) & 1) DOUT_HIGH(); else DOUT_LOW();
    BCK_HIGH(); BCK_LOW();
  }
  DOUT_LOW();
  for (int b = 0; b < 15; b++) { BCK_HIGH(); BCK_LOW(); }
}

void setup() {
  Serial.begin(115200);
  delay(500);
  Serial.println("ILI9488 + PCM5102A TEST");
  pinMode(TFT_CS, OUTPUT);
  pinMode(TFT_DC, OUTPUT);
  digitalWrite(TFT_CS, HIGH);
  digitalWrite(TFT_DC, HIGH);
  SPI.begin(TFT_SCLK, TFT_MISO, TFT_MOSI, TFT_CS);
  SPI.beginTransaction(SPISettings(20000000, MSBFIRST, SPI_MODE0));
  tftInit();
  //tft.setRotation(0);
  Serial.println("Łączenie z wirtualnym WiFi Wokwi...");
  WiFi.begin("Wokwi-GUEST", "");
  int timeout_counter = 0;
  while (WiFi.status() != WL_CONNECTED && timeout_counter < 20) {
    delay(500);
    Serial.print(".");
    timeout_counter++;
  }
  if (WiFi.status() == WL_CONNECTED) {
    Serial.println("\nPołączono z siecią WiFi Wokwi!");
    Serial.print("Adres IP: ");
    Serial.println(WiFi.localIP());
  } else {
    Serial.println("\nBłąd: Nie udało się połączyć z WiFi Wokwi.");
  }
  tftFillScreen(BLACK);
  tftDrawText(20, 30, "ILI9488 OK", WHITE, 3);
  tftDrawText(20, 80, "PCM5102A", WHITE, 3);
  tftDrawText(20, 130, "I2S 44100 Hz", WHITE, 2);
  tftDrawText(20, 165, "16 BIT STEREO", WHITE, 2);
  tftDrawText(20, 200, "TONE 440 Hz", WHITE, 3);
  initI2S();
  tftDrawText(20, 260, "AUDIO ON", WHITE, 3);
  String ipStr = "IP: " + WiFi.localIP().toString();
  tftDrawText(20, 290, ipStr.c_str(), WHITE, 2);
  Serial.println("ILI9488 OK");
  Serial.println("PCM5102A OK");
  Serial.println("I2S 44100 Hz / 16 bit / stereo");
  Serial.println("440 Hz tone started");
}

void loop() {
  static float phase = 0.0f;
  const float step = 2.0f * PI * TONE_FREQ / SAMPLE_RATE;
  int16_t sample = (int16_t)(sinf(phase) * AUDIO_AMPLITUDE);
  phase += step;
  if (phase >= 2.0f * PI) phase -= 2.0f * PI;
  sendI2SSampleFast(sample, sample);
}