#include <stdlib.h>
#include <string.h>
#include "gen_labelset.h"
#include "gen_constants.h"

const unsigned char B_bytes[] = {
  0x58, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 
  0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 
  0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 
  0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66,
};

unsigned char* buffer_add(unsigned char* bufptr, const unsigned char* bufend,
                          const unsigned char* in, const unsigned long in_len)
{
  unsigned long count = 0;

  if (bufptr == NULL || bufend == NULL || bufptr > bufend)
    return NULL;
  if (in == NULL && in_len != 0)
    return NULL;
  if (bufend - bufptr < in_len)
    return NULL;

  for (count=0; count < in_len; count++) {
    if (bufptr >= bufend)
      return NULL;
    *bufptr++ = *in++;
  }
  return bufptr;
}

unsigned char* buffer_pad(const unsigned char* buf, unsigned char* bufptr, const unsigned char* bufend)
{
  unsigned long count = 0;
  unsigned long pad_len = 0;

  if (buf == NULL || bufptr == NULL || bufend == NULL || bufptr >= bufend || bufptr < buf)
    return NULL;

  pad_len = (BLOCKLEN - ((bufptr-buf) % BLOCKLEN)) % BLOCKLEN;
  if (bufend - bufptr < pad_len)
    return NULL;

  for (count=0; count < pad_len; count++) {
    if (bufptr >= bufend)
      return NULL;
    *bufptr++ = 0;
  }
  return bufptr;
}

int labelset_new(unsigned char* labelset, unsigned long* labelset_len, const unsigned long labelset_maxlen,
                 const unsigned char* protocol_name, const unsigned char protocol_name_len,
                 const unsigned char* customization_label, const unsigned char customization_label_len)
{
  unsigned char* bufptr;

  *labelset_len = 0;
  if (labelset == NULL)
    return -1;
  if (labelset_len == NULL)
    return -1;
  if (labelset_maxlen > LABELSETMAXLEN)
    return -1;
  if (labelset_maxlen < 3 + protocol_name_len + customization_label_len)
    return -1;
  if (protocol_name == NULL && protocol_name_len != 0)
    return -1;
  if (customization_label == NULL && customization_label_len != 0)
    return -1;
  if (protocol_name_len > LABELMAXLEN)
    return -1;
  if (customization_label_len > LABELMAXLEN)
    return -1;

  bufptr = labelset;
  *bufptr++ = 2;
  *bufptr++ = protocol_name_len;
  bufptr = buffer_add(bufptr, labelset + labelset_maxlen, protocol_name, protocol_name_len); 
  if (bufptr != NULL && bufptr < labelset + labelset_maxlen)
    *bufptr++ = customization_label_len;
  bufptr = buffer_add(bufptr, labelset + labelset_maxlen, 
                      customization_label, customization_label_len);

  if (bufptr != NULL && bufptr - labelset == 3 + protocol_name_len + customization_label_len) {
    *labelset_len = bufptr - labelset;
    return 0;
  }
  return -1;
}


int labelset_add(unsigned char* labelset, unsigned long* labelset_len, const unsigned long labelset_maxlen,
              const unsigned char* label, const unsigned char label_len)
{
  unsigned char* bufptr;
  if (labelset_len == NULL)
    return -1;
  if (*labelset_len > LABELSETMAXLEN || labelset_maxlen > LABELSETMAXLEN)
    return -1;
  if (*labelset_len >= labelset_maxlen || *labelset_len + label_len + 1 > labelset_maxlen)
    return -1;
  if (*labelset_len < 3 || labelset_maxlen < 4)
    return -1;
  if (label_len > LABELMAXLEN)
    return -1;

  labelset[0]++;
  labelset[*labelset_len] = label_len;
  bufptr = labelset + *labelset_len + 1;
  bufptr = buffer_add(bufptr, labelset + labelset_maxlen, label, label_len);
  if (bufptr == NULL)
    return -1;
  if (bufptr - labelset >= labelset_maxlen)
    return -1;
  if (bufptr - labelset != *labelset_len + 1 + label_len)
    return -1;

  *labelset_len += (1 + label_len);
  return 0;
}

int labelset_validate(const unsigned char* labelset, const unsigned long labelset_len)
{
  unsigned char num_labels = 0;
  unsigned char count = 0;
  unsigned long offset = 0;
  unsigned char label_len = 0;

  if (labelset == NULL)
    return -1;
  if (labelset_len < 3 || labelset_len > LABELSETMAXLEN)
    return -1;

  num_labels = labelset[0];
  offset = 1;
  for (count = 0; count < num_labels; count++) {
    label_len = labelset[offset];
    if (label_len > LABELMAXLEN)
      return -1;
    offset += 1 + label_len;
    if (offset > labelset_len)
      return -1;
  }
  if (offset != labelset_len)
    return -1;
  return 0;
}

int labelset_is_empty(const unsigned char* labelset, const unsigned long labelset_len)
{
  if (labelset_len != 3)
    return 0;
  return 1;
}