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 "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.67"
#define RTP_PORT 4000

#define WIFI_SSID ""
#define WIFI_PASS ""

#define ASTERISK_IP ""
#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 8000
#define SAMPLES_PER_PACKET 160   // 20 ms
#define TONE_FREQ 1000


#define MIC_FRAME_SAMPLES 320
#define AEC_MAX_FRAME_SAMPLES 512
#define FAR_END_FIFO_SAMPLES 2048

static int32_t mic_buffer[320];
static int16_t mic_pcm16[SAMPLES_PER_PACKET];
static uint8_t payload[SAMPLES_PER_PACKET];

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 int16_t far_end_fifo[FAR_END_FIFO_SAMPLES];
static size_t far_end_read = 0;
static size_t far_end_write = 0;
static portMUX_TYPE far_end_mux = portMUX_INITIALIZER_UNLOCKED;
static int16_t aec_mic_pending[AEC_MAX_FRAME_SAMPLES];
static int16_t aec_ref_pending[AEC_MAX_FRAME_SAMPLES];
static size_t aec_pending_samples = 0;
static int16_t aec_output_fifo[FAR_END_FIFO_SAMPLES];
static size_t aec_output_samples = 0;

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 far_end_push(const int16_t *samples, size_t count)
{
    portENTER_CRITICAL(&far_end_mux);
    for (size_t i = 0; i < count; ++i) {
        size_t next = (far_end_write + 1) % FAR_END_FIFO_SAMPLES;
        if (next == far_end_read) {
            far_end_read = (far_end_read + 1) % FAR_END_FIFO_SAMPLES;
        }
        far_end_fifo[far_end_write] = samples[i];
        far_end_write = next;
    }
    portEXIT_CRITICAL(&far_end_mux);
}

static void far_end_pop(int16_t *samples, size_t count)
{
    portENTER_CRITICAL(&far_end_mux);
    for (size_t i = 0; i < count; ++i) {
        if (far_end_read == far_end_write) {
            samples[i] = 0;
        } else {
            samples[i] = far_end_fifo[far_end_read];
            far_end_read = (far_end_read + 1) % FAR_END_FIFO_SAMPLES;
        }
    }
    portEXIT_CRITICAL(&far_end_mux);
}

static bool aec_process_capture(const int16_t *near_end,
                                const int16_t *far_end,
                                size_t sample_count)
{
    int frame_size = bsp_aec_get_frame_size();
    if (frame_size <= 0 || frame_size > AEC_MAX_FRAME_SAMPLES) {
        return false;
    }

    for (size_t i = 0; i < sample_count; ++i) {
        if (aec_pending_samples == AEC_MAX_FRAME_SAMPLES) {
            return false;
        }
        aec_mic_pending[aec_pending_samples] = near_end[i];
        aec_ref_pending[aec_pending_samples] = far_end[i];
        ++aec_pending_samples;

        if (aec_pending_samples == (size_t)frame_size) {
            int16_t processed[AEC_MAX_FRAME_SAMPLES];
            if (bsp_aec_process_frame(aec_mic_pending,
                                      aec_ref_pending,
                                      processed) != ESP_OK) {
                return false;
            }

            /* AEC runs at 16 kHz. The negotiated PCMU stream is 8 kHz,
             * so retain every second cleaned sample for RTP encoding. */
            for (int sample = 0; sample < frame_size; sample += 2) {
                if (aec_output_samples < FAR_END_FIFO_SAMPLES) {
                    aec_output_fifo[aec_output_samples++] = processed[sample];
                }
            }

            aec_pending_samples = 0;
        }
    }
    return true;
}

static bool aec_output_pop(int16_t *samples, size_t count)
{
    if (aec_output_samples < count) {
        return false;
    }
    memcpy(samples, aec_output_fifo, count * sizeof(int16_t));
    memmove(aec_output_fifo,
            aec_output_fifo + count,
            (aec_output_samples - count) * sizeof(int16_t));
    aec_output_samples -= 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) {
        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 microphone_read(int16_t *samples, size_t count)
{
    // ESP_LOGI(TAG,
    //          "mic ptr=%p samples=%d bytes=%d",
    //          samples,
    //          count,
    //          count * sizeof(int16_t));

    esp_err_t ret = esp_get_feed_data(false, samples, count);





    


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

    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 uint32_t call_count = 0;
/*
ESP_LOGI(TAG,
         "sip_send_rtp_packet() #%lu",
         (unsigned long)call_count++);*/

         
    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_peer_port == 0)
{
    ESP_LOGW(TAG,
             "No RTP peer port set!");
    return;
}

    if(s_rtp_sock < 0)
    {
        s_rtp_sock = socket(AF_INET,
                    SOCK_DGRAM,
                    IPPROTO_UDP);

                    ESP_LOGI(TAG,
         "RTP socket=%d",
         s_rtp_sock);
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 = INADDR_ANY;

if(bind(s_rtp_sock,
        (struct sockaddr *)&local_rtp,
        sizeof(local_rtp)) < 0)
{
    ESP_LOGE(TAG, "RTP bind failed errno=%d", errno);
}
else
{
    ESP_LOGI(TAG,
             "RTP listening on port %d",
             RTP_PORT);
}

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



    uint8_t payload[SAMPLES_PER_PACKET];


    // Read one 20 ms stereo capture block (320 mono samples at 16 kHz).
    if (!microphone_read((int16_t *)mic_buffer, sizeof(mic_buffer)))
{
    return;
}
    int16_t *mic16 = (int16_t *)mic_buffer;
    int16_t near_end[MIC_FRAME_SAMPLES];
    int16_t far_end[MIC_FRAME_SAMPLES];

    for (int i = 0; i < MIC_FRAME_SAMPLES; ++i) {
        int32_t left = mic16[i * 2];
        int32_t right = mic16[i * 2 + 1];
        near_end[i] = (int16_t)((left + right) / 2);
    }

    far_end_pop(far_end, MIC_FRAME_SAMPLES);
    if (!aec_process_capture(near_end, far_end, MIC_FRAME_SAMPLES) ||
        !aec_output_pop(mic_pcm16, SAMPLES_PER_PACKET)) {
        return;
    }


// Debug microphone level
int max = 0;

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

    if(value > max)
        max = value;
}
/*
ESP_LOGI("MIC", "max=%d", max);*/


// PCM16 -> PCMU (G711 u-law)
for(int i = 0; i < SAMPLES_PER_PACKET; i++)
{
    payload[i] = linear2ulaw(mic_pcm16[i]);
}



//testing multiple packets
/*
ESP_LOGI("MIC",
         "%6d %6d %6d %6d %6d %6d %6d %6d "
         "%6d %6d %6d %6d %6d %6d %6d %6d "
         "%6d %6d %6d %6d",
         mic_pcm16[0], mic_pcm16[1], mic_pcm16[2], mic_pcm16[3],
         mic_pcm16[4], mic_pcm16[5], mic_pcm16[6], mic_pcm16[7],
         mic_pcm16[8], mic_pcm16[9], mic_pcm16[10], mic_pcm16[11],
         mic_pcm16[12], mic_pcm16[13], mic_pcm16[14], mic_pcm16[15],
         mic_pcm16[16], mic_pcm16[17], mic_pcm16[18], mic_pcm16[19]);
*/

    uint8_t packet[12 + SAMPLES_PER_PACKET];


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


    packet[0] = 0x80;       // RTP version
    packet[1] = 0x00;       // PCMU


    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,
           SAMPLES_PER_PACKET);



    struct sockaddr_in peer = s_rtp_peer;

peer.sin_port = htons(s_rtp_peer_port);
/*ESP_LOGI(TAG,
         "RTP destination IP: %s",
         inet_ntoa(peer.sin_addr));

ESP_LOGI(TAG,
         "Sending RTP to %s:%d",
         inet_ntoa(peer.sin_addr),
         ntohs(peer.sin_port));*/

esp_netif_ip_info_t ip;
esp_netif_t *netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF");

esp_netif_get_ip_info(netif, &ip);

/*ESP_LOGI(TAG, "Current IP: " IPSTR, IP2STR(&ip.ip));*/
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);
}
else
{
   /* ESP_LOGI(TAG,
             "RTP sent bytes=%d seq=%d",
             ret,
             seq);*/
}

    /*if(ret > 0)
    {
        ESP_LOGI(TAG,
                 "RTP sent seq=%d",
                 seq);
    }*/


    seq++;

    timestamp += SAMPLES_PER_PACKET;
}



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








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


    // TEMP: create RTP socket for RX-only testing
    if (s_rtp_sock < 0)
    {
        s_rtp_sock = socket(AF_INET,
                            SOCK_DGRAM,
                            IPPROTO_UDP);

        if (s_rtp_sock < 0)
        {
            ESP_LOGE(TAG,
                     "RTP socket failed errno=%d",
                     errno);
            vTaskDelete(NULL);
            return;
        }


        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 = 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;

            vTaskDelete(NULL);
            return;
        }


        ESP_LOGI(TAG,
                 "RTP RX listening on port %d",
                 RTP_PORT);
    }


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




//comment this block out for testing rx only
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;
}

if (s_rtp_peer_port != 0 &&
    (src.sin_port != htons(s_rtp_peer_port) ||
     src.sin_addr.s_addr != s_rtp_peer.sin_addr.s_addr))
{
    ESP_LOGW(TAG,
             "Ignoring RTP from unexpected peer %s:%d",
             inet_ntoa(src.sin_addr),
             ntohs(src.sin_port));
    continue;
}

/* Ignore packets that are clearly looped back to the local device. */
esp_netif_ip_info_t local_ip;
esp_netif_t *netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF");
esp_netif_get_ip_info(netif, &local_ip);
if (src.sin_addr.s_addr == local_ip.ip.addr)
{
    ESP_LOGW(TAG, "Ignoring looped RTP packet from local IP");
    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 far_end_ref[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;
    }

    // sample 1
    play_buf[4*i + 0] = s1; // Left
    play_buf[4*i + 1] = s1; // Right

    // interpolated sample
    play_buf[4*i + 2] = s2; // Left
    play_buf[4*i + 3] = s2; // Right

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

    // The reference must be the mono 16 kHz signal sent to the speaker,
    // before it is duplicated into the codec's stereo output.
    far_end_push(far_end_ref, payload_size * 2);

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, "esp_audio_play() returned %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);
}

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)
{
    call_active = true;

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

        xTaskCreate(
    rtp_rx_task,
    "rtp_rx",
    16384,
    NULL,
    5,
    NULL);
//temp comment out
    //ESP_LOGI(TAG, "rtp task create result=%d", 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;
    }

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