#include <stdlib.h>
#include <stdio.h>
#include <sys/time.h>

#include "packet.h"
#include "params.h"
#include "window_state.h"
#include "receive.h"

int pdebug = 0;

/*
 * The object of the code in this file is to create a valid
 * H261 bitsream for XIL in case a frame is dropped during
 * broadcast. For this example (xil_broadcast/xil_receive),
 * an XIL H261 frame is broken into packets and broadcasted.
 * The last packet of a frame actual contains the first
 * four bytes of the next frame (which contains the PSC for
 * the next frame). 
 *
 * If all of the packets of a frame do not arrive, the complete
 * frame is dropped. If a packet from a future frame arrives
 * before the packets from the current frame arrive, the
 * current frame is dropped. This is all handled by xilvc.c.
 *
 * The situation we are trying to correct is when a complete
 * frame is dropped. For a given frame X successfully received,
 * we know how the frame ends because we also received the
 * first 4 bytes of the frame X+1 which was trasmitted as part
 * of frame X in the last packet of X. These bytes of X+1 are
 * saved as PREV.
 *
 * Now assume that a frame Y arrives. This may actually be
 * frame X+1 if no frames were dropped by the network. If
 * some number of frames N were dropped, then Y is actually
 * frame X+N. Let the first 4 bytes of frame Y be NEW.
 *
 * A simple comparison of PREV and NEW can tell if a frame
 * was dropped by the network. If PREV != NEW then at least
 * one frame was dropped by the network and the bitstream
 * needs to be repaired. If PREV == NEW, either Y is indeed
 * frame X+1 (no frame drop) or the beginning of Y is the
 * same as that of frame X+1. In either case, no repair is
 * required.
 *
 * The repair of the bitstream can be handled in three ways.
 * The easiest method is by adding 0 bytes to the beginning
 * of frame Y. This is only possible if the H261 decoder can
 * handle zero stuffing. Alas, XIL's s/w decoder can not.
 * 
 * The second method is by bitshifting the new frame Y to the 
 * same PSC bit boundary as frame X+1 (which we know because 
 * of the value of PREV). Bitshifting waste CPU cycles. 
 *
 * The final method is to add macroblock stuffing. The
 * stuffing that is added at frame Y will actually be
 * a part of frame X so when X is added to the XIL cis,
 * it must not be put in as a 'full' frame but as an
 * 'unknown number of frames'. You must also keep XIL
 * from decoding X until Y and its stuffing for X have
 * been added.
 *
 * A macroblock stuffing pattern is an odd number of bits.
 * Thus it's possible to re-bitalign two no-adjacent frames
 * by adding some number of the macroblock stuffing pattern
 * to the beginning of Y. The number of 'stuffers' is determined
 * by examining how many bits of the PSC there were in frame X+1
 * as compared to how many are in frame Y. Recall that we have
 * frame X+1 PSC (or some part of it) because it was sent as
 * part of frame X.
 */

/*
 * Note: within comments below...
 *
 *  frame X    : previous received frame
 *  data X+1   : transmitted in the last packet of frame X are the
 *                 1st 4 bytes on the frame that follows X
 *  frame Y    : the current received frame
 *  data  Y+1  : transmitted in the last packet of frame Y are the
 *                 1st 4 bytes on the frame that follows Y
 *
 *  Variables:
 *    prev = data X+1
 *    new  = 1st 4 bytes of frame Y
 */

/*
  Below, the LAST BYTE X represent the last byte of the previous frame X
  where the S bits represent bits that are part of X. The PSC is (in
  binary: 0000 0000 0000 0001 0000. From data X+1 we can determine
  the S bits in the last byte of X. If stuffing is required, the
  binary bytes following the LAST BYTE X pattern below reprent the
  stuffing pattern. From the first bytes of frame Y, we know how many
  leading zeros are required to make a valid PSC. Thus, given the
  LAST BYTE X, we add bytes from its stuffing pattern until we have
  the required number of leading zeros.

  LAST BYTE X      1        2         3         4         5         6         7         8         9        10         11
  -----------  --------  --------  --------  --------  --------  --------  --------  --------  --------  -------   --------
0  SSSS SSSS:  00000001  11100000  00111100  00000111  10000000  11110000  00011110  00000011  11000000  01111000  00001111  

  LAST BYTE X      1        2         3         4         5         6         7         8         9        10         11
  -----------  --------  --------  --------  --------  --------  --------  --------  --------  --------  -------   --------
1  SSSS SSS0:  00000011  11000000  01111000  00001111  00000001  11100000  00111100  00000111  10000000  11110000  00011110  

  LAST BYTE X      1        2         3         4         5         6         7         8         9        10         11
  -----------  --------  --------  --------  --------  --------  --------  --------  --------  --------  -------   --------
2  SSSS SS00:  00000111  10000000  11110000  00011110  00000011  11000000  01111000  00001111  00000001  11100000  00111100  

  LAST BYTE X      1        2         3         4         5         6         7         8         9        10         11
  -----------  --------  --------  --------  --------  --------  --------  --------  --------  --------  -------   --------
3  SSSS S000:  00001111  00000001  11100000  00111100  00000111  10000000  11110000  00011110  00000011  11000000  01111000  

  LAST BYTE X      1        2         3         4         5         6         7         8         9        10         11
  -----------  --------  --------  --------  --------  --------  --------  --------  --------  --------  -------   --------
4  SSSS 0000:  00011110  00000011  11000000  01111000  00001111  00000001  11100000  00111100  00000111  10000000  11110000  

  LAST BYTE X      1        2         3         4         5         6         7         8         9        10         11
  -----------  --------  --------  --------  --------  --------  --------  --------  --------  --------  -------   --------
5  SSS0 0000:  00111100  00000111  10000000  11110000  00011110  00000011  11000000  01111000  00001111  00000001  11100000  

  LAST BYTE X      1        2         3         4         5         6         7         8         9        10         11
  -----------  --------  --------  --------  --------  --------  --------  --------  --------  --------  -------   --------
6  SS00 0000:  01111000  00001111  00000001  11100000  00111100  00000111  10000000  11110000  00011110  00000011  11000000  

  LAST BYTE X      1        2         3         4         5         6         7         8         9        10         11
  -----------  --------  --------  --------  --------  --------  --------  --------  --------  --------  -------   --------
7  S000 0000:  11110000  00011110  00000011  11000000  01111000  00001111  00000001  11100000  00111100  00000111  10000000  

*/

unsigned char stuff_data[8][11] = {
  { 0x01, 0xE0, 0x3C, 0x07, 0x80, 0xF0, 0x1E, 0x03, 0xC0, 0x78, 0x0F },
  { 0x03, 0xC0, 0x78, 0x0F, 0x01, 0xE0, 0x3C, 0x07, 0x80, 0xF0, 0x1E },
  { 0x07, 0x80, 0xF0, 0x1E, 0x03, 0xC0, 0x78, 0x0F, 0x01, 0xE0, 0x3C },
  { 0x0F, 0x01, 0xE0, 0x3C, 0x07, 0x80, 0xF0, 0x1E, 0x03, 0xC0, 0x78 },
  { 0x1E, 0x03, 0xC0, 0x78, 0x0F, 0x01, 0xE0, 0x3C, 0x07, 0x80, 0xF0 },
  { 0x3C, 0x07, 0x80, 0xF0, 0x1E, 0x03, 0xC0, 0x78, 0x0F, 0x01, 0xE0 },
  { 0x78, 0x0F, 0x01, 0xE0, 0x3C, 0x07, 0x80, 0xF0, 0x1E, 0x03, 0xC0 },  
  { 0xF0, 0x1E, 0x03, 0xC0, 0x78, 0x0F, 0x01, 0xE0, 0x3C, 0x07, 0x80 },
};


/*
 * These are leading zero masks. For example, if we need
 * 0 leading zeros, we add bytes from the stuffing pattern
 * until the last byte we have added is 0x0F.
 */

unsigned char stuff_mask[8] = {
  0x0F, 0x1E, 0x3C, 0x78,
  0xF0, 0xE0, 0xC0, 0x80
};

typedef struct _pcs_mask_t{
  unsigned int mask;
  unsigned int psc;
} pcs_mask_t;

pcs_mask_t pcs_masks[8] = {
  { 0xFFF80000, 0x00800000 },
  { 0xFFFC0000, 0x00400000 },
  { 0xFFFE0000, 0x00200000 },
  { 0xFFFF0000, 0x00100000 },
  { 0xFFFF8000, 0x00080000 },
  { 0xFFFFC000, 0x00040000 },
  { 0xFFFFE000, 0x00020000 },
  { 0xFFFFF000, 0x00010000 }
};



/*
 * findp:
 *  Determine position of PSC. There are only 8 possibilities.
 *  The number of leading PSC zeros is (return value + 8).
 *  The number of PSC zeros not present is (7 - return value).
 *  
 */
int findp(int value)
{
  int i;

  for (i=0; i<8; i++){
    if ((value & pcs_masks[i].mask) == pcs_masks[i].psc){
      if (pdebug) printf("[%d]  v: 0x%.8X  m: 0x%.8X  p: 0x%.8X\n",
                         i, value,(value & pcs_masks[i].mask),pcs_masks[i].psc);     
      break;
    }
  }

  if (i==8)
    printf("did not find psc in 0x%.8X\n",value);

  return i;
}
  
void create_big_h261_packet(BigPacketType * big, PacketType * packetlist[])
{
    int i;
    unsigned char *dstptr;
    int total_size = 0;
    int size;
    int last_packet_index;
    int ph, nh;
    
    static unsigned int prev = 0x00010000; /* initial prev == a FULL PSC code */
    unsigned int new = 0x00010000;
    unsigned char* tmpptr;

    if (pdebug) printf("###############################################\n");
    
    /*
     * Determine index for last packet which contains data X+1
     */
    for (i = 0; i < MAXPACKETS; i++) {
      if (packetlist[i])
        last_packet_index = i;
    }

    
    dstptr = big->data;

    /*
     * Get 1st 4 bytes of frame Y
     */
    if (packetlist[0]){
      tmpptr = packetlist[0]->data;
      new = (tmpptr[0]<<24) | (tmpptr[1]<<16) | (tmpptr[2]<<8) | (tmpptr[3]);
    } else {
      fprintf(stderr,"internal failure: no first packet data\n");
    }
    
    /* Get PSC placement for X+1 */
    ph = findp(prev);
    /* Get PSC placement for Y */
    nh = findp(new);

    if (pdebug) printf("new %.8x prev %.8x ph %d nh %d\n",new,prev,ph,nh);
    
    if ((new == prev) || (ph == nh)){
      /*
       * Either
       *  (new == prev)  no dropped frame or identical 1st 4 bytes
       *  (ph == nh)     X+1 and Y are PSC aligned
       *
       * Thus just add frame Y as is
       */

      if (pdebug) printf("OK to ADD Frame\n");
      
    } else {

      unsigned char stuff_buffer[15];
      int mask;
      int pattern;
      int bytes;
      int i;
      
      if (pdebug) printf("****************** STUFFING *******************\n");

      /*
       * mask    = 7 - nh = # additional leading zeros needed for PSC Y
       * pattern = 7 - ph = # zeros in LAST BYTE X of PSC X+1
       */

      mask    = stuff_mask[7 - nh];      
      pattern = 7 - ph;

      i = bytes = 0;
      do {
        stuff_buffer[i] = stuff_data[pattern][i];
        bytes++;
      } while ((stuff_buffer[i] != mask) && (++i)<11);
      
      if (i==11){
        fprintf(stderr,"internal failure: end stuff mask not found\n");
      }

      if (pdebug) printf("stuffing: ");
      for (i=0; i<bytes; i++)
        if (pdebug) printf("%.2x",stuff_buffer[i]);
      if (pdebug) printf("\n");
      
      /* copy in stuff buffer */
      size = bytes;
      if (pdebug) printf("copy in stuff_buffer %d\n",size);
      memcpy(dstptr, stuff_buffer, size);
      total_size += size;
      dstptr += size;

      
    }

    /* copy in frame Y */      
    for (i = 0; i < last_packet_index; i++) {
      if (packetlist[i]) {
        size = packetlist[i]->header.size;
        memcpy(dstptr, packetlist[i]->data, size);
        total_size += size;
        dstptr += size;
      }
    }
    
    big->header = packetlist[0]->header;    
    big->header.size = total_size;

    if (packetlist[last_packet_index]){
      tmpptr = packetlist[last_packet_index]->data;
      prev = (tmpptr[0]<<24) | (tmpptr[1]<<16) | (tmpptr[2]<<8) | (tmpptr[3]);
    } else {
      fprintf(stderr,"internal failure: no last packet data\n");
    }
    
    if (pdebug) printf("next frame 0x%.8X\n",prev);
    if (pdebug) printf("===============================================\n");
}
