#ifndef CN_ENCODER_C
#define CN_ENCODER_C

#ifdef  __cplusplus
extern "C" {
#endif
#ifdef EMACS_INDENTATION_HELPER
} /* Duh. */
#endif

#include <string.h>
#include <stdbool.h>
#include <assert.h>
#include <limits.h>
#include <math.h>
#include "cn-cbor.h"
#include "cbor.h"

// Provide our own implementation of htons() and htonl() to avoid inclusion of inet.h or arpa/inet,h,
// which can create unintended depedencies.

static uint16_t htons(const uint16_t host16) {
  uint16_t be16;
  uint8_t *be16_ptr = (uint8_t*)&be16;

  be16_ptr[0] = (host16 >> 8) & 0xff;
  be16_ptr[1] = (host16 >> 0) & 0xff;

  return be16;
}

static uint32_t htonl(const uint32_t host32) {
  uint32_t be32;
  uint8_t *be32_ptr = (uint8_t*)&be32;

  be32_ptr[0] = (host32 >> 24) & 0xff;
  be32_ptr[1] = (host32 >> 16) & 0xff;
  be32_ptr[2] = (host32 >> 8)  & 0xff;
  be32_ptr[3] = (host32 >> 0)  & 0xff;

  return be32;
}

#define hton8p(p) (*(uint8_t*)(p))
#define hton16p(p) (htons(*(uint16_t*)(p)))
#define hton32p(p) (htonl(*(uint32_t*)(p)))
static uint64_t hton64p(const uint8_t *p) {
  /* TODO: does this work on both BE and LE systems? */
  uint64_t ret = hton32p(p);
  ret <<= 32;
  ret |= hton32p(p+4);
  return ret;
}

typedef struct _write_state
{
  uint8_t *buf;
  int offset;
  int size;
} cn_write_state;

#define ensure_writable(sz) if ((ws->offset<0) || (ws->offset + (sz) > ws->size)) { \
	ws->offset = -1; \
	return; \
}

#define write_byte_and_data(b, data, sz) \
if (ws->buf == NULL) { ws->offset += sz + 1; } \
else { \
ws->buf[ws->offset++] = (b); \
memcpy(ws->buf + ws->offset, (data), (sz)); \
ws->offset += sz; \
}

#define write_byte(b) \
if (ws->buf == NULL) { ws->offset++; } \
else { ws->buf[ws->offset++] = (b); } \

#define write_byte_ensured(b) \
ensure_writable(1); \
write_byte(b); \

static uint8_t _xlate[] = {
  IB_FALSE,    /* CN_CBOR_FALSE */
  IB_TRUE,     /* CN_CBOR_TRUE */
  IB_NIL,      /* CN_CBOR_NULL */
  IB_UNDEF,    /* CN_CBOR_UNDEF */
  IB_UNSIGNED, /* CN_CBOR_UINT */
  IB_NEGATIVE, /* CN_CBOR_INT */
  IB_BYTES,    /* CN_CBOR_BYTES */
  IB_TEXT,     /* CN_CBOR_TEXT */
  IB_BYTES,    /* CN_CBOR_BYTES_CHUNKED */
  IB_TEXT,     /* CN_CBOR_TEXT_CHUNKED */
  IB_ARRAY,    /* CN_CBOR_ARRAY */
  IB_MAP,      /* CN_CBOR_MAP */
  IB_TAG,      /* CN_CBOR_TAG */
  IB_PRIM,     /* CN_CBOR_SIMPLE */
  0xFF,        /* CN_CBOR_DOUBLE */
  0xFF         /* CN_CBOR_INVALID */
};

static inline bool is_indefinite(const cn_cbor *cb)
{
  return (cb->flags & CN_CBOR_FL_INDEF) != 0;
}

static void _write_positive(cn_write_state *ws, cn_cbor_type typ, uint64_t val) {
  uint8_t ib;
  assert((size_t)typ < sizeof(_xlate));

  ib = _xlate[typ];
  if (ib == 0xFF) {
    ws->offset = -1;
    return;
  }

  if (val < 24) {
    ensure_writable(1);
    write_byte(ib | val);
  } else if (val < 256) {
    ensure_writable(2);
    write_byte(ib | 24);
    write_byte((uint8_t)val);
  } else if (val < 65536) {
    uint16_t be16 = (uint16_t)val;
    ensure_writable(3);
    be16 = hton16p(&be16);
    write_byte_and_data(ib | 25, (const void*)&be16, 2);
  } else if (val < 0x100000000L) {
    uint32_t be32 = (uint32_t)val;
    ensure_writable(5);
    be32 = hton32p(&be32);
    write_byte_and_data(ib | 26, (const void*)&be32, 4);
  } else {
    uint64_t be64;
    ensure_writable(9);
    be64 = hton64p((const uint8_t*)&val);
    write_byte_and_data(ib | 27, (const void*)&be64, 8);
  }
}

#ifndef CBOR_NO_FLOAT
static void _write_double(cn_write_state *ws, double val)
{
  float float_val = val;
  
  /* NOTE - the following code block assures portability across
            different OSs, it is even necessary when running
	    different OSs under same toolchain. */
  if (isnan(val)) {
      float_val = NAN;
  } else if (isinf(val) && isgreater(val, 0)) {
      float_val = INFINITY;
  } else if (isinf(val) && isless(val, 0)) {
      float_val = -INFINITY;
  }

  if (((float_val == val) && !isnan(val)) || (isinf(val) && isinf(float_val))) { /* 32 bits is enough and we aren't NaN or INFINITY */
    uint32_t be32;
    uint16_t be16, u16;
    union {
      float f;
      uint32_t u;
    } u32;
    u32.f = float_val;
    if ((u32.u & 0x1FFF) == 0) { /* worth trying half */
      int s16 = (u32.u >> 16) & 0x8000;
      int exp = (u32.u >> 23) & 0xff;
      int mant = u32.u & 0x7fffff;
      if (exp == 0 && mant == 0)
        ;              /* 0.0, -0.0 */
      else if (exp >= 113 && exp <= 142) /* normalized */
        s16 += ((exp - 112) << 10) + (mant >> 13);
      else if (exp >= 103 && exp < 113) { /* denorm, exp16 = 0 */
        if (mant & ((1 << (126 - exp)) - 1))
          goto float32;         /* loss of precision */
        s16 += ((mant + 0x800000) >> (126 - exp));
      } else if (exp == 255 && mant == 0) { /* Inf */
        s16 += 0x7c00;
      } else
        goto float32;           /* loss of range */

      ensure_writable(3);
      u16 = s16;
      be16 = hton16p((const uint8_t*)&u16);

      write_byte_and_data(IB_PRIM | 25, (const void*)&be16, 2);
      return;
    }
  float32:
    ensure_writable(5);
    be32 = hton32p((const uint8_t*)&u32.u);

    write_byte_and_data(IB_PRIM | 26, (const void*)&be32, 4);

  } else if (isnan(val)) {      /* NaN -- we always write a half NaN*/
    ensure_writable(3);
    write_byte_and_data(IB_PRIM | 25, (const void*)"\x7e\x00", 2);
  } else {
    uint64_t be64;
    /* Copy the same problematic implementation from the decoder. */
    union {
      double d;
      uint64_t u;
    } u64;

    u64.d = val;

    ensure_writable(9);
    be64 = hton64p((const uint8_t*)&u64.u);

    write_byte_and_data(IB_PRIM | 27, (const void*)&be64, 8);

  }
}
#endif /* CBOR_NO_FLOAT */

// TODO: make public?
typedef void (*cn_visit_func)(const cn_cbor *cb, int depth, void *context);
static void _visit(const cn_cbor *cb,
                   cn_visit_func visitor,
                   cn_visit_func breaker,
                   void *context)
{
    const cn_cbor *p = cb;
    int depth = 0;
    while (p)
    {
    visit:

      visitor(p, depth, context);

      if (p->first_child) {

        p = p->first_child;
        depth++;
      } else{

        // Empty indefinite
        if (is_indefinite(p)) {

          breaker(p->parent, depth, context);
        }
        if (p->next) {
          p = p->next;
        } else {
          while (p->parent) {
            depth--;
            if (is_indefinite(p->parent)) {
              breaker(p->parent, depth, context);
            }
            if (p->parent->next) {
              p = p->parent->next;
              goto visit;
            }
            p = p->parent;
          }
          return;
        }
      }
    }
}

#define CHECK(st) (st); \
if (ws->offset < 0) {return;}

void _encoder_visitor(const cn_cbor *cb, int depth, void *context)
{
  cn_write_state *ws = context;
  UNUSED_PARAM(depth);

  switch (cb->type) {
  case CN_CBOR_ARRAY:
    if (is_indefinite(cb)) {
      write_byte_ensured(IB_ARRAY | AI_INDEF);
    } else {
      CHECK(_write_positive(ws, CN_CBOR_ARRAY, cb->length));
    }
    break;
  case CN_CBOR_MAP:
    if (is_indefinite(cb)) {
      write_byte_ensured(IB_MAP | AI_INDEF);
    } else {
      CHECK(_write_positive(ws, CN_CBOR_MAP, cb->length/2));
    }
    break;
  case CN_CBOR_BYTES_CHUNKED:
  case CN_CBOR_TEXT_CHUNKED:
    write_byte_ensured(_xlate[cb->type] | AI_INDEF);
    break;

  case CN_CBOR_TEXT:
  case CN_CBOR_BYTES:
    CHECK(_write_positive(ws, cb->type, cb->length));
    ensure_writable(cb->length);
    if (ws->buf != NULL)
    {
	    memcpy(ws->buf + ws->offset, cb->v.str, cb->length);
    }

    ws->offset += cb->length;

    break;

  case CN_CBOR_FALSE:
  case CN_CBOR_TRUE:
  case CN_CBOR_NULL:
  case CN_CBOR_UNDEF:
    write_byte_ensured(_xlate[cb->type]);
    break;

  case CN_CBOR_TAG:
  case CN_CBOR_UINT:
  case CN_CBOR_SIMPLE:
    CHECK(_write_positive(ws, cb->type, cb->v.uint));
    break;

  case CN_CBOR_INT:
    assert(cb->v.sint < 0);
    CHECK(_write_positive(ws, CN_CBOR_INT, ~(cb->v.sint)));
    break;

  case CN_CBOR_DOUBLE:
#ifndef CBOR_NO_FLOAT
    CHECK(_write_double(ws, cb->v.dbl));
#endif /* CBOR_NO_FLOAT */
    break;

  case CN_CBOR_INVALID:
    ws->offset = -1;
    break;
  }
}

void _encoder_breaker(const cn_cbor *cb, int depth, void *context)
{
  cn_write_state *ws = context;
  UNUSED_PARAM(cb);
  UNUSED_PARAM(depth);
  write_byte_ensured(IB_BREAK);
}

/*
If called with a NULL buf, will run as usual but will not copy any data.
This is done in order to acquire the size of the buffer we will allocate for the decoded buffer.
*/
static int cn_cbor_encoder(uint8_t *buf,
	size_t buf_offset,
	size_t buf_size,
	const cn_cbor *cb)
{
	int ret;
	cn_write_state ws = { buf, buf_offset, buf_size };
	_visit(cb, _encoder_visitor, _encoder_breaker, &ws);
	
	if (ws.offset < 0) { return -1; }
	ret = ws.offset - buf_offset;
	return ret;
}

int cn_cbor_get_encoded_size(const cn_cbor *cb, cn_cbor_errback *err)
{
	int ret;

	if (cb == NULL) {
		err->err = CN_CBOR_ERR_INVALID_PARAMETER;
		return -1;
	}

	ret = cn_cbor_encoder(NULL, 0, INT_MAX, cb);
	if (ret <= 0) {
		err->err = CN_CBOR_ERR_ENCODER;
		return -1;
	}

	err->err = CN_CBOR_NO_ERROR;
	return ret;
}

int cn_cbor_encoder_write(const cn_cbor *cb, uint8_t *bufOut, int bufSize, cn_cbor_errback *err)
{
	int bytesWritten;
	
	if (bufOut == NULL) {
		err->err = CN_CBOR_ERR_INVALID_PARAMETER;
		return -1;
	}
	
	bytesWritten = cn_cbor_encoder(bufOut, 0, bufSize, cb);
	if (bytesWritten <= 0)
	{
		err->err = CN_CBOR_ERR_ENCODER;
	}
	else
	{
		err->err = CN_CBOR_NO_ERROR;

	}

	return bytesWritten;
}

int cn_cbor_get_encoded_container_size(const cn_cbor *cb_container) 
{
    int ret;
    cn_cbor_errback err;
    cn_cbor *sibling_backup = cb_container->next;
    cn_cbor *parent_backup = cb_container->parent;

    // Make sure the CBOR is a container
    if (cb_container->type != CN_CBOR_MAP &&
        cb_container->type != CN_CBOR_ARRAY) {
        return -1;
    }

    // Disconnect parent sibling temporarily. We first cast to size_t to avoid compiler const warnings
    ((cn_cbor*)(size_t)cb_container)->next = NULL;
    ((cn_cbor*)(size_t)cb_container)->parent = NULL;

    ret = cn_cbor_get_encoded_size(cb_container, &err);

    // Restore sibling
    ((cn_cbor*)(size_t)cb_container)->next = sibling_backup;
    ((cn_cbor*)(size_t)cb_container)->parent = parent_backup;

    return ret;
}

#ifdef  __cplusplus
}
#endif

#endif  /* CN_CBOR_C */
