#include "../../lib/md2_impls/md2_0.h"

#include "../../lib/md2_impls/md2_common.h"

void md2_checksum_0(size_t len, uint8_t* buf) {
  uint8_t l = 0;

  for (size_t i = 0; i < len / 16; i++) {
    for (int j = 0; j < 16; j++) {
      u_int8_t c = buf[i * 16 + j];
      // reference is wrong. It says: Set C[j] to S[c xor L]. But it should be:
      buf[len + j] ^= MD2_PI_SUBST[c ^ l];
      l = buf[len + j];
    }
  }
}

void md2_hash_0(size_t len, const uint8_t buf[len], uint8_t out[16]) {
  // === step 1 ===
  int paddingNeeded = 16 - (len % 16);
  uint8_t originalPadding = paddingNeeded;
  len += paddingNeeded;

  // printf("len: %d\n", len);

  // +16 for the checksum
  uint8_t* newBuf = calloc(len + 16, sizeof(uint8_t));
  if (newBuf == NULL) {
    return;
  }
  memcpy(newBuf, buf, len - paddingNeeded);

  // printBuf(len + 16, newBuf);

  while (paddingNeeded > 0) {
    newBuf[len - paddingNeeded] = originalPadding;
    paddingNeeded--;
  }
  // printf("buf with padding: ");
  // printBuf(len + 16, newBuf);

  // === step 2 ===
  md2_checksum_0(len, newBuf);

  // printf("buf with cecksum: ");
  // printBuf(len + 16, newBuf);

  // === step 3 ===
  uint8_t* messageDigestBuf = calloc(48, sizeof(uint8_t));
  if (messageDigestBuf == NULL) {
    return;
  }


  // === step 4 ===
  // <= because we need to hash the last block too
  for (size_t i = 0; i <= (len + 16) / 16 - 1; i++) {
    for (int j = 0; j < 16; j++) {
      messageDigestBuf[16 + j] = newBuf[i * 16 + j];
      messageDigestBuf[32 + j] =
          (messageDigestBuf[16 + j] ^ messageDigestBuf[j]);
    }

    u_int8_t t = 0;

    for (int j = 0; j < 18; j++) {
      for (int k = 0; k < 48; k++) {
        t = messageDigestBuf[k] = messageDigestBuf[k] ^ MD2_PI_SUBST[t];
      }
      t = (t + j) % 256;
    }
  }
  // printf("messageDigestBuf: \n");
  // printBuf(16, messageDigestBuf);

  memcpy(out, messageDigestBuf, 16);

  free(messageDigestBuf);
  free(newBuf);
}