/* -*- Mode: C++; tab-width: 2; indent-tabs-mode: nil; c-basic-offset: 2 -*- */
/* vim: set et sw=2 ts=2: */
/***************************************************************************
 *            rtp_profile_jpeg.cc
 *
 *  Wed Dec 11 08:09:16 CET 2013
 *  Copyright 2013 Bent Bisballe Nyeng
 *  deva@aasimon.org
 ****************************************************************************/
/*
 *  This file is part of lrtp.
 *
 * lrtp is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * lrtp 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
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with lrtp; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
 */

/*
 * Large portions of this file have been taken from the gstreamer jpeg rtp
 * pay/depay implementation version 1.1.3:
 *  gst-plugins-good-1.1.3/gst/rtp/gstrtpjpegpay.c
 * and
 *  gst-plugins-good-1.1.3/gst/rtp/gstrtpjpegdepay.c
 * Copyright (C) 2008 Wim Taymans <wim.taymans@gmail.com>
 * Copyright (C) 2008 Axis Communications <dev-gstreamer@axis.com>
 * @author Bjorn Ostby <bjorn.ostby@axis.com>
 * @author Peter Kjellerstedt <peter.kjellerstedt@axis.com>
 */
#include "rtp_profile_jpeg.h"

#include "rtp_profile.h"

#include <stdio.h>
#include <string.h>

#include <list>
#include <map>
#include <string>

// For htons
#ifdef WIN32
#include <winsock2.h>
#else
#include <arpa/inet.h>
#endif

#if 0
#define GST_LOG(fmt ...) printf("\nLOG\t"fmt)
#define GST_WARNING(fmt ...) printf("\nWARN:\t"fmt)
#define GST_ELEMENT_WARNING(fmt ...) printf("\nWARN\t"fmt)
#define GST_WARNING_OBJECT(fmt ...) printf("\nWARN\t"fmt)
#define GST_LOG_OBJECT(fmt ...) printf("\nLOG:\t"fmt)
#define GST_DEBUG_OBJECT(fmt ...) printf("\nDEBUG:\t"fmt)
#else
#define GST_LOG(fmt, ...)
#define GST_WARNING(fmt, ...)
#define GST_ELEMENT_WARNING(fmt, ...)
#define GST_WARNING_OBJECT(fmt, ...)
#define GST_LOG_OBJECT(fmt, ...)
#define GST_DEBUG_OBJECT(fmt, ...)
#endif

#define GST_ROUND_UP_8(num)  (((num)+7)&~7)

#define G_LIKELY(x) x
#define G_UNLIKELY(x) x

#define g_htons(x) htons(x)

#ifndef TRUE
#define TRUE true
#define FALSE false
#endif

typedef unsigned char guint8;
typedef unsigned int guint;
typedef int gint;
typedef unsigned long gulong;
typedef bool gboolean;
typedef unsigned long long GstClockTime;
typedef unsigned short guint16;

typedef enum {
  /* custom success starts here */
  GST_FLOW_CUSTOM_SUCCESS_2 = 102,
  GST_FLOW_CUSTOM_SUCCESS_1 = 101,
  GST_FLOW_CUSTOM_SUCCESS = 100,

  /* core predefined */
  GST_FLOW_OK		  =  0,
  /* expected failures */
  GST_FLOW_NOT_LINKED     = -1,
  GST_FLOW_FLUSHING       = -2,
  /* error cases */
  GST_FLOW_EOS            = -3,
  GST_FLOW_NOT_NEGOTIATED = -4,
  GST_FLOW_ERROR	  = -5,
  GST_FLOW_NOT_SUPPORTED  = -6,

  /* custom error starts here */
  GST_FLOW_CUSTOM_ERROR   = -100,
  GST_FLOW_CUSTOM_ERROR_1 = -101,
  GST_FLOW_CUSTOM_ERROR_2 = -102
} GstFlowReturn;

///--------------------------------------------------------------------------

////////////////////////////////////////////////////
/// Begin if pay
////////////////////////////////////////////////////



/*
 * QUANT_PREFIX_LEN:
 *
 * Prefix length in the header before the quantization tables:
 * Two size bytes and one byte for precision
 */
#define QUANT_PREFIX_LEN     3

/*
 * RtpJpegMarker:
 * @JPEG_MARKER: Prefix for JPEG marker
 * @JPEG_MARKER_SOI: Start of Image marker
 * @JPEG_MARKER_JFIF: JFIF marker
 * @JPEG_MARKER_CMT: Comment marker
 * @JPEG_MARKER_DQT: Define Quantization Table marker
 * @JPEG_MARKER_SOF: Start of Frame marker
 * @JPEG_MARKER_DHT: Define Huffman Table marker
 * @JPEG_MARKER_SOS: Start of Scan marker
 * @JPEG_MARKER_EOI: End of Image marker
 * @JPEG_MARKER_DRI: Define Restart Interval marker
 * @JPEG_MARKER_H264: H264 marker
 *
 * Identifers for markers in JPEG header
 */
typedef enum {
  JPEG_MARKER = 0xFF,
  JPEG_MARKER_SOI = 0xD8,
  JPEG_MARKER_JFIF = 0xE0,
  JPEG_MARKER_CMT = 0xFE,
  JPEG_MARKER_DQT = 0xDB,
  JPEG_MARKER_SOF = 0xC0,
  JPEG_MARKER_DHT = 0xC4,
  JPEG_MARKER_SOS = 0xDA,
  JPEG_MARKER_EOI = 0xD9,
  JPEG_MARKER_DRI = 0xDD,
  JPEG_MARKER_H264 = 0xE4
} RtpJpegMarker;

#define DEFAULT_JPEG_QUANT    255

#define DEFAULT_JPEG_QUALITY  255
#define DEFAULT_JPEG_TYPE     1

enum
{
  PROP_0,
  PROP_JPEG_QUALITY,
  PROP_JPEG_TYPE,
  PROP_LAST
};

enum
{
  Q_TABLE_0 = 0,
  Q_TABLE_1,
  Q_TABLE_MAX                   /* only support for two tables at the moment */
};

typedef struct _RtpJpegHeader RtpJpegHeader;

/*
 * RtpJpegHeader:
 * @type_spec: type specific
 * @offset: fragment offset
 * @type: type field
 * @q: quantization table for this frame
 * @width: width of image in 8-pixel multiples
 * @height: height of image in 8-pixel multiples
 *
 * 0                   1                   2                   3
 * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 * | Type-specific |              Fragment Offset                  |
 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 * |      Type     |       Q       |     Width     |     Height    |
 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 */
struct _RtpJpegHeader
{
  unsigned int type_spec:8;
  unsigned int offset:24;
  unsigned char type;
  unsigned char q;
  unsigned char width;
  unsigned char height;
};

/*
 * RtpQuantHeader
 * @mbz: must be zero
 * @precision: specify size of quantization tables
 * @length: length of quantization data
 *
 * 0                   1                   2                   3
 * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 * |      MBZ      |   Precision   |             Length            |
 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 * |                    Quantization Table Data                    |
 * |                              ...                              |
 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 */
typedef struct
{
  unsigned char mbz;
  unsigned char precision;
  unsigned short length;
} RtpQuantHeader;

typedef struct
{
  unsigned char size;
  const unsigned char *data;
} RtpQuantTable;

/*
 * RtpRestartMarkerHeader:
 * @restartInterval: number of MCUs that appear between restart markers
 * @restartFirstLastCount: a combination of the first packet mark in the chunk
 *                         last packet mark in the chunk and the position of the
 *                         first restart interval in the current "chunk"
 *
 *    0                   1                   2                   3
 *   0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |       Restart Interval        |F|L|       Restart Count       |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *
 *  The restart marker header is implemented according to the following
 *  methodology specified in section 3.1.7 of rfc2435.txt.
 *
 *  "If the restart intervals in a frame are not guaranteed to be aligned
 *  with packet boundaries, the F (first) and L (last) bits MUST be set
 *  to 1 and the Restart Count MUST be set to 0x3FFF.  This indicates
 *  that a receiver MUST reassemble the entire frame before decoding it."
 *
 */

typedef struct
{
  unsigned short restart_interval;
  unsigned short restart_count;
} RtpRestartMarkerHeader;

typedef struct
{
  unsigned char id;
  unsigned char samp;
  unsigned char qt;
} CompInfo;

typedef struct {
  //GstRTPBasePayload payload;

  unsigned char quality;
  unsigned char type;

  int height;
  int width;

  unsigned char quant;
} GstRtpJPEGPay;

//typedef char GstRTPBasePayload;
typedef struct {
  const unsigned char *data;
  size_t size;
  unsigned long long timestamp;
} GstBuffer;

#include <vector>

typedef std::vector<GstBuffer> GstBufferList;

#define QUANT_TABLE_SIZE 15

static unsigned int gst_rtp_jpeg_pay_header_size(const unsigned char *data,
                                                 unsigned int offset)
{
  return data[offset] << 8 | data[offset + 1];
}

static unsigned int gst_rtp_jpeg_pay_read_quant_table(const unsigned char *data,
                                                      unsigned int size,
                                                      unsigned int offset,
                                                      RtpQuantTable tables[])
{
  unsigned int quant_size, tab_size;
  unsigned char prec;
  unsigned char id;

  if (offset + 2 > size)
    goto too_small;

  quant_size = gst_rtp_jpeg_pay_header_size (data, offset);
  if (quant_size < 2)
    goto small_quant_size;

  /* clamp to available data */
  if (offset + quant_size > size)
    quant_size = size - offset;

  offset += 2;
  quant_size -= 2;

  while (quant_size > 0) {
    /* not enough to read the id */
    if (offset + 1 > size)
      break;

    id = data[offset] & 0x0f;
    if (id == 15)
      /* invalid id received - corrupt data */
      goto invalid_id;

    prec = (data[offset] & 0xf0) >> 4;
    if (prec)
      tab_size = 128;
    else
      tab_size = 64;

    /* there is not enough for the table */
    if (quant_size < tab_size + 1)
      goto no_table;

    GST_LOG("read quant table %d, tab_size %d, prec %02x",
            (int)id, (int)tab_size, (unsigned int)prec);

    tables[id].size = tab_size;
    tables[id].data = &data[offset + 1];

    tab_size += 1;
    quant_size -= tab_size;
    offset += tab_size;
  }
done:
  return offset + quant_size;

  /* ERRORS */
too_small:
  {
    GST_WARNING ("not enough data");
    return size;
  }
small_quant_size:
  {
    GST_WARNING ("quant_size too small (%u < 2)", quant_size);
    return size;
  }
invalid_id:
  {
    GST_WARNING ("invalid id");
    goto done;
  }
no_table:
  {
    GST_WARNING ("not enough data for table (%u < %u)", quant_size,
        tab_size + 1);
    goto done;
  }
}

static bool
gst_rtp_jpeg_pay_read_sof (GstRtpJPEGPay * pay, const guint8 * data,
    guint size, guint * offset, CompInfo info[], RtpQuantTable tables[],
    gulong tables_elements)
{
  guint sof_size, off;
  guint width, height, infolen;
  CompInfo elem;
  gint i, j;

  off = *offset;

  /* we need at least 17 bytes for the SOF */
  if (off + 17 > size)
    goto wrong_size;

  sof_size = gst_rtp_jpeg_pay_header_size (data, off);
  if (sof_size < 17)
    goto wrong_length;

  *offset += sof_size;

  /* skip size */
  off += 2;

  /* precision should be 8 */
  if (data[off++] != 8)
    goto bad_precision;

  /* read dimensions */
  height = data[off] << 8 | data[off + 1];
  width = data[off + 2] << 8 | data[off + 3];
  off += 4;

  GST_LOG_OBJECT("got dimensions %ux%u", height, width);

  if (height == 0) {
    goto invalid_dimension;
  }
  if (height > 2040) {
    height = 0;
  }
  if (width == 0) {
    goto invalid_dimension;
  }
  if (width > 2040) {
    width = 0;
  }

  if (height == 0 || width == 0) {
    pay->height = 0;
    pay->width = 0;
  } else {
    pay->height = GST_ROUND_UP_8 (height) / 8;
    pay->width = GST_ROUND_UP_8 (width) / 8;
  }

  /* we only support 3 components */
  if (data[off++] != 3)
    goto bad_components;

  infolen = 0;
  for (i = 0; i < 3; i++) {
    elem.id = data[off++];
    elem.samp = data[off++];
    elem.qt = data[off++];
    GST_LOG_OBJECT("got comp %d, samp %02x, qt %d",elem.id, elem.samp, elem.qt);
    /* insertion sort from the last element to the first */
    for (j = infolen; j > 1; j--) {
      if (G_LIKELY (info[j - 1].id < elem.id))
        break;
      info[j] = info[j - 1];
    }
    info[j] = elem;
    infolen++;
  }

  /* see that the components are supported */
  if (info[0].samp == 0x21)
    pay->type = 0;
  else if (info[0].samp == 0x22)
    pay->type = 1;
  else {
    GST_LOG_OBJECT("info[0].samp := %02x\n", (unsigned char)info[0].samp);
    goto invalid_comp;
  }

  if (!(info[1].samp == 0x11)) {
    GST_LOG_OBJECT("!(info[1].samp == 0x11)\n");
    goto invalid_comp;
  }

  if (!(info[2].samp == 0x11)) {
    GST_LOG_OBJECT("!(info[2].samp == 0x11)\n");
    goto invalid_comp;
  }

  /* the other components are free to use any quant table but they have to
   * have the same table id */
  if (info[1].qt != info[2].qt) {
    /* Some MJPG (like the one from the Logitech C-920 camera) uses different
     * quant tables for component 1 and 2 but both tables contain the exact
     * same data, so we could consider them as being the same tables */
    if (!(info[1].qt < tables_elements &&
            info[2].qt < tables_elements &&
            tables[info[1].qt].size > 0 &&
            tables[info[1].qt].size == tables[info[2].qt].size &&
            memcmp (tables[info[1].qt].data, tables[info[2].qt].data,
                tables[info[1].qt].size) == 0))
      goto invalid_comp;
  }

  return TRUE;

  /* ERRORS */
wrong_size:
  {
    GST_ELEMENT_WARNING("Wrong size %u (needed %u).", size, off + 17);
    return FALSE;
  }
wrong_length:
  {
    GST_ELEMENT_WARNING("Wrong SOF length %u.", sof_size);
    return FALSE;
  }
bad_precision:
  {
    GST_ELEMENT_WARNING("Wrong precision, expecting 8.");
    return FALSE;
  }
invalid_dimension:
  {
    GST_ELEMENT_WARNING("Wrong dimension, size %ux%u", width, height);
    return FALSE;
  }
bad_components:
  {
    GST_ELEMENT_WARNING("Wrong number of components");
    return FALSE;
  }
invalid_comp:
  {
    GST_ELEMENT_WARNING("Invalid component");
    return FALSE;
  }
}

static gboolean
gst_rtp_jpeg_pay_read_dri (GstRtpJPEGPay * pay, const guint8 * data,
    guint size, guint * offset, RtpRestartMarkerHeader * dri)
{
  guint dri_size, off;

  off = *offset;

  /* we need at least 4 bytes for the DRI */
  if (off + 4 > size)
    goto wrong_size;

  dri_size = gst_rtp_jpeg_pay_header_size (data, off);
  if (dri_size < 4)
    goto wrong_length;

  *offset += dri_size;
  off += 2;

  dri->restart_interval = g_htons ((data[off] << 8) | (data[off + 1]));
  dri->restart_count = g_htons (0xFFFF);

  return dri->restart_interval > 0;

wrong_size:
  {
    GST_WARNING ("not enough data for DRI");
    *offset = size;
    return FALSE;
  }
wrong_length:
  {
    GST_WARNING ("DRI size too small (%u)", dri_size);
    *offset += dri_size;
    return FALSE;
  }
}

static RtpJpegMarker
gst_rtp_jpeg_pay_scan_marker (const guint8 * data, guint size, guint * offset)
{
  while ((data[(*offset)++] != JPEG_MARKER) && ((*offset) < size));

  if (G_UNLIKELY ((*offset) >= size)) {
    GST_LOG ("found EOI marker");
    return JPEG_MARKER_EOI;
  } else {
    guint8 marker;

    marker = data[*offset];
    GST_LOG ("found 0x%02x marker at offset %u", marker, *offset);
    (*offset)++;
    return (RtpJpegMarker)marker;
  }
}

static GstFlowReturn
gst_rtp_jpeg_pay_handle_buffer(GstRtpJPEGPay *basepayload,
                               GstBuffer *buffer, std::list<RTP> &rtplist)
{
  GstRtpJPEGPay *pay;
  GstClockTime timestamp;
  GstFlowReturn ret = GST_FLOW_OK;//ERROR;
  RtpJpegHeader jpeg_header;
  RtpQuantHeader quant_header;
  RtpRestartMarkerHeader restart_marker_header;
  RtpQuantTable tables[QUANT_TABLE_SIZE] = { {0, NULL}, };
  CompInfo info[3] = { {0,}, };
  guint quant_data_size;
  //GstMapInfo map;
  const guint8 *data;
  size_t size;
  guint mtu;
  guint bytes_left;
  guint jpeg_header_size = 0;
  guint offset;
  gboolean frame_done;
  gboolean sos_found, sof_found, dqt_found, dri_found;
  gint i;
  //GstBufferList *list = NULL;

  pay = basepayload;

  // Max packet size (std. 1400)
  mtu = 1400;// basepayload->mtu;//GST_RTP_BASE_PAYLOAD_MTU (pay);

  //gst_buffer_map (buffer, &map, GST_MAP_READ);
  data = buffer->data;
  size = buffer->size;
  timestamp = buffer->timestamp;//GST_BUFFER_TIMESTAMP (buffer);
  offset = 0;

  GST_LOG_OBJECT("got buffer size %d timestamp %d", (int)size, (int)timestamp);

  /* parse the jpeg header for 'start of scan' and read quant tables if needed */
  sos_found = FALSE;
  dqt_found = FALSE;
  sof_found = FALSE;
  dri_found = FALSE;

  while (!sos_found && (offset < size)) {
    GST_LOG_OBJECT("checking from offset %u", offset);
    int marker = gst_rtp_jpeg_pay_scan_marker (data, size, &offset);
    GST_LOG_OBJECT("Marker: 0x%02x", marker);
    switch (marker) {
      case JPEG_MARKER_JFIF:
        GST_LOG_OBJECT("skipping marker (JFIF)");
        offset += gst_rtp_jpeg_pay_header_size (data, offset);
        break;
      case JPEG_MARKER_CMT:
        GST_LOG_OBJECT("skipping marker (CMT)");
        offset += gst_rtp_jpeg_pay_header_size (data, offset);
        break;
      case JPEG_MARKER_DHT:
        GST_LOG_OBJECT("skipping marker (DHT: define Huffman table)");
        offset += gst_rtp_jpeg_pay_header_size (data, offset);
        break;
      case JPEG_MARKER_H264:
        GST_LOG_OBJECT("skipping marker (H264)");
        offset += gst_rtp_jpeg_pay_header_size (data, offset);
        break;
      case JPEG_MARKER_SOF:
        if (!gst_rtp_jpeg_pay_read_sof(pay, data, size, &offset, info, tables,
                                       QUANT_TABLE_SIZE))
          goto invalid_format;
        sof_found = TRUE;
        break;
      case JPEG_MARKER_DQT:
        GST_LOG ("DQT found");
        offset = gst_rtp_jpeg_pay_read_quant_table (data, size, offset, tables);
        dqt_found = TRUE;
        break;
      case JPEG_MARKER_SOS:
        sos_found = TRUE;
        GST_LOG_OBJECT("SOS found");
        jpeg_header_size = offset + gst_rtp_jpeg_pay_header_size (data, offset);
        break;
      case JPEG_MARKER_EOI:
        GST_WARNING_OBJECT("EOI reached before SOS!");
        break;
      case JPEG_MARKER_SOI:
        GST_LOG_OBJECT("SOI found");
        break;
      case JPEG_MARKER_DRI:
        GST_LOG_OBJECT("DRI found");
        if (gst_rtp_jpeg_pay_read_dri (pay, data, size, &offset,
                &restart_marker_header))
          dri_found = TRUE;
        break;
      default:
        break;
    }
  }
  if (!dqt_found || !sof_found)
    goto unsupported_jpeg;

  /* by now we should either have negotiated the width/height or the SOF header
   * should have filled us in */
  if (pay->width < 0 || pay->height < 0) {
    goto no_dimension;
  }

  GST_LOG_OBJECT("header size %u", jpeg_header_size);

  size -= jpeg_header_size;
  data += jpeg_header_size;
  offset = 0;

  if (dri_found)
    pay->type += 64;

  /* prepare stuff for the jpeg header */
  jpeg_header.type_spec = 0;
  jpeg_header.type = pay->type;
  jpeg_header.q = pay->quant;
  jpeg_header.width = pay->width;
  jpeg_header.height = pay->height;

  /* collect the quant headers sizes */
  quant_header.mbz = 0;
  quant_header.precision = 0;
  quant_header.length = 0;
  quant_data_size = 0;

  if (pay->quant > 127) {
    /* for the Y and U component, look up the quant table and its size. quant
     * tables for U and V should be the same */
    for (i = 0; i < 2; i++) {
      guint qsize;
      guint qt;

      qt = info[i].qt;
      if (qt >= QUANT_TABLE_SIZE)
        goto invalid_quant;

      qsize = tables[qt].size;
      if (qsize == 0)
        goto invalid_quant;

      quant_header.precision |= (qsize == 64 ? 0 : (1 << i));
      quant_data_size += qsize;
    }
    quant_header.length = g_htons (quant_data_size);
    quant_data_size += sizeof (quant_header);
  }

  GST_LOG_OBJECT("quant_data size %u", quant_data_size);

  //list = gst_buffer_list_new ();

  bytes_left = sizeof (jpeg_header) + quant_data_size + size;

  if (dri_found)
    bytes_left += sizeof (restart_marker_header);

  frame_done = FALSE;
  do {
    //GstBuffer *outbuf;
    //char *outbuf;
    guint8 *payload;
    guint payload_size = (bytes_left < mtu ? bytes_left : mtu);
    guint header_size;
    //GstBuffer *paybuf;
    //GstRTPBuffer rtp = { NULL };
    RTP rtp;

    header_size = sizeof (jpeg_header) + quant_data_size;
    if (dri_found)
      header_size += sizeof (restart_marker_header);


    //outbuf = gst_rtp_buffer_new_allocate (header_size, 0, 0);
    //outbuf = malloc(header_size);

    //gst_rtp_buffer_map (outbuf, GST_MAP_WRITE, &rtp);

    if (payload_size == bytes_left) {
      GST_LOG_OBJECT("last packet of frame");
      frame_done = TRUE;
      //gst_rtp_buffer_set_marker (&rtp, 1);
      rtp.setMarker(true);
    }


    //gst_rtp_buffer_get_payload (&rtp);
    GST_LOG_OBJECT("-------- %d\n", header_size);
    unsigned char *payload_base =
      (unsigned char*)malloc(/*header_size*/16*1024);
    payload = payload_base;
    
    
    /* update offset */
#if (G_BYTE_ORDER == G_LITTLE_ENDIAN)
    jpeg_header.offset = ((offset & 0x0000FF) << 16) |
        ((offset & 0xFF0000) >> 16) | (offset & 0x00FF00);
#else
    jpeg_header.offset = offset;
#endif
    memcpy (payload, &jpeg_header, sizeof (jpeg_header));
    payload += sizeof (jpeg_header);
    payload_size -= sizeof (jpeg_header);

    if (dri_found) {
      memcpy (payload, &restart_marker_header, sizeof (restart_marker_header));
      payload += sizeof (restart_marker_header);
      payload_size -= sizeof (restart_marker_header);
    }

    /* only send quant table with first packet */
    if (G_UNLIKELY (quant_data_size > 0)) {
      memcpy (payload, &quant_header, sizeof (quant_header));
      payload += sizeof (quant_header);

      /* copy the quant tables for luma and chrominance */
      for (i = 0; i < 2; i++) {
        guint qsize;
        guint qt;

        qt = info[i].qt;
        qsize = tables[qt].size;
        memcpy (payload, tables[qt].data, qsize);

        GST_LOG_OBJECT("component %d using quant %d, size %d", i, qt, qsize);

        payload += qsize;
      }
      payload_size -= quant_data_size;
      bytes_left -= quant_data_size;
      quant_data_size = 0;
    }
    GST_LOG_OBJECT("sending payload size %d", payload_size);
    //gst_rtp_buffer_unmap (&rtp);
    
    /* create a new buf to hold the payload */
    //paybuf = gst_buffer_copy_region (buffer, GST_BUFFER_COPY_MEMORY,
    //                                 jpeg_header_size + offset, payload_size);

    /* join memory parts */
    //outbuf = gst_buffer_append (outbuf, paybuf);
    memcpy(payload, buffer->data + jpeg_header_size + offset, payload_size);
    payload += payload_size;

    rtp.setPayload((char*)payload_base, payload - payload_base);
    free(payload_base);

    //GST_BUFFER_TIMESTAMP (outbuf) = timestamp;
    rtp.setTimestamp((uint32_t)timestamp);

    /* and add to list */
    //gst_buffer_list_insert (list, -1, outbuf);
    rtplist.push_back(rtp);

    bytes_left -= payload_size;
    offset += payload_size;
    data += payload_size;
  }
  while (!frame_done);

  /* push the whole buffer list at once */
  //ret = gst_rtp_base_payload_push_list (basepayload, list);

  //  gst_buffer_unmap (buffer, &map);
  //  gst_buffer_unref (buffer);

  return ret;

  /* ERRORS */
unsupported_jpeg:
  {
    GST_ELEMENT_WARNING("Unsupported JPEG");
    //    gst_buffer_unmap (buffer, &map);
    //    gst_buffer_unref (buffer);
    return GST_FLOW_ERROR;
  }
no_dimension:
  {
    GST_ELEMENT_WARNING("No size given");
    //    gst_buffer_unmap (buffer, &map);
    //    gst_buffer_unref (buffer);
    return GST_FLOW_ERROR;
  }
invalid_format:
  {
    /* error was posted */
    //    gst_buffer_unmap (buffer, &map);
    //    gst_buffer_unref (buffer);
    return GST_FLOW_ERROR;
  }
invalid_quant:
  {
    GST_ELEMENT_WARNING("Invalid quant tables");
    //    gst_buffer_unmap (buffer, &map);
    //    gst_buffer_unref (buffer);
    return GST_FLOW_ERROR;
  }
}

static void
gst_rtp_jpeg_pay_init (GstRtpJPEGPay * pay)
{
  pay->quality = DEFAULT_JPEG_QUALITY;
  pay->quant = DEFAULT_JPEG_QUANT;
  pay->type = DEFAULT_JPEG_TYPE;
  pay->width = -1;
  pay->height = -1;
}


////////////////////////////////////////////////////
/// End of pay
////////////////////////////////////////////////////

///--------------------------------------------------------------------------

////////////////////////////////////////////////////
/// Begin of depay
////////////////////////////////////////////////////


// TODO
typedef std::string GstAdapter;

typedef struct {
  //  GstRTPBaseDepayload depayload;

  GstAdapter adapter;
  gboolean    discont;

  /* cached quant tables */
  //guint8 * qtables[255];
  std::map<int, guint8 *> qtables;
  gint frate_num;
  gint frate_denom;
  gint media_width;
  gint media_height;
  gint width, height;
} GstRtpJPEGDepay;







static void
gst_rtp_jpeg_depay_init (GstRtpJPEGDepay * rtpjpegdepay)
{
  //  rtpjpegdepay->adapter = 0;//TODO: gst_adapter_new ();
}

/*
static void
gst_rtp_jpeg_depay_reset (GstRtpJPEGDepay * depay)
{
  gint i;

  depay->width = 0;
  depay->height = 0;
  depay->media_width = 0;
  depay->media_height = 0;
  depay->frate_num = 0;
  depay->frate_denom = 1;
  depay->discont = TRUE;

  for (i = 0; i < 255; i++) {
    free (depay->qtables[i]);
    depay->qtables[i] = NULL;
  }

  //TODO: gst_adapter_clear (depay->adapter);
}
*/

static const int zigzag[] = {
  0, 1, 8, 16, 9, 2, 3, 10,
  17, 24, 32, 25, 18, 11, 4, 5,
  12, 19, 26, 33, 40, 48, 41, 34,
  27, 20, 13, 6, 7, 14, 21, 28,
  35, 42, 49, 56, 57, 50, 43, 36,
  29, 22, 15, 23, 30, 37, 44, 51,
  58, 59, 52, 45, 38, 31, 39, 46,
  53, 60, 61, 54, 47, 55, 62, 63
};

/*
 * Table K.1 from JPEG spec.
 */
static const int jpeg_luma_quantizer[64] = {
  16, 11, 10, 16, 24, 40, 51, 61,
  12, 12, 14, 19, 26, 58, 60, 55,
  14, 13, 16, 24, 40, 57, 69, 56,
  14, 17, 22, 29, 51, 87, 80, 62,
  18, 22, 37, 56, 68, 109, 103, 77,
  24, 35, 55, 64, 81, 104, 113, 92,
  49, 64, 78, 87, 103, 121, 120, 101,
  72, 92, 95, 98, 112, 100, 103, 99
};

/*
 * Table K.2 from JPEG spec.
 */
static const int jpeg_chroma_quantizer[64] = {
  17, 18, 24, 47, 99, 99, 99, 99,
  18, 21, 26, 66, 99, 99, 99, 99,
  24, 26, 56, 99, 99, 99, 99, 99,
  47, 66, 99, 99, 99, 99, 99, 99,
  99, 99, 99, 99, 99, 99, 99, 99,
  99, 99, 99, 99, 99, 99, 99, 99,
  99, 99, 99, 99, 99, 99, 99, 99,
  99, 99, 99, 99, 99, 99, 99, 99
};

#define CLAMP(x, l, u) (x<l?l:(x>u)?u:x)

/* Call MakeTables with the Q factor and a guint8[128] return array
 */
static void
MakeTables (GstRtpJPEGDepay * rtpjpegdepay, gint Q, guint8 qtable[128])
{
  gint i;
  guint factor;

  factor = CLAMP (Q, 1, 99);

  if (Q < 50)
    Q = 5000 / factor;
  else
    Q = 200 - factor * 2;

  for (i = 0; i < 64; i++) {
    gint lq = (jpeg_luma_quantizer[zigzag[i]] * Q + 50) / 100;
    gint cq = (jpeg_chroma_quantizer[zigzag[i]] * Q + 50) / 100;

    /* Limit the quantizers to 1 <= q <= 255 */
    qtable[i] = CLAMP (lq, 1, 255);
    qtable[i + 64] = CLAMP (cq, 1, 255);
  }
}

static const guint8 lum_dc_codelens[] = {
  0, 1, 5, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0
};

static const guint8 lum_dc_symbols[] = {
  0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11
};

static const guint8 lum_ac_codelens[] = {
  0, 2, 1, 3, 3, 2, 4, 3, 5, 5, 4, 4, 0, 0, 1, 0x7d
};

static const guint8 lum_ac_symbols[] = {
  0x01, 0x02, 0x03, 0x00, 0x04, 0x11, 0x05, 0x12,
  0x21, 0x31, 0x41, 0x06, 0x13, 0x51, 0x61, 0x07,
  0x22, 0x71, 0x14, 0x32, 0x81, 0x91, 0xa1, 0x08,
  0x23, 0x42, 0xb1, 0xc1, 0x15, 0x52, 0xd1, 0xf0,
  0x24, 0x33, 0x62, 0x72, 0x82, 0x09, 0x0a, 0x16,
  0x17, 0x18, 0x19, 0x1a, 0x25, 0x26, 0x27, 0x28,
  0x29, 0x2a, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39,
  0x3a, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49,
  0x4a, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59,
  0x5a, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68, 0x69,
  0x6a, 0x73, 0x74, 0x75, 0x76, 0x77, 0x78, 0x79,
  0x7a, 0x83, 0x84, 0x85, 0x86, 0x87, 0x88, 0x89,
  0x8a, 0x92, 0x93, 0x94, 0x95, 0x96, 0x97, 0x98,
  0x99, 0x9a, 0xa2, 0xa3, 0xa4, 0xa5, 0xa6, 0xa7,
  0xa8, 0xa9, 0xaa, 0xb2, 0xb3, 0xb4, 0xb5, 0xb6,
  0xb7, 0xb8, 0xb9, 0xba, 0xc2, 0xc3, 0xc4, 0xc5,
  0xc6, 0xc7, 0xc8, 0xc9, 0xca, 0xd2, 0xd3, 0xd4,
  0xd5, 0xd6, 0xd7, 0xd8, 0xd9, 0xda, 0xe1, 0xe2,
  0xe3, 0xe4, 0xe5, 0xe6, 0xe7, 0xe8, 0xe9, 0xea,
  0xf1, 0xf2, 0xf3, 0xf4, 0xf5, 0xf6, 0xf7, 0xf8,
  0xf9, 0xfa
};

static const guint8 chm_dc_codelens[] = {
  0, 3, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0
};

static const guint8 chm_dc_symbols[] = {
  0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11
};

static const guint8 chm_ac_codelens[] = {
  0, 2, 1, 2, 4, 4, 3, 4, 7, 5, 4, 4, 0, 1, 2, 0x77
};

static const guint8 chm_ac_symbols[] = {
  0x00, 0x01, 0x02, 0x03, 0x11, 0x04, 0x05, 0x21,
  0x31, 0x06, 0x12, 0x41, 0x51, 0x07, 0x61, 0x71,
  0x13, 0x22, 0x32, 0x81, 0x08, 0x14, 0x42, 0x91,
  0xa1, 0xb1, 0xc1, 0x09, 0x23, 0x33, 0x52, 0xf0,
  0x15, 0x62, 0x72, 0xd1, 0x0a, 0x16, 0x24, 0x34,
  0xe1, 0x25, 0xf1, 0x17, 0x18, 0x19, 0x1a, 0x26,
  0x27, 0x28, 0x29, 0x2a, 0x35, 0x36, 0x37, 0x38,
  0x39, 0x3a, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48,
  0x49, 0x4a, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58,
  0x59, 0x5a, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68,
  0x69, 0x6a, 0x73, 0x74, 0x75, 0x76, 0x77, 0x78,
  0x79, 0x7a, 0x82, 0x83, 0x84, 0x85, 0x86, 0x87,
  0x88, 0x89, 0x8a, 0x92, 0x93, 0x94, 0x95, 0x96,
  0x97, 0x98, 0x99, 0x9a, 0xa2, 0xa3, 0xa4, 0xa5,
  0xa6, 0xa7, 0xa8, 0xa9, 0xaa, 0xb2, 0xb3, 0xb4,
  0xb5, 0xb6, 0xb7, 0xb8, 0xb9, 0xba, 0xc2, 0xc3,
  0xc4, 0xc5, 0xc6, 0xc7, 0xc8, 0xc9, 0xca, 0xd2,
  0xd3, 0xd4, 0xd5, 0xd6, 0xd7, 0xd8, 0xd9, 0xda,
  0xe2, 0xe3, 0xe4, 0xe5, 0xe6, 0xe7, 0xe8, 0xe9,
  0xea, 0xf2, 0xf3, 0xf4, 0xf5, 0xf6, 0xf7, 0xf8,
  0xf9, 0xfa
};

static guint8 *
MakeQuantHeader (guint8 * p, const guint8 * qt, gint size, gint tableNo)
{
  *p++ = 0xff;
  *p++ = 0xdb;                  /* DQT */
  *p++ = 0;                     /* length msb */
  *p++ = size + 3;              /* length lsb */
  *p++ = tableNo;
  memcpy (p, qt, size);

  return (p + size);
}

static guint8 *
MakeHuffmanHeader (guint8 * p, const guint8 * codelens, int ncodes,
    const guint8 * symbols, int nsymbols, int tableNo, int tableClass)
{
  *p++ = 0xff;
  *p++ = 0xc4;                  /* DHT */
  *p++ = 0;                     /* length msb */
  *p++ = 3 + ncodes + nsymbols; /* length lsb */
  *p++ = (tableClass << 4) | tableNo;
  memcpy (p, codelens, ncodes);
  p += ncodes;
  memcpy (p, symbols, nsymbols);
  p += nsymbols;

  return (p);
}

static guint8 *
MakeDRIHeader (guint8 * p, guint16 dri)
{
  *p++ = 0xff;
  *p++ = 0xdd;                  /* DRI */
  *p++ = 0x0;                   /* length msb */
  *p++ = 4;                     /* length lsb */
  *p++ = dri >> 8;              /* dri msb */
  *p++ = dri & 0xff;            /* dri lsb */

  return (p);
}

/*
 *  Arguments:
 *    type, width, height: as supplied in RTP/JPEG header
 *    qt: quantization tables as either derived from
 *        the Q field using MakeTables() or as specified
 *        in section 4.2.
 *    dri: restart interval in MCUs, or 0 if no restarts.
 *
 *    p: pointer to return area
 *
 *  Return value:
 *    The length of the generated headers.
 *
 *    Generate a frame and scan headers that can be prepended to the
 *    RTP/JPEG data payload to produce a JPEG compressed image in
 *    interchange format (except for possible trailing garbage and
 *    absence of an EOI marker to terminate the scan).
 */
static guint
MakeHeaders (guint8 * p, int type, int width, int height, const guint8 * qt,
    guint precision, guint16 dri)
{
  guint8 *start = p;
  gint size;

  *p++ = 0xff;
  *p++ = 0xd8;                  /* SOI */

  size = ((precision & 1) ? 128 : 64);
  p = MakeQuantHeader (p, qt, size, 0);
  qt += size;

  size = ((precision & 2) ? 128 : 64);
  p = MakeQuantHeader (p, qt, size, 1);
  qt += size;

  if (dri != 0)
    p = MakeDRIHeader (p, dri);

  *p++ = 0xff;
  *p++ = 0xc0;                  /* SOF */
  *p++ = 0;                     /* length msb */
  *p++ = 17;                    /* length lsb */
  *p++ = 8;                     /* 8-bit precision */
  *p++ = height >> 8;           /* height msb */
  *p++ = height;                /* height lsb */
  *p++ = width >> 8;            /* width msb */
  *p++ = width;                 /* width lsb */
  *p++ = 3;                     /* number of components */
  *p++ = 0;                     /* comp 0 */
  if ((type & 0x3f) == 0)
    *p++ = 0x21;                /* hsamp = 2, vsamp = 1 */
  else
    *p++ = 0x22;                /* hsamp = 2, vsamp = 2 */
  *p++ = 0;                     /* quant table 0 */
  *p++ = 1;                     /* comp 1 */
  *p++ = 0x11;                  /* hsamp = 1, vsamp = 1 */
  *p++ = 1;                     /* quant table 1 */
  *p++ = 2;                     /* comp 2 */
  *p++ = 0x11;                  /* hsamp = 1, vsamp = 1 */
  *p++ = 1;                     /* quant table 1 */

  p = MakeHuffmanHeader (p, lum_dc_codelens,
      sizeof (lum_dc_codelens), lum_dc_symbols, sizeof (lum_dc_symbols), 0, 0);
  p = MakeHuffmanHeader (p, lum_ac_codelens,
      sizeof (lum_ac_codelens), lum_ac_symbols, sizeof (lum_ac_symbols), 0, 1);
  p = MakeHuffmanHeader (p, chm_dc_codelens,
      sizeof (chm_dc_codelens), chm_dc_symbols, sizeof (chm_dc_symbols), 1, 0);
  p = MakeHuffmanHeader (p, chm_ac_codelens,
      sizeof (chm_ac_codelens), chm_ac_symbols, sizeof (chm_ac_symbols), 1, 1);

  *p++ = 0xff;
  *p++ = 0xda;                  /* SOS */
  *p++ = 0;                     /* length msb */
  *p++ = 12;                    /* length lsb */
  *p++ = 3;                     /* 3 components */
  *p++ = 0;                     /* comp 0 */
  *p++ = 0;                     /* huffman table 0 */
  *p++ = 1;                     /* comp 1 */
  *p++ = 0x11;                  /* huffman table 1 */
  *p++ = 2;                     /* comp 2 */
  *p++ = 0x11;                  /* huffman table 1 */
  *p++ = 0;                     /* first DCT coeff */
  *p++ = 63;                    /* last DCT coeff */
  *p++ = 0;                     /* sucessive approx. */

  return (p - start);
};

//static GstBuffer *
static int
gst_rtp_jpeg_depay_process(GstRtpJPEGDepay *rtpjpegdepay,
                           const RTP &rtp)
{
  //TODO:  GstBuffer *outbuf;
  gint payload_len, header_len;
  const guint8 *payload;
  guint frag_offset;
  gint Q;
  gint type, width, height;
  guint16 dri, precision, length;
  const guint8 *qtable;
  // TODO: GstRTPBuffer _rtp = { NULL };

  /* // TODO:
  if (GST_BUFFER_IS_DISCONT (buf)) {
    gst_adapter_clear (rtpjpegdepay->adapter);
    rtpjpegdepay->discont = TRUE;
  }
  */

  //gst_rtp_buffer_map(buf, GST_MAP_READ, &_rtp);
  payload_len = rtp.payloadSize();//gst_rtp_buffer_get_payload_len (&_rtp);

  if (payload_len < 8)
    goto empty_packet;

  //payload = gst_rtp_buffer_get_payload(&_rtp);
  payload = (const guint8 *)rtp.payloadData();
  header_len = 0;

  /*  0                   1                   2                   3
   *  0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
   * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
   * | Type-specific |              Fragment Offset                  |
   * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
   * |      Type     |       Q       |     Width     |     Height    |
   * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
   */
  frag_offset = (payload[1] << 16) | (payload[2] << 8) | payload[3];
  type = payload[4];
  Q = payload[5];
  width = payload[6] * 8;
  height = payload[7] * 8;

  /* allow frame dimensions > 2040, passed in SDP session or media attributes
   * from gstrtspsrc.c (gst_rtspsrc_sdp_attributes_to_caps), or in caps */
  /* // No!
  if (!width)
    width = rtpjpegdepay->media_width;

  if (!height)
    height = rtpjpegdepay->media_height;
  */

  if (width == 0 || height == 0)
    goto invalid_dimension;

  GST_DEBUG_OBJECT("frag %u, type %u, Q %d, width %u, height %u",
                   frag_offset, type, Q, width, height);

  header_len += 8;
  payload += 8;
  payload_len -= 8;

  dri = 0;
  if (type > 63) {
    if (payload_len < 4)
      goto empty_packet;

    /*  0                   1                   2                   3
     *  0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     * |       Restart Interval        |F|L|       Restart Count       |
     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     */
    dri = (payload[0] << 8) | payload[1];

    GST_DEBUG_OBJECT("DRI %d", dri);

    payload += 4;
    header_len += 4;
    payload_len -= 4;
  }

  if (Q >= 128 && frag_offset == 0) {
    if (payload_len < 4)
      goto empty_packet;

    /*  0                   1                   2                   3
     *  0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     * |      MBZ      |   Precision   |             Length            |
     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     * |                    Quantization Table Data                    |
     * |                              ...                              |
     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     */
    precision = payload[1];
    length = (payload[2] << 8) | payload[3];

    GST_DEBUG_OBJECT("precision %04x, length %d", precision, length);

    if (Q == 255 && length == 0)
      goto empty_packet;

    payload += 4;
    header_len += 4;
    payload_len -= 4;

    if (length > payload_len)
      goto empty_packet;

    if (length > 0) {
      GST_LOG_OBJECT("--- Use qtable from payload %d bytes", length);
      qtable = (guint8*)payload;
    } else {
      if(rtpjpegdepay->qtables.find(Q) == rtpjpegdepay->qtables.end()) {
        GST_LOG_OBJECT("Ooops! Missing qtable for Q: %d", Q);
      }
      GST_LOG_OBJECT("--- %d qtables\n", rtpjpegdepay->qtables.size());
      qtable = rtpjpegdepay->qtables[Q];
    }

    payload += length;
    header_len += length;
    payload_len -= length;
  } else {
    length = 0;
    qtable = NULL;
    precision = 0;
  }

  if (frag_offset == 0) {
    //    GstMapInfo map;
    guint size;

    if (rtpjpegdepay->width != width || rtpjpegdepay->height != height) {
      /*
      GstCaps *outcaps;

      outcaps =
          gst_caps_new_simple ("image/jpeg", "framerate", GST_TYPE_FRACTION,
          rtpjpegdepay->frate_num, rtpjpegdepay->frate_denom, "width",
          G_TYPE_INT, width, "height", G_TYPE_INT, height, NULL);
      gst_pad_set_caps (depayload->srcpad, outcaps);
      gst_caps_unref (outcaps);
      */
      rtpjpegdepay->width = width;
      rtpjpegdepay->height = height;
    }

    GST_LOG_OBJECT("first packet, length %d", length);

    /* first packet */
    if (length == 0) {
      if (Q < 128) {
        /* no quant table, see if we have one cached */
        qtable = rtpjpegdepay->qtables[Q];
        if (!qtable) {
          GST_DEBUG_OBJECT("making Q %d table", Q);
          /* make and cache the table */
          guint8 *_qtable = (guint8*)malloc(sizeof(guint8) * 128);
          MakeTables(rtpjpegdepay, Q, _qtable);
          rtpjpegdepay->qtables[Q] = _qtable;
        } else {
          GST_DEBUG_OBJECT("using cached table for Q %d", Q);
        }
        /* all 8 bit quantizers */
        precision = 0;
      } else {
        if (!qtable)
          goto no_qtable;
      }
    }
    /* max header length, should be big enough */
    /*
    outbuf = gst_buffer_new_and_alloc (1000);
    gst_buffer_map (outbuf, &map, GST_MAP_WRITE);
    size = MakeHeaders (map.data, type, width, height, qtable, precision, dri);
    gst_buffer_unmap (outbuf, &map);
    gst_buffer_resize (outbuf, 0, size);
    */
    guint8 *_outbuf = (guint8*)malloc(1000);
    size = MakeHeaders(_outbuf, type, width, height, qtable, precision, dri);
    //    _outbuf = (guint8*)realloc(_outbuf, size);

    GST_DEBUG_OBJECT("pushing %u bytes of header", size);

    // TODO: gst_adapter_push (rtpjpegdepay->adapter, _outbuf);
    rtpjpegdepay->adapter.append((char*)_outbuf, size);

    free(_outbuf);
  }

  /* take JPEG data, push in the adapter */
  GST_DEBUG_OBJECT("pushing data at offset %d", header_len);
  /*
  outbuf = gst_rtp_buffer_get_payload_subbuffer (&_rtp, header_len, -1);
  gst_adapter_push (rtpjpegdepay->adapter, outbuf);
  outbuf = NULL;
  */
  rtpjpegdepay->adapter.append(rtp.payloadData() + header_len,
                               rtp.payloadSize() - header_len);

  //  if (gst_rtp_buffer_get_marker (&_rtp)) {
  if(rtp.marker()) {
    guint avail;
    guint8 end[2];
    //GstMapInfo map;

    /* last buffer take all data out of the adapter */
    // TODO: avail = gst_adapter_available (rtpjpegdepay->adapter);
    avail = rtpjpegdepay->adapter.size();

    GST_DEBUG_OBJECT("marker set, last buffer");

    if (avail < 2)
      goto invalid_packet;

    /* take the last bytes of the jpeg data to see if there is an EOI
     * marker */
    //TODO: gst_adapter_copy (rtpjpegdepay->adapter, end, avail - 2, 2);
    end[0] = rtpjpegdepay->adapter[avail - 2];
    end[1] = rtpjpegdepay->adapter[avail - 1];

    if (end[0] != 0xff && end[1] != 0xd9) {
      GST_DEBUG_OBJECT("no EOI marker, adding one");

      /* no EOI marker, add one */
      /*
      outbuf = gst_buffer_new_and_alloc (2);
      gst_buffer_map (outbuf, &map, GST_MAP_WRITE);
      map.data[0] = 0xff;
      map.data[1] = 0xd9;
      gst_buffer_unmap (outbuf, &map);
      */
      char buf[] = { 0xff, 0xd9 };

      // TODO: gst_adapter_push (rtpjpegdepay->adapter, buf, sizeof(buf));
      rtpjpegdepay->adapter.append(buf, sizeof(buf));

      avail += 2;
    }

    // TODO: outbuf = gst_adapter_take_buffer (rtpjpegdepay->adapter, avail);

    if (rtpjpegdepay->discont) {
      // TODO: GST_BUFFER_FLAG_SET(outbuf, GST_BUFFER_FLAG_DISCONT);
      rtpjpegdepay->discont = FALSE;
    }

    GST_DEBUG_OBJECT("returning %u bytes", avail);
    return avail;
  }

  //gst_rtp_buffer_unmap (&_rtp);

  return 0; //TODO: outbuf;

  /* ERRORS */
empty_packet:
  {
    GST_ELEMENT_WARNING("Empty Payload.");
    //gst_rtp_buffer_unmap (&_rtp);
    return 0;
  }
invalid_dimension:
  {
    GST_ELEMENT_WARNING("Invalid Dimension %dx%d.", width, height);
    //gst_rtp_buffer_unmap (&_rtp);
    return 0;
  }
no_qtable:
  {
    GST_WARNING_OBJECT("no qtable");
    //gst_rtp_buffer_unmap (&_rtp);
    return 0;
  }
invalid_packet:
  {
    GST_WARNING_OBJECT("invalid packet");
    // TODO: gst_adapter_flush (rtpjpegdepay->adapter, gst_adapter_available (rtpjpegdepay->adapter));
    //gst_rtp_buffer_unmap (&_rtp);
    return 0;
  }
}






////////////////////////////////////////////////////
/// End of depay
////////////////////////////////////////////////////

///--------------------------------------------------------------------------

struct lrtp_profile_jpeg_t {
  struct lrtp_profile_t profile; // 'Inherit' lrtp_profile_t

  std::list<RTP> rtplist;
  GstRtpJPEGDepay depay;
  uint32_t timestamp;
};


int profile_jpeg_pack(struct lrtp_profile_t *profile,
                      const char *frame, size_t framesize,
                      RTP &rtp)
{
  struct lrtp_profile_jpeg_t *p = (struct lrtp_profile_jpeg_t *)profile;

  // We assume that all frames in the same stream have unique timestamps...
  if(rtp.timestamp() != p->timestamp) {

    // TODO: If p->rtplist is not empty, clear it and issue a warning...

    GstBuffer buffer;
    buffer.data = (const unsigned char*)frame;
    buffer.size = framesize;

    GstRtpJPEGPay basepayload;
    gst_rtp_jpeg_pay_init(&basepayload);
    int ret = gst_rtp_jpeg_pay_handle_buffer(&basepayload, &buffer, p->rtplist);

    p->timestamp = rtp.timestamp();

    GST_LOG_OBJECT("ret: %d - list: %d\n", ret, p->rtplist.size());

    if(ret != GST_FLOW_OK) {
      rtp.setValid(false);
      return -1;
    }
  }

  if(p->rtplist.size()) {
    RTP r = p->rtplist.front();
    p->rtplist.pop_front();
    rtp.setMarker(r.marker());
    rtp.setPayload(r.payloadData(), r.payloadSize());

    if(p->rtplist.size()) return 1; // signal more packets
    else return framesize; // signal 'all data used'
  }

  rtp.setValid(false);

  return framesize;
}

int profile_jpeg_unpack(struct lrtp_profile_t *profile,
                        const RTP &rtp,
                        oframelist_t &framelist)
{
  struct lrtp_profile_jpeg_t *p = (struct lrtp_profile_jpeg_t *)profile;

  {
    int ret = gst_rtp_jpeg_depay_process(&p->depay, rtp);
    if(ret > 0) {
      outputframe_t *of = new outputframe_t();

      of->size = ret;
      char *buf = (char*)malloc(of->size);

      memcpy(buf, p->depay.adapter.data(), ret);
      p->depay.adapter.clear();

      of->data = buf;
      of->ts = rtp.timestamp();

      framelist.push_back(of);
    }
  }

  return 0;
}

void profile_jpeg_destroy(struct lrtp_profile_t *profile)
{
  struct lrtp_profile_jpeg_t *p = (struct lrtp_profile_jpeg_t *)profile;
  delete p;
}

struct lrtp_profile_t *profile_jpeg_create(struct lrtp_t *lrtp,
                                           unsigned int csrc,
                                           va_list vp)
{
  struct lrtp_profile_jpeg_t *p = new struct lrtp_profile_jpeg_t;

  p->profile.pack = profile_jpeg_pack;
  p->profile.unpack = profile_jpeg_unpack;
  p->profile.destroy = profile_jpeg_destroy;

  p->timestamp = 0xffffffff;

  gst_rtp_jpeg_depay_init(&p->depay);

  while(true) {
    int type = va_arg(vp, int);
    if(type == OPTION_END) break;

    switch(type) {
    default:
      // TODO: Unknown arg type
      break;
    }
  }

  return &p->profile;
}