// sb-send: Sonobus audio sender

// Just enough of the Sonobus protocol to do what we need.
// Writing OSC packets, invites, slip.

#include "sb-send.h"

uint64_t get_ntp_time(void) {
#ifdef _WIN32
    FILETIME ft;
    GetSystemTimeAsFileTime(&ft);
    uint64_t t = ((uint64_t)ft.dwHighDateTime << 32) | ft.dwLowDateTime;
    uint64_t unix_100ns = t - 116444736000000000ULL;
    uint64_t secs = unix_100ns / 10000000ULL;
    uint64_t frac = ((unix_100ns % 10000000ULL) << 32) / 10000000ULL;
    return ((secs + 2208988800ULL) << 32) | frac;
#else
    struct timespec ts;
    clock_gettime(CLOCK_REALTIME, &ts);
    uint64_t secs = (uint64_t)ts.tv_sec + 2208988800ULL;
    uint64_t frac = ((uint64_t)ts.tv_nsec << 32) / 1000000000ULL;
    return (secs << 32) | frac;
#endif
}

int osc_pad4(int len) { return (len + 3) & ~3; }

int osc_write_string(unsigned char *buf, const char *s) {
    int len = strlen(s) + 1;
    int padded = osc_pad4(len);
    memcpy(buf, s, len);
    memset(buf + len, 0, padded - len);
    return padded;
}

int osc_write_int32(unsigned char *buf, int32_t val) {
    buf[0] = (val >> 24) & 0xFF;
    buf[1] = (val >> 16) & 0xFF;
    buf[2] = (val >> 8) & 0xFF;
    buf[3] = val & 0xFF;
    return 4;
}

int osc_write_int64(unsigned char *buf, int64_t val) {
    buf[0] = (val >> 56) & 0xFF;
    buf[1] = (val >> 48) & 0xFF;
    buf[2] = (val >> 40) & 0xFF;
    buf[3] = (val >> 32) & 0xFF;
    buf[4] = (val >> 24) & 0xFF;
    buf[5] = (val >> 16) & 0xFF;
    buf[6] = (val >> 8) & 0xFF;
    buf[7] = val & 0xFF;
    return 8;
}

int osc_write_double(unsigned char *buf, double val) {
    union { double d; uint64_t u; } conv = { .d = val };
    return osc_write_int64(buf, (int64_t)conv.u);
}

int osc_write_timetag(unsigned char *buf) {
    return osc_write_int64(buf, (int64_t)get_ntp_time());
}

int osc_write_blob(unsigned char *buf, const unsigned char *data, int len) {
    int padded = osc_pad4(len);
    osc_write_int32(buf, len);
    memcpy(buf + 4, data, len);
    if (padded > len) {
        memset(buf + 4 + len, 0, padded - len);
    }
    return 4 + padded;
}

int osc_read_string(const unsigned char *buf, char *out, int max) {
    int i = 0;
    while (buf[i] && i < max - 1) { out[i] = buf[i]; i++; }
    out[i] = '\0';
    return osc_pad4(i + 1);
}

int32_t osc_read_int32(const unsigned char *buf) {
    return ((int32_t)buf[0] << 24) | ((int32_t)buf[1] << 16) |
           ((int32_t)buf[2] << 8) | buf[3];
}

int64_t osc_read_int64(const unsigned char *buf) {
    return ((int64_t)buf[0] << 56) | ((int64_t)buf[1] << 48) |
           ((int64_t)buf[2] << 40) | ((int64_t)buf[3] << 32) |
           ((int64_t)buf[4] << 24) | ((int64_t)buf[5] << 16) |
           ((int64_t)buf[6] << 8) | buf[7];
}

int slip_encode(const unsigned char *in, int inlen,
               unsigned char *out, int maxout) {
    int j = 0;
    if (j >= maxout) {
        return -1;
    }
    out[j++] = SLIP_END;
    for (int i = 0; i < inlen; i++) {
        if (in[i] == SLIP_END) {
            if (j + 2 > maxout) {
                return -1;
            }
            out[j++] = SLIP_ESC;
            out[j++] = SLIP_ESC_END;
        } else if (in[i] == SLIP_ESC) {
            if (j + 2 > maxout) {
                return -1;
            }
            out[j++] = SLIP_ESC;
            out[j++] = SLIP_ESC_ESC;
        } else {
            if (j + 1 > maxout) {
                return -1;
            }
            out[j++] = in[i];
        }
    }
    if (j >= maxout) {
        return -1;
    }
    out[j++] = SLIP_END;
    return j;
}

void slip_decoder_init(SlipDecoder *d) { d->len = 0; d->escape = 0; }

int slip_decode_byte(SlipDecoder *d, unsigned char b,
                     unsigned char **msg, int *msglen) {
    if (b == SLIP_END) {
        if (d->len > 0) {
            *msg = d->buf; *msglen = d->len;
            d->len = 0; d->escape = 0;
            return 1;
        }
        return 0;
    }
    if (d->escape) {
        d->escape = 0;
        if (b == SLIP_ESC_END) {
            b = SLIP_END;
        } else if (b == SLIP_ESC_ESC) {
            b = SLIP_ESC;
        }
        if (d->len < MAX_OSC_SIZE) {
            d->buf[d->len++] = b;
        }
        return 0;
    }
    if (b == SLIP_ESC) { d->escape = 1; return 0; }
    if (d->len < MAX_OSC_SIZE) {
        d->buf[d->len++] = b;
    }
    return 0;
}

int send_tcp_slip(sock_t sock, const unsigned char *msg, int len) {
    unsigned char slip_buf[MAX_OSC_SIZE * 2];
    int slip_len = slip_encode(msg, len, slip_buf, sizeof(slip_buf));
    if (slip_len < 0) {
        return -1;
    }
    int sent = 0;
    while (sent < slip_len) {
        int n = send(sock, (const char*)(slip_buf + sent), slip_len - sent, MSG_NOSIGNAL);
        if (n <= 0) {
            return -1;
        }
        sent += n;
    }
    return sent;
}

int build_login_msg(AppState *state, unsigned char *buf) {
    SB_ASSERT(state->nickname[0] != '\0');
    SB_ASSERT(state->public_ip[0] != '\0');
    int pos = 0;
    pos += osc_write_string(buf + pos, "/aoo/server/login");
    pos += osc_write_string(buf + pos, ",sssisih");
    pos += osc_write_string(buf + pos, state->nickname);
    pos += osc_write_string(buf + pos, state->password_hash);
    pos += osc_write_string(buf + pos, state->public_ip);
    pos += osc_write_int32(buf + pos, state->public_port);
    pos += osc_write_string(buf + pos, state->public_ip);
    pos += osc_write_int32(buf + pos, state->local_port);
    pos += osc_write_int64(buf + pos, state->client_token);
    return pos;
}

int build_group_join_msg(AppState *state, unsigned char *buf) {
    int pos = 0;
    pos += osc_write_string(buf + pos, "/aoo/server/group/join");
    pos += osc_write_string(buf + pos, ",ssF");
    pos += osc_write_string(buf + pos, state->group);
    pos += osc_write_string(buf + pos, state->password_hash);
    return pos;
}

int build_group_leave_msg(AppState *state, unsigned char *buf) {
    int pos = 0;
    pos += osc_write_string(buf + pos, "/aoo/server/group/leave");
    pos += osc_write_string(buf + pos, ",s");
    pos += osc_write_string(buf + pos, state->group);
    return pos;
}

int build_udp_request(unsigned char *buf) {
    int pos = 0;
    pos += osc_write_string(buf + pos, "/aoo/server/request");
    pos += osc_write_string(buf + pos, ",");
    return pos;
}

int build_invite_msg(int32_t our_sink_id, unsigned char *buf) {
    int pos = 0;
    pos += osc_write_string(buf + pos, "/aoo/src/0/invite");
    pos += osc_write_string(buf + pos, ",ii");
    pos += osc_write_int32(buf + pos, our_sink_id);
    pos += osc_write_int32(buf + pos, AOO_PROTOCOL_FLAG_COMPACT_DATA);
    return pos;
}

int build_uninvite_msg(int32_t source_id, int32_t our_sink_id, unsigned char *buf) {
    int pos = 0;
    char addr[64];
    snprintf(addr, sizeof(addr), "/aoo/src/%d/uninvite", source_id);
    pos += osc_write_string(buf + pos, addr);
    pos += osc_write_string(buf + pos, ",i");
    pos += osc_write_int32(buf + pos, our_sink_id);
    return pos;
}

int build_peer_ping_msg(int64_t token, unsigned char *buf) {
    int pos = 0;
    pos += osc_write_string(buf + pos, "/aoo/peer/ping");
    pos += osc_write_string(buf + pos, ",h");
    pos += osc_write_int64(buf + pos, token);
    return pos;
}

int build_format_msg(AppState *state, int32_t sink_id,
                     int32_t source_id, int32_t salt,
                     unsigned char *buf) {
    int pos = 0;
    char addr[64];
    snprintf(addr, sizeof(addr), "/aoo/sink/%d/format", sink_id);
    pos += osc_write_string(buf + pos, addr);
    pos += osc_write_string(buf + pos, ",iiiiiisb");
    pos += osc_write_int32(buf + pos, source_id);
    pos += osc_write_int32(buf + pos, (AOO_VERSION_MAJOR << 24) |
                                      (AOO_VERSION_MINOR << 16) |
                                      AOO_PROTOCOL_FLAG_COMPACT_DATA);
    pos += osc_write_int32(buf + pos, salt);
    pos += osc_write_int32(buf + pos, state->channels);
    pos += osc_write_int32(buf + pos, SAMPLE_RATE);
    pos += osc_write_int32(buf + pos, state->frame_size);
    pos += osc_write_string(buf + pos, "opus");
    unsigned char opus_opts[16];
    osc_write_int32(opus_opts, state->recv_bitrate * state->channels);
    osc_write_int32(opus_opts + 4, 10);
    osc_write_int32(opus_opts + 8, 3002);
    if (state->voice) {
        osc_write_int32(opus_opts + 12, OPUS_APPLICATION_VOIP);
    } else {
        osc_write_int32(opus_opts + 12, OPUS_APPLICATION_AUDIO);
    }
    pos += osc_write_blob(buf + pos, opus_opts, 16);
    return pos;
}

int build_data_msg(int32_t sink_id, int32_t source_id,
                   int32_t salt, int32_t seq,
                   const unsigned char *data, int data_len,
                   unsigned char *buf) {
    int pos = 0;
    char addr[64];
    snprintf(addr, sizeof(addr), "/aoo/sink/%d/data", sink_id);
    pos += osc_write_string(buf + pos, addr);
    pos += osc_write_string(buf + pos, ",iiidiiiib");
    pos += osc_write_int32(buf + pos, source_id);
    pos += osc_write_int32(buf + pos, salt);
    pos += osc_write_int32(buf + pos, seq);
    pos += osc_write_double(buf + pos, (double)SAMPLE_RATE);
    pos += osc_write_int32(buf + pos, 0);
    pos += osc_write_int32(buf + pos, data_len);
    pos += osc_write_int32(buf + pos, 1);
    pos += osc_write_int32(buf + pos, 0);
    if (pos + 4 + data_len + 3 > MAX_PACKET_SIZE) {
        fprintf(stderr, "build_data_msg: data too large (%d + %d > %d)\n",
                pos, data_len, MAX_PACKET_SIZE);
        return -1;
    }
    pos += osc_write_blob(buf + pos, data, data_len);
    return pos;
}

int build_source_ping_msg(int32_t sink_id, int32_t source_id,
                          unsigned char *buf) {
    int pos = 0;
    char addr[64];
    snprintf(addr, sizeof(addr), "/aoo/sink/%d/ping", sink_id);
    pos += osc_write_string(buf + pos, addr);
    pos += osc_write_string(buf + pos, ",it");
    pos += osc_write_int32(buf + pos, source_id);
    pos += osc_write_timetag(buf + pos);
    return pos;
}

int parse_peer_join(const unsigned char *buf, int len,
                    char *name, char *ip, int *port, int64_t *token) {
    char tags[32], addr[64], group[64], local_ip[64];
    int pos = 0;
    if (pos + 4 > len) { return -1; }
    pos += osc_read_string(buf + pos, addr, sizeof(addr));
    if (pos + 4 > len) { return -1; }
    pos += osc_read_string(buf + pos, tags, sizeof(tags));
    if (strcmp(tags, ",sssisih") != 0) {
        return -1;
    }
    if (pos + 4 > len) { return -1; }
    pos += osc_read_string(buf + pos, group, sizeof(group));
    if (pos + 4 > len) { return -1; }
    pos += osc_read_string(buf + pos, name, 64);
    if (pos + 4 > len) { return -1; }
    pos += osc_read_string(buf + pos, ip, 64);
    if (pos + 4 > len) { return -1; }
    *port = osc_read_int32(buf + pos); pos += 4;
    if (pos + 4 > len) { return -1; }
    pos += osc_read_string(buf + pos, local_ip, sizeof(local_ip));
    if (pos + 4 > len) { return -1; }
    pos += 4;
    if (pos + 8 > len) { return -1; }
    *token = osc_read_int64(buf + pos);
    return 0;
}

int parse_peer_leave(const unsigned char *buf, int len, char *name) {
    char tags[32], addr[64], group[64];
    int pos = 0;
    if (pos + 4 > len) { return -1; }
    pos += osc_read_string(buf + pos, addr, sizeof(addr));
    if (pos + 4 > len) { return -1; }
    pos += osc_read_string(buf + pos, tags, sizeof(tags));
    if (strcmp(tags, ",ss") != 0) {
        return -1;
    }
    if (pos + 4 > len) { return -1; }
    pos += osc_read_string(buf + pos, group, sizeof(group));
    if (pos + 4 > len) { return -1; }
    pos += osc_read_string(buf + pos, name, 64);
    return 0;
}

int discover_public_ip(AppState *state, struct sockaddr_in *srv) {
    unsigned char buf[256];
    int len = build_udp_request(buf);
    for (int attempt = 0; attempt < 5; attempt++) {
        sendto(state->udp_sock, (const char*)buf, len, 0, (struct sockaddr *)srv, sizeof(*srv));
        fd_set fds;
        struct timeval tv = {1, 0};
        FD_ZERO(&fds);
        FD_SET(state->udp_sock, &fds);
        if (select(state->udp_sock + 1, &fds, NULL, NULL, &tv) > 0) {
            unsigned char reply[512];
            struct sockaddr_in from;
            socklen_t fromlen = sizeof(from);
            int n = recvfrom(state->udp_sock, (char*)reply, sizeof(reply), 0,
                             (struct sockaddr *)&from, &fromlen);
            if (n > 0) {
                char addr[64];
                int pos = osc_read_string(reply, addr, sizeof(addr));
                if (strcmp(addr, "/aoo/client/reply") == 0) {
                    char tags[16];
                    pos += osc_read_string(reply + pos, tags, sizeof(tags));
                    if (tags[0] == ',' && tags[1] == 's' && tags[2] == 'i') {
                        pos += osc_read_string(reply + pos, state->public_ip, 64);
                        state->public_port = osc_read_int32(reply + pos);
                        fprintf(stderr, "Public address: %s:%d\n", state->public_ip, state->public_port);
                        return 0;
                    }
                }
            }
        }
    }
    return -1;
}

void handle_udp_message(AppState *state, unsigned char *buf, int len,
                        struct sockaddr_in *from) {
    SB_ASSERT(state->udp_sock != SOCK_INVALID);
    if (len < 4) { return; }
    char addr[128];
    int pos = 0;
    pos += osc_read_string(buf, addr, sizeof(addr));
    SB_ASSERT(pos > 0 && pos <= len);

    dbg(state, "udp: %s from %s:%d\n", addr,
        inet_ntoa(from->sin_addr), ntohs(from->sin_port));

    if (strncmp(addr, "/aoo/src/", 9) == 0) {
        char *slash = strchr(addr + 9, '/');
        if (slash && strcmp(slash, "/invite") == 0) {
            int32_t their_source_id = atoi(addr + 9);
            char tags[16];
            if (pos + 4 > len) { return; }
            pos += osc_read_string(buf + pos, tags, sizeof(tags));
            int32_t their_sink_id = 0;
            if (strlen(tags) >= 2 && tags[1] == 'i') {
                if (pos + 4 > len) { return; }
                their_sink_id = osc_read_int32(buf + pos);
            }

            pthread_mutex_lock(&state->peer_mutex);
            Peer *p = find_peer_by_addr_locked(state, from);
            if (p) {
                p->last_packet_time = time(NULL);
                if (p->is_sb_send) {
                    pthread_mutex_unlock(&state->peer_mutex);
                    return;
                }
                bool was_connected = p->connected;
                bool sink_changed = (p->remote_sink_id != their_sink_id);
                p->remote_sink_id = their_sink_id;
                p->our_source_id = their_sink_id;
                if (their_source_id >= 0) {
                    p->remote_source_id = their_source_id;
                }
                p->connected = true;
                p->uninvited = false;
                bool should_send_format = !was_connected || sink_changed;
                if (should_send_format) {
                    p->salt = generate_salt(p);
                    p->salt_initialized = true;
                    atomic_store(&p->sequence, 0);
                    p->format_sent = true;
                    const char *status_str;
                    if (was_connected) {
                        status_str = "sink changed";
                    } else {
                        status_str = "connected";
                    }
                    fprintf(stderr, "Peer %s %s: sink=%d src=%d salt=%d\n",
                           p->name, status_str,
                           p->remote_sink_id, p->our_source_id, p->salt);
                }
                Peer pcopy = *p;
                pthread_mutex_unlock(&state->peer_mutex);
                if (should_send_format) {
                    send_format_to_peer(state, &pcopy);
                }
                if (pcopy.remote_source_id >= 0) {
                    send_uninvite_to_peer(state, &pcopy);
                }
            } else {
                pthread_mutex_unlock(&state->peer_mutex);
                fprintf(stderr, "sb-send: invite from unknown peer %s:%d (source_id=%d), sending format anyway\n",
                        inet_ntoa(from->sin_addr), ntohs(from->sin_port), their_source_id);
                int32_t our_sink_id = 0;
                {
                    pthread_mutex_lock(&state->peer_mutex);
                    our_sink_id = ++state->next_sink_id;
                    pthread_mutex_unlock(&state->peer_mutex);
                }
                if (our_sink_id > 0) {
                    unsigned char fmt_buf[512];
                    int salt = (int32_t)(time(NULL) ^ rand());
                    int fmt_len = build_format_msg(state, their_source_id, our_sink_id, salt, fmt_buf);
                    sendto(state->udp_sock, (const char*)fmt_buf, fmt_len, 0,
                           (struct sockaddr *)from, sizeof(*from));
                    unsigned char uninv_buf[128];
                    int uninv_len = build_uninvite_msg(their_source_id, our_sink_id, uninv_buf);
                    sendto(state->udp_sock, (const char*)uninv_buf, uninv_len, 0,
                           (struct sockaddr *)from, sizeof(*from));
                }
            }
        }
        else if (slash && strcmp(slash, "/uninvite") == 0) {
            pthread_mutex_lock(&state->peer_mutex);
            Peer *p = find_peer_by_addr_locked(state, from);
            if (p) {
                p->connected = false;
                p->format_sent = false;
                fprintf(stderr, "Uninvited by peer %s, stopping audio\n", p->name);
            }
            pthread_mutex_unlock(&state->peer_mutex);
        }
    }
    else if (strncmp(addr, "/aoo/sink/", 10) == 0 && strstr(addr + 10, "/format")) {
        char tags[16];
        if (pos + 4 > len) { return; }
        pos += osc_read_string(buf + pos, tags, sizeof(tags));
        int32_t remote_source_id = 0;
        if (strlen(tags) >= 2 && tags[1] == 'i') {
            if (pos + 4 > len) { return; }
            remote_source_id = osc_read_int32(buf + pos);
        }
        if (remote_source_id != 0) {
            pthread_mutex_lock(&state->peer_mutex);
            Peer *p = find_peer_by_addr_locked(state, from);
            if (p) {
                p->remote_source_id = remote_source_id;
                p->uninvited = false;
            }
            pthread_mutex_unlock(&state->peer_mutex);
        }
    }
    else if (strcmp(addr, "/d") == 0) {
    }
    else if (strcmp(addr, "/aoo/peer/ping") == 0) {
        char ping_tags[32];
        if (pos + 4 > len) { return; }
        pos += osc_read_string(buf + pos, ping_tags, sizeof(ping_tags));
        int64_t their_token = 0;
        if (strcmp(ping_tags, ",h") == 0) {
            if (pos + 8 > len) { return; }
            their_token = osc_read_int64(buf + pos);
        }
        if (their_token & ((int64_t)1 << 63)) {
            pthread_mutex_lock(&state->peer_mutex);
            Peer *p = find_peer_by_addr_locked(state, from);
            if (p && !p->is_sb_send) {
                p->is_sb_send = true;
                fprintf(stderr, "Peer %s identified as sb-send via ping token, ignoring\n", p->name);
            }
            pthread_mutex_unlock(&state->peer_mutex);
            return;
        }
        pthread_mutex_lock(&state->peer_mutex);
        Peer *p = find_peer_by_addr_locked(state, from);
        time_t now = time(NULL);
        if (p) {
            p->last_packet_time = now;
            if (now - p->last_ping_reply >= 1) {
                p->last_ping_reply = now;
                pthread_mutex_unlock(&state->peer_mutex);
                unsigned char ping_buf[64];
                int plen = build_peer_ping_msg(state->client_token, ping_buf);
                sendto(state->udp_sock, (const char*)ping_buf, plen, 0,
                       (struct sockaddr *)from, sizeof(*from));
            } else {
                pthread_mutex_unlock(&state->peer_mutex);
            }
        } else {
            pthread_mutex_unlock(&state->peer_mutex);
            unsigned char ping_buf[64];
            int plen = build_peer_ping_msg(state->client_token, ping_buf);
            sendto(state->udp_sock, (const char*)ping_buf, plen, 0,
                   (struct sockaddr *)from, sizeof(*from));
        }
    }
}

void process_tcp_message(AppState *state, unsigned char *buf, int len) {
    SB_ASSERT(state->tcp_sock != SOCK_INVALID);
    if (len < 4) { return; }
    char addr[128], tags[32];
    int pos = 0;
    pos += osc_read_string(buf, addr, sizeof(addr));
    SB_ASSERT(pos > 0 && pos <= len);

    dbg(state, "tcp: %s\n", addr);

    if (strcmp(addr, "/aoo/client/login") == 0) {
        if (pos + 4 > len) { return; }
        pos += osc_read_string(buf + pos, tags, sizeof(tags));
        if (pos + 4 > len) { return; }
        int32_t status = osc_read_int32(buf + pos);
        if (status > 0) {
            atomic_store(&state->logged_in, true);
            fprintf(stderr, "Login successful\n");
        } else {
            fprintf(stderr, "Login failed\n");
            atomic_store(&state->running, false);
        }
    }
    else if (strcmp(addr, "/aoo/client/group/join") == 0) {
        if (pos + 4 > len) { return; }
        pos += osc_read_string(buf + pos, tags, sizeof(tags));
        char grp[64];
        if (pos + 4 > len) { return; }
        pos += osc_read_string(buf + pos, grp, sizeof(grp));
        if (pos + 4 > len) { return; }
        int32_t status = osc_read_int32(buf + pos);
        if (status > 0) {
            atomic_store(&state->group_joined, true);
            fprintf(stderr, "Joined group: %s\n", grp);
        } else {
            fprintf(stderr, "Failed to join group\n");
            atomic_store(&state->running, false);
        }
    }
    else if (strcmp(addr, "/aoo/client/peer/join") == 0) {
        char name[64], ip[64];
        int port;
        int64_t token;
        if (parse_peer_join(buf, len, name, ip, &port, &token) == 0) {
            pthread_mutex_lock(&state->peer_mutex);
            Peer *p = add_peer_locked(state, name, ip, port, token);
            Peer pcopy;
            bool need_invite = false;
            if (p) {
                pcopy = *p;
                p->last_invite_time = time(NULL);
                if (token & ((int64_t)1 << 63)) {
                    p->is_sb_send = true;
                    fprintf(stderr, "Peer joined: %s (%s:%d) [sb-send, ignoring]\n", name, ip, port);
                } else {
                    p->invite_count++;
                    need_invite = true;
                    fprintf(stderr, "Peer joined: %s (%s:%d)\n", name, ip, port);
                }
            }
            pthread_mutex_unlock(&state->peer_mutex);
            if (need_invite) {
                send_invite_to_peer(state, &pcopy);
            }
        }
    }
    else if (strcmp(addr, "/aoo/client/peer/leave") == 0) {
        char name[64];
        if (parse_peer_leave(buf, len, name) == 0) {
            pthread_mutex_lock(&state->peer_mutex);
            remove_peer_locked(state, name);
            pthread_mutex_unlock(&state->peer_mutex);
        }
    }
}
// EOF
