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

#include "logutils.h"
#include "packetbuffer.h"

#define LOGCHANNEL "PACKETBUFFER"

packetbuffer_t *packetbuffer_create(int maxitems, int buffersize)
{
  packetbuffer_t *pb=(packetbuffer_t*) calloc(sizeof(packetbuffer_t),1);

  pthread_mutexattr_t mutexAttr;
  pthread_mutexattr_init(&mutexAttr);
  pthread_mutexattr_settype(&mutexAttr, PTHREAD_MUTEX_RECURSIVE_NP);

  pthread_condattr_t condAttr;
  pthread_condattr_init(&condAttr);

  pthread_mutex_init(&pb->mutex, &mutexAttr);
  pthread_cond_init(&pb->cond, &condAttr);

  pb->maxbuffersize=buffersize;
  pb->maxitems=maxitems;
  pb->nextitem=0;
  pb->nextseqno=1;

  pb->item=(packetbufferitem_t*) calloc(sizeof(packetbufferitem_t), maxitems);

  for (int i=0;i<maxitems;i++)
    {
      pb->item[i].packetlen=0;
      pb->item[i].seqno=-1;
      pb->item[i].packet=(char*) malloc(buffersize);
    }

  return pb;
}

packetbufferstate_t *packetbuffer_createstate(packetbuffer_t *pb)
{
  packetbufferstate_t *pbs=(packetbufferstate_t*) calloc(sizeof(packetbufferstate_t),1);

  pthread_mutex_lock(&pb->mutex);

  pbs->nextitem=pb->nextitem;
  pbs->nextseqno=pb->nextseqno;

  pthread_mutex_unlock(&pb->mutex);

  return pbs;
}

void packetbuffer_deletestate(packetbuffer_t *pb, packetbufferstate_t *pbs)
{
  pb=NULL; // squelch unused argument warning.
  free(pbs);
}

void packetbuffer_addpacket(packetbuffer_t *pb, void *buf, int packetlen)
{
  int idx;

  assert(packetlen>=0);
  assert(packetlen<=pb->maxbuffersize);

  pthread_mutex_lock(&pb->mutex);

  // find our entry in the circular buffer
  idx=pb->nextitem;
  pb->nextitem++;
  if (pb->nextitem>=pb->maxitems)
    pb->nextitem=0;

  pb->item[idx].packetlen=packetlen;
  pb->item[idx].seqno=pb->nextseqno;
  memcpy(pb->item[idx].packet, buf, packetlen);

  pb->nextseqno++;

  pthread_cond_broadcast(&pb->cond);
  pthread_mutex_unlock(&pb->mutex);
  
}

void packetbuffer_getpacket(packetbuffer_t *pb, packetbufferstate_t *pbs, void *buf, int *packetlen)
{
  pthread_mutex_lock(&pb->mutex);

  // if we haven't received any packets yet, wait.
  while (pb->item[pbs->nextitem].seqno<pbs->nextseqno)
    {
      pthread_cond_wait(&pb->cond, &pb->mutex);
    }

  // check if we can grab the next packet right now (common/easy case)
  if (pb->item[pbs->nextitem].seqno==pbs->nextseqno)
    {
      memcpy(buf, pb->item[pbs->nextitem].packet, pb->item[pbs->nextitem].packetlen);
      *packetlen=pb->item[pbs->nextitem].packetlen;

      pbs->nextseqno++;
      pbs->nextitem++;
      if (pbs->nextitem>=pb->maxitems)
	pbs->nextitem=0;

      pthread_mutex_unlock(&pb->mutex);
      return;
    }

  // we've dropped at least one packet. Search for the lowest sequence
  // number that is >= pbs->nextseqno note: we're guaranteed to find a
  // newer packet.
  int bestitem=0;

  for (int i=1;i<pb->maxitems;i++)
    {
      if (pb->item[i].seqno >= pbs->nextseqno && 
	  pb->item[i].seqno < pb->item[bestitem].seqno)
	{
	  bestitem=i;
	}
    }

  log(LOG_WARN, LOGCHANNEL,"%08x: dropped %i packets!", pbs, pb->item[bestitem].seqno-pbs->nextseqno);

  memcpy(buf, pb->item[bestitem].packet, pb->item[bestitem].packetlen);
  pbs->nextseqno=pb->item[bestitem].seqno+1;
  pbs->nextitem=bestitem+1;

  if (pbs->nextitem>=pb->maxitems)
    pbs->nextitem=0;
  
  pthread_mutex_unlock(&pb->mutex);
}