#include "digest.h"
#include "endian.h"

#include <string.h>
#include <stdio.h>
#include <inttypes.h>

#define L2_SHA1_H0 UINT32_C(0x67452301)
#define L2_SHA1_H1 UINT32_C(0xEFCDAB89)
#define L2_SHA1_H2 UINT32_C(0x98BADCFE)
#define L2_SHA1_H3 UINT32_C(0x10325476)
#define L2_SHA1_H4 UINT32_C(0xC3D2E1F0)

#define L2_SHA1_K0 UINT32_C(0x5A827999)
#define L2_SHA1_K1 UINT32_C(0x6ED9EBA1)
#define L2_SHA1_K2 UINT32_C(0x8F1BBCDC)
#define L2_SHA1_K3 UINT32_C(0xCA62C1D6)

void l2_sha1_init(l2_sha1_state_t *st)
{
  st->state[0] = L2_SHA1_H0;
  st->state[1] = L2_SHA1_H1;
  st->state[2] = L2_SHA1_H2;
  st->state[3] = L2_SHA1_H3;
  st->state[4] = L2_SHA1_H4;

  memset(st->chunk, 0, L2_SHA1_BLOCKLEN);
  st->nchunk = 0;
  st->totallen = 0;
}

uint32_t l2_sha1__rol(uint32_t in, uint32_t v)
{
  return (in << v) | (in >> (32 - v));
}

void l2_sha1__update_int(l2_sha1_state_t *st)
{
  uint32_t w[80];

  for (int i = 0; i < 16; ++i) {
    memcpy(w + i, st->chunk + (i * 4), 4);
    w[i] = l2_betoh32(w[i]);
  }

  for (unsigned i = 16; i < 80; ++i) {
    w[i] = l2_sha1__rol((w[i-3] ^ w[i-8] ^ w[i-14] ^ w[i-16]), 1);
  }

  uint32_t a, b, c, d, e, f, k, temp;
  a = st->state[0];
  b = st->state[1];
  c = st->state[2];
  d = st->state[3];
  e = st->state[4];

  for (unsigned i = 0; i < 80; ++i) {
    if (i < 20) {
      f = (b & c) ^ ((~b) & d);
      k = L2_SHA1_K0;
    } else if (i < 40) {
      f = b ^ c ^ d;
      k = L2_SHA1_K1;
    } else if (i < 60) {
      f = (b & c) ^ (b & d) ^ (c & d);
      k = L2_SHA1_K2;
    } else {
      f = b ^ c ^ d;
      k = L2_SHA1_K3;
    }

    temp = l2_sha1__rol(a, 5) + f + e + k + w[i];
    e = d;
    d = c;
    c = l2_sha1__rol(b, 30);
    b = a;
    a = temp;
  }

  st->state[0] += a;
  st->state[1] += b;
  st->state[2] += c;
  st->state[3] += d;
  st->state[4] += e;
}

void l2_sha1_update(l2_sha1_state_t *st, const void *data, size_t sz)
{
  const uint8_t *dbytes = data;
  size_t rem;
  st->totallen += sz;
  
  while (sz >= (rem = (L2_SHA1_BLOCKLEN - st->nchunk))) {
    memcpy(st->chunk + st->nchunk, dbytes, rem);
    l2_sha1__update_int(st);

    st->nchunk = 0;
    dbytes += rem;
    sz -= rem;
  }

  if (sz > 0) {
    memcpy(st->chunk + st->nchunk, dbytes, sz);
    st->nchunk += sz;
  }
}

void l2_sha1_finalize(l2_sha1_state_t *st, l2_sha1_digest_t *digest)
{
  st->chunk[st->nchunk] = UINT8_C(0x80);
  ++st->nchunk;
  if (st->nchunk > (L2_SHA1_BLOCKLEN - 8)) {
    /* must pad the rest of the way */
    memset(st->chunk + st->nchunk, 0, L2_SHA1_BLOCKLEN - st->nchunk);
    l2_sha1__update_int(st);
    st->nchunk = 0;
  }

  size_t rem = (L2_SHA1_BLOCKLEN - st->nchunk) - 8;
  uint64_t len = l2_htobe64(st->totallen * 8);
  memset(st->chunk + st->nchunk, 0, rem);
  st->nchunk += rem;

  memcpy(st->chunk + st->nchunk, (uint8_t *)&len, 8);
  l2_sha1__update_int(st);

  digest->state[0] = st->state[0];
  digest->state[1] = st->state[1];
  digest->state[2] = st->state[2];
  digest->state[3] = st->state[3];
  digest->state[4] = st->state[4];
}

int l2_sha1_digest_compare(const l2_sha1_digest_t *d1, const l2_sha1_digest_t *d2)
{
  return memcmp(d1, d2, sizeof(l2_sha1_digest_t));
}

void l2_sha1_digest_copy(l2_sha1_digest_t *restrict dest, const l2_sha1_digest_t *restrict src)
{
  memcpy(dest, src, sizeof(l2_sha1_digest_t));
}

void l2_sha1_digest_to_hex(const l2_sha1_digest_t *dg, char *out)
{
  snprintf(out, L2_SHA1_HEX_STRLEN + 1,
           "%08" PRIx32 "%08" PRIx32 "%08" PRIx32 "%08" PRIx32 "%08" PRIx32,
           dg->state[0], dg->state[1], dg->state[2], dg->state[3], dg->state[4]);
}

int l2_sha1__hex_to_uint32(const char *in, uint32_t *out);

int l2_sha1_digest_from_hex(l2_sha1_digest_t *dg, const char *in)
{
  size_t len = strlen(in);
  l2_sha1_digest_t dg1;

  if (len != L2_SHA1_HEX_STRLEN) return -1;

  for (unsigned i = 0; i < 5; ++i) {
    if (l2_sha1__hex_to_uint32(in + (i * 8), dg1.state + i) < 0) return -1;
  }

  l2_sha1_digest_copy(dg, &dg1);
  return 0;
}

int l2_sha1__hex_to_uint32(const char *in, uint32_t *out)
{
  uint32_t ret = 0;

  for (unsigned i = 0; i < 8; ++i) {
    switch (in[i]) {
      case '0':
      case '1':
      case '2':
      case '3':
      case '4':
      case '5':
      case '6':
      case '7':
      case '8':
      case '9':
        ret |= (in[i] - '0') << ((7 - i) << 2);
        break;
      case 'a':
      case 'b':
      case 'c':
      case 'd':
      case 'e':
      case 'f':
        ret |= (in[i] - 'a' + 0xa) << ((7 - i) << 2);
        break;
      case 'A':
      case 'B':
      case 'C':
      case 'D':
      case 'E':
      case 'F':
        ret |= (in[i] - 'A' + 0xA) << ((7 - i) << 2);
        break;
      default:
        return -1;
    }
  }

  *out = ret;

  return 0;
}

#if L2_SHA1_STANDALONE_TEST
int main(int argc, char **argv)
{
  if (argc < 2) return 1;

  FILE *fp = fopen(argv[1], "rb");
  if (!fp) {
    fputs("oh no\n", stderr);
    return 1;
  }

  char b[1024];
  size_t nread = 0;
  l2_sha1_state_t st;
  l2_sha1_digest_t dg;

  l2_sha1_init(&st);

  while ((nread = fread(b, 1, 1024, fp))) {
    printf("%zu\n", nread);
    l2_sha1_update(&st, b, nread);
  }
  
  l2_sha1_finalize(&st, &dg);

  char hex[L2_SHA1_HEX_STRLEN + 1];
  l2_sha1_digest_to_hex(&dg, hex);
  printf("%s\n", hex);

  printf("%d\n", l2_sha1_digest_from_hex(&dg, "2fd4e1c67a2d28fced849ee1bb76e7391b93eb12"));
  l2_sha1_digest_to_hex(&dg, hex);
  printf("%s\n", hex);

  return 0;
}
#endif