#include <WiFi.h>
#include <WiFiUdp.h>
#include <driver/i2s_std.h> 
//Pin configurations for I2S
#define SCK_PIN GPIO_NUM_11
#define LRCK_PIN GPIO_NUM_12
#define SDOUT_PIN GPIO_NUM_13
#define CONFIG_LWIP_UDP_RECVMBOX_SIZE 64
//task function declarations
void udpReceiveTask(void *args);
void i2sTask(void *args);
void i2sInit();
i2s_chan_handle_t tx_handle;
const char* ssid = "ESP32-ACCESS-POINT";
const char* password = "Denis171ESP32";
WiFiUDP udp;
//Set up wifi addresses
const int udpPort = 3333;
IPAddress local_ip(192,168,4,1);
IPAddress gateway(192,168,4,1);
IPAddress subnet(255,255,255,0);
QueueHandle_t packetQueue;
void setup() {
  //Start serial to log activity 
  Serial.begin(115200);
  delay(1000);
  Serial.println("BOOT OK");
  //Make a queue to store & transmit packets
  packetQueue = xQueueCreate(20,sizeof(int32_t)*256);
  Serial.println("Setting up WiFi config...");

  WiFi.setSleep(false);
  //set a designated ip 
  WiFi.softAPConfig(local_ip,gateway,subnet);
  //set ssid and password for the AP
  WiFi.softAP(ssid,password,6);

  delay(500);  // ← give the AP time to come up
  Serial.println(WiFi.softAPIP()); 
  
  udp.begin(udpPort);
  Serial.printf("UDP listening on port:%d\n",udpPort);
  i2sInit();
  xTaskCreatePinnedToCore(udpReceiveTask,"recTask",8192,NULL,5,NULL,0);
  xTaskCreatePinnedToCore(i2sTask,"i2sTask",8192,NULL,4,NULL,1);
}
void loop() { 
vTaskDelay(portMAX_DELAY);
}
void udpReceiveTask(void *args){
  static uint8_t buffer[1024];
  //static int packetCount = 0;
  //data = will hold 32bit representation of buffer
  static int32_t data[256] = {0};
  while(true){
    
    if(udp.parsePacket()){
      int count = 0;
      int len = udp.read(buffer,sizeof(buffer));
      //if the length isn't divisible by 4 the packet is not at least 32 bits
      if(len != 1024){
        Serial.printf("RX packet size: %d\n", len);
        continue;
      }
    //packetCount++;
    //logging 
    /*if (packetCount <= 10) {
      Serial.printf("\nPacket #%d len=%d\n", packetCount, len);
      for (int i = 0; i < 32; i++) {
        Serial.printf("%02X ", buffer[i]);
      }
      Serial.println();
    } */
      count = len / 4;
      for(int i=0; i<count; i++){
        memcpy(&data[i],&buffer[i*4],4);
      }
      //args = queue, data, non-blocking
      xQueueSend(packetQueue,data,0);
    }
    vTaskDelay(1);
  }
  
}
void i2sTask(void *args){
  while(true){
    size_t bytesWritten;
    //args = queue, data, non-blocking
    int32_t buffer[256] = {0};
    xQueueReceive(packetQueue,buffer,portMAX_DELAY);
    // for(int i=0; i<sizeof(buffer)/sizeof(buffer[0]); i++){
    //   buffer[i] = __builtin_bswap32(buffer[i]);
    // }
    //Write to the I2S
    i2s_channel_write(tx_handle,buffer,sizeof(buffer),&bytesWritten,portMAX_DELAY);
      if(bytesWritten >= 32){
        for (int i = 0; i < 32; i++) {
          Serial.printf("%02X ", buffer[i]);
        }
      }
  }
}
void i2sInit(){
  i2s_chan_config_t chan_config = I2S_CHANNEL_DEFAULT_CONFIG(I2S_NUM_0,I2S_ROLE_MASTER);
  ESP_ERROR_CHECK(i2s_new_channel(&chan_config,&tx_handle,NULL));
  i2s_std_config_t std_cfg = {
    .clk_cfg = I2S_STD_CLK_DEFAULT_CONFIG(44100),
    .slot_cfg = I2S_STD_MSB_SLOT_DEFAULT_CONFIG(I2S_DATA_BIT_WIDTH_32BIT,I2S_SLOT_MODE_STEREO),
    // .slot_cfg = I2S_STD_PHILIPS_SLOT_DEFAULT_CONFIG(I2S_DATA_BIT_WIDTH_32BIT,I2S_SLOT_MODE_STEREO),
    .gpio_cfg = {
        .mclk = I2S_GPIO_UNUSED,
        .bclk = SCK_PIN,
        .ws   = LRCK_PIN,
        .dout = SDOUT_PIN,
        .din  = I2S_GPIO_UNUSED,
    },
  };
  ESP_ERROR_CHECK(i2s_channel_init_std_mode(tx_handle, &std_cfg));
  ESP_ERROR_CHECK(i2s_channel_enable(tx_handle));
}