2_way_Tochie/main/sip/sip.c

// in Asterisk pjsip.conf, under 1001, set direct_media=no

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <math.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 "bsp_board.h"
#include "tca9555_driver.h"

#include "freertos/semphr.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.69"
#define RTP_PORT 4000

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

#define ASTERISK_IP "192.168.68.61"
#define SIP_PORT 5060
#define SIP_LOCAL_IP "0.0.0.0"
#define SIP_LOCAL_PORT 5062
#define SIP_USER "1001"
#define SIP_PASSWORD "secret123"

#define SAMPLE_RATE 16000
#define RTP_SAMPLE_RATE         8000

#define SAMPLES_PER_PACKET 160   
#define TONE_FREQ 1000

#define AEC_FRAME_SAMPLES       256
//#define AEC_OUTPUT_SAMPLES      320
#define RTP_SAMPLES             160

#define MIC_BLOCK_SAMPLES       320       
#define REF_BLOCK_SAMPLES       320       


#define REF_BUFFER_SAMPLES      4096
#define MIC_BUFFER_SAMPLES      2048
//
#define MIC_FRAME_SAMPLES       160

#define REF_DELAY_SAMPLES 320
#define AEC_DELAY_SAMPLES       320     

static size_t ref_delay_count = AEC_DELAY_SAMPLES;
static uint64_t mic_total_processed = 0;
static uint64_t ref_total_written = 0;

static int16_t ref_delay_buffer[AEC_DELAY_SAMPLES];


static int16_t ref_buffer[REF_BUFFER_SAMPLES];
static size_t ref_write = 0;
static size_t ref_read = 0;
static size_t ref_count = 0;

static size_t ref_delay_write = 0;
//static size_t ref_delay_count = 0;

static int16_t mic_aec_buffer[AEC_FRAME_SAMPLES];
static int16_t ref_aec_buffer[AEC_FRAME_SAMPLES];

static int16_t aec_output_buffer[MIC_BUFFER_SAMPLES];
static size_t aec_output_count = 0;

static int32_t mic_buffer[320];

static int16_t mic_pcm16[RTP_SAMPLES];

static SemaphoreHandle_t aec_mutex = NULL;
static int16_t ref_pending[AEC_FRAME_SAMPLES];

static size_t ref_pending_count = 0;

#include "esp_afe_config.h"
#include "esp_afe_sr_iface.h"
#include "esp_afe_sr_models.h"
#include "esp_afe_aec.h"

static int16_t mic_fifo[MIC_BUFFER_SAMPLES];
static int16_t ref_fifo[MIC_BUFFER_SAMPLES];

static size_t mic_fifo_count = 0;
static size_t ref_fifo_count = 0;


static uint8_t payload[MIC_FRAME_SAMPLES];

static uint8_t rx_packet[172];
static int16_t speaker_pcm[160];

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 volatile bool call_active = false;
static volatile bool s_audio_hw_ready = false;

static afe_aec_handle_t *aec_handle = NULL;

esp_err_t esp_get_feed_data(
    bool is_get_raw_channel,
    int16_t *buffer,
    int buffer_len);

    esp_err_t esp_audio_play(const int16_t *data,
                         int length,
                         uint32_t ticks_to_wait);
esp_err_t sip_media_init(void);

static void rtp_tx_task(void *pvParameters);
static void rtp_rx_task(void *pvParameters);
static void s_aec_process(
    int16_t *mic,
    int16_t *ref
    //int samples
);
static bool aec_reference_get(
    int16_t *out,
    size_t count
);

/* ================= AEC ================= */
static bool aec_reference_push(
    const int16_t *samples,
    size_t count)
{
    if (samples == NULL || count == 0)
        return false;

    
    if (ref_count + count > REF_BUFFER_SAMPLES)
    {
        ESP_LOGE(
            TAG,
            "AEC REF OVERFLOW: count=%u incoming=%u",
            (unsigned)ref_count,
            (unsigned)count
        );

        return false;
    }

    for (size_t i = 0; i < count; i++)
    {
        ref_buffer[ref_write] = samples[i];

        ref_write++;

        if (ref_write >= REF_BUFFER_SAMPLES)
            ref_write = 0;

        ref_count++;
        ref_total_written++;
    }

    ESP_LOGI(
        TAG,
        "AEC REF PUSH: count=%u fifo=%u total=%llu",
        (unsigned)count,
        (unsigned)ref_count,
        (unsigned long long)ref_total_written
    );

    return true;
}

static bool aec_reference_get(
    int16_t *out,
    size_t count)
{
   
        size_t required =
        AEC_DELAY_SAMPLES + count;

    if (ref_count < AEC_DELAY_SAMPLES + count)
    {
        return false;
    }

    for (size_t i = 0; i < count; i++)
    {
        out[i] = ref_buffer[ref_read];

        ref_read++;

        if (ref_read >= REF_BUFFER_SAMPLES)
            ref_read = 0;
    }

    ref_count -= count;

    return true;
}

bool aec_init(void)
{
    memset(ref_buffer, 0, sizeof(ref_buffer));

    ref_write = 0;
    ref_read = 0;
    ref_count = 0;

ref_pending_count = 0;
memset(ref_pending, 0, sizeof(ref_pending));

    ref_total_written = 0;
    mic_total_processed = 0;

    aec_output_count = 0;

    aec_handle = afe_aec_create(
        "MR",
        4,
        AFE_TYPE_VC,
        AFE_MODE_HIGH_PERF
    );

    if (aec_handle == NULL)
    {
        ESP_LOGE(TAG, "AEC init failed");
        return false;
    }

    ESP_LOGI(
        TAG,
        "AEC initialized: frame_size=%d sample_rate=%d",
        aec_handle->frame_size,
        SAMPLE_RATE
    );

    if (aec_handle->frame_size != AEC_FRAME_SAMPLES)
    {
        ESP_LOGE(
            TAG,
            "AEC frame mismatch: handle=%d expected=%d",
            aec_handle->frame_size,
            AEC_FRAME_SAMPLES
        );

        afe_aec_destroy(aec_handle);
        aec_handle = NULL;

        return false;
    }

    return true;
}

static void s_aec_process(
    int16_t *mic,
    int16_t *ref)
{
    if (aec_handle == NULL)
        return;

    int16_t input[AEC_FRAME_SAMPLES * 2];
    int16_t output[AEC_FRAME_SAMPLES];

    for (int i = 0; i < AEC_FRAME_SAMPLES; i++)
    {
        input[2 * i]     = mic[i];
        input[2 * i + 1] = ref[i];
    }

    afe_aec_process(
        aec_handle,
        input,
        output
    );

    if (aec_output_count + AEC_FRAME_SAMPLES >
        MIC_BUFFER_SAMPLES)
    {
        ESP_LOGE(
            TAG,
            "AEC OUTPUT FIFO OVERFLOW: "
            "count=%u adding=%u max=%u",
            (unsigned)aec_output_count,
            (unsigned)AEC_FRAME_SAMPLES,
            (unsigned)MIC_BUFFER_SAMPLES
        );

        return;
    }

    memcpy(
        &aec_output_buffer[aec_output_count],
        output,
        AEC_FRAME_SAMPLES * sizeof(int16_t)
    );

    aec_output_count += AEC_FRAME_SAMPLES;
}


void aec_deinit()
{
    if(aec_handle)
    {
        afe_aec_destroy(aec_handle);
        aec_handle=NULL;
    }
}


static bool process_aec_320(
    const int16_t *mic320,
    int16_t *out320)
{
    static int16_t mic_pending[AEC_FRAME_SAMPLES];
    static size_t mic_pending_count = 0;

    static int16_t ref_pending[AEC_FRAME_SAMPLES];
static size_t ref_pending_count = 0;

    size_t input_pos = 0;

    while (input_pos < MIC_BLOCK_SAMPLES)
    {
        if (mic_pending_count == AEC_FRAME_SAMPLES)
        {
            int16_t ref_frame[AEC_FRAME_SAMPLES];

            if (!aec_reference_get(
                    ref_frame,
                    AEC_FRAME_SAMPLES))
            {
                ESP_LOGW(
                    TAG,
                    "AEC waiting for reference: "
                    "fifo=%u need=%u",
                    (unsigned)ref_count,
                    (unsigned)(
                        AEC_DELAY_SAMPLES +
                        AEC_FRAME_SAMPLES)
                );

                break;
            }

            ESP_LOGI(
                TAG,
                "AEC PROCESS: mic=256 ref=256 "
                "ref_remaining=%u",
                (unsigned)ref_count
            );

            s_aec_process(
                mic_pending,
                ref_frame
            );

            mic_total_processed += AEC_FRAME_SAMPLES;

            mic_pending_count = 0;

            continue;
        }

        size_t needed =
            AEC_FRAME_SAMPLES -
            mic_pending_count;

        size_t available =
            MIC_BLOCK_SAMPLES -
            input_pos;

        size_t copy_count =
            (needed < available)
                ? needed
                : available;

        memcpy(
            &mic_pending[mic_pending_count],
            &mic320[input_pos],
            copy_count * sizeof(int16_t)
        );

        mic_pending_count += copy_count;
        input_pos += copy_count;
    }

    if (aec_output_count < MIC_BLOCK_SAMPLES)
    {
        return false;
    }

    memcpy(
        out320,
        aec_output_buffer,
        MIC_BLOCK_SAMPLES * sizeof(int16_t)
    );

    size_t remaining =
        aec_output_count -
        MIC_BLOCK_SAMPLES;

    if (remaining > 0)
    {
        memmove(
            aec_output_buffer,
            &aec_output_buffer[MIC_BLOCK_SAMPLES],
            remaining * sizeof(int16_t)
        );
    }

    aec_output_count = remaining;

    return true;
}


static void downsample_16k_to_8k(
    const int16_t *input,
    int16_t *output)
{
    for (int i = 0; i < 160; i++)
    {
        
        output[i] = input[i * 2];
    }
}



/* ================= 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) {
        ESP_LOGI(TAG, "WiFi connected!");
        wifi_ready = true;
    }

    esp_netif_ip_info_t ip;
esp_netif_get_ip_info(esp_netif_get_handle_from_ifkey("WIFI_STA_DEF"), &ip);

ESP_LOGI(TAG, "ESP32 IP: " IPSTR, IP2STR(&ip.ip));
}

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", 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,
        ESP32_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",
        ESP32_IP,
        ESP32_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;

    
}

/* ================= RTP TX ================= */

static bool rtp_socket_init(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,
            "RTP socket failed errno=%d",
            errno
        );

        return false;
    }

    struct sockaddr_in local_rtp = {0};

    local_rtp.sin_family = AF_INET;
    local_rtp.sin_port = htons(RTP_PORT);
    local_rtp.sin_addr.s_addr = htonl(INADDR_ANY);

    if (bind(
            s_rtp_sock,
            (struct sockaddr *)&local_rtp,
            sizeof(local_rtp)) < 0)
    {
        ESP_LOGE(
            TAG,
            "RTP bind failed errno=%d",
            errno
        );

        close(s_rtp_sock);
        s_rtp_sock = -1;

        return false;
    }

    ESP_LOGI(
        TAG,
        "RTP socket ready on port %d",
        RTP_PORT
    );

    return true;
}

static bool microphone_read_320(int16_t *out)
{
    int16_t buffer1[320];
    int16_t buffer2[320];

    esp_err_t ret;

    ret = esp_get_feed_data(
        false,
        buffer1,
        sizeof(buffer1));

    if (ret != ESP_OK)
    {
        ESP_LOGE(TAG,
                 "mic read 1 failed: %s",
                 esp_err_to_name(ret));
        return false;
    }

   ret = esp_get_feed_data(
    false,
    buffer2,
    sizeof(buffer2));

if (ret != ESP_OK)
{
    ESP_LOGE(TAG,
             "mic read 2 failed: %s",
             esp_err_to_name(ret));
    return false;
}


/* ================= MIC CHANNEL DEBUG ================= */

int max_l = 0;
int max_r = 0;

for (int i = 0; i < 160; i++)
{
    int l = abs(buffer1[i * 2]);
    int r = abs(buffer1[i * 2 + 1]);

    if (l > max_l)
        max_l = l;

    if (r > max_r)
        max_r = r;
}

ESP_LOGI(TAG,
         "MIC channels: L=%d R=%d",
         max_l,
         max_r);

    for (int i = 0; i < 160; i++)
    {
        out[i] = buffer1[i * 2];
        out[160 + i] = buffer2[i * 2];
    }

    return true;
}


static uint8_t linear2ulaw(int16_t pcm)
{
    const int16_t BIAS = 0x84;
    const int16_t CLIP = 32635;

    uint8_t mask;
    uint8_t seg;
    uint8_t uval;

    int16_t sample = pcm;

    if (sample < 0)
    {
        sample = -sample;
        mask = 0x7F;
    }
    else
    {
        mask = 0xFF;
    }

    if (sample > CLIP)
        sample = CLIP;

    sample += BIAS;


    if (sample <= 0xFF)
        seg = 0;
    else if (sample <= 0x1FF)
        seg = 1;
    else if (sample <= 0x3FF)
        seg = 2;
    else if (sample <= 0x7FF)
        seg = 3;
    else if (sample <= 0xFFF)
        seg = 4;
    else if (sample <= 0x1FFF)
        seg = 5;
    else if (sample <= 0x3FFF)
        seg = 6;
    else
        seg = 7;


    uval = (seg << 4) |
           ((sample >> (seg + 3)) & 0x0F);


    return uval ^ mask;
}

static int16_t ulaw2linear(uint8_t u_val)
{
    u_val = ~u_val;

    int t = ((u_val & 0x0F) << 3) + 0x84;
    t <<= ((unsigned)u_val & 0x70) >> 4;

    if (u_val & 0x80)
        return 0x84 - t;
    else
        return t - 0x84;
}

static void sip_send_rtp_packet(void)
{
    static uint16_t seq = 0;
    static uint32_t timestamp = 0;

    if (s_rtp_peer_port == 0)
    {
        ESP_LOGW(TAG, "No RTP peer");
        return;
    }

    if (s_rtp_sock < 0)
{
    ESP_LOGE(TAG, "RTP socket not initialized");
    return;
}

    static int16_t mic320[320];
    static int16_t aec320[320];

  if (!microphone_read_320(mic320))
    return;

int raw_mic_max = 0;

for (int i = 0; i < 320; i++)
{
    int v = abs(mic320[i]);

    if (v > raw_mic_max)
        raw_mic_max = v;
}

ESP_LOGI(TAG,
         "MIC16 max=%d first=%d %d %d %d",
         raw_mic_max,
         mic320[0],
         mic320[1],
         mic320[2],
         mic320[3]);

xSemaphoreTake(aec_mutex, portMAX_DELAY);

    bool ready = process_aec_320(
        mic320,
        aec320);

    xSemaphoreGive(aec_mutex);

    if (!ready)
    {
        return;
    }

    downsample_16k_to_8k(
        aec320,
        mic_pcm16);

    int max = 0;

    for (int i = 0; i < RTP_SAMPLES; i++)
    {
        int value = abs(mic_pcm16[i]);

        if (value > max)
            max = value;
    }

    ESP_LOGI(
        "MIC",
        "AEC output max=%d first=%d %d %d %d",
        max,
        mic_pcm16[0],
        mic_pcm16[1],
        mic_pcm16[2],
        mic_pcm16[3]);

    
    uint8_t payload[RTP_SAMPLES];

    for (int i = 0; i < RTP_SAMPLES; i++)
    {
        payload[i] = linear2ulaw(
            mic_pcm16[i]);
    }

    uint8_t packet[12 + RTP_SAMPLES];

    memset(packet, 0, sizeof(packet));

    packet[0] = 0x80;
    packet[1] = 0x00;

    packet[2] = seq >> 8;
    packet[3] = seq & 0xff;

    packet[4] = timestamp >> 24;
    packet[5] = timestamp >> 16;
    packet[6] = timestamp >> 8;
    packet[7] = timestamp;

    uint32_t ssrc = 0x12345678;

    packet[8]  = (ssrc >> 24) & 0xff;
    packet[9]  = (ssrc >> 16) & 0xff;
    packet[10] = (ssrc >> 8) & 0xff;
    packet[11] = ssrc & 0xff;

    memcpy(
        packet + 12,
        payload,
        RTP_SAMPLES);

    struct sockaddr_in peer = s_rtp_peer;

    peer.sin_port = htons(
        s_rtp_peer_port);

    int ret = sendto(
        s_rtp_sock,
        packet,
        sizeof(packet),
        0,
        (struct sockaddr *)&peer,
        sizeof(peer));

    if (ret < 0)
    {
        ESP_LOGE(
            TAG,
            "RTP send failed errno=%d",
            errno);
    }

    seq++;

    timestamp += RTP_SAMPLES;
}



static void rtp_tx_task(void *pvParameters)
{
    ESP_LOGI(TAG, "RTP TX started");

    TickType_t last_wake = xTaskGetTickCount();

while(call_active)
{/*ESP_LOGI(TAG, "RTP loop");*/
    sip_send_rtp_packet();

    vTaskDelayUntil(
        &last_wake,
        pdMS_TO_TICKS(20)
    );
}
    ESP_LOGI(TAG, "RTP TX stopped");

    vTaskDelete(NULL);
}

// ================= RTP RX =================

static void rtp_rx_task(void *pvParameters)
{
    ESP_LOGI(TAG, "RTP RX started");

    while (call_active)
    {
        struct sockaddr_in src;
        socklen_t len = sizeof(src);

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

        if (n < 0)
        {
            ESP_LOGE(TAG,
                     "recvfrom failed errno=%d",
                     errno);
            continue;
        }
/*
        ESP_LOGI(TAG,
                 "Received RTP packet (%d bytes)",
                 n);
*/
        if (n <= 12)
        {
            ESP_LOGW(TAG, "Packet too small");
            continue;
        }
/*
        ESP_LOGI(TAG,
                 "RTP Header: %02X %02X %02X %02X",
                 rx_packet[0],
                 rx_packet[1],
                 rx_packet[2],
                 rx_packet[3]);

        ESP_LOGI(TAG,
                 "RTP Payload: %02X %02X %02X %02X %02X %02X %02X %02X",
                 rx_packet[12],
                 rx_packet[13],
                 rx_packet[14],
                 rx_packet[15],
                 rx_packet[16],
                 rx_packet[17],
                 rx_packet[18],
                 rx_packet[19]);
*/
        int payload_size = n - 12;

        if (payload_size > SAMPLES_PER_PACKET)
        {
            payload_size = SAMPLES_PER_PACKET;
        }

        int16_t play_buf[640];
       
        int16_t ref_frame[320];

        for (int i = 0; i < payload_size; i++)
        {
            int16_t s1 =
                ulaw2linear(rx_packet[12 + i]);

            int16_t s2 = s1;

            if (i < payload_size - 1)
            {
                int16_t next =
                    ulaw2linear(
                        rx_packet[12 + i + 1]);

                s2 = (s1 + next) / 2;
            }
            play_buf[4 * i + 0] = s1;
            play_buf[4 * i + 1] = s1;

            play_buf[4 * i + 2] = s2;
            play_buf[4 * i + 3] = s2;

            ref_frame[2 * i + 0] = s1;
            ref_frame[2 * i + 1] = s2;
        }


xSemaphoreTake(
    aec_mutex,
    portMAX_DELAY);

size_t ref_samples = payload_size * 2;

size_t ref_pos = 0;

while (ref_pos < ref_samples)
{
    size_t needed =
        AEC_FRAME_SAMPLES - ref_pending_count;

    size_t available =
        ref_samples - ref_pos;

    size_t copy_count =
        (needed < available)
            ? needed
            : available;

    memcpy(
        &ref_pending[ref_pending_count],
        &ref_frame[ref_pos],
        copy_count * sizeof(int16_t)
    );

    ref_pending_count += copy_count;
    ref_pos += copy_count;

    if (ref_pending_count == AEC_FRAME_SAMPLES)
    {
        aec_reference_push(
            ref_pending,
            AEC_FRAME_SAMPLES
        );

        ref_pending_count = 0;
    }
}

uint64_t ref_written =
    ref_total_written;

size_t ref_level =
    ref_count;

xSemaphoreGive(
    aec_mutex);

    ESP_LOGI(
    TAG,
    "AEC REF: pushed=%d fifo=%d",
    payload_size * 2,
    (int)ref_level);

ESP_LOGI(
    TAG,
    "AEC REF PUSH: samples=%d "
    "ref_count=%d total_written=%llu",
    payload_size * 2,
    (int)ref_level,
    (unsigned long long)ref_written);

        
        esp_err_t ret = esp_audio_play(
            play_buf,
            payload_size * 4 * sizeof(int16_t),
            portMAX_DELAY
        );
/*
        ESP_LOGI(TAG,
                 "esp_audio_play() = %s",
                 esp_err_to_name(ret));*/
    }

    ESP_LOGI(TAG, "RTP RX stopped");

    vTaskDelete(NULL);
}

/* ================= 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);
    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);

    if (sock < 0) {
        ESP_LOGE(TAG, "Socket failed");
        vTaskDelete(NULL);
        return;
    }

    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,
        SIP_LOCAL_IP,
        SIP_LOCAL_PORT,
        SIP_USER,
        ASTERISK_IP,
        SIP_USER,
        ASTERISK_IP,
        SIP_USER,
        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,
                SIP_LOCAL_IP,
                SIP_LOCAL_PORT,
                SIP_USER,
                ASTERISK_IP,
                SIP_USER,
                ASTERISK_IP,
                SIP_USER,
                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");
    ESP_LOGI(TAG, "stack remaining: %u",
        uxTaskGetStackHighWaterMark(NULL));

    /* =====================================================
     * 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';
        ESP_LOGI(TAG, "\n===== RAW INVITE =====\n%s\n=======================\n", rx_buf);

        // 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");
char *test = strstr(rx_buf, "m=audio");

if(test)
{
    ESP_LOGI(TAG, "Found m=audio SDP:");
    ESP_LOGI(TAG, "%s", test);
}
else
{
    ESP_LOGE(TAG, "No m=audio found");
}
    /* ================= EXTRACT SIP HEADERS ================= */

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

char *header = NULL;

header = strstr(rx_buf, "Via:");
if (header) {
    sscanf(header, "Via: %511[^\r\n]", via);
}

header = strstr(rx_buf, "From:");
if (header) {
    sscanf(header, "From: %255[^\r\n]", from);
}

header = strstr(rx_buf, "To:");
if (header) {
    sscanf(header, "To: %255[^\r\n]", to);
}

header = strstr(rx_buf, "Call-ID:");
if (header) {
    sscanf(header, "Call-ID: %255[^\r\n]", callid);
}

header = strstr(rx_buf, "CSeq:");
if (header) {
    sscanf(header, "CSeq: %127[^\r\n]", cseq);
}

    /* ================= 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");
    ESP_LOGI(TAG, "stack remaining: %u",
      uxTaskGetStackHighWaterMark(NULL));

char ok[4096];
char sdp[1024];

sip_build_sdp(sdp, sizeof(sdp));

if (sip_extract_rtp_port(rx_buf, &s_rtp_peer_port))
{
    s_rtp_peer.sin_family = AF_INET;
    s_rtp_peer.sin_port =
        htons(s_rtp_peer_port);

    s_rtp_peer.sin_addr.s_addr =
        inet_addr(ASTERISK_IP);

    ESP_LOGI(
        TAG,
        "RTP peer port=%d",
        s_rtp_peer_port
    );

    if (!rtp_socket_init())
    {
        ESP_LOGE(
            TAG,
            "RTP socket initialization failed"
        );

        continue;
    }
}



snprintf(ok, sizeof(ok),
    "SIP/2.0 200 OK\r\n"
    "Via: %s\r\n"
    "From: %s\r\n"
    "To: %s;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,
    ESP32_IP,
    SIP_LOCAL_PORT,
    strlen(sdp),
    sdp);

ESP_LOGI(TAG, "\n========== 200 OK ==========\n%s\n============================\n", ok);

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");




uint8_t test_values[] = {0xFF, 0x7F, 0x00, 0x80};

for(int i = 0; i < 4; i++)
{
    ESP_LOGI(TAG,
             "ulaw %02X -> %d",
             test_values[i],
             ulaw2linear(test_values[i]));
}



if (!call_active)
{
    if (s_rtp_sock < 0)
    {
        ESP_LOGE(
            TAG,
            "Cannot start RTP: socket not initialized"
        );

        continue;
    }

    if (s_rtp_peer_port == 0)
    {
        ESP_LOGE(
            TAG,
            "Cannot start RTP: peer port unknown"
        );

        continue;
    }

    call_active = true;

    BaseType_t tx_ret = xTaskCreate(
        rtp_tx_task,
        "rtp_tx",
        16384,
        NULL,
        5,
        NULL
    );

    BaseType_t rx_ret = xTaskCreate(
        rtp_rx_task,
        "rtp_rx",
        16384,
        NULL,
        5,
        NULL
    );

    ESP_LOGI(
        TAG,
        "RTP tasks: TX=%d RX=%d",
        tx_ret,
        rx_ret
    );
}

    



}
        else if (strncmp(rx_buf, "BYE ", 4) == 0)
{
    ESP_LOGI(TAG, "Received BYE");

    call_active = false;

    sip_send_200_ok(sock, &server, rx_buf, NULL, 0);
}
        else {

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

    close(sock);
    vTaskDelete(NULL);
}


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

    aec_mutex = xSemaphoreCreateMutex();

    if (aec_mutex == NULL)
    {
        ESP_LOGE(TAG, "AEC mutex failed");
        return ESP_FAIL;
    }

    ESP_LOGI(TAG, "AEC mutex=%p", aec_mutex);


    esp_err_t ret = esp_board_init(16000, 2, 32);



    if (ret != ESP_OK)
    {
        ESP_LOGE(TAG,
                 "esp_board_init failed: %s",
                 esp_err_to_name(ret));

        return ret;
    }


    tca9555_driver_init();

aec_init();

    s_audio_hw_ready = true;

    ESP_LOGI(TAG,
             "Audio board initialized for live RTP media");


    return ESP_OK;
}