/*---------------------------------------------------------------------------*/
/* jitter_buffer.cpp                                                         */
/* copyright (c) innovaphone 2017                                            */
/*---------------------------------------------------------------------------*/

#include <platform.h>

#include "sdk/common/interface/socket.h"
#include "sdk/common/interface/media.h"
#include "jitter_buffer.h"

#define JB_DEB(x) /* Only activate if debug print is allowed from context calling Pull() ! */

IJitterBuffer * IJitterBuffer::Create(class UJitterBuffer * user)
{
    return new class jitter_buffer(user);
}

/*---------------------------------------------------------------------------*/
/* jitter_buffer                                                             */
/*---------------------------------------------------------------------------*/

jitter_buffer::jitter_buffer(class UJitterBuffer * user)
{
    int i;

    this->user = user;
    coder = AudioCoder::G711_A;
    config_pending = false;
    read_descriptor_ex = 0;
    write_descriptor_ex = 0;
    count = 0;
    adjusts = 0;
    timestamp_set = false;
    for(i=0;i<JB_MAX_DESCRIPTORS;i++) {
        xmit_descriptors[i].control = 0;
        xmit_descriptors[i].buffer_pos = 0;
    }
}

unsigned jitter_buffer::Configure(enum AudioCoder coder, unsigned mediaPacketizationMs, unsigned pullInterval8khz,
                                  unsigned minBufferMs, unsigned initialBufferMs, unsigned maxBufferMs)
{
    int i;
    dword d;

    JB_DEB(debug->printf("JB: Configure(coder=%u,media_packet=%u,min=%u,init=%u,max=%u)",coder,mediaPacketizationMs,minBufferMs,initialBufferMs,maxBufferMs));
    timestamp_set = false;

    this->coder = coder;
    this->payload_type = 0;
    this->min_buffer = minBufferMs*8;
    this->max_buffer = maxBufferMs*8;
    timestamp_rate_multiplier = 1;
    adjust_interval = 200;

    i = 0;
    while((i < JB_MAX_DESCRIPTORS) && (count != 0)) {
        d = (write_descriptor_ex + i) & JB_DESCRIPTORS_MASK;
        if(xmit_descriptors[d].control > 1) {
            xmit_descriptors[d].control = 0;
            count -= xmit_descriptors[d].time;
        }
        i++;
    }

    switch(coder) {
    case AudioCoder::G711_A:
    case AudioCoder::G711_U:
    case AudioCoder::CLEARCHANNEL:
    case AudioCoder::G722:
        descriptor_time = pullInterval8khz;
        descriptor_size = pullInterval8khz;
        break;

    case AudioCoder::AMR_WB:
        timestamp_rate_multiplier = 5;
        descriptor_time = pullInterval8khz;
        descriptor_size = pullInterval8khz - 1;
        break;

    case AudioCoder::G723_53:
    case AudioCoder::G723_63:
        descriptor_time = 30*8;
        descriptor_size = 24;
        break;

    case AudioCoder::G729:
    case AudioCoder::G729A:
    case AudioCoder::G729B:
    case AudioCoder::G729AB:
        descriptor_time = 10*8;
        descriptor_size = 10;
        break;

    case AudioCoder::G726_40:
    case AudioCoder::G726_32:
    case AudioCoder::G726_24:
    case AudioCoder::G726_16:
        descriptor_time = 10*8;
        descriptor_size = pullInterval8khz;
        break;

    case AudioCoder::OPUS_NB:
    case AudioCoder::OPUS_WB:
        timestamp_rate_multiplier = 6;
        descriptor_time = 5*8/2; /* 2.5ms */
        descriptor_size = 48; /* Account for OPUS high sample rates */
        adjust_interval *= (20*8) / (5*8/2);
        break;

    default:
        return 0;
    }

    initial_buffers = initialBufferMs*8 / descriptor_time;
    descriptors_num = JB_MAX_TIME / descriptor_time;

    if(this->min_buffer<((unsigned) descriptor_time)) this->min_buffer = descriptor_time;

    config_pending = true;

    JB_DEB(if(initialBufferMs*8 % descriptor_time != 0) debug->printf("JB: Init buffer not correct!"));
    return descriptor_time;
}

unsigned jitter_buffer::get_head(const void *buf, size_t len, unsigned *pos, byte *get_buf, unsigned get_len)
{
    unsigned l;

    l = len - *pos;
    if (l > get_len) {
        l = get_len;
    }
    memcpy(get_buf, &((const byte *) buf)[*pos], l);
    *pos += l;
    return l;
}

void jitter_buffer::Push(const void * buf, size_t len, dword timestamp)
{
	int offset;
    unsigned pos;
    dword i, n, l, d;
    byte payload = 0;

    n = (dword)(read_descriptor_ex - write_descriptor_ex);
    i = 0;
    while((i < n) && (count != 0)) {
        d = (write_descriptor_ex + i) & JB_DESCRIPTORS_MASK;
        if(xmit_descriptors[d].control != 0) {
            xmit_descriptors[d].control = 0;
            count -= xmit_descriptors[d].time;
        }
        i++;
    }
    write_descriptor_ex += n;

    if(((payload &0x7f) != payload_type) || config_pending || !coder) {
        JB_DEB(debug->printf("JB: type not set or payload types do not match, discard."));
        return;
    }

        // if this is the first packet or if there is an underun, we start with
        // a fresh jitter-buffer
    if(!timestamp_set) {
        if(count != 0) {
            JB_DEB(debug->printf("JB: re-init, not empty"));
            return;
        }
        timestamp_set = true;
        count_max = min_buffer;
        count_min = max_buffer;

        d = xmit_descriptors[write_descriptor_ex & JB_DESCRIPTORS_MASK].buffer_pos;
        for(i = 0; i < JB_MAX_DESCRIPTORS; i++) {
            if(d + descriptor_size > JB_XMIT_SIZE) {
                d = 0;
            }
            xmit_descriptors[(write_descriptor_ex + i) & JB_DESCRIPTORS_MASK].buffer_pos = d;
            d += descriptor_size;
        }
        next_timestamp = timestamp - timestamp_rate_multiplier * initial_buffers * descriptor_time;
    }
    else {
        d = xmit_descriptors[(write_descriptor_ex - n - 1) & JB_DESCRIPTORS_MASK].buffer_pos;
        for(i = JB_MAX_DESCRIPTORS - n; i < JB_MAX_DESCRIPTORS; i++) {
            d += descriptor_size;
            if(d + descriptor_size > JB_XMIT_SIZE) {
                d = 0;
            }
            xmit_descriptors[(write_descriptor_ex + i) & JB_DESCRIPTORS_MASK].buffer_pos = d;
        }
        next_timestamp += timestamp_rate_multiplier * n * descriptor_time;
    }

    if(adjust_count < adjust_interval) {
        if(count>count_max) count_max = count;
        if(count<count_min) count_min = count;
    }

    offset = (int)(dword)(timestamp-next_timestamp) / timestamp_rate_multiplier;
    JB_DEB(debug->printf("JB: %i %08lx %i %i",payload,timestamp,len,offset));
    if(offset<-JB_MAX_TIME) {
        JB_DEB(debug->printf("JB: re-init, jump back"));
        next_timestamp = timestamp - timestamp_rate_multiplier * initial_buffers * descriptor_time;
        offset = initial_buffers * descriptor_time;
    }
    else if(offset<0) {
        JB_DEB(debug->printf("JB: discard, late"));
        return;
    }
    else if(offset>JB_MAX_TIME-descriptor_time) {
        JB_DEB(debug->printf("JB: re-init, jump forward"));
        next_timestamp = timestamp - timestamp_rate_multiplier * initial_buffers * descriptor_time;
        offset = initial_buffers * descriptor_time;
    }
    d = (write_descriptor_ex + offset/descriptor_time) & JB_DESCRIPTORS_MASK;

    pos = 0;
    if(coder==AudioCoder::G711_A || coder==AudioCoder::G711_U || coder==AudioCoder::G722 || coder==AudioCoder::CLEARCHANNEL) {
        dword r = offset%descriptor_time;
        if(r) {
            if(xmit_descriptors[d].control == 1) {
                pos = (pos + descriptor_time-r < len) ? pos + descriptor_time-r : len;
            }
            else {
                if(!xmit_descriptors[d].control) {
                    count += descriptor_time;
                    xmit_descriptors[d].time = descriptor_time;
                    xmit_descriptors[d].payload_type = payload_type;
                    xmit_descriptors[d].length = 0;
                }
                xmit_descriptors[d].length += get_head(buf, len, &pos, &xmit_buffer[xmit_descriptors[d].buffer_pos + r], descriptor_time-r);
                if(xmit_descriptors[d].length>=descriptor_time) {
                    xmit_descriptors[d].length = descriptor_time;
                    xmit_descriptors[d].control = 1;
                }
                else {
                    xmit_descriptors[d].control = 2;
                }
            }
            d = (d + 1) & JB_DESCRIPTORS_MASK;
            offset += descriptor_time-r;
        }
        while(offset<=JB_MAX_TIME-descriptor_time && (pos < len)) {
            if(xmit_descriptors[d].control == 1) {
                pos = (pos + descriptor_time < len) ? pos + descriptor_time : len;
            }
            else {
                if(!xmit_descriptors[d].control) {
                    count += descriptor_time;
                    xmit_descriptors[d].time = descriptor_time;
                    xmit_descriptors[d].payload_type = payload_type;
                    xmit_descriptors[d].length = 0;
                }
                xmit_descriptors[d].length += get_head(buf, len, &pos, &xmit_buffer[xmit_descriptors[d].buffer_pos], descriptor_time);
                if(xmit_descriptors[d].length>=descriptor_time) {
                    xmit_descriptors[d].length = descriptor_time;
                    xmit_descriptors[d].control = 1;
                }
                else {
                    xmit_descriptors[d].control = 3;
                }
            }
            d = (d + 1) & JB_DESCRIPTORS_MASK;
            offset += descriptor_time;
        }
    }
    else if((coder==AudioCoder::G723_53) || (coder==AudioCoder::G723_63)) {
        // reality check: ignore packets which contain more then 200ms speech. Workaround for a IP200
        // problem which did send big g711 data as g723/g729
        if(len<=160) {
            do {
                word length = get_head(buf, len, &pos, &xmit_buffer[xmit_descriptors[d].buffer_pos], 4);
                if(length==4) {
                    word available;
                    if(xmit_buffer[xmit_descriptors[d].buffer_pos] &2) length = 4 -4;
                    else if(xmit_buffer[xmit_descriptors[d].buffer_pos] &1) length = 20 -4;
                    else length = 24 -4;
                    available = get_head(buf, len, &pos, &xmit_buffer[xmit_descriptors[d].buffer_pos + 4], length);
                    if((available==length) && (xmit_descriptors[d].control != 1)) {
                        if(!xmit_descriptors[d].control) {
                            count += descriptor_time;
                            xmit_descriptors[d].time = descriptor_time;
                        }
                        xmit_descriptors[d].payload_type = payload_type;
                        xmit_descriptors[d].length = 4+length;
                        xmit_descriptors[d].control = 1;
                    }
                }
                d = (d + 1) & JB_DESCRIPTORS_MASK;
                offset += descriptor_time;
            } while(offset<=JB_MAX_TIME-descriptor_time && (pos < len));
        }
    }
    else if((coder==AudioCoder::OPUS_NB) || (coder==AudioCoder::OPUS_WB)) {
        l = len;
        n = (l + descriptor_size - 1) / descriptor_size;
        if(offset + n * descriptor_time <= JB_MAX_TIME) {
            if((xmit_descriptors[d].control == 1) && (xmit_descriptors[d].length == 0)) {
                debug->printf("JB: Too large Opus frame %i",l);
            }
            else {
                i = 0;
                while((i < n) && (xmit_descriptors[d].control != 1)) {
                    d = (d + 1) & JB_DESCRIPTORS_MASK;
                    i++;
                }
                if(i < n) {
                    debug->printf("JB: Too large late Opus frame %i",l);
                }
                else {
                    d = (write_descriptor_ex + offset/descriptor_time) & JB_DESCRIPTORS_MASK;
                    for(i = 0; i < n; i++) {
                        xmit_descriptors[d].payload_type = payload_type;
                        get_head(buf, len, &pos, &xmit_buffer[xmit_descriptors[d].buffer_pos], descriptor_size);
                        xmit_descriptors[d].length = l;
                        l = 0;
                        d = (d + 1) & JB_DESCRIPTORS_MASK;
                    }
                    d = write_descriptor_ex & JB_DESCRIPTORS_MASK;
                    for(i = 0; i < ((dword) offset)/descriptor_time; i++) {
                        if(!xmit_descriptors[d].control) {
                            count += descriptor_time;
                            xmit_descriptors[d].time = descriptor_time;
                            xmit_descriptors[d].control = 2;
                        }
                        d = (d + 1) & JB_DESCRIPTORS_MASK;
                    }
                    for(i = 0; i < n; i++) {
                        if(!xmit_descriptors[d].control) {
                            count += descriptor_time;
                            xmit_descriptors[d].time = descriptor_time;
                        }
                        xmit_descriptors[d].control = 1;
                        d = (d + 1) & JB_DESCRIPTORS_MASK;
                    }
                }
            }
        }
    }
    else {
        do {
            if(xmit_descriptors[d].control != 1) {
                if(!xmit_descriptors[d].control) {
                    count += descriptor_time;
                    xmit_descriptors[d].time = descriptor_time;
                }
                xmit_descriptors[d].payload_type = payload_type;
                xmit_descriptors[d].length = get_head(buf, len, &pos, &xmit_buffer[xmit_descriptors[d].buffer_pos], descriptor_size);
                xmit_descriptors[d].control = 1;
            }
            d = (d + 1) & JB_DESCRIPTORS_MASK;
            offset += descriptor_time;
        } while(offset<=JB_MAX_TIME-descriptor_time && (pos < len));
    }
}

bool jitter_buffer::Pull(bool discard)
{
    /* Might be entered from other thread */
    bool ret = false;
    dword d;
    word i, n, l;

    if(!coder) {
        if(!discard) user->JitterBufferPulled(this, 0, 0);
        return false;
    }
    if(config_pending) {
        d = read_descriptor_ex;
        while(((dword)(d - write_descriptor_ex)) < JB_MAX_DESCRIPTORS) {
            if(xmit_descriptors[d & JB_DESCRIPTORS_MASK].control == 1) {
                read_descriptor_ex = d+1;
            }
            d++;
        }
        config_pending = false;
        adjust_count = adjust_interval + initial_buffers;
        adjusts = 0;
        if(!discard) user->JitterBufferPulled(this, 0, 0);
    } else if(adjusts > 0) {
        adjusts--;
        if(!discard) user->JitterBufferPulled(this, 0, 0);
    }
    else {
        if(adjusts < 0) {
            adjusts++;
        }
        d = read_descriptor_ex & JB_DESCRIPTORS_MASK;
        if(!discard) {
            if((((dword)(read_descriptor_ex - write_descriptor_ex)) < descriptors_num) && (xmit_descriptors[d].control == 1)) {
                l = xmit_descriptors[d].length;
                if(l > descriptor_size) {
                    i = 0;
                    do {
                        n = l - i;
                        if(n > descriptor_size) {
                            n = descriptor_size;
                        }
                        memcpy(&linear_buffer[i], &xmit_buffer[xmit_descriptors[d].buffer_pos], n);
                        d = (d + 1) & JB_DESCRIPTORS_MASK;
                        i += n;
                    }
                    while(i < l);
                    user->JitterBufferPulled(this, linear_buffer, l);
                }
                else {
                    user->JitterBufferPulled(this, &xmit_buffer[xmit_descriptors[d].buffer_pos], l);
                }
                ret = true;
            }
            else {
                user->JitterBufferPulled(this, 0, 0);
            }
        }
        else {
                // if we can not transmit, but there was data we return true
                // this is used to implement "tone: off_if_date"
            if((((dword)(read_descriptor_ex - write_descriptor_ex)) < descriptors_num) && (xmit_descriptors[d].control == 1)) {
                ret = true;
            }
        }
        read_descriptor_ex++;

        if(timestamp_set) {
            adjust_count--;
            if(adjust_count < adjust_interval) {
                if(count <= ((dword)(read_descriptor_ex - write_descriptor_ex)) * descriptor_time) {
                    JB_DEB(debug->printf("JB: empty, re-init"));
                    adjust_count = adjust_interval + initial_buffers;
                    timestamp_set = false;
                }
                else {
                    if(adjust_count==0) {
                        adjust_count = adjust_interval;

                        if(count_max>max_buffer && count_min>min_buffer) {
                            JB_DEB(debug->printf("JB-- (min=%u/%u,max=%u/%u)",count_min,min_buffer,count_max,max_buffer));
                            adjusts -= ((coder==AudioCoder::OPUS_NB) || (coder==AudioCoder::OPUS_WB)) ? (20*8) / (5*8/2) : 1;
                            read_descriptor_ex += ((coder==AudioCoder::OPUS_NB) || (coder==AudioCoder::OPUS_WB)) ? (20*8) / (5*8/2) : 1;
                        }
                        else if(count_min<min_buffer) {
                            JB_DEB(debug->printf("JB++ (min=%u/%u)",count_min,min_buffer));
                            adjusts += ((coder==AudioCoder::OPUS_NB) || (coder==AudioCoder::OPUS_WB)) ? (20*8) / (5*8/2) : 1;
                        }
                        count_max = min_buffer;
                        count_min = max_buffer;
                    }
                }
            }
        }
    }
    return ret;
}

void jitter_buffer::Adjust(unsigned initialBufferMs)
{
    /* Might be entered from other thread */
    unsigned n;

    initial_buffers = initialBufferMs*8 / descriptor_time;
    n = adjusts + count / descriptor_time;

    JB_DEB(debug->printf("JB Adjust %u %u %i %i", initial_buffers, timestamp_set, adjusts, n));
    if(timestamp_set && ((adjusts >= 0) || (initial_buffers < n))) {

        adjusts += initial_buffers - n;
        if(adjusts < 0) {
            read_descriptor_ex -= adjusts;
        }
        if(initial_buffers < n) {
            JB_DEB(debug->printf("JB-- (adjust=%u %u %i)",initial_buffers,n,adjusts));
        }
        else if(initial_buffers > n) {
            JB_DEB(debug->printf("JB++ (adjust=%u %u %i)",initial_buffers,n,adjusts));
        }
    }
}

/*---------------------------------------------------------------------------*/
