#include <WiFi.h>
#include <WiFiUdp.h>
#include <math.h>
#include <driver/i2s.h>
#include <driver/touch_pad.h>

const char* ssid = "Fios-8EKF5";
const char* password = "hero816cars7050jon";
const char* DEST_IP = "192.168.1.190";
const int UDP_PORT = 5004;

const int OUTPUT_RATE = 8000; // 8kHz rtp payload rate
const int MIC_RATE = 8000;   // capture rate inmp441
const int FRAME_SIZE  = 160;  // 20ms frames @ 8kHz
const float TONE_HZ   = 440.0;
const float MIC_GAIN = 4.0f;

const touch_pad_t TOUCH_PIN = TOUCH_PAD_NUM4; // gpio13
const int TOUCH_THRESHOLD = 500;

const int I2S_WS  = 25; 
const int I2S_SCK = 22; 
const int I2S_SD  = 21; 

WiFiUDP udp;
uint16_t seq = 0;
uint32_t ts = 0;
bool tone_mode = true; // start with tone

struct rtp_hdr {
  uint8_t vpxcc;
  uint8_t mpt;
  uint16_t seq;
  uint32_t ts;
  uint32_t ssrc;
};

static const int16_t exp_lut[256] = {
  0,0,1,1,2,2,2,2,3,3,3,3,3,3,3,3,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,
  5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,
  6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,
  6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,
  7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
  7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
  7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
  7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7
};

uint8_t linear2ulaw(int16_t pcm_val) {
  int sign = (pcm_val >> 8) & 0x80;
  if (sign) pcm_val = (short)-pcm_val;
  if (pcm_val > 32635) pcm_val = 32635;
  pcm_val = pcm_val + 0x84;
  int exponent = exp_lut[(pcm_val >> 7) & 0xFF];
  int mantissa = (pcm_val >> (exponent + 3)) & 0x0F;
  return ~(sign | (exponent << 4) | mantissa);
}

void setup_i2s_mic() {
  i2s_config_t i2s_config = {
    .mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_RX),
    .sample_rate = MIC_RATE,
    .bits_per_sample = I2S_BITS_PER_SAMPLE_32BIT,
    .channel_format = I2S_CHANNEL_FMT_ONLY_LEFT,
    .communication_format = I2S_COMM_FORMAT_STAND_I2S,
    .intr_alloc_flags = ESP_INTR_FLAG_LEVEL1,
    .dma_buf_count = 8,
    .dma_buf_len = 256,
    .use_apll = false,
    .tx_desc_auto_clear = false,
    .fixed_mclk = 0
  };

  i2s_pin_config_t pin_config = {
    .bck_io_num = I2S_SCK,
    .ws_io_num = I2S_WS,
    .data_out_num = I2S_PIN_NO_CHANGE,
    .data_in_num = I2S_SD
  };

  i2s_driver_install(I2S_NUM_0, &i2s_config, 0, NULL);
  i2s_set_pin(I2S_NUM_0, &pin_config);
  i2s_zero_dma_buffer(I2S_NUM_0);
}

void setup() {
  Serial.begin(115200);
  
  touch_pad_init();
  touch_pad_set_voltage(TOUCH_HVOLT_2V7, TOUCH_LVOLT_0V5, TOUCH_HVOLT_ATTEN_1V);
  touch_pad_config(TOUCH_PIN, 0);
  
  setup_i2s_mic();

  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) { delay(500); Serial.print("."); }
  Serial.printf("\nconnected, ip=%s\n", WiFi.localIP().toString().c_str());
  udp.begin(UDP_PORT);
  
  Serial.println("tap pin 13 to switch modes");
}

void loop() {
  static unsigned long last_send = 0; // timer
  static unsigned long last_touch_check = 0;  // timer
  static bool last_touch_state = false;
  static const unsigned long FRAME_INTERVAL = 20; // 20ms
  static const unsigned long TOUCH_CHECK_INTERVAL = 50; // check every 50ms
  
  // check touch state periodically
  unsigned long now = millis();
  if (now - last_touch_check > TOUCH_CHECK_INTERVAL) {
    uint16_t touch_val;
    touch_pad_read(TOUCH_PIN, &touch_val);
    bool touched = touch_val < TOUCH_THRESHOLD;
    
    // detect touch press (not hold)
    if (touched && !last_touch_state) {
      tone_mode = !tone_mode;
      Serial.printf("%s mode active\n", tone_mode ? "tone" : "mic");
      delay(300); // debounce
    }
    
    last_touch_state = touched;
    last_touch_check = now;
  }
  
  // frame buffer
  if (now - last_send < FRAME_INTERVAL) {
    yield();
    return;
  }
  
  uint8_t packet[12 + FRAME_SIZE];
  int16_t samples[FRAME_SIZE];
  
  if (tone_mode) {
    // fixed tone generation with integer phase accumulator
    static uint32_t phase_acc = 0;
    const uint32_t phase_inc_int = (uint32_t)(TONE_HZ * (1ULL << 32) / OUTPUT_RATE);
    
    for (int i = 0; i < FRAME_SIZE; i++) {
      float phase = (phase_acc >> 16) * (2.0 * M_PI / 65536.0);
      samples[i] = (int16_t)(16000.0 * sinf(phase));
      phase_acc += phase_inc_int;
    }
    
  } else {
    int32_t mic_buf[FRAME_SIZE];
    size_t bytes_read = 0;
    i2s_read(I2S_NUM_0, mic_buf, sizeof(mic_buf), &bytes_read, portMAX_DELAY);

    if (bytes_read != sizeof(mic_buf)) {
      memset(samples, 0, sizeof(samples));
    } else {
      for (int i = 0; i < FRAME_SIZE; i++) {
        float y = (mic_buf[i] >> 16) * MIC_GAIN; //gain
        if (y > 32767.0f) y = 32767.0f;
        if (y < -32768.0f) y = -32768.0f;
        samples[i] = (int16_t)y;
      }
    }
  }
  for (int i = 0; i < FRAME_SIZE; i++) {
    packet[12 + i] = linear2ulaw(samples[i]);
  }

  rtp_hdr* hdr = (rtp_hdr*)packet;
  hdr->vpxcc = 0x80;
  hdr->mpt   = 0x00;
  hdr->seq   = htons(seq++);
  hdr->ts    = htonl(ts);
  hdr->ssrc  = htonl(0x12345678);

  udp.beginPacket(DEST_IP, UDP_PORT);
  udp.write(packet, sizeof(packet));
  udp.endPacket();

  ts += FRAME_SIZE;
  last_send = now;

  yield();
}
