/* 
 * Copyright (C) 2008-2011 Teluu Inc. (http://www.teluu.com)
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA 
 */
#include <pjmedia-codec/h263_packetizer.h>
#include <pjmedia/types.h>
#include <pj/assert.h>
#include <pj/errno.h>
#include <pj/string.h>


#if defined(PJMEDIA_HAS_VIDEO) && (PJMEDIA_HAS_VIDEO != 0)


#define THIS_FILE       "h263_packetizer.c"


/* H.263 packetizer definition */
struct pjmedia_h263_packetizer {
    /* Current settings */
    pjmedia_h263_packetizer_cfg cfg;
    
    /* Unpacketizer state */
    unsigned        unpack_last_sync_pos;
    pj_bool_t       unpack_prev_lost;
};


/*
 * Find synchronization point (PSC, slice, GSBC, EOS, EOSBS) in H.263 
 * bitstream.
 */
static pj_uint8_t* find_sync_point(pj_uint8_t *data,
                                   pj_size_t data_len)
{
    pj_uint8_t *p = data, *end = data+data_len-1;

    while (p < end && (*p || *(p+1)))
        ++p;

    if (p == end)
        return NULL;
        
    return p;
}


/*
 * Find synchronization point (PSC, slice, GSBC, EOS, EOSBS) in H.263 
 * bitstream, in reversed manner.
 */
static pj_uint8_t* find_sync_point_rev(pj_uint8_t *data,
                                       pj_size_t data_len)
{
    pj_uint8_t *p = data+data_len-2;

    while (p >= data && (*p || *(p+1)))
        --p;

    if (p < data)
        return (data + data_len);
        
    return p;
}


/*
 * Create H263 packetizer.
 */
PJ_DEF(pj_status_t) pjmedia_h263_packetizer_create(
                                pj_pool_t *pool,
                                const pjmedia_h263_packetizer_cfg *cfg,
                                pjmedia_h263_packetizer **p)
{
    pjmedia_h263_packetizer *p_;

    PJ_ASSERT_RETURN(pool && p, PJ_EINVAL);

    if (cfg && cfg->mode != PJMEDIA_H263_PACKETIZER_MODE_RFC4629)
        return PJ_ENOTSUP;

    p_ = PJ_POOL_ZALLOC_T(pool, pjmedia_h263_packetizer);
    if (cfg) {
        pj_memcpy(&p_->cfg, cfg, sizeof(*cfg));
    } else {
        p_->cfg.mode = PJMEDIA_H263_PACKETIZER_MODE_RFC4629;
        p_->cfg.mtu = PJMEDIA_MAX_VID_PAYLOAD_SIZE;
    }

    *p = p_;

    return PJ_SUCCESS;
}


/*
 * Generate an RTP payload from H.263 frame bitstream, in-place processing.
 */
PJ_DEF(pj_status_t) pjmedia_h263_packetize(pjmedia_h263_packetizer *pktz,
                                           pj_uint8_t *bits,
                                           pj_size_t bits_len,
                                           unsigned *pos,
                                           pj_uint8_t **payload,
                                           pj_size_t *payload_len)
{
    pj_uint8_t *p, *end;

    pj_assert(pktz && bits && pos && payload && payload_len);
    pj_assert(*pos <= bits_len);

    p = bits + *pos;
    end = bits + bits_len;

    /* Put two octets payload header */
    if ((end-p > 2) && *p==0 && *(p+1)==0) {
        /* The bitstream starts with synchronization point, just override
         * the two zero octets (sync point mark) for payload header.
         */
        *p = 0x04;
    } else {
        /* Not started in synchronization point, we will use two octets
         * preceeding the bitstream for payload header!
         */

        if (*pos < 2) {
            /* Invalid H263 bitstream, it's not started with PSC */
            return PJ_EINVAL;
        }

        p -= 2;
        *p = 0;
    }
    *(p+1) = 0;

    /* When bitstream truncation needed because of payload length/MTU 
     * limitation, try to use sync point for the payload boundary.
     */
    if (end-p > pktz->cfg.mtu) {
        end = find_sync_point_rev(p+2, pktz->cfg.mtu-2);
    }

    *payload = p;
    *payload_len = end-p;
    *pos = (unsigned)(end - bits);

    return PJ_SUCCESS;
}


/*
 * Append an RTP payload to a H.263 picture bitstream.
 */
PJ_DEF(pj_status_t) pjmedia_h263_unpacketize (pjmedia_h263_packetizer *pktz,
                                              const pj_uint8_t *payload,
                                              pj_size_t payload_len,
                                              pj_uint8_t *bits,
                                              pj_size_t bits_size,
                                              unsigned *pos)
{
    pj_uint8_t P, V, PLEN;
    const pj_uint8_t *p = payload;
    pj_uint8_t *q;

    q = bits + *pos;

    /* Check if this is a missing/lost packet */
    if (payload == NULL) {
        pktz->unpack_prev_lost = PJ_TRUE;
        return PJ_SUCCESS;
    }

    /* H263 payload header size is two octets */
    if (payload_len < 2) {
        /* Invalid bitstream, discard this payload */
        pktz->unpack_prev_lost = PJ_TRUE;
        return PJ_EINVAL;
    }

    /* Reset last sync point for every new picture bitstream */
    if (*pos == 0)
        pktz->unpack_last_sync_pos = 0;

    /* Get payload header info */
    P = *p & 0x04;
    V = *p & 0x02;
    PLEN = ((*p & 0x01) << 5) + ((*(p+1) & 0xF8)>>3);

    /* Get start bitstream pointer */
    p += 2;         /* Skip payload header */
    if (V)
        p += 1;     /* Skip VRC data */
    if (PLEN)
        p += PLEN;  /* Skip extra picture header data */

    /* Get bitstream length */
    if (payload_len > (pj_size_t)(p - payload)) {
        payload_len -= (p - payload);
    } else {
        /* Invalid bitstream, discard this payload */
        pktz->unpack_prev_lost = PJ_TRUE;
        return PJ_EINVAL;
    }

    /* Validate bitstream length */
    if (bits_size < *pos + payload_len + 2) {
        /* Insufficient bistream buffer, discard this payload */
        pj_assert(!"Insufficient H.263 bitstream buffer");
        pktz->unpack_prev_lost = PJ_TRUE;
        return PJ_ETOOSMALL;
    }

    /* Start writing bitstream */

    /* No sync point flag */
    if (!P) {
        if (*pos == 0) {
            /* Previous packet must be lost */
            pktz->unpack_prev_lost = PJ_TRUE;

            /* If there is extra picture header, let's use it. */
            if (PLEN) {
                /* Write two zero octets for PSC */
                *q++ = 0;
                *q++ = 0;
                /* Copy the picture header */
                p -= PLEN;
                pj_memcpy(q, p, PLEN);
                p += PLEN;
                q += PLEN;
            }
        } else if (pktz->unpack_prev_lost) {
            /* If prev packet was lost, revert the bitstream pointer to
             * the last sync point.
             */
            pj_assert(pktz->unpack_last_sync_pos <= *pos);
            q = bits + pktz->unpack_last_sync_pos;
        }

        /* There was packet lost, see if this payload contain sync point
         * (usable data).
         */
        if (pktz->unpack_prev_lost) {
            pj_uint8_t *sync;
            sync = find_sync_point((pj_uint8_t*)p, payload_len);
            if (sync) {
                /* Got sync point, update P/sync-point flag */
                P = 1;
                /* Skip the two zero octets */
                sync += 2;
                /* Update payload length and start bitstream pointer */
                payload_len -= (sync - p);
                p = sync;
            } else {
                /* No sync point in it, just discard this payload */
                return PJ_EIGNORED;
            }
        }
    }

    /* Write two zero octets when payload flagged with sync point */
    if (P) {
        pktz->unpack_last_sync_pos = (unsigned)(q - bits);
        *q++ = 0;
        *q++ = 0;
    }

    /* Write the payload to the bitstream */
    pj_memcpy(q, p, payload_len);
    q += payload_len;

    /* Update the bitstream writing offset */
    *pos = (unsigned)(q - bits);

    pktz->unpack_prev_lost = PJ_FALSE;

    return PJ_SUCCESS;
}


#endif /* PJMEDIA_HAS_VIDEO */
