2_way_Tochie/main/sip/sip.c

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <unistd.h>

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

#include "esp_wifi.h"
#include "esp_event.h"
#include "esp_log.h"
#include "nvs_flash.h"

#include "lwip/sockets.h"

#include <stdbool.h>
#include "esp_rom_md5.h"
#include "esp_err.h"

#include "bsp_board.h"
#include "tca9555_driver.h"

#include "esp_audio_enc_default.h"
#include "esp_audio_enc.h"
#include "esp_audio_dec_default.h"
#include "esp_audio_dec.h"
#include "esp_g711_enc.h"
#include "esp_g711_dec.h"

#define ESP32_IP "192.168.68.79"
#define RTP_PORT 4000

#define WIFI_SSID "TOCHTECH"
#define WIFI_PASS "Smarturns2017"

#define ASTERISK_IP "192.168.68.72"
#define SIP_PORT 5060
#define SIP_LOCAL_IP_DEFAULT "0.0.0.0"
#define SIP_LOCAL_PORT 5062
#define SIP_USER "1001"
#define SIP_PASSWORD "secret123"

#define SAMPLE_RATE 8000
#define SAMPLES_PER_PACKET 160   // 20 ms
#define RTP_HEADER_SIZE 12
#define RTP_PAYLOAD_PCMU 0
#define MIC_CAPTURE_RATE 16000
#define MIC_CAPTURE_FRAMES_20MS (MIC_CAPTURE_RATE / 50)
#define RTP_PTIME_MS 20

// Troubleshooting knobs for mic conversion and downsample quality.
#define SIP_PCM_SHIFT_BITS 12
#define SIP_DOWNSAMPLE_MODE_DROP 1
#define SIP_DOWNSAMPLE_MODE_AVG2 0
#define SIP_DOWNSAMPLE_MODE SIP_DOWNSAMPLE_MODE_AVG2
#define SIP_CAPTURE_DIAG_EVERY_N_FRAMES 100
#define SIP_RX_JITTER_BUFFER_FRAMES 4
#define SIP_RX_JITTER_BUFFER_TARGET 3
#define SIP_RX_ENABLE_JITTER_BUFFER 0
#define SIP_RX_ENABLE_PLC 0
#define SIP_SEND_RINGING 1

static int32_t s_mic_raw_32[MIC_CAPTURE_FRAMES_20MS];

static const char *TAG = "SIP";
volatile bool wifi_ready = false;
static int s_rtp_sock = -1;
static struct sockaddr_in s_rtp_peer = {0};
static uint16_t s_rtp_peer_port = 0;
static uint16_t s_rtp_sequence = 0;
static uint32_t s_rtp_timestamp = 0;
static const uint32_t s_rtp_ssrc = 0x45535033;
static volatile bool s_call_active = false;
static volatile bool s_audio_hw_ready = false;
static esp_audio_enc_handle_t s_audio_enc_handle = NULL;
static esp_audio_dec_handle_t s_audio_dec_handle = NULL;
static TaskHandle_t s_rtp_tx_task_handle = NULL;
static TaskHandle_t s_rtp_rx_task_handle = NULL;
static char s_sip_local_ip[16] = SIP_LOCAL_IP_DEFAULT;
static uint32_t s_rtp_tx_packets = 0;
static uint32_t s_rtp_tx_bad_payload = 0;
static uint32_t s_rtp_tx_bad_ts_step = 0;
static uint32_t s_rtp_tx_last_log_ms = 0;
static uint32_t s_rtp_rx_packets = 0;
static uint32_t s_rtp_rx_gap_packets = 0;
static uint32_t s_rtp_rx_out_of_order = 0;
static uint32_t s_rtp_rx_duplicates = 0;
static uint32_t s_rtp_rx_non_pcmu = 0;
static uint32_t s_rtp_rx_timeouts = 0;
static uint32_t s_rtp_rx_plc_frames = 0;
static uint32_t s_rtp_rx_last_log_ms = 0;

static int sip_upsample_8k_to_16k_linear(const int16_t *in,
                                         int in_samples,
                                         int16_t *out,
                                         int out_capacity_samples)
{
    if (in_samples <= 0 || out_capacity_samples < (in_samples * 2)) {
        return 0;
    }

    for (int i = 0; i < in_samples; i++) {
        out[i * 2] = in[i];

        if (i + 1 < in_samples) {
            int32_t a = in[i];
            int32_t b = in[i + 1];
            out[(i * 2) + 1] = (int16_t)((a + b) / 2);
        }
        else {
            out[(i * 2) + 1] = in[i];
        }
    }

    return in_samples * 2;
}

static void sip_rx_queue_push(int16_t queue[][SAMPLES_PER_PACKET * 2],
                              int *bytes_queue,
                              int *head,
                              int *tail,
                              int *count,
                              const int16_t *frame,
                              int frame_bytes)
{
    if (frame_bytes <= 0) {
        return;
    }

    if (*count >= SIP_RX_JITTER_BUFFER_FRAMES) {
        *head = (*head + 1) % SIP_RX_JITTER_BUFFER_FRAMES;
        (*count)--;
    }

    memcpy(queue[*tail], frame, (size_t)frame_bytes);
    bytes_queue[*tail] = frame_bytes;
    *tail = (*tail + 1) % SIP_RX_JITTER_BUFFER_FRAMES;
    (*count)++;
}

static void sip_rx_queue_drain(int16_t queue[][SAMPLES_PER_PACKET * 2],
                               int *bytes_queue,
                               int *head,
                               int *count,
                               int target_level)
{
    while (*count > target_level) {
        esp_audio_play(queue[*head], bytes_queue[*head], portMAX_DELAY);
        *head = (*head + 1) % SIP_RX_JITTER_BUFFER_FRAMES;
        (*count)--;
    }
}

static int16_t sip_pcm16_from_shifted(int32_t sample, uint32_t *clip_counter)
{
    int32_t shifted = sample >> SIP_PCM_SHIFT_BITS;

    if (shifted > 32767) {
        (*clip_counter)++;
        return 32767;
    }

    if (shifted < -32768) {
        (*clip_counter)++;
        return -32768;
    }

    return (int16_t)shifted;
}

static bool sip_capture_pcm16_frame(int16_t *pcm_frame)
{
    static uint32_t frame_counter = 0;

    // Read 20 ms of raw microphone data and downsample 16 kHz -> 8 kHz.
    if (esp_get_feed_data(false, (int16_t *)s_mic_raw_32, sizeof(s_mic_raw_32)) != ESP_OK) {
        return false;
    }

    uint32_t clip_count = 0;
    int16_t max_abs = 0;

    for (int i = 0; i < SAMPLES_PER_PACKET; i++) {
        int32_t sample;

#if SIP_DOWNSAMPLE_MODE == SIP_DOWNSAMPLE_MODE_AVG2
        int32_t s0 = s_mic_raw_32[i * 2];
    int32_t s1 = s_mic_raw_32[(i * 2) + 1];
    sample = (s0 + s1) / 2;
#else
        sample = s_mic_raw_32[i * 2];
#endif

        pcm_frame[i] = sip_pcm16_from_shifted(sample, &clip_count);

        int16_t v = pcm_frame[i];
        int16_t abs_v = (v < 0) ? (int16_t)(-v) : v;
        if (abs_v > max_abs) {
            max_abs = abs_v;
        }
    }

    frame_counter++;
    if ((frame_counter % SIP_CAPTURE_DIAG_EVERY_N_FRAMES) == 0) {
        ESP_LOGI(TAG,
                 "cap diag: shift=%d mode=%d max=%d clips=%u",
                 SIP_PCM_SHIFT_BITS,
                 SIP_DOWNSAMPLE_MODE,
                 max_abs,
                 (unsigned)clip_count);
    }

    return true;
}

/* ================= WIFI ================= */

static void wifi_event_handler(void *arg, esp_event_base_t event_base,
                               int32_t event_id, void *event_data)
{
    if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_START) {
        esp_wifi_connect();
    }
    else if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_DISCONNECTED) {
        ESP_LOGI(TAG, "WiFi disconnected, retrying...");
        wifi_ready = false;
        esp_wifi_connect();
    }
    else if (event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP) {
        ip_event_got_ip_t *ip = (ip_event_got_ip_t *)event_data;
        snprintf(s_sip_local_ip, sizeof(s_sip_local_ip), IPSTR, IP2STR(&ip->ip_info.ip));
        ESP_LOGI(TAG, "ESP IP: " IPSTR, IP2STR(&ip->ip_info.ip));
        // ESP_LOGI(TAG, "WiFi connected!");
        wifi_ready = true;

    }
}

void wifi_init(void)
{
    nvs_flash_init();
    esp_netif_init();
    esp_event_loop_create_default();
    esp_netif_create_default_wifi_sta();

    wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
    esp_wifi_init(&cfg);

    esp_event_handler_register(WIFI_EVENT, ESP_EVENT_ANY_ID, &wifi_event_handler, NULL);
    esp_event_handler_register(IP_EVENT, IP_EVENT_STA_GOT_IP, &wifi_event_handler, NULL);

    wifi_config_t wifi_config = {
        .sta = {
            .ssid = WIFI_SSID,
            .password = WIFI_PASS,
        },
    };

    esp_wifi_set_mode(WIFI_MODE_STA);
    esp_wifi_set_config(WIFI_IF_STA, &wifi_config);
    esp_wifi_start();
}

/* ================= MD5 HELPERS ================= */

static void md5_calc(const unsigned char *input, size_t len, unsigned char output[16])
{
    md5_context_t ctx;
    esp_rom_md5_init(&ctx);
    esp_rom_md5_update(&ctx, input, len);
    esp_rom_md5_final(output, &ctx);
}

static void md5_to_hex(unsigned char *md5, char *out)
{
    for (int i = 0; i < 16; i++) {
        sprintf(out + i * 2, "%02x", md5[i]);
    }
}

/* ================= SIP DIGEST ================= */

void sip_compute_response(
    const char *username,
    const char *realm,
    const char *password,
    const char *nonce,
    const char *uri,
    char *out_response)
{
    unsigned char ha1_md5[16], ha2_md5[16], final_md5[16];
    char ha1_hex[64], ha2_hex[64], final_str[256];
    char ha1[128], ha2[128];

    snprintf(ha1, sizeof(ha1), "%s:%s:%s", username, realm, password);
    md5_calc((unsigned char*)ha1, strlen(ha1), ha1_md5);
    md5_to_hex(ha1_md5, ha1_hex);

    snprintf(ha2, sizeof(ha2), "REGISTER:%s", uri);
    md5_calc((unsigned char*)ha2, strlen(ha2), ha2_md5);
    md5_to_hex(ha2_md5, ha2_hex);

    snprintf(final_str, sizeof(final_str),
             "%s:%s:%s", ha1_hex, nonce, ha2_hex);

    md5_calc((unsigned char*)final_str, strlen(final_str), final_md5);
    md5_to_hex(final_md5, out_response);
}

/* ================= SIP HELPERS ================= */

static bool sip_extract_header(const char *msg, const char *header, char *out, size_t out_len)
{
    const char *start = strstr(msg, header);
    if (!start) {
        return false;
    }

    start += strlen(header);
    while (*start == ' ' || *start == '\t') {
        start++;
    }

    const char *end = start;
    while (*end != '\0' && *end != '\r' && *end != '\n') {
        end++;
    }

    size_t len = (size_t)(end - start);
    if (len >= out_len) {
        len = out_len - 1;
    }

    memcpy(out, start, len);
    out[len] = '\0';
    return true;
}

static void sip_send_200_ok(int sock, const struct sockaddr_in *remote, const char *sip_msg,
                           const char *sdp_body, size_t sdp_len)
{
    char via[512] = {0};
    char from[256] = {0};
    char to[256] = {0};
    char callid[256] = {0};
    char cseq[128] = {0};
    char response[4096];

    if (!sip_extract_header(sip_msg, "Via:", via, sizeof(via))) {
        snprintf(via, sizeof(via), "SIP/2.0/UDP %s:%d", s_sip_local_ip, SIP_LOCAL_PORT);
    }
    if (!sip_extract_header(sip_msg, "From:", from, sizeof(from))) {
        snprintf(from, sizeof(from), "<sip:%s@%s>", SIP_USER, ASTERISK_IP);
    }
    if (!sip_extract_header(sip_msg, "To:", to, sizeof(to))) {
        snprintf(to, sizeof(to), "<sip:%s@%s>", SIP_USER, ASTERISK_IP);
    }
    if (!sip_extract_header(sip_msg, "Call-ID:", callid, sizeof(callid))) {
        snprintf(callid, sizeof(callid), "esp32-call");
    }
    if (!sip_extract_header(sip_msg, "CSeq:", cseq, sizeof(cseq))) {
        snprintf(cseq, sizeof(cseq), "1 REGISTER");
    }

    int body_len = sdp_body ? (int)sdp_len : 0;
    int written = snprintf(response, sizeof(response),
        "SIP/2.0 200 OK\r\n"
        "Via: %s\r\n"
        "From: %s\r\n"
        "To: %s;tag=esp32\r\n"
        "Call-ID: %s\r\n"
        "CSeq: %s\r\n"
        "Contact: <sip:%s@%s:%d>\r\n"
        "%s"
        "Content-Length: %d\r\n"
        "\r\n"
        "%s",
        via,
        from,
        to,
        callid,
        cseq,
        SIP_USER,
        s_sip_local_ip,
        SIP_LOCAL_PORT,
        sdp_body ? "Content-Type: application/sdp\r\n" : "",
        body_len,
        sdp_body ? sdp_body : "");

    if (written < 0 || written >= (int)sizeof(response)) {
        ESP_LOGW(TAG, "SIP 200 OK response too large");
        return;
    }

    sendto(sock, response, (size_t)written, 0,
           (struct sockaddr *)remote, sizeof(*remote));
}

static void sip_build_sdp(char *out, size_t out_len)
{
    snprintf(out, out_len,
        "v=0\r\n"
    "o=ESP32 1234 1234 IN IP4 %s\r\n"
        "s=ESP32 SIP Call\r\n"
        "c=IN IP4 %s\r\n"
        "t=0 0\r\n"
        "m=audio %d RTP/AVP 0\r\n"
        "a=rtpmap:0 PCMU/8000\r\n"
        "a=ptime:20\r\n"
        "a=sendrecv\r\n",
    s_sip_local_ip,
    s_sip_local_ip,
        RTP_PORT);
}

static bool sip_extract_rtp_port(const char *msg, uint16_t *out_port)
{
    const char *m = strstr(msg, "m=audio ");
    if (!m) {
        return false;
    }

    m += strlen("m=audio ");
    char port_buf[16] = {0};
    size_t i = 0;
    while (*m && *m != ' ' && *m != '\r' && *m != '\n' && i < sizeof(port_buf) - 1) {
        port_buf[i++] = *m++;
    }

    port_buf[i] = '\0';
    *out_port = (uint16_t)atoi(port_buf);
    return *out_port != 0;
}

static bool sip_open_audio_media_codecs(void)
{
    if (s_audio_enc_handle == NULL) {
        esp_audio_err_t reg_ret = esp_audio_enc_register_default();
        if (reg_ret != ESP_AUDIO_ERR_OK && reg_ret != ESP_AUDIO_ERR_ALREADY_EXIST) {
            ESP_LOGE(TAG, "Failed to register default encoders: %d", reg_ret);
            return false;
        }

        esp_g711_enc_config_t enc_cfg = ESP_G711_ENC_CONFIG_DEFAULT();
        enc_cfg.sample_rate = SAMPLE_RATE;
        enc_cfg.channel = 1;
        enc_cfg.bits_per_sample = 16;
        enc_cfg.frame_duration = 20;

        esp_audio_enc_config_t audio_enc_cfg = {
            .type = ESP_AUDIO_TYPE_G711U,
            .cfg = &enc_cfg,
            .cfg_sz = sizeof(enc_cfg),
        };

        esp_audio_err_t enc_ret = esp_audio_enc_open(&audio_enc_cfg, &s_audio_enc_handle);
        if (enc_ret != ESP_AUDIO_ERR_OK) {
            ESP_LOGE(TAG, "Failed to open audio encoder: %d", enc_ret);
            s_audio_enc_handle = NULL;
            return false;
        }
    }

    if (s_audio_dec_handle == NULL) {
        esp_audio_err_t reg_ret = esp_audio_dec_register_default();
        if (reg_ret != ESP_AUDIO_ERR_OK && reg_ret != ESP_AUDIO_ERR_ALREADY_EXIST) {
            ESP_LOGE(TAG, "Failed to register default decoders: %d", reg_ret);
            if (s_audio_enc_handle != NULL) {
                esp_audio_enc_close(s_audio_enc_handle);
                s_audio_enc_handle = NULL;
            }
            return false;
        }

        esp_g711_dec_cfg_t dec_cfg = ESP_G711_DEC_CONFIG_DEFAULT();
        dec_cfg.channel = 1;

        esp_audio_dec_cfg_t audio_dec_cfg = {
            .type = ESP_AUDIO_TYPE_G711U,
            .cfg = &dec_cfg,
            .cfg_sz = sizeof(dec_cfg),
        };

        esp_audio_err_t dec_ret = esp_audio_dec_open(&audio_dec_cfg, &s_audio_dec_handle);
        if (dec_ret != ESP_AUDIO_ERR_OK) {
            ESP_LOGE(TAG, "Failed to open audio decoder: %d", dec_ret);
            s_audio_dec_handle = NULL;
            if (s_audio_enc_handle != NULL) {
                esp_audio_enc_close(s_audio_enc_handle);
                s_audio_enc_handle = NULL;
            }
            return false;
        }
    }

    return true;
}

static bool sip_ensure_rtp_socket(void)
{
    if (s_rtp_sock >= 0) {
        return true;
    }

    s_rtp_sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
    if (s_rtp_sock < 0) {
        ESP_LOGE(TAG, "Failed to create RTP socket");
        return false;
    }

    struct sockaddr_in local_addr = {0};
    local_addr.sin_family = AF_INET;
    local_addr.sin_port = htons(RTP_PORT);
    local_addr.sin_addr.s_addr = INADDR_ANY;

    if (bind(s_rtp_sock, (struct sockaddr *)&local_addr, sizeof(local_addr)) < 0) {
        ESP_LOGE(TAG, "Failed to bind RTP port %d", RTP_PORT);
        close(s_rtp_sock);
        s_rtp_sock = -1;
        return false;
    }

    struct timeval timeout = {
        .tv_sec = 0,
        .tv_usec = 200000,
    };
    setsockopt(s_rtp_sock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout));

    ESP_LOGI(TAG, "RTP socket bound on %s:%d", s_sip_local_ip, RTP_PORT);

    return true;
}

static void sip_send_rtp_packet(const uint8_t *payload, size_t payload_len)
{
    if (s_rtp_peer_port == 0) {
        return;
    }

    if (!sip_ensure_rtp_socket()) {
        return;
    }

    uint8_t packet[RTP_HEADER_SIZE + SAMPLES_PER_PACKET] = {0};
    size_t packet_len = RTP_HEADER_SIZE + payload_len;

    packet[0] = 0x80;
    packet[1] = 0x00;
    packet[2] = (uint8_t)(s_rtp_sequence >> 8);
    packet[3] = (uint8_t)(s_rtp_sequence & 0xFF);
    packet[4] = (uint8_t)(s_rtp_timestamp >> 24);
    packet[5] = (uint8_t)(s_rtp_timestamp >> 16);
    packet[6] = (uint8_t)(s_rtp_timestamp >> 8);
    packet[7] = (uint8_t)(s_rtp_timestamp & 0xFF);
    packet[8] = (uint8_t)(s_rtp_ssrc >> 24);
    packet[9] = (uint8_t)(s_rtp_ssrc >> 16);
    packet[10] = (uint8_t)(s_rtp_ssrc >> 8);
    packet[11] = (uint8_t)(s_rtp_ssrc & 0xFF);
    memcpy(packet + RTP_HEADER_SIZE, payload, payload_len);

    struct sockaddr_in peer = {0};
    peer.sin_family = AF_INET;
    peer.sin_port = htons(s_rtp_peer_port);
    peer.sin_addr = s_rtp_peer.sin_addr;

    sendto(s_rtp_sock, packet, packet_len, 0,
           (struct sockaddr *)&peer, sizeof(peer));

    s_rtp_sequence++;
    s_rtp_timestamp += SAMPLES_PER_PACKET;
}

static bool sip_extract_rtp_payload(const uint8_t *packet,
                                    int packet_len,
                                    uint8_t *payload_type,
                                    const uint8_t **payload,
                                    int *payload_len)
{
    if (packet_len < RTP_HEADER_SIZE) {
        return false;
    }

    uint8_t version = (uint8_t)(packet[0] >> 6);
    uint8_t csrc_count = (uint8_t)(packet[0] & 0x0F);
    bool has_extension = (packet[0] & 0x10) != 0;

    if (version != 2) {
        return false;
    }

    int header_len = RTP_HEADER_SIZE + (csrc_count * 4);
    if (packet_len < header_len) {
        return false;
    }

    if (has_extension) {
        if (packet_len < header_len + 4) {
            return false;
        }

        int ext_len_words = (packet[header_len + 2] << 8) | packet[header_len + 3];
        header_len += 4 + (ext_len_words * 4);
        if (packet_len < header_len) {
            return false;
        }
    }

    *payload_type = (uint8_t)(packet[1] & 0x7F);
    *payload = packet + header_len;
    *payload_len = packet_len - header_len;
    return *payload_len > 0;
}


static void sip_rtp_tx_task(void *pvParameters)
{
    (void)pvParameters;

    int16_t pcm_frame[SAMPLES_PER_PACKET] = {0};
    uint8_t payload[SAMPLES_PER_PACKET] = {0};

    esp_audio_enc_in_frame_t in_frame = {
        .buffer = (uint8_t *)pcm_frame,
        .len = sizeof(pcm_frame),
    };
    esp_audio_enc_out_frame_t out_frame = {
        .buffer = payload,
        .len = sizeof(payload),
    };

    TickType_t last_wake = xTaskGetTickCount();
    uint32_t last_ts = s_rtp_timestamp;
    s_rtp_tx_packets = 0;
    s_rtp_tx_bad_payload = 0;
    s_rtp_tx_bad_ts_step = 0;
    s_rtp_tx_last_log_ms = 0;

    while (s_call_active) {
        vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(RTP_PTIME_MS));

        if (!sip_capture_pcm16_frame(pcm_frame)) {
            continue;
        }

        if (s_audio_enc_handle == NULL) {
            continue;
        }

        out_frame.encoded_bytes = 0;
        esp_audio_err_t enc_ret = esp_audio_enc_process(s_audio_enc_handle, &in_frame, &out_frame);

        if (enc_ret != ESP_AUDIO_ERR_OK) {
            ESP_LOGW(TAG, "Audio encode failed: %d", enc_ret);
            continue;
        }

        if (out_frame.encoded_bytes != SAMPLES_PER_PACKET) {
            s_rtp_tx_bad_payload++;
        }

        sip_send_rtp_packet(payload, out_frame.encoded_bytes);

        uint32_t ts_step = s_rtp_timestamp - last_ts;
        if (ts_step != SAMPLES_PER_PACKET) {
            s_rtp_tx_bad_ts_step++;
        }
        last_ts = s_rtp_timestamp;
        s_rtp_tx_packets++;

        uint32_t now_ms = (uint32_t)(xTaskGetTickCount() * portTICK_PERIOD_MS);
        if (s_rtp_tx_last_log_ms == 0) {
            s_rtp_tx_last_log_ms = now_ms;
        }
        else if ((now_ms - s_rtp_tx_last_log_ms) >= 1000) {
            ESP_LOGI(TAG,
                     "rtp tx: pps=%u bad_payload=%u bad_ts=%u last_bytes=%u",
                     (unsigned)s_rtp_tx_packets,
                     (unsigned)s_rtp_tx_bad_payload,
                     (unsigned)s_rtp_tx_bad_ts_step,
                     (unsigned)out_frame.encoded_bytes);
            s_rtp_tx_packets = 0;
            s_rtp_tx_bad_payload = 0;
            s_rtp_tx_bad_ts_step = 0;
            s_rtp_tx_last_log_ms = now_ms;
        }
    }

    s_rtp_tx_task_handle = NULL;
    vTaskDelete(NULL);
}

static void sip_rtp_rx_task(void *pvParameters)
{
    (void)pvParameters;

    uint8_t packet[1720] = {0};
    int16_t pcm[SAMPLES_PER_PACKET] = {0};
    int16_t pcm_16k[SAMPLES_PER_PACKET * 2] = {0};
    int16_t last_good_pcm_16k[SAMPLES_PER_PACKET * 2] = {0};
    bool have_last_good_frame = false;
    bool have_last_seq = false;
    uint16_t last_seq = 0;
    uint16_t last_logged_seq = 0;
    uint32_t ooo_streak = 0;
    int16_t playback_queue[SIP_RX_JITTER_BUFFER_FRAMES][SAMPLES_PER_PACKET * 2] = {0};
    int playback_bytes[SIP_RX_JITTER_BUFFER_FRAMES] = {0};
    int playback_head = 0;
    int playback_tail = 0;
    int playback_count = 0;

    s_rtp_rx_packets = 0;
    s_rtp_rx_gap_packets = 0;
    s_rtp_rx_out_of_order = 0;
    s_rtp_rx_duplicates = 0;
    s_rtp_rx_non_pcmu = 0;
    s_rtp_rx_timeouts = 0;
    s_rtp_rx_plc_frames = 0;
    s_rtp_rx_last_log_ms = 0;

    while (s_call_active) {
        struct sockaddr_in src;
        socklen_t srclen = sizeof(src);

        int len = recvfrom(s_rtp_sock,
                        packet,
                        sizeof(packet),
                        0,
                        (struct sockaddr *)&src,
                        &srclen);

        uint32_t now_ms = (uint32_t)(xTaskGetTickCount() * portTICK_PERIOD_MS);
        if (s_rtp_rx_last_log_ms == 0) {
            s_rtp_rx_last_log_ms = now_ms;
        }
        else if ((now_ms - s_rtp_rx_last_log_ms) >= 1000) {
            ESP_LOGI(TAG,
                     "rtp rx: pps=%u gaps=%u ooo=%u dup=%u non_pcmu=%u to=%u plc=%u last_seq=%u",
                     (unsigned)s_rtp_rx_packets,
                     (unsigned)s_rtp_rx_gap_packets,
                     (unsigned)s_rtp_rx_out_of_order,
                     (unsigned)s_rtp_rx_duplicates,
                     (unsigned)s_rtp_rx_non_pcmu,
                     (unsigned)s_rtp_rx_timeouts,
                     (unsigned)s_rtp_rx_plc_frames,
                     (unsigned)last_logged_seq);
            s_rtp_rx_packets = 0;
            s_rtp_rx_gap_packets = 0;
            s_rtp_rx_out_of_order = 0;
            s_rtp_rx_duplicates = 0;
            s_rtp_rx_non_pcmu = 0;
            s_rtp_rx_timeouts = 0;
            s_rtp_rx_plc_frames = 0;
            s_rtp_rx_last_log_ms = now_ms;
        }

        if (len <= 0) {
            s_rtp_rx_timeouts++;
            continue;
        }

        uint8_t payload_type = 0;
        const uint8_t *payload = NULL;
        int payload_len = 0;

        if (!sip_extract_rtp_payload(packet, len, &payload_type, &payload, &payload_len)) {
            continue;
        }

        if (payload_type != RTP_PAYLOAD_PCMU) {
            s_rtp_rx_non_pcmu++;
            continue;
        }

        uint16_t seq = (uint16_t)(((uint16_t)packet[2] << 8) | (uint16_t)packet[3]);
        last_logged_seq = seq;

        if (!have_last_seq) {
            have_last_seq = true;
            last_seq = seq;
        }
        else {
            int16_t order_diff = (int16_t)(seq - last_seq);
            if (order_diff > 0) {
                if (order_diff > 1) {
                    uint32_t missing = (uint32_t)(order_diff - 1);
                    s_rtp_rx_gap_packets += missing;

                    if (SIP_RX_ENABLE_PLC && have_last_good_frame) {
                        uint32_t plc_to_play = missing;
                        if (plc_to_play > 3) {
                            plc_to_play = 3;
                        }
                        for (uint32_t i = 0; i < plc_to_play; i++) {
                            sip_rx_queue_push(playback_queue,
                                              playback_bytes,
                                              &playback_head,
                                              &playback_tail,
                                              &playback_count,
                                              last_good_pcm_16k,
                                              (int)sizeof(last_good_pcm_16k));
                            s_rtp_rx_plc_frames++;
                        }
                    }
                }
                last_seq = seq;
                ooo_streak = 0;
            }
            else if (order_diff == 0) {
                s_rtp_rx_duplicates++;
                continue;
            }
            else {
                s_rtp_rx_out_of_order++;
                ooo_streak++;
                if (ooo_streak >= 10) {
                    // Recover quickly if the sender changes sequence domain.
                    last_seq = seq;
                    ooo_streak = 0;
                }
                continue;
            }
        }

        s_rtp_rx_packets++;

        if (s_audio_dec_handle == NULL) {
            continue;
        }

        esp_audio_dec_in_raw_t raw = {
            .buffer = (uint8_t *)payload,
            .len = (uint32_t)payload_len,
            .consumed = 0,
            .frame_recover = ESP_AUDIO_DEC_RECOVERY_NONE,
        };

        while (raw.len > 0) {
            esp_audio_dec_out_frame_t frame = {
                .buffer = (uint8_t *)pcm,
                .len = sizeof(pcm),
                .needed_size = 0,
                .decoded_size = 0,
            };

            esp_audio_err_t dec_ret = esp_audio_dec_process(s_audio_dec_handle, &raw, &frame);

            if (dec_ret != ESP_AUDIO_ERR_OK) {
                ESP_LOGW(TAG, "Audio decode failed: %d", dec_ret);
                break;
            }

            int decoded_samples = (int)frame.decoded_size / (int)sizeof(int16_t);
            int up_samples = sip_upsample_8k_to_16k_linear(
                pcm,
                decoded_samples,
                pcm_16k,
                (int)(sizeof(pcm_16k) / sizeof(pcm_16k[0])));

            if (up_samples > 0) {
                int play_bytes = up_samples * (int)sizeof(int16_t);

                if (play_bytes <= (int)sizeof(last_good_pcm_16k)) {
                    memcpy(last_good_pcm_16k, pcm_16k, (size_t)play_bytes);
                    have_last_good_frame = true;
                }

#if SIP_RX_ENABLE_JITTER_BUFFER
                sip_rx_queue_push(playback_queue,
                                  playback_bytes,
                                  &playback_head,
                                  &playback_tail,
                                  &playback_count,
                                  pcm_16k,
                                  play_bytes);

                sip_rx_queue_drain(playback_queue,
                                   playback_bytes,
                                   &playback_head,
                                   &playback_count,
                                   SIP_RX_JITTER_BUFFER_TARGET);
#else
                esp_audio_play(pcm_16k, play_bytes, portMAX_DELAY);
#endif
            }

            if (raw.consumed == 0) {
                break;
            }

            raw.buffer += raw.consumed;
            raw.len -= raw.consumed;
        }
    }

    s_rtp_rx_task_handle = NULL;
    vTaskDelete(NULL);
}

static void sip_start_media_tasks(void)
{
    if (!s_audio_hw_ready) {
        ESP_LOGW(TAG, "Audio hardware is not ready, media tasks not started");
        return;
    }

    if (s_rtp_peer_port == 0) {
        ESP_LOGW(TAG, "No RTP peer port, media tasks not started");
        return;
    }

    if (!sip_ensure_rtp_socket()) {
        return;
    }

    s_rtp_sequence = 0;
    s_rtp_timestamp = 0;

    ESP_LOGI(TAG,
             "media cfg: shift=%d downsample=%d jitter=%d plc=%d",
             SIP_PCM_SHIFT_BITS,
             SIP_DOWNSAMPLE_MODE,
             SIP_RX_ENABLE_JITTER_BUFFER,
             SIP_RX_ENABLE_PLC);
    ESP_LOGI(TAG,
             "media peer: %s:%u <- local %s:%d",
             inet_ntoa(s_rtp_peer.sin_addr),
             (unsigned)s_rtp_peer_port,
             s_sip_local_ip,
             RTP_PORT);

    if (s_rtp_tx_task_handle == NULL) {
        if (xTaskCreate(sip_rtp_tx_task, "rtp_tx", 8192, NULL, 5, &s_rtp_tx_task_handle) != pdPASS) {
            ESP_LOGE(TAG, "Failed to start RTP TX task");
            s_rtp_tx_task_handle = NULL;
        }
    }

    if (s_rtp_rx_task_handle == NULL) {
        if (xTaskCreate(sip_rtp_rx_task, "rtp_rx", 8192, NULL, 5, &s_rtp_rx_task_handle) != pdPASS) {
            ESP_LOGE(TAG, "Failed to start RTP RX task");
            s_rtp_rx_task_handle = NULL;
        }
    }
}

static void sip_stop_media_tasks(void)
{
    s_call_active = false;
}

esp_err_t sip_media_init(void)
{
    if (s_audio_hw_ready) {
        return ESP_OK;
    }

    esp_err_t ret = esp_board_init(MIC_CAPTURE_RATE, 2, 16);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "esp_board_init failed: %s", esp_err_to_name(ret));
        return ret;
    }

    tca9555_driver_init();
    if (!sip_open_audio_media_codecs()) {
        return ESP_FAIL;
    }
    s_audio_hw_ready = true;
    ESP_LOGI(TAG, "Audio board initialized for live RTP media");
    return ESP_OK;
}

/* ================= SIP TASK ================= */

void sip_register_task(void *pvParameters)
{
    struct sockaddr_in server = {0};
    server.sin_family = AF_INET;
    server.sin_port = htons(SIP_PORT);
    server.sin_addr.s_addr = inet_addr(ASTERISK_IP);

    int sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
    if (sock < 0) {
        ESP_LOGE(TAG, "Socket failed");
        vTaskDelete(NULL);
        return;
    }

    struct sockaddr_in local_addr = {0};

    local_addr.sin_family = AF_INET;
    local_addr.sin_port = htons(SIP_LOCAL_PORT);
    local_addr.sin_addr.s_addr = INADDR_ANY;

    if (bind(sock,
             (struct sockaddr *)&local_addr,
             sizeof(local_addr)) < 0) {

        ESP_LOGE(TAG, "Failed to bind SIP port %d", SIP_LOCAL_PORT);
        close(sock);
        vTaskDelete(NULL);
        return;
    }

    ESP_LOGI(TAG, "SIP listening on UDP %d", SIP_LOCAL_PORT);

    struct timeval timeout = {
        .tv_sec = 300,
        .tv_usec = 0
    };

    setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout));

    /* ================= FIRST REGISTER ================= */

    char register_msg[512];

    snprintf(register_msg, sizeof(register_msg),
        "REGISTER sip:%s SIP/2.0\r\n"
        "Via: SIP/2.0/UDP %s:%d\r\n"
        "From: <sip:%s@%s>;tag=1234\r\n"
        "To: <sip:%s@%s>\r\n"
        "Call-ID: esp32-0001\r\n"
        "CSeq: 1 REGISTER\r\n"
        "Contact: <sip:%s@%s:%d>\r\n"
        "Expires: 300\r\n"
        "Content-Length: 0\r\n"
        "\r\n",
        ASTERISK_IP,
        s_sip_local_ip,
        SIP_LOCAL_PORT,
        SIP_USER,
        ASTERISK_IP,
        SIP_USER,
        ASTERISK_IP,
        SIP_USER,
        s_sip_local_ip,
        SIP_LOCAL_PORT);

    ESP_LOGI(TAG, "Sending REGISTER...");
    sendto(sock, register_msg, strlen(register_msg), 0,
           (struct sockaddr*)&server, sizeof(server));

    /* ================= RECEIVE 401 / 200 ================= */

    char response[1024];
    socklen_t addr_len = sizeof(server);
    bool registered = false;

    while (!registered) {
        int len = recvfrom(sock, response, sizeof(response) - 1, 0,
                           (struct sockaddr *)&server, &addr_len);

        if (len <= 0) {
            ESP_LOGE(TAG, "No response");
            close(sock);
            vTaskDelete(NULL);
            return;
        }

        response[len] = '\0';
        // printf("SIP RESPONSE:\n%s\n", response);
        ESP_LOGI(TAG, "Received SIP response (%d bytes)", len);

        if (strstr(response, "SIP/2.0 401")) {
            char sip_nonce[256] = {0};
            char sip_realm[128] = {0};

            char *n = strstr(response, "nonce=\"");
            if (n) {
                n += 7;
                char *end = strchr(n, '"');
                if (end) {
                    size_t nlen = end - n;
                    if (nlen < sizeof(sip_nonce)) {
                        strncpy(sip_nonce, n, nlen);
                        sip_nonce[nlen] = '\0';
                    }
                }
            }

            char *r = strstr(response, "realm=\"");
            if (r) {
                r += 7;
                char *rend = strchr(r, '"');
                if (rend) {
                    size_t rlen = rend - r;
                    if (rlen < sizeof(sip_realm)) {
                        strncpy(sip_realm, r, rlen);
                        sip_realm[rlen] = '\0';
                    }
                }
            }

            if (sip_realm[0] == '\0' || sip_nonce[0] == '\0') {
                ESP_LOGE(TAG, "Missing realm or nonce in challenge");
                close(sock);
                vTaskDelete(NULL);
                return;
            }

            ESP_LOGI(TAG, "Nonce: %s", sip_nonce);
            ESP_LOGI(TAG, "Realm: %s", sip_realm);

            char response_hash[64];
            char sip_uri[64];
            snprintf(sip_uri, sizeof(sip_uri), "sip:%s", ASTERISK_IP);

            sip_compute_response(
                SIP_USER,
                sip_realm,
                SIP_PASSWORD,
                sip_nonce,
                sip_uri,
                response_hash
            );

            char auth[1024];
            snprintf(auth, sizeof(auth),
                "REGISTER sip:%s SIP/2.0\r\n"
                "Via: SIP/2.0/UDP %s:%d\r\n"
                "From: <sip:%s@%s>;tag=1234\r\n"
                "To: <sip:%s@%s>\r\n"
                "Call-ID: esp32-0001\r\n"
                "CSeq: 2 REGISTER\r\n"
                "Contact: <sip:%s@%s:%d>\r\n"
                "Authorization: Digest username=\"%s\", realm=\"%s\", nonce=\"%s\", uri=\"sip:%s\", response=\"%s\"\r\n"
                "Expires: 300\r\n"
                "Content-Length: 0\r\n"
                "\r\n",
                ASTERISK_IP,
                s_sip_local_ip,
                SIP_LOCAL_PORT,
                SIP_USER,
                ASTERISK_IP,
                SIP_USER,
                ASTERISK_IP,
                SIP_USER,
                s_sip_local_ip,
                SIP_LOCAL_PORT,
                SIP_USER,
                sip_realm,
                sip_nonce,
                ASTERISK_IP,
                response_hash);

            ESP_LOGI(TAG, "Sending AUTH REGISTER...");
            sendto(sock, auth, strlen(auth), 0,
                   (struct sockaddr *)&server, sizeof(server));
        }
        else if (strstr(response, "SIP/2.0 200")) {
            registered = true;
        }
        else if (strstr(response, "SIP/2.0 486")) {
            ESP_LOGW(TAG, "Server returned 486 Busy");
        }
        else {
            ESP_LOGI(TAG, "Ignoring SIP response");
        }
    }

    if (!registered) {
        ESP_LOGW(TAG, "Registration was not completed");
        close(sock);
        vTaskDelete(NULL);
        return;
    }

    ESP_LOGI(TAG, "🎉 SIP REGISTER SUCCESS");

    /* =====================================================
     * KEEP SOCKET OPEN AND WAIT FOR SIP REQUESTS
     * ===================================================== */

    ESP_LOGI(TAG, "Waiting for incoming SIP requests...");

    while (1) {

        char rx_buf[2048];

        int rx_len = recvfrom(sock,
                              rx_buf,
                              sizeof(rx_buf) - 1,
                              0,
                              (struct sockaddr *)&server,
                              &addr_len);

        if (rx_len <= 0) {
            continue;
        }

        rx_buf[rx_len] = '\0';

        // printf("\n========================================\n");
        // printf("SIP MESSAGE RECEIVED:\n");
        // printf("%s\n", rx_buf);
        ESP_LOGI(TAG, "Received SIP packet (%d bytes)", rx_len);
        // printf("========================================\n");

        if (strncmp(rx_buf, "REGISTER ", 9) == 0) {
            ESP_LOGI(TAG, "Received REGISTER; sending 200 OK");
            sip_send_200_ok(sock, &server, rx_buf, NULL, 0);
            continue;
        }
        else if (strncmp(rx_buf, "INVITE ", 7) == 0) {

    ESP_LOGI(TAG, "📞 Incoming INVITE");

    /* ================= EXTRACT SIP HEADERS ================= */

    char via[512] = {0};
    char from[256] = {0};
    char to[256] = {0};
    char callid[256] = {0};
    char cseq[128] = {0};

        if (!sip_extract_header(rx_buf, "Via:", via, sizeof(via)) ||
         !sip_extract_header(rx_buf, "From:", from, sizeof(from)) ||
         !sip_extract_header(rx_buf, "To:", to, sizeof(to)) ||
         !sip_extract_header(rx_buf, "Call-ID:", callid, sizeof(callid)) ||
         !sip_extract_header(rx_buf, "CSeq:", cseq, sizeof(cseq))) {
         ESP_LOGW(TAG, "INVITE missing required headers");
         continue;
        }

    /* ================= SEND 100 TRYING ================= */

    char trying[4096];

    snprintf(trying, sizeof(trying),
        "SIP/2.0 100 Trying\r\n"
        "Via: %s\r\n"
        "From: %s\r\n"
        "To: %s\r\n"
        "Call-ID: %s\r\n"
        "CSeq: %s\r\n"
        "Content-Length: 0\r\n"
        "\r\n",
        via,
        from,
        to,
        callid,
        cseq);

    sendto(sock,
           trying,
           strlen(trying),
           0,
           (struct sockaddr *)&server,
           sizeof(server));

        ESP_LOGI(TAG, "100 Trying sent");

    #if SIP_SEND_RINGING
    char ringing[2048];

    snprintf(ringing, sizeof(ringing),
        "SIP/2.0 180 Ringing\r\n"
        "Via: %s\r\n"
        "From: %s\r\n"
        "To: %s;tag=esp32\r\n"
        "Call-ID: %s\r\n"
        "CSeq: %s\r\n"
        "Contact: <sip:%s@%s:%d>\r\n"
        "Content-Length: 0\r\n"
        "\r\n",
        via,
        from,
        to,
        callid,
        cseq,
        SIP_USER,
        s_sip_local_ip,
        SIP_LOCAL_PORT);

    sendto(sock,
           ringing,
           strlen(ringing),
           0,
           (struct sockaddr *)&server,
           sizeof(server));

    ESP_LOGI(TAG, "180 Ringing sent");
    #endif

    char ok[4096];
    char sdp[1024];
    char peer_ip[32] = {0};

    sip_build_sdp(sdp, sizeof(sdp));

    if (sip_extract_rtp_port(rx_buf, &s_rtp_peer_port)) {
        s_rtp_peer = server;
    }

    inet_ntop(AF_INET,
              &s_rtp_peer.sin_addr,
              peer_ip,
              sizeof(peer_ip));

    ESP_LOGI(TAG,
             "INVITE SDP peer %s:%u, local SDP %s:%d",
             peer_ip,
             (unsigned)s_rtp_peer_port,
             s_sip_local_ip,
             RTP_PORT);

snprintf(ok, sizeof(ok),
    "SIP/2.0 200 OK\r\n"
    "Via: %s\r\n"
    "From: %s\r\n"
    "To: %s;tag=esp32\r\n"
    "Call-ID: %s\r\n"
    "CSeq: %s\r\n"
    "Contact: <sip:%s@%s:%d>\r\n"
    "Content-Type: application/sdp\r\n"
    "Content-Length: %d\r\n"
    "\r\n"
    "%s",
    via,
    from,
    to,
    callid,
    cseq,
    SIP_USER,
    s_sip_local_ip,
    SIP_LOCAL_PORT,
    strlen(sdp),
    sdp);

sendto(sock,
       ok,
       strlen(ok),
       0,
       (struct sockaddr *)&server,
       sizeof(server));

ESP_LOGI(TAG, "200 OK sent");
}
        else if (strncmp(rx_buf, "OPTIONS ", 8) == 0) {

            ESP_LOGI(TAG, "Received OPTIONS");
        }
        else if (strncmp(rx_buf, "ACK ", 4) == 0) {
            ESP_LOGI(TAG, "Received ACK");
            s_call_active = true;
            sip_start_media_tasks();
        }
        else if (strncmp(rx_buf, "BYE ", 4) == 0) {

            ESP_LOGI(TAG, "Received BYE");
            sip_stop_media_tasks();
            sip_send_200_ok(sock, &server, rx_buf, NULL, 0);
        }
        else {

            ESP_LOGI(TAG, "Unknown SIP message");
        }
    }

    close(sock);
    vTaskDelete(NULL);
}