#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 = 16000;   // capture rate inmp441
const int FRAME_SIZE  = 160;  // 20ms frames @ 8kHz
const float TONE_HZ   = 440.0;

const touch_pad_t TOUCH_PIN = TOUCH_PAD_NUM4; // gpio13
const int TOUCH_THRESHOLD = 800; // adjust as needed

// inmp441 pins - change as needed
const int I2S_WS  = 22; // lrcl / ws
const int I2S_SCK = 26; // bclk
const int I2S_SD  = 21; // data from mic to esp32

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() {
  Serial.begin(115200);
  
  // init touch
  touch_pad_init();
  touch_pad_set_voltage(TOUCH_HVOLT_2V7, TOUCH_LVOLT_0V5, TOUCH_HVOLT_ATTEN_1V);
  touch_pad_config(TOUCH_PIN, 0);
  
  // init i2s for adc sampling
  i2s_config_t i2s_config = {
    .mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_RX | I2S_MODE_ADC_BUILT_IN),
    .sample_rate = OUTPUT_RATE,
    .bits_per_sample = I2S_BITS_PER_SAMPLE_16BIT,
    .channel_format = I2S_CHANNEL_FMT_ONLY_LEFT,
    .communication_format = I2S_COMM_FORMAT_STAND_MSB,
    .intr_alloc_flags = ESP_INTR_FLAG_LEVEL1,
    .dma_buf_count = 8,
    .dma_buf_len = 128,
    .use_apll = false
  };
  
  i2s_driver_install(I2S_NUM_0, &i2s_config, 0, NULL);
  i2s_set_adc_mode(ADC_UNIT_1, ADC1_CHANNEL_0); // gpio36
  i2s_adc_enable(I2S_NUM_0);
  
  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;
  static unsigned long last_touch_check = 0;
  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("switched to %s mode\n", tone_mode ? "tone" : "mic");
      delay(200); // debounce
    }
    
    last_touch_state = touched;
    last_touch_check = now;
  }
  
  // only send if it's time
  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 {
    // read from i2s adc buffer - need multiple reads to fill frame
    static float dc_filter = 2048.0; // dc offset removal filter
    int samples_filled = 0;
    
    while (samples_filled < FRAME_SIZE) {
      size_t bytes_read;
      uint16_t i2s_buffer[64]; // bigger buffer
      i2s_read(I2S_NUM_0, i2s_buffer, 128, &bytes_read, 10);
      
      int samples_read = bytes_read / 2; // 2 bytes per sample
      for (int i = 0; i < samples_read && samples_filled < FRAME_SIZE; i++) {
        // i2s adc gives 12-bit data in upper bits of 16-bit word
        int adc_val = (i2s_buffer[i] >> 4) & 0x0FFF;
        
        // dc offset removal with simple iir filter
        dc_filter = dc_filter * 0.999 + adc_val * 0.001;
        
        // scale and store
        samples[samples_filled] = (int16_t)((adc_val - dc_filter) * 16);
        samples_filled++;
      }
    }
  }
  
  // µ-law encode
  for (int i = 0; i < FRAME_SIZE; i++) {
    packet[12 + i] = linear2ulaw(samples[i]);
  }
  
  // build rtp header
  rtp_hdr* hdr = (rtp_hdr*)packet;
  hdr->vpxcc = 0x80;
  hdr->mpt   = 0x00;
  hdr->seq   = htons(seq++);
  hdr->ts    = htonl(ts);
  hdr->ssrc  = htonl(0x12345678);
  
  // send packet
  udp.beginPacket(DEST_IP, UDP_PORT);
  udp.write(packet, sizeof(packet));
  
  ts += FRAME_SIZE;
  last_send = now;
  
  yield();
}