#include "packet.h"
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <err.h>

#include <iconv.h>

_Static_assert(sizeof(((union packet*)0)->buf) >= sizeof(((union packet*)0)->hdr), "packet.hdr must fit packet");
_Static_assert(sizeof(((union packet*)0)->buf) == sizeof(*((union packet*)0)), "packet.buf must be sizeof(packet)");

bool
packet_verify(const union packet *packet)
{
   assert(packet);

   if (packet->hdr.size < sizeof(packet->hdr) || packet->hdr.size > sizeof(packet->buf))
      return false;

   static const uint8_t hdr[] = { 0x02, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff }; // 0xff are wildcards
   _Static_assert(sizeof(packet->hdr) >= sizeof(hdr), "sizeof(packet->hdr) != sizeof(hdr)");

   for (const uint8_t *b = packet->buf, *h = hdr; (size_t)(h - hdr) < sizeof(hdr) ; ++b, ++h) {
       if (*h != 0xff && *b != *h)
           return false;
    }

   return true;
}

void
packet_crypt(union packet *packet)
{
   assert(packet);
   static const uint8_t key[] = { 0x57, 0x19, 0xc6, 0x2d, 0x56, 0x68, 0x3a, 0xcc, 0x60, 0x3b, 0x0b, 0xb1, 0x90, 0x5c, 0x4a, 0xf8, 0x80, 0x28, 0xb1, 0x45, 0xb6, 0x85, 0xe7, 0x4c, 0x06, 0x2d, 0x55, 0x83, 0xaf, 0x44, 0x99 };
   const int16_t salt = packet->hdr.salt + packet->hdr.salt + packet->hdr.salt * 2;
   for (size_t i = 5 /* skip salt && size && unknown[0] */, c = 0; i < packet->hdr.size; ++i, ++c) {
      const uint32_t hi = (uint64_t)(0x55555556 * c) >> 32;
      int32_t magic = (hi >> sizeof(key)) + hi;
      magic = salt - (magic + magic * 2);
      assert(magic + c < sizeof(key));
      packet->buf[i] ^= key[magic + c];
   }
}

static size_t
utf16_to_utf8(const uint8_t *utf16, const size_t utf16_sz, uint8_t *utf8, const size_t utf8_sz)
{
   assert(utf16 && utf8 && utf8_sz);

   if (!utf16_sz) {
      utf8[0] = 0;
      return 0;
   }

   static iconv_t cd;
   if (!cd && !(cd = iconv_open("utf8", "utf16le")))
      err(EXIT_FAILURE, "iconv_open");

   size_t isz = utf16_sz, osz = utf8_sz - 1;
   iconv(cd, (char*[]){(uint8_t*)utf16}, &isz, (char*[]){utf8}, &osz);
   const size_t len = (utf8_sz - 1) - osz;
   utf8[len] = 0;
   return len;
}

static size_t
utf8_to_utf16(const uint8_t *utf8, const size_t utf8_sz, uint8_t *utf16, const size_t utf16_sz)
{
   assert(utf8 && utf16 && utf16_sz);

   if (!utf8_sz)
      return 0;

   static iconv_t cd;
   if (!cd && !(cd = iconv_open("utf16le", "utf8")))
      err(EXIT_FAILURE, "iconv_open");

   size_t isz = utf8_sz, osz = utf16_sz;
   iconv(cd, (char*[]){(uint8_t*)utf8}, &isz, (char*[]){utf16}, &osz);
   const size_t len = utf16_sz - osz;
   return len;
}

void
pbuf_flush(struct pbuf *pbuf)
{
   pbuf->packet->hdr.size = pbuf->cursor;
   pbuf->cursor = 0;
}

void
pbuf_write(struct pbuf *pbuf, const void *data, const size_t sz)
{
   assert(pbuf && pbuf->packet);
   assert(sz <= sizeof(pbuf->packet->buf) && pbuf->cursor <= sizeof(pbuf->packet->buf) - sz);
   memcpy(pbuf->packet->buf + pbuf->cursor, data, sz);
   pbuf->cursor += sz;
}

void
pbuf_write_str_len(struct pbuf *pbuf, const void *str, const uint16_t len)
{
   pbuf_write(pbuf, &len, sizeof(len));
   pbuf_write(pbuf, str, len);
}

void
pbuf_write_str(struct pbuf *pbuf, const char *str)
{
   assert(str);
   pbuf_write_str_len(pbuf, str, strlen(str));
}

void
pbuf_write_str_len_utf16(struct pbuf *pbuf, const void *str, const uint16_t len)
{
   struct pbuf_string u16;
   u16.len = utf8_to_utf16(str, len, u16.data, sizeof(u16.data));
   pbuf_write(pbuf, &u16.len, sizeof(u16.len));
   pbuf_write(pbuf, u16.data, u16.len);
}

void
pbuf_write_str_utf16(struct pbuf *pbuf, const char *str)
{
   assert(str);
   pbuf_write_str_len_utf16(pbuf, str, strlen(str));
}

size_t
pbuf_read_safe(struct pbuf *pbuf, void *data, const size_t data_sz, const size_t sz)
{
   assert(pbuf && pbuf->packet);
   assert(sz <= sizeof(pbuf->packet->buf) && pbuf->cursor <= sizeof(pbuf->packet->buf) - sz);
   const size_t to_copy = (sz > data_sz ? data_sz : sz);
   memcpy(data, pbuf->packet->buf + pbuf->cursor, to_copy);
   pbuf->cursor += sz;
   return to_copy;
}

void
pbuf_read(struct pbuf *pbuf, void *data, const size_t sz)
{
   pbuf_read_safe(pbuf, data, sz, sz);
}

void
pbuf_read_str(struct pbuf *pbuf, struct pbuf_string *str)
{
   assert(str);
   pbuf_read(pbuf, &str->len, sizeof(str->len));
   str->len = pbuf_read_safe(pbuf, str->data, sizeof(str->data) - 1, str->len);
   assert(str->len < sizeof(str->data));
   str->data[str->len] = 0;
}

void
pbuf_read_str_utf16(struct pbuf *pbuf, struct pbuf_string *str)
{
   assert(str);
   struct pbuf_string u16;
   pbuf_read(pbuf, &u16.len, sizeof(u16.len));
   u16.len = pbuf_read_safe(pbuf, u16.data, sizeof(u16.data) - 1, u16.len);
   assert(u16.len < sizeof(u16.data));
   str->len = utf16_to_utf8(u16.data, u16.len, str->data, sizeof(str->data));
}