#include "lib/tunnel_encaps.h"
#include "filter/data.h"

static int
walk_tlvlist(const struct te_tlvlist *set, const struct te_visitor *visitor, void *opaque)
{
  if (!set)
    return 0;

  struct te_tlv *t;
  struct te_context ctx = { .opaque = opaque, };

  WALK_LIST(t, set->tlv)
  {
    if (visitor->visit_tlv && visitor->visit_tlv(t->type, &ctx) < 0)
      continue;

    for (uint j=0; j < t->len; j++)
    {
      const struct te_subtlv *st = &t->st[j];

      if (visitor->visit_subtlv && visitor->visit_subtlv(st->type, &ctx) < 0)
        continue;

      switch (st->type)
      {
      case TLV_ENCAPSULATION:
        if (visitor->visit_encap && visitor->visit_encap(&st->u.tunnel_encap, &ctx) < 0)
          return -1;
        break;
      case TLV_COLOR:
        if (visitor->visit_color && visitor->visit_color(st->u.color, &ctx) < 0)
          return -1;
        break;
      case TLV_TUNNEL_ENDPOINT:
        if (visitor->visit_ep && visitor->visit_ep(&st->u.tunnel_endpoint, &ctx) < 0)
          return -1;
        break;
      case TLV_UDP_DEST_PORT:
        if (visitor->visit_udp_dest_port && visitor->visit_udp_dest_port(st->u.udp_dest_port, &ctx) < 0)
          return -1;
        break;
      default:
        if (visitor->visit_unknown && visitor->visit_unknown(st->type, &st->u.unknown, &ctx) < 0)
          return -1;
        break;
      }

      if (visitor->visit_subtlv_end)
        visitor->visit_subtlv_end(st->type, &ctx); /* TODO use return value? */
    }

    if (visitor->visit_tlv_end)
      visitor->visit_tlv_end(&ctx); /* TODO use return value? */
  }

  return 0;
}

static void calc_elems(const struct f_tree UNUSED *t, void *ptr)
{
  uint *len = ptr;
  (*len)++;
}

static void put_elems(const struct f_tree *t, void *ptr)
{
  struct te_tlv *tlv = ptr;
  const struct te_subtlv *st = &t->from.val.st;
  tlv->len--;
  tlv->st[tlv->len] = *st;
  if (st->type == TLV_ENCAPSULATION)
    tlv->type = st->u.tunnel_encap.type;
  DBG("put_elems: type %d\n", st->type);
}

const struct te_tlv *tlv_alloc(struct linpool *pool, const struct f_tree *set)
{
  uint len = 0;

  tree_walk(set, calc_elems, &len);

  if (!len)
    return NULL;

  struct te_tlv *res = lp_alloc(pool, sizeof(struct te_tlv) + len * sizeof(struct te_subtlv));

  res->len = len;
  tree_walk(set, put_elems, res);

  res->len = len;		/* Len was reset during tree_walk. */

  for (uint i=0; i < len; i++)
  {
    DBG("tlv_alloc: type[%d]: %d\n", i, res->st[i].type);
  }
  return res;
}

int tlv_set_format(const struct te_tlvlist *set, int UNUSED from, byte *buf, uint size)
{
  const struct te_visitor *visitor = te_get_format_visitor();
  struct te_buffer buffer = {.pos=buf, .size=size, .tlv=0, .subtlv=0};

  if (set)
    walk_tlvlist(set, visitor, &buffer);

  int l = bsnprintf(buffer.pos, buffer.size, "%s", buffer.tlv?"}":"<empty>");
  if (l < 0)
    return -1;
  ADVANCE(buffer.pos, buffer.size, l);
  return buffer.pos - buf;
}

int
tlv_set_contains(const struct te_tlvlist *list, const struct te_tlv UNUSED *val)
{
  if (!list)
    return 0;

  /* FIXME */
  return 0;
}

static void tlv_add_copy(struct linpool *pool, struct te_tlvlist *list, const struct te_tlv *t)
{
  uint size = sizeof(struct te_tlv) + t->len * sizeof(struct te_subtlv);
  struct te_tlv *tlv = lp_alloc(pool, size);
  memcpy(tlv, t, size);
  memset(&tlv->n, 0, sizeof(tlv->n));
  add_tail(&list->tlv, &tlv->n);
  DBG("tlv_add_copy: src type %d, dst type %d\n", t->st[0].type, tlv->st[0].type);
}

const struct te_tlvlist *
tlv_set_add(struct linpool *pool, const struct te_tlvlist *list, const struct te_tlv *val)
{
  if (tlv_set_contains(list, val))
    return list;

  struct te_tlvlist *res = lp_alloc(pool, sizeof(struct te_tlvlist));

  init_list(&res->tlv);

  DBG("tlv_set_add: first type %d, second type %d\n", list?((const struct te_tlv *)HEAD(list->tlv))->st[0].type:-1, val->st[0].type);

  if (list)
  {
    struct te_tlv *n;
    WALK_LIST(n, list->tlv)
    {
      tlv_add_copy(pool, res, n);
    }
  }

  tlv_add_copy(pool, res, val);
  return res;
}

const struct te_tlvlist *
tlv_set_union(struct linpool UNUSED *pool, const struct te_tlvlist *l1, const struct te_tlvlist *l2)
{
  if (!l1)
    return l2;
  if (!l2)
    return l1;

  /* FIXME */
  void *res = NULL;
  return res;
}

static int
subtlv_same(const struct te_subtlv *st1, const struct te_subtlv *st2)
{
  if (st1 == NULL && st2 == NULL)
    return 1;

  if (st1 == NULL || st2 == NULL)
    return 0;

  if (st1->type != st2->type)
    return 0;

  switch(st1->type)
  {
  case TLV_ENCAPSULATION:
    if (st1->u.tunnel_encap.type != st2->u.tunnel_encap.type ||
	st1->u.tunnel_encap.length != st2->u.tunnel_encap.length ||
	memcmp(st1->u.tunnel_encap.data, st2->u.tunnel_encap.data, st1->u.tunnel_encap.length))
      return 0;
    break;
  case TLV_COLOR:
    if (st1->u.color != st2->u.color)
      return 0;
    break;
  case TLV_TUNNEL_ENDPOINT:
    if (st1->u.tunnel_endpoint.reserved != st2->u.tunnel_endpoint.reserved ||
	st1->u.tunnel_endpoint.af != st2->u.tunnel_endpoint.af ||
	st1->u.tunnel_endpoint.af && ipa_compare(st1->u.tunnel_endpoint.ip, st2->u.tunnel_endpoint.ip))
      return 0;
    break;
  case TLV_UDP_DEST_PORT:
    if (st1->u.udp_dest_port != st2->u.udp_dest_port)
      return 0;
    break;
  default:
    if (st1->u.unknown.length != st2->u.unknown.length ||
	memcmp(st1->u.unknown.data, st2->u.unknown.data, st1->u.unknown.length))
      return 0;
    break;
  }

  return 1;
}

static int
tlv_same(const struct te_tlv *t1, const struct te_tlv *t2)
{
  if (t1 == NULL && t2 == NULL)
    return 1;

  if (t1 == NULL || t2 == NULL)
    return 0;

  if (t1->type != t2->type ||
      t1->len != t2->len)
    return 0;

  for (uint i=0; i < t1->len; i++)
    {
      if (!subtlv_same(&t1->st[i], &t2->st[i]))
	return 0;
    }

  return 1;
}

int
tlvlist_same(const struct te_tlvlist *tl1, const struct te_tlvlist *tl2)
{
  if (tl1 == NULL && tl2 == NULL)
    return 1;

  if (tl1 == NULL || tl2 == NULL)
    return 0;

  const struct te_tlv *t1 = HEAD(tl1->tlv);
  const struct te_tlv *t2 = HEAD(tl2->tlv);

  while (NODE_VALID(t1) || NODE_VALID(t2))
  {
    if (!NODE_VALID(t1) || !NODE_VALID(t2))
      return 0;

    /* Both are valid. */
    if (!tlv_same(t1, t2))
      return 0;
  }

  return 1;
}

static int
tlvlist_calc_tunnel_encap_new(const struct te_tlvlist *t, struct adata *ad)
{
  if (t == NULL)
    return 0;

  uint size = ad ? ad->length : 0;
  byte *p = ad ? ad->data : NULL;

  uint buf_size = te_init_encode_visitor(NULL, NULL, 0);
  struct tlv_buffer *buf = alloca(buf_size);

  if (te_init_encode_visitor(buf, p, size) < 0)
    return -1;

  if (walk_tlvlist(t, te_get_encode_visitor(), buf) < 0)
    return -1;

  return te_get_encode_length(buf);
}

struct adata *
tlvlist_encode_tunnel_encap(struct linpool *pool, const struct te_tlvlist *tl)
{
  int len = tlvlist_calc_tunnel_encap_new(tl, NULL);

  if (len < 0)
    return NULL;

  struct adata *ad = lp_alloc(pool, sizeof(struct adata) + len);
  ad->length = len;

  if (tlvlist_calc_tunnel_encap_new(tl, ad) < 0)
    return NULL;

  return ad;
}

struct decoder_buf
{
  struct linpool *pool;
  struct te_tlvlist *tl;
  struct te_tlv *cur_tlv;
  struct te_subtlv *cur_subtlv;
  int cur_subtlv_index;
};

static int visitor_decode_tlv(int type, struct te_context *ctx)
{
  struct decoder_buf *buf = ctx->opaque;

  int count = te_count_subtlvs(ctx);

  if (count < 0)
    return -1;

  buf->cur_tlv = lp_allocz(buf->pool, sizeof(struct te_tlv) + count * sizeof(struct te_subtlv));
  buf->cur_tlv->type = type;
  return 0;
}

static int visitor_decode_tlv_end(struct te_context *ctx)
{
  struct decoder_buf *buf = ctx->opaque;

  add_tail(&buf->tl->tlv, &buf->cur_tlv->n);
  buf->cur_tlv->len = buf->cur_subtlv_index;
  buf->cur_tlv = NULL;
  return 0;
}

static int
visitor_decode_subtlv(int type, struct te_context *ctx){
  struct decoder_buf *buf = ctx->opaque;

  buf->cur_subtlv = &buf->cur_tlv->st[buf->cur_subtlv_index];
  buf->cur_subtlv->type = type;
  return 0;
}

static int
visitor_decode_subtlv_end(int UNUSED type, struct te_context *ctx){
  struct decoder_buf *buf = ctx->opaque;

  buf->cur_subtlv_index++;
  return 0;
}

static int
visitor_decode_encap(const struct te_encap *encap, struct te_context *ctx)
{
  struct decoder_buf *buf = ctx->opaque;

  buf->cur_subtlv->u.tunnel_encap = *encap;
  return 0;
}

static int
visitor_decode_ep(const struct te_endpoint *ep, struct te_context *ctx)
{
  struct decoder_buf *buf = ctx->opaque;

  buf->cur_subtlv->u.tunnel_endpoint = *ep;
  return 0;
}

static int
visitor_decode_color(u32 color, struct te_context *ctx)
{
  struct decoder_buf *buf = ctx->opaque;

  buf->cur_subtlv->u.color = color;
  return 0;
}

static int
visitor_decode_udp_dest_port(u16 port, struct te_context *ctx)
{
  struct decoder_buf *buf = ctx->opaque;

  buf->cur_subtlv->u.udp_dest_port = port;
  return 0;
}

static int
visitor_decode_unknown(int UNUSED type, const struct te_unknown *unknown, struct te_context *ctx)
{
  struct decoder_buf *buf = ctx->opaque;

  buf->cur_subtlv->u.unknown = *unknown;
  return 0;
}

struct te_tlvlist *tlvlist_decode_tunnel_encap(struct linpool *pool,
					       const struct adata *ad)
{
  struct te_tlvlist *tl = lp_alloc(pool, sizeof(struct te_tlvlist));
  struct decoder_buf buf = { .pool = pool, .tl = tl, };
  struct te_visitor visitor = {
    .visit_tlv = visitor_decode_tlv,
    .visit_tlv_end = visitor_decode_tlv_end,
    .visit_subtlv = visitor_decode_subtlv,
    .visit_subtlv_end = visitor_decode_subtlv_end,
    .visit_encap = visitor_decode_encap,
    .visit_ep = visitor_decode_ep,
    .visit_color = visitor_decode_color,
    .visit_udp_dest_port = visitor_decode_udp_dest_port,
    .visit_unknown = visitor_decode_unknown,
  };

  init_list(&tl->tlv);

  int res = walk_tunnel_encap(ad, &visitor, &buf);
  if (res < 0)
    return NULL;

  return tl;
}