EVS-Embedded-Voice-System/src/main.cpp

#include <Arduino.h>
#include <WiFi.h>
#include <WiFiUdp.h>
#include <ArduinoWebsockets.h>
#include <PubSubClient.h>
#include <math.h>
#include "driver/i2s.h"
#include "secrets.h"
using namespace websockets;

#ifndef EVS_ENABLE_MIC
#define EVS_ENABLE_MIC 1
#endif

#ifndef EVS_ENABLE_SPEAKER
#define EVS_ENABLE_SPEAKER 1
#endif

#ifndef EVS_DEFAULT_IO_MODE
#define EVS_DEFAULT_IO_MODE "mic"
#endif

#ifndef EVS_SPK_DAC_PIN
#define EVS_SPK_DAC_PIN 25
#endif

static constexpr bool kSerialCommandEcho = EVS_SERIAL_COMMAND_ECHO;
static constexpr bool kMicUseRightChannel = EVS_MIC_USE_RIGHT_CHANNEL;
static constexpr int kMicS24ToS16Shift = EVS_MIC_S24_TO_S16_SHIFT;
static constexpr bool kMicHardwareAvailable = (EVS_ENABLE_MIC != 0);
static constexpr bool kSpeakerHardwareAvailable = (EVS_ENABLE_SPEAKER != 0);
static constexpr float MIC_GAIN_MIN = 0.1f;
static constexpr float MIC_GAIN_MAX = 8.0f;

// ---------------------------
// Project config
// ---------------------------
static constexpr i2s_port_t MIC_I2S_PORT = I2S_NUM_0;
static constexpr uint32_t MIC_SAMPLE_RATE = 16000;
static constexpr i2s_bits_per_sample_t MIC_BITS = I2S_BITS_PER_SAMPLE_32BIT;
static constexpr size_t MIC_FRAME_SAMPLES = 256;

// INMP441 -> ESP32
static constexpr int PIN_I2S_WS = 25;
static constexpr int PIN_I2S_SCK = 26;
static constexpr int PIN_I2S_SD = 33;

// ESP32 DAC -> amplifier IN (L+ or R+)
static constexpr uint32_t SPEAKER_SAMPLE_RATE = 16000;
static constexpr uint32_t MIC_TELEMETRY_INTERVAL_MS = 1000;
static constexpr float PI_F = 3.14159265358979323846f;
static constexpr uint32_t MQTT_RECONNECT_MS = 5000;
static constexpr uint16_t MQTT_BUFFER_SIZE = 512;
static constexpr uint16_t UDP_MAX_PACKET_BYTES = MIC_FRAME_SAMPLES * sizeof(int16_t);
static constexpr uint32_t UDP_STATUS_INTERVAL_MS = 2000;

// WiFi / WebSocket

enum class DeviceMode : uint8_t {
  Idle,
  StreamToServer,  // Ship PCM to remote STT/LLM/TTS service
};

enum class IoMode : uint8_t {
  Mic,
  Speaker,
};

static DeviceMode g_mode = DeviceMode::Idle;
static IoMode g_ioMode = IoMode::Mic;
static int32_t g_micBuffer[MIC_FRAME_SAMPLES];
static WebsocketsClient g_ws;
static bool g_wsConnected = false;
static uint32_t g_lastConnectTryMs = 0;
static uint32_t g_nextOutUs = 0;
static bool g_streamingActive = false;
static uint32_t g_lastMicTelemetryMs = 0;
static WiFiClient g_mqttNet;
static PubSubClient g_mqtt(g_mqttNet);
static bool g_mqttConnected = false;
static uint32_t g_lastMqttConnectTryMs = 0;
static WiFiUDP g_udp;
static bool g_udpEnabled = false;
static String g_udpHost;
static uint16_t g_udpPort = 0;
static IPAddress g_udpIp;
static bool g_udpIpValid = false;
static uint32_t g_udpPacketsSent = 0;
static float g_micGain = EVS_MIC_GAIN;
static String g_mqttCmdTopic;
static String g_mqttStatusTopic;
static String g_mqttUdpStatusTopic;
static uint32_t g_lastUdpStatusMs = 0;

static constexpr size_t RX_SAMPLES_CAP = 16000;
static int16_t g_rxSamples[RX_SAMPLES_CAP];
static size_t g_rxHead = 0;
static size_t g_rxTail = 0;
static size_t g_rxCount = 0;

static void setMode(DeviceMode mode);
static void setIoMode(IoMode mode);
static void publishClientStatus();
static void publishUdpStatus(bool force = false);
static void handleUdpCommand(const String& msg);
static bool parseJsonStringField(const String& msg, const char* key, String& out);
static bool parseJsonIntField(const String& msg, const char* key, int& out);
static bool parseJsonBoolField(const String& msg, const char* key, bool& out);
static bool parseJsonFloatField(const String& msg, const char* key, float& out);
static bool isMicModeActive() { return kMicHardwareAvailable && g_ioMode == IoMode::Mic; }
static bool isSpeakerModeActive() { return kSpeakerHardwareAvailable && g_ioMode == IoMode::Speaker; }
static bool isDacPinValid(int pin) { return pin == 25 || pin == 26; }

static bool initMicI2s() {
  if (!kMicHardwareAvailable) {
    return true;
  }
  const i2s_config_t i2sConfig = {
      .mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_RX),
      .sample_rate = MIC_SAMPLE_RATE,
      .bits_per_sample = MIC_BITS,
      .channel_format = kMicUseRightChannel ? I2S_CHANNEL_FMT_ONLY_RIGHT : I2S_CHANNEL_FMT_ONLY_LEFT,
      .communication_format = I2S_COMM_FORMAT_STAND_I2S,
      .intr_alloc_flags = 0,
      .dma_buf_count = 8,
      .dma_buf_len = 256,
      .use_apll = false,
      .tx_desc_auto_clear = false,
      .fixed_mclk = 0,
  };

  const i2s_pin_config_t pinConfig = {
      .bck_io_num = PIN_I2S_SCK,
      .ws_io_num = PIN_I2S_WS,
      .data_out_num = I2S_PIN_NO_CHANGE,
      .data_in_num = PIN_I2S_SD,
  };

  if (i2s_driver_install(MIC_I2S_PORT, &i2sConfig, 0, nullptr) != ESP_OK) {
    return false;
  }
  if (i2s_set_pin(MIC_I2S_PORT, &pinConfig) != ESP_OK) {
    return false;
  }
  return true;
}

static bool initSpeakerOut() {
  if (!kSpeakerHardwareAvailable) {
    return true;
  }
  return isDacPinValid(EVS_SPK_DAC_PIN);
}

static inline void speakerWriteU8(uint8_t value) {
  if (!kSpeakerHardwareAvailable) {
    return;
  }
  if (!isDacPinValid(EVS_SPK_DAC_PIN)) {
    return;
  }
  dacWrite(EVS_SPK_DAC_PIN, value);
}

// Convert signed 16-bit PCM to unsigned 8-bit DAC domain.
static inline uint8_t pcm16ToU8(int16_t s) {
  return (uint16_t)(s + 32768) >> 8;
}

static bool enqueuePcmSample(int16_t s) {
  if (g_rxCount >= RX_SAMPLES_CAP) {
    return false;
  }
  g_rxSamples[g_rxTail] = s;
  g_rxTail = (g_rxTail + 1) % RX_SAMPLES_CAP;
  ++g_rxCount;
  return true;
}

static bool dequeuePcmSample(int16_t& out) {
  if (g_rxCount == 0) {
    return false;
  }
  out = g_rxSamples[g_rxHead];
  g_rxHead = (g_rxHead + 1) % RX_SAMPLES_CAP;
  --g_rxCount;
  return true;
}

static void enqueuePcmFrame(const int16_t* frame, size_t count) {
  if (!kSpeakerHardwareAvailable) {
    return;
  }
  for (size_t i = 0; i < count; ++i) {
    if (!enqueuePcmSample(frame[i])) {
      break;
    }
  }
}

static void enqueueTone(uint16_t freqHz, uint16_t durationMs, int16_t amplitude) {
  if (!kSpeakerHardwareAvailable) {
    return;
  }
  if (freqHz == 0 || durationMs == 0) {
    return;
  }
  const uint32_t sampleCount = (uint32_t)SPEAKER_SAMPLE_RATE * durationMs / 1000U;
  const float phaseStep = 2.0f * PI_F * (float)freqHz / (float)SPEAKER_SAMPLE_RATE;
  float phase = 0.0f;
  for (uint32_t i = 0; i < sampleCount; ++i) {
    const float s = sinf(phase);
    const int16_t sample = (int16_t)(s * (float)amplitude);
    if (!enqueuePcmSample(sample)) {
      return;
    }
    phase += phaseStep;
    if (phase > 2.0f * PI_F) {
      phase -= 2.0f * PI_F;
    }
  }
}

static void enqueueSilenceMs(uint16_t durationMs) {
  if (!kSpeakerHardwareAvailable) {
    return;
  }
  const uint32_t sampleCount = (uint32_t)SPEAKER_SAMPLE_RATE * durationMs / 1000U;
  for (uint32_t i = 0; i < sampleCount; ++i) {
    if (!enqueuePcmSample(0)) {
      return;
    }
  }
}

static void playStartTone() {
  // Ascending double beep: A5 -> C6.
  enqueueTone(880, 90, 5000);
  enqueueSilenceMs(35);
  enqueueTone(1047, 110, 6000);
}

static void playStopTone() {
  // Descending double beep: C6 -> A5.
  enqueueTone(1047, 90, 5000);
  enqueueSilenceMs(35);
  enqueueTone(880, 110, 6000);
}

static void onWsMessageCallback(WebsocketsMessage message) {
  if (!isSpeakerModeActive()) {
    return;
  }
  if (!message.isBinary()) {
    return;
  }
  const String payload = message.data();
  const size_t n = payload.length();
  if (n < 2) {
    return;
  }

  const uint8_t* bytes = reinterpret_cast<const uint8_t*>(payload.c_str());
  const size_t sampleCount = n / 2;
  for (size_t i = 0; i < sampleCount; ++i) {
    const uint8_t lo = bytes[2 * i];
    const uint8_t hi = bytes[2 * i + 1];
    const int16_t s = (int16_t)((uint16_t)hi << 8 | lo);
    if (!enqueuePcmSample(s)) {
      break;
    }
  }
}

static void onWsEventCallback(WebsocketsEvent event, String) {
  if (event == WebsocketsEvent::ConnectionOpened) {
    g_wsConnected = true;
    // Mic-mode clients stream immediately on connect.
    if (isMicModeActive()) {
      setMode(DeviceMode::StreamToServer);
    } else {
      setMode(DeviceMode::Idle);
    }
    Serial.println("WS connected");
    publishClientStatus();
  } else if (event == WebsocketsEvent::ConnectionClosed) {
    const bool wasStreaming = g_streamingActive;
    g_wsConnected = false;
    g_streamingActive = false;
    // Connection-driven mode: always idle on disconnect.
    setMode(DeviceMode::Idle);
    if (wasStreaming) {
      playStopTone();
    }
    Serial.println("WS disconnected");
    publishClientStatus();
  }
}

static String makeWsUrl() {
  String url = "ws://";
  url += EVS_BRIDGE_HOST;
  url += ":";
  url += String(EVS_WS_PORT);
  url += EVS_WS_PATH;
  url += "?device_id=";
  url += EVS_DEVICE_ID;
  return url;
}

static void ensureConnectivity() {
  const uint32_t now = millis();
  if ((now - g_lastConnectTryMs) < EVS_RECONNECT_MS) {
    return;
  }
  g_lastConnectTryMs = now;

  if (WiFi.status() != WL_CONNECTED) {
    Serial.println("WiFi connecting...");
    WiFi.mode(WIFI_STA);
    WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
    return;
  }

  if (!g_wsConnected) {
    const String url = makeWsUrl();
    Serial.print("WS connecting: ");
    Serial.println(url);
    g_ws.connect(url);
  }
}

static bool resolveUdpHost() {
  if (g_udpHost.length() == 0 || g_udpPort == 0) {
    g_udpIpValid = false;
    return false;
  }
  // Fast path for numeric IPv4 literals to avoid DNS dependency.
  if (g_udpIp.fromString(g_udpHost)) {
    g_udpIpValid = true;
    return true;
  }
  if (WiFi.hostByName(g_udpHost.c_str(), g_udpIp)) {
    g_udpIpValid = true;
    return true;
  }
  g_udpIpValid = false;
  return false;
}

static String mqttClientId() {
  String id = "evs-client-";
  id += EVS_DEVICE_ID;
  return id;
}

static void mqttMessageCallback(char* topic, uint8_t* payload, unsigned int length) {
  String msg;
  msg.reserve(length);
  for (unsigned int i = 0; i < length; ++i) msg += (char)payload[i];
  Serial.print("MQTT cmd [");
  Serial.print(topic);
  Serial.print("]: ");
  Serial.println(msg);

  String cmd;
  if (!parseJsonStringField(msg, "cmd", cmd)) {
    return;
  }
  cmd.toLowerCase();
  if (cmd == "status") {
    publishClientStatus();
    return;
  }
  if (cmd == "mode") {
    String value;
    if (parseJsonStringField(msg, "value", value)) {
      value.toLowerCase();
      if (value == "idle") {
        setMode(DeviceMode::Idle);
      } else if (value == "stream" || value == "streamtoserver") {
        setMode(DeviceMode::StreamToServer);
      }
      publishClientStatus();
    }
    return;
  }
  if (cmd == "io_mode") {
    String value;
    if (parseJsonStringField(msg, "value", value)) {
      value.toLowerCase();
      if (value == "mic") {
        setIoMode(IoMode::Mic);
      } else if (value == "spk" || value == "speaker") {
        setIoMode(IoMode::Speaker);
      }
      publishClientStatus();
      publishUdpStatus(true);
    }
    return;
  }
  if (cmd == "udp_stream") {
    if (!isMicModeActive()) {
      g_udpEnabled = false;
      g_udpIpValid = false;
      Serial.println("UDP stream ignored: io_mode is not mic");
      publishUdpStatus(true);
      publishClientStatus();
      return;
    }
    handleUdpCommand(msg);
    publishClientStatus();
    return;
  }
  if (cmd == "mic_gain") {
    if (!isMicModeActive()) {
      Serial.println("MIC gain command ignored: io_mode is not mic");
      return;
    }
    float value = 0.0f;
    bool changed = false;
    if (parseJsonFloatField(msg, "value", value)) {
      g_micGain = value;
      changed = true;
    }

    float delta = 0.0f;
    if (parseJsonFloatField(msg, "delta", delta)) {
      g_micGain += delta;
      changed = true;
    }

    String action;
    if (parseJsonStringField(msg, "action", action)) {
      action.toLowerCase();
      float step = 0.1f;
      parseJsonFloatField(msg, "step", step);
      if (step < 0.0f) step = -step;
      if (action == "up" || action == "inc" || action == "increase") {
        g_micGain += step;
        changed = true;
      } else if (action == "down" || action == "dec" || action == "decrease") {
        g_micGain -= step;
        changed = true;
      }
    }

    if (g_micGain < MIC_GAIN_MIN) g_micGain = MIC_GAIN_MIN;
    if (g_micGain > MIC_GAIN_MAX) g_micGain = MIC_GAIN_MAX;
    if (changed) {
      Serial.print("MIC gain set to ");
      Serial.println(g_micGain, 3);
    }
    publishClientStatus();
    return;
  }
}

static void ensureMqttConnectivity() {
  if (WiFi.status() != WL_CONNECTED) return;
  if (g_mqttConnected && g_mqtt.connected()) return;

  const uint32_t now = millis();
  if ((now - g_lastMqttConnectTryMs) < MQTT_RECONNECT_MS) return;
  g_lastMqttConnectTryMs = now;

  g_mqtt.setServer(EVS_MQTT_HOST, EVS_MQTT_PORT);
  g_mqtt.setCallback(mqttMessageCallback);

  String clientId = mqttClientId();
  bool ok = false;
  if (strlen(EVS_MQTT_USER) > 0) {
    ok = g_mqtt.connect(clientId.c_str(), EVS_MQTT_USER, EVS_MQTT_PASSWORD);
  } else {
    ok = g_mqtt.connect(clientId.c_str());
  }
  g_mqttConnected = ok;
  if (!ok) {
    Serial.print("MQTT connect failed, state=");
    Serial.println(g_mqtt.state());
    return;
  }
  Serial.println("MQTT connected");
  g_mqtt.subscribe(g_mqttCmdTopic.c_str());
  publishClientStatus();
}

static void setMode(DeviceMode mode) {
  if (g_mode == mode) {
    return;
  }
  if (isMicModeActive() && g_mode == DeviceMode::StreamToServer && g_wsConnected && g_streamingActive) {
    g_ws.send("{\"type\":\"stop\"}");
    g_streamingActive = false;
    if (isSpeakerModeActive()) playStopTone();
  }
  g_mode = mode;
  if (isMicModeActive() && g_mode == DeviceMode::StreamToServer && g_wsConnected && !g_streamingActive) {
    g_ws.send("{\"type\":\"start\"}");
    g_streamingActive = true;
    if (isSpeakerModeActive()) playStartTone();
  }
}

static void setIoMode(IoMode mode) {
  if (mode == g_ioMode) {
    return;
  }
  if (mode == IoMode::Mic && !kMicHardwareAvailable) {
    Serial.println("io_mode mic rejected: mic hardware disabled");
    return;
  }
  if (mode == IoMode::Speaker && !kSpeakerHardwareAvailable) {
    Serial.println("io_mode spk rejected: speaker hardware disabled");
    return;
  }

  if (g_streamingActive && g_wsConnected) {
    g_ws.send("{\"type\":\"stop\"}");
    g_streamingActive = false;
  }
  g_udpEnabled = false;
  g_udpIpValid = false;
  g_udpPacketsSent = 0;

  g_ioMode = mode;
  g_rxHead = g_rxTail = g_rxCount = 0;

  if (isMicModeActive() && g_wsConnected) {
    setMode(DeviceMode::StreamToServer);
  } else {
    setMode(DeviceMode::Idle);
  }

  Serial.print("io_mode set to ");
  Serial.println(g_ioMode == IoMode::Mic ? "mic" : "spk");
}

static bool parseJsonStringField(const String& msg, const char* key, String& out) {
  String pattern = "\"";
  pattern += key;
  pattern += "\"";
  int k = msg.indexOf(pattern);
  if (k < 0) return false;
  int colon = msg.indexOf(':', k + pattern.length());
  if (colon < 0) return false;
  int q1 = msg.indexOf('"', colon + 1);
  if (q1 < 0) return false;
  int q2 = msg.indexOf('"', q1 + 1);
  if (q2 < 0) return false;
  out = msg.substring(q1 + 1, q2);
  return true;
}

static bool parseJsonIntField(const String& msg, const char* key, int& out) {
  String pattern = "\"";
  pattern += key;
  pattern += "\"";
  int k = msg.indexOf(pattern);
  if (k < 0) return false;
  int colon = msg.indexOf(':', k + pattern.length());
  if (colon < 0) return false;
  int start = colon + 1;
  while (start < (int)msg.length() && (msg[start] == ' ' || msg[start] == '\t')) start++;
  int end = start;
  while (end < (int)msg.length() && isDigit(msg[end])) end++;
  if (end <= start) return false;
  out = msg.substring(start, end).toInt();
  return true;
}

static bool parseJsonBoolField(const String& msg, const char* key, bool& out) {
  String pattern = "\"";
  pattern += key;
  pattern += "\"";
  int k = msg.indexOf(pattern);
  if (k < 0) return false;
  int colon = msg.indexOf(':', k + pattern.length());
  if (colon < 0) return false;
  int start = colon + 1;
  while (start < (int)msg.length() && (msg[start] == ' ' || msg[start] == '\t')) start++;
  if (msg.startsWith("true", start)) {
    out = true;
    return true;
  }
  if (msg.startsWith("false", start)) {
    out = false;
    return true;
  }
  return false;
}

static bool parseJsonFloatField(const String& msg, const char* key, float& out) {
  String pattern = "\"";
  pattern += key;
  pattern += "\"";
  int k = msg.indexOf(pattern);
  if (k < 0) return false;
  int colon = msg.indexOf(':', k + pattern.length());
  if (colon < 0) return false;
  int start = colon + 1;
  while (start < (int)msg.length() && (msg[start] == ' ' || msg[start] == '\t')) start++;
  int end = start;
  while (end < (int)msg.length()) {
    const char c = msg[end];
    if ((c >= '0' && c <= '9') || c == '-' || c == '+' || c == '.' || c == 'e' || c == 'E') {
      end++;
    } else {
      break;
    }
  }
  if (end <= start) return false;
  out = msg.substring(start, end).toFloat();
  return true;
}

static void handleUdpCommand(const String& msg) {
  bool enabled = g_udpEnabled;
  const bool hasEnabled = parseJsonBoolField(msg, "enabled", enabled);

  String host;
  const bool hasHost = parseJsonStringField(msg, "target_host", host);
  if (hasHost) {
    g_udpHost = host;
  }
  int p = 0;
  const bool hasPort = parseJsonIntField(msg, "target_port", p);
  if (hasPort && p > 0 && p <= 65535) {
    g_udpPort = (uint16_t)p;
  }

  Serial.print("UDP cmd parsed enabled=");
  Serial.print(hasEnabled ? (enabled ? "true" : "false") : "<missing>");
  Serial.print(" host=");
  Serial.print(hasHost ? host : "<missing>");
  Serial.print(" port=");
  Serial.println(hasPort ? String(p) : "<missing>");

  if (enabled) {
    if (!resolveUdpHost()) {
      Serial.println("UDP target resolve failed");
      g_udpEnabled = false;
      return;
    }
    g_udpEnabled = true;
    Serial.print("UDP target resolved: ");
    Serial.print(g_udpIp);
    Serial.print(":");
    Serial.println(g_udpPort);
  } else {
    g_udpEnabled = false;
    Serial.println("UDP stream disabled");
  }
  publishUdpStatus(true);
}

static void publishClientStatus() {
  if (!g_mqttConnected || !g_mqtt.connected()) return;
  String msg = "{\"type\":\"client_status\",\"device_id\":\"";
  msg += EVS_DEVICE_ID;
  msg += "\",\"mic_enabled\":";
  msg += isMicModeActive() ? "true" : "false";
  msg += ",\"speaker_enabled\":";
  msg += isSpeakerModeActive() ? "true" : "false";
  msg += ",\"io_mode\":\"";
  msg += (g_ioMode == IoMode::Mic) ? "mic" : "spk";
  msg += "\",\"mode\":\"";
  msg += (g_mode == DeviceMode::StreamToServer) ? "stream" : "idle";
  msg += "\",\"ws_connected\":";
  msg += g_wsConnected ? "true" : "false";
  msg += ",\"mqtt_connected\":";
  msg += (g_mqttConnected && g_mqtt.connected()) ? "true" : "false";
  msg += "}";
  const bool ok = g_mqtt.publish(g_mqttStatusTopic.c_str(), msg.c_str(), true);
  if (!ok) {
    Serial.print("MQTT status publish failed, len=");
    Serial.print(msg.length());
    Serial.print(" state=");
    Serial.println(g_mqtt.state());
  }
}

static void publishUdpStatus(bool force) {
  if (!g_mqttConnected || !g_mqtt.connected()) return;
  const uint32_t now = millis();
  if (!force && (now - g_lastUdpStatusMs) < UDP_STATUS_INTERVAL_MS) return;
  g_lastUdpStatusMs = now;

  String msg = "{\"type\":\"udp_status\",\"device_id\":\"";
  msg += EVS_DEVICE_ID;
  msg += "\",\"enabled\":";
  msg += g_udpEnabled ? "true" : "false";
  msg += ",\"target_resolved\":";
  msg += g_udpIpValid ? "true" : "false";
  msg += ",\"target_host\":\"";
  msg += g_udpHost;
  msg += "\",\"target_port\":";
  msg += String(g_udpPort);
  msg += ",\"packets_sent\":";
  msg += String(g_udpPacketsSent);
  msg += "}";

  const bool ok = g_mqtt.publish(g_mqttUdpStatusTopic.c_str(), msg.c_str(), true);
  if (!ok) {
    Serial.print("MQTT udp status publish failed, len=");
    Serial.print(msg.length());
    Serial.print(" state=");
    Serial.println(g_mqtt.state());
  }
}

// Send PCM16 mono frame to server.
static void handleFrameForServer(const int16_t* frame, size_t count) {
  if (!g_wsConnected || count == 0) {
    return;
  }
  g_ws.sendBinary(reinterpret_cast<const char*>(frame), count * sizeof(int16_t));
}

static void publishMicTelemetryIfDue(const int16_t* frame, size_t count) {
  if (!isMicModeActive()) {
    return;
  }
  if (count == 0) {
    return;
  }

  int16_t peak = 0;
  int64_t sum_abs = 0;
  for (size_t i = 0; i < count; ++i) {
    int16_t v = frame[i];
    int16_t av = (v < 0) ? (int16_t)(-v) : v;
    if (av > peak) peak = av;
    sum_abs += av;
  }
  const uint32_t avg_abs = (uint32_t)(sum_abs / (int64_t)count);

  const uint32_t now = millis();
  if ((now - g_lastMicTelemetryMs) < MIC_TELEMETRY_INTERVAL_MS) {
    return;
  }
  g_lastMicTelemetryMs = now;

  Serial.print("MIC peak=");
  Serial.print(peak);
  Serial.print(" avg_abs=");
  Serial.print(avg_abs);
  Serial.print(" gain=");
  Serial.print(g_micGain, 3);
  Serial.print(" ws=");
  Serial.println(g_wsConnected ? "connected" : "disconnected");

  if (g_wsConnected) {
    String msg = "{\"type\":\"mic_level\",\"peak\":";
    msg += String(peak);
    msg += ",\"avg_abs\":";
    msg += String(avg_abs);
    msg += ",\"samples\":";
    msg += String((unsigned)count);
    msg += ",\"mic_gain\":";
    msg += String(g_micGain, 3);
    msg += "}";
    g_ws.send(msg);
  }
}

static inline int16_t convertMicSampleToPcm16(int32_t raw32) {
  if (!isMicModeActive()) {
    return 0;
  }
  // INMP441 uses 24-bit signed samples packed into 32-bit I2S slots.
  int32_t s24 = raw32 >> 8;
  int32_t s16 = s24 >> kMicS24ToS16Shift;
  float scaled = (float)s16 * g_micGain;
  if (scaled > 32767.0f) scaled = 32767.0f;
  if (scaled < -32768.0f) scaled = -32768.0f;
  return (int16_t)scaled;
}

static void serviceSpeaker() {
  if (!isSpeakerModeActive()) {
    return;
  }
  const uint32_t periodUs = 1000000UL / SPEAKER_SAMPLE_RATE;
  uint32_t now = micros();
  int processed = 0;
  // Catch up if we lagged behind, but cap work per loop iteration.
  while ((int32_t)(now - g_nextOutUs) >= 0 && processed < 8) {
    g_nextOutUs += periodUs;
    int16_t s = 0;
    if (dequeuePcmSample(s)) {
      speakerWriteU8(pcm16ToU8(s));
    } else {
      speakerWriteU8(128);
    }
    now = micros();
    ++processed;
  }
}

static void printHelp() {
  Serial.println();
  Serial.println("Commands:");
  Serial.println("  p = print network status");
  Serial.println("  h = help");
  Serial.println("MQTT cmd:");
  Serial.println("  {\"cmd\":\"io_mode\",\"value\":\"mic|spk\"}");
  Serial.println("Connection policy:");
  Serial.println("  connect -> StreamToServer (start)");
  Serial.println("  disconnect -> Idle");
}

static void handleSerialCommands() {
  while (Serial.available()) {
    const char c = (char)Serial.read();
    if (kSerialCommandEcho && c != '\r' && c != '\n') {
      Serial.print("RX cmd: ");
      Serial.println(c);
    }
    if (c == 'p') {
      Serial.print("WiFi: ");
      Serial.print((WiFi.status() == WL_CONNECTED) ? "connected " : "disconnected ");
      Serial.print("IP=");
      Serial.println(WiFi.localIP());
      Serial.print("WS: ");
      Serial.println(g_wsConnected ? "connected" : "disconnected");
      Serial.print("Mode: ");
      Serial.println((g_mode == DeviceMode::StreamToServer) ? "StreamToServer" : "Idle");
    } else if (c == 'h') {
      printHelp();
    } else if (c != '\r' && c != '\n') {
      Serial.println("Unknown command");
    }
  }
}

void setup() {
  Serial.begin(115200);
  delay(300);
  Serial.println("EVS Client boot");

  if (!initMicI2s()) {
    Serial.println("ERROR: I2S init failed");
    while (true) delay(1000);
  }
  if (!initSpeakerOut()) {
    Serial.println("ERROR: speaker DAC init failed (EVS_SPK_DAC_PIN must be 25 or 26)");
    while (true) delay(1000);
  }

  g_ws.onMessage(onWsMessageCallback);
  g_ws.onEvent(onWsEventCallback);
  g_mqtt.setBufferSize(MQTT_BUFFER_SIZE);
  g_mqttCmdTopic = String(EVS_MQTT_BASE_TOPIC) + "/" + EVS_DEVICE_ID + "/command";
  g_mqttStatusTopic = String(EVS_MQTT_BASE_TOPIC) + "/" + EVS_DEVICE_ID + "/status";
  g_mqttUdpStatusTopic = String(EVS_MQTT_BASE_TOPIC) + "/" + EVS_DEVICE_ID + "/udp_status";
  g_nextOutUs = micros();

  String defaultMode = EVS_DEFAULT_IO_MODE;
  defaultMode.toLowerCase();
  if (defaultMode == "spk" || defaultMode == "speaker") {
    g_ioMode = IoMode::Speaker;
  } else {
    g_ioMode = IoMode::Mic;
  }

  // Wait in idle until WS connect event switches to StreamToServer.
  setMode(DeviceMode::Idle);
  Serial.println("Audio init ok");
  Serial.print("Hardware: mic=");
  Serial.print(kMicHardwareAvailable ? "yes" : "no");
  Serial.print(" speaker=");
  Serial.println(kSpeakerHardwareAvailable ? "yes" : "no");
  Serial.print("Active io_mode: ");
  Serial.println(g_ioMode == IoMode::Mic ? "mic" : "spk");
  Serial.print("Speaker DAC pin: ");
  Serial.println(EVS_SPK_DAC_PIN);
  Serial.println("Set local environment values in include/secrets.h");
  printHelp();
}

void loop() {
  handleSerialCommands();
  ensureConnectivity();
  ensureMqttConnectivity();
  g_ws.poll();
  if (g_mqtt.connected()) {
    g_mqtt.loop();
    g_mqttConnected = true;
  } else {
    g_mqttConnected = false;
  }
  publishUdpStatus(false);
  serviceSpeaker();

  if (!isMicModeActive()) {
    delay(5);
    return;
  }

  size_t bytesRead = 0;
  const esp_err_t res = i2s_read(
      MIC_I2S_PORT, g_micBuffer, sizeof(g_micBuffer), &bytesRead, 0);
  if (res != ESP_OK || bytesRead == 0) {
    return;
  }

  const size_t sampleCount = bytesRead / sizeof(int32_t);
  static int16_t pcm16[MIC_FRAME_SAMPLES];
  for (size_t i = 0; i < sampleCount; ++i) {
    pcm16[i] = convertMicSampleToPcm16(g_micBuffer[i]);
  }

  if (g_mode == DeviceMode::StreamToServer) {
    handleFrameForServer(pcm16, sampleCount);
    publishMicTelemetryIfDue(pcm16, sampleCount);
  }

  if (g_udpEnabled && g_udpIpValid && g_udpPort > 0) {
    const size_t nBytes = sampleCount * sizeof(int16_t);
    if (nBytes <= UDP_MAX_PACKET_BYTES) {
      g_udp.beginPacket(g_udpIp, g_udpPort);
      g_udp.write(reinterpret_cast<const uint8_t*>(pcm16), nBytes);
      g_udp.endPacket();
      ++g_udpPacketsSent;
    }
  }
}