/* -*- Mode: C++; tab-width: 2; indent-tabs-mode: nil; c-basic-offset: 2 -*- */
/* vim: set et sw=2 ts=2: */
/***************************************************************************
 *            rtp_profile_amrwb.cc
 *
 *  Wed Oct  2 14:43:05 CEST 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
 */
#include "rtp_profile_amrwb.h"

#include "rtp_profile.h"
#include "rtp.h"

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

/*
The timestamp clock frequency is the same as the sampling frequency, so the
timestamp unit is in samples.

The duration of one speech frame-block is 20 ms for both AMR and AMR-WB.  For
AMR, the sampling frequency is 8 kHz, corresponding to 160 encoded speech
samples per frame from each channel.  For AMR-WB, the sampling frequency is
16 kHz, corresponding to 320 samples per frame from each channel.  Thus, the
timestamp is increased by 160 for AMR and 320 for AMR-WB for each consecutive
frame-block.

QUESTION: Should we use 16kHz or 19351Hz?
ANSWER: The encoder and decoder respectively should use 19531Hz, the rest of
the system can assume 16kHz without loss of quality.
 */

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

  int frame_type_index;
};

// Frame sizes based on frame type index:
static const int wb_frame_size[16] = {
  17, 23, 32, 36, 40, 46, 50, 58,
  60, 5, -1, -1, -1, -1, -1, 0
};

/*
 *   0 1 2 3 4 5 6 7
 *  +-+-+-+-+-+-+-+-+
 *  |F|  FT   |Q|P|P|
 *  +-+-+-+-+-+-+-+-+
 */

/*
 *  F (1 bit): If set to 1, indicates that this frame is followed by
 *     another speech frame in this payload; if set to 0, indicates that
 *     this frame is the last frame in this payload.
 */
static int is_last_frame(const char *frame_header)
{
  return ( (*frame_header >> 7) & 0x1 ) == 0;
}

static void set_is_last_frame(char *frame_header, int last)
{
  last = 1 - last; // Invert 'last': 0 means last, 1 means more
  *frame_header = (*frame_header & 0x7f) | ( (last & 0x1) << 7);
}

/*
 * FT (4 bits): Frame type index, indicating either the AMR or AMR-WB
 *     speech coding mode or comfort noise (SID) mode of the
 *     corresponding frame carried in this payload.
 */
static int frame_type_index(const char *frame_header)
{
  return (*frame_header >> 3) & 0xf;
}

static void set_frame_type_index(char *frame_header, int index)
{
  *frame_header = (*frame_header & 0x87) | ( (index & 0xf) << 3);
}

/*
 *  Q (1 bit): Frame quality indicator.  If set to 0, indicates the
 *     corresponding frame is severely damaged and the receiver should
 *     set the RX_TYPE (see [6]) to either SPEECH_BAD or SID_BAD
 *     depending on the frame type (FT).
 */
/*
static int quality_indicator(const char *frame_header)
{
  return (*frame_header >> 2) & 0x1;
}
*/
static void set_quality_indicator(char *frame_header, int quality)
{
  *frame_header = (*frame_header & 0xf8) | ( (quality & 0x1) << 2);
}


// http://tools.ietf.org/html/rfc3267#section-4.4
int profile_amrwb_pack(struct lrtp_profile_t *profile,
                       const char *input, size_t inputsize,
                       RTP &rtp)
{
  // NOTE: This implementation does not support frame interleaving...

  struct lrtp_profile_amrwb_t *p = (struct lrtp_profile_amrwb_t *)profile;

  size_t frameheader_size = 1;
  size_t cmr_size = 1;

  // Frame size based on frame_type_index,
  size_t frame_size = wb_frame_size[p->frame_type_index];

  // This is the maximum number of frames we have room for in a single RTP
  // payload.
  size_t max_num_frames =
    (MAX_RTP_PAYLOAD_SIZE - cmr_size) / (frame_size + frameheader_size);

  size_t cpsz = inputsize;

  //printf("cpsz: %d\n", cpsz);

  size_t num_frames = cpsz / frame_size;

  if(num_frames == 0) {
    printf("Garbage at the end of the buffer\n");
    rtp.setValid(false);
    return -1;
  }

  if(num_frames > max_num_frames) {
    num_frames = max_num_frames;
  }

  // Make sure num_frames > 1 is an even number (don't split stereo frames)
  if(num_frames != 1) num_frames = (num_frames / 2) * 2;

  cpsz = num_frames * frame_size; // Actually consumed bytes.

  char *payload = (char*)malloc(MAX_RTP_PAYLOAD_SIZE);

  /*   0                   1
   *   0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5
   *  +-+-+-+-+-+-+-+-+- - - - - - - -
   *  |  CMR  |R|R|R|R|  ILL  |  ILP  |
   *  +-+-+-+-+-+-+-+-+- - - - - - - -
   */
  // If a terminal has no preference in which mode to receive, it SHOULD set
  // CMR=15 in all its outbound payloads.
  char *CMR = payload;
  *CMR = 15;
 
  // The ILL and ILP fields are not used (see note above).

  char *frame_header = CMR + cmr_size;
  for(size_t i = 0; i < num_frames; i++) {
    // 0 if this is the last frame.
    set_is_last_frame(frame_header, i == (num_frames - 1) );

    // Frame type index (4 bits):
    set_frame_type_index(frame_header, p->frame_type_index);

    // Frame quality indicator (1 bit): Set to 0 to indicate damaged frame.
    set_quality_indicator(frame_header, 1);

    frame_header += frameheader_size;
  }

  char *frame_data = frame_header;
  
  size_t payload_size = cmr_size;
  const char *frame_offset = input;
  for(size_t i = 0; i < num_frames; i++) {
    memcpy(frame_data, frame_offset, frame_size);
    frame_data += frame_size;
    frame_offset += frame_size;
    payload_size += frameheader_size + frame_size;
  }  
  /*
  printf("---------------------------------------- New packet:\n");
  printf("payload (pack):\n");
  for(int i = 0; i < payload_size; i++) {
    printf("%02x ", (unsigned char)payload[i]);
  }
  printf("\n");
  */
  //size_t payload_size = cmr_size + num_frames*(frame_size + frameheader_size);
  rtp.setPayload(payload, payload_size);
  rtp.setValid(true);
  /*
  printf("payload (pack):\n");
  for(int i = 0; i < payload_size; i++) {
    printf("%02x ", (unsigned char)payload[i]);
  }
  printf("\n");
  */
  free(payload);

  return cpsz;
}

int profile_amrwb_unpack(struct lrtp_profile_t *profile,
                         const RTP &rtp,
                         std::list<outputframe_t *> &framelist)
{
  //struct lrtp_profile_amrwb_t *p = (struct lrtp_profile_amrwb_t *)profile;

  //size_t size = rtp.payloadSize();
  const char *payload = rtp.payloadData();

  unsigned int timestamp = rtp.timestamp();

  /*
  printf("---------------------------------------- New packet:\n");
  printf("payload (unpack):\n");
  for(int i = 0; i < size; i++) {
    printf("%02x ", (unsigned char)payload[i]);
  }
  printf("\n");
  */
  // Read CMR:
  size_t cmr_size = 1;
  char CMR = payload[0];
  if(CMR != 15) {
    printf("CMR not 15\n");
    return UNPACK_ERROR;
  }

  // TODO: What to do if there are 0 frames (and therefore no header to set the
  // 'last frame' bit in....)

  // Read ToC:
  size_t frameheader_size = 1;
  const char *frame_toc;
  frame_toc = payload + cmr_size;

  size_t frame_num = 0;
  const char *frame_data = frame_toc;
  while(!is_last_frame(frame_data)) {
    frame_num++;
    frame_data += frameheader_size;
  }
  frame_num++;
  frame_data += frameheader_size;

  while(frame_num) {
    size_t frame_size_idx = frame_type_index(frame_toc);
    size_t frame_size = wb_frame_size[frame_size_idx];

    outputframe_t *of = new outputframe_t();
    of->ts = timestamp;
    of->size = frame_size;
    char *buf = (char*)malloc(of->size);
    memcpy(buf, frame_data, frame_size);
    of->data = buf;

    //printf("push amrwb frame (size: %d)\n", of->size);
    framelist.push_back(of);

    if(is_last_frame(frame_toc)) return 0;

    frame_toc += frameheader_size;
    frame_data += frame_size;

    int num_samples = 1; // TODO: Figure out how many samples were in this package
    timestamp += num_samples;

    frame_num--;
  }

  printf("Error, missed the last_frame bit\n");

  return 1; // Error, missed the last_frame bit
}

void profile_amrwb_destroy(struct lrtp_profile_t *profile)
{
  struct lrtp_profile_amrwb_t *p = (struct lrtp_profile_amrwb_t *)profile;
  delete p;
}

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

  p->profile.pack = profile_amrwb_pack;
  p->profile.unpack = profile_amrwb_unpack;
  p->profile.destroy = profile_amrwb_destroy;

  p->frame_type_index = 8; // Default: AMR-WB 23.85 kbit/s

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

    switch(type) {
    case OPTION_AMRWB_FRAME_TYPE_INDEX:
      p->frame_type_index = va_arg(vp, int);
      break;
    default:
      // TODO: Unknown arg type
      break;
    }
  }

  return (struct lrtp_profile_t *)p;
}