//replace IP on line 283 with ASTERISK_IP
#include <stdio.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 "mbedtls/md5.h"

#define ESP32_IP "10.0.0.15"
#define RTP_PORT 4000

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

#define ASTERISK_IP "192.168.68.64"
#define SIP_PORT 5060

static const char *TAG = "SIP";
static volatile bool wifi_ready = false;

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

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])
{
    mbedtls_md5_context ctx;
    mbedtls_md5_init(&ctx);
    mbedtls_md5_starts(&ctx);
    mbedtls_md5_update(&ctx, input, len);
    mbedtls_md5_finish(&ctx, output);
    mbedtls_md5_free(&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 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(5062);
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 5062");
    close(sock);
    vTaskDelete(NULL);
    return;
}

ESP_LOGI(TAG, "SIP listening on UDP 5062");

    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 0.0.0.0:5062\r\n"
        "From: <sip:1001@%s>;tag=1234\r\n"
        "To: <sip:1001@%s>\r\n"
        "Call-ID: esp32-0001\r\n"
        "CSeq: 1 REGISTER\r\n"
        "Contact: <sip:1001@0.0.0.0:5062>\r\n"
        "Expires: 300\r\n"
        "Content-Length: 0\r\n"
        "\r\n",
        ASTERISK_IP,
        ASTERISK_IP,
        ASTERISK_IP);

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

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

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

    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("401 RESPONSE:\n%s\n", response);

    if (!strstr(response, "401")) {
        ESP_LOGE(TAG, "Expected 401 Unauthorized");
        close(sock);
        vTaskDelete(NULL);
        return;
    }

    /* ================= EXTRACT NONCE ================= */

    char sip_nonce[256] = {0};

    char *n = strstr(response, "nonce=\"");
    if (!n) {
        ESP_LOGE(TAG, "No nonce found");
        close(sock);
        vTaskDelete(NULL);
        return;
    }

    n += 7;

    char *end = strchr(n, '"');
    if (!end) {
        ESP_LOGE(TAG, "Bad nonce format");
        close(sock);
        vTaskDelete(NULL);
        return;
    }

    size_t nlen = end - n;

    if (nlen >= sizeof(sip_nonce)) {
        ESP_LOGE(TAG, "Nonce too long");
        close(sock);
        vTaskDelete(NULL);
        return;
    }

    strncpy(sip_nonce, n, nlen);
    sip_nonce[nlen] = '\0';

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

    /* ================= EXTRACT REALM ================= */

    char sip_realm[128] = {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') {
        ESP_LOGE(TAG, "Realm not found");
        close(sock);
        vTaskDelete(NULL);
        return;
    }

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

    /* ================= AUTH DIGEST ================= */

    char response_hash[64];

    sip_compute_response(
        "1001",
        sip_realm,
        "secret123",
        sip_nonce,
        "sip:192.168.68.64", // replace this 192.168.68.64 with ASTERISK_IP
        response_hash
    );

    /* ================= AUTH REGISTER ================= */

    char auth[1024];

    snprintf(auth, sizeof(auth),
        "REGISTER sip:%s SIP/2.0\r\n"
        "Via: SIP/2.0/UDP 0.0.0.0:5062\r\n"
        "From: <sip:1001@%s>;tag=1234\r\n"
        "To: <sip:1001@%s>\r\n"
        "Call-ID: esp32-0001\r\n"
        "CSeq: 2 REGISTER\r\n"
        "Contact: <sip:1001@0.0.0.0:5062>\r\n"
        "Authorization: Digest username=\"1001\", realm=\"%s\", nonce=\"%s\", uri=\"sip:%s\", response=\"%s\"\r\n"
        "Expires: 300\r\n"
        "Content-Length: 0\r\n"
        "\r\n",
        ASTERISK_IP,
        ASTERISK_IP,
        ASTERISK_IP,
        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));

    /* ================= FINAL RESPONSE ================= */

    /* ================= FINAL RESPONSE ================= */

    char final_resp[1024];

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

    if (len2 <= 0) {
        ESP_LOGW(TAG, "No final response");
        close(sock);
        vTaskDelete(NULL);
        return;
    }

    final_resp[len2] = '\0';

    printf("FINAL RESPONSE:\n%s\n", final_resp);

    if (!strstr(final_resp, "200 OK")) {
        ESP_LOGW(TAG, "REGISTER FAILED");
        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);
        printf("========================================\n");

        if (strstr(rx_buf, "INVITE")) {

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

    sscanf(strstr(rx_buf, "Via:"),
           "Via: %[^\r\n]",
           via);

    sscanf(strstr(rx_buf, "From:"),
           "From: %[^\r\n]",
           from);

    sscanf(strstr(rx_buf, "To:"),
           "To: %[^\r\n]",
           to);

    sscanf(strstr(rx_buf, "Call-ID:"),
           "Call-ID: %[^\r\n]",
           callid);

    sscanf(strstr(rx_buf, "CSeq:"),
           "CSeq: %[^\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");

    char ringing[2048];

snprintf(ringing, sizeof(ringing),
    "SIP/2.0 180 Ringing\r\n"
    "Via: %s\r\n"
    "From: %s\r\n"
    "To: %s\r\n"
    "Call-ID: %s\r\n"
    "CSeq: %s\r\n"
    "Contact: <sip:1001@10.0.0.15:5062>\r\n"
    "Content-Length: 0\r\n"
    "\r\n",
    via,
    from,
    to,
    callid,
    cseq);

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

ESP_LOGI(TAG, "180 Ringing sent");

char ok[2048];

const char *sdp =
"v=0\r\n"
"o=ESP32 1234 1234 IN IP4 " ESP32_IP "\r\n"
"s=ESP32 SIP Call\r\n"
"c=IN IP4 " ESP32_IP "\r\n"
"t=0 0\r\n"
"m=audio 4000 RTP/AVP 0 101\r\n"
"a=rtpmap:0 PCMU/8000\r\n"
"a=rtpmap:101 telephone-event/8000\r\n"
"a=fmtp:101 0-16\r\n"
"a=ptime:20\r\n"
"a=sendrecv\r\n";


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:1001@" ESP32_IP ":5062>\r\n"
    "Content-Type: application/sdp\r\n"
    "Content-Length: %d\r\n"
    "\r\n"
    "%s",
    via,
    from,
    to,
    callid,
    cseq,
    strlen(sdp),
    sdp);


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

ESP_LOGI(TAG, "200 OK sent");
}
        else if (strstr(rx_buf, "OPTIONS")) {

            ESP_LOGI(TAG, "Received OPTIONS");
        }
        else if (strstr(rx_buf, "ACK")) {

            ESP_LOGI(TAG, "Received ACK");
        }
        else if (strstr(rx_buf, "BYE")) {

            ESP_LOGI(TAG, "Received BYE");
        }
        else {

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

    close(sock);
    vTaskDelete(NULL);
}

/* ================= MAIN ================= */

void app_main(void)
{
    ESP_LOGI(TAG, "Starting SIP client...");

    wifi_init();

    while (!wifi_ready) {
        vTaskDelay(pdMS_TO_TICKS(500));
    }

    xTaskCreate(sip_register_task,
                "sip_register",
                20480,
                NULL,
                5,
                NULL);
}