#include <assert.h>

#include "base32.h"
#include "common.h"

static inline bool b32blktoa(uint8_t *restrict, const uint8_t *restrict)
	__attribute__((always_inline));

static const uint8_t lookup[] = {
	/* [00…07] = */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	/* [08…0F] = */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	/* [10…17] = */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	/* [18…1F] = */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	/* [20…27] = */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	/* [28…2F] = */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	/* [30…37] = */ 0xFF, 0xFF,   26,   27,   28,   29,   30,   31,
	/* [38…3F] = */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,    0, 0xFF, 0xFF,
	/* [40…47] = */ 0xFF,    0,    1,    2,    3,    4,    5,    6,
	/* [48…4F] = */    7,    8,    9,   10,   11,   12,   13,   14,
	/* [50…57] = */   15,   16,   17,   18,   19,   20,   21,   22,
	/* [58…5F] = */   23,   24,  25,  0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	/* [60…67] = */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	/* [68…6F] = */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	/* [70…77] = */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	/* [78…7F] = */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	/* [80…87] = */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	/* [88…8F] = */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	/* [90…97] = */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	/* [98…9F] = */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	/* [A0…A7] = */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	/* [A8…AF] = */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	/* [B0…B7] = */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	/* [B8…BF] = */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	/* [C0…C7] = */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	/* [C8…CF] = */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	/* [D0…D7] = */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	/* [D8…DF] = */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	/* [E0…E7] = */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	/* [E8…EF] = */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	/* [F0…F7] = */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	/* [F8…FF] = */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
};

bool
b32toa(uint8_t *restrict dst, const char *restrict src, size_t len)
{
	assert(len != 0);

	size_t i, j;
	for (i = j = 0; len - i >= 8; i += 8, j += 5) {
		uint8_t bits[] = {
			lookup[(uint8_t)src[i + 0]],
			lookup[(uint8_t)src[i + 1]],
			lookup[(uint8_t)src[i + 2]],
			lookup[(uint8_t)src[i + 3]],
			lookup[(uint8_t)src[i + 4]],
			lookup[(uint8_t)src[i + 5]],
			lookup[(uint8_t)src[i + 6]],
			lookup[(uint8_t)src[i + 7]],
		};
		if (!b32blktoa(dst + j, bits))
			return false;
	}

	uint8_t bits[8] = {0};
	switch (len - i) {
	case 7: bits[6] = lookup[(uint8_t)src[i + 6]]; /* fallthrough */
	case 6: bits[5] = lookup[(uint8_t)src[i + 5]]; /* fallthrough */
	case 5: bits[4] = lookup[(uint8_t)src[i + 4]]; /* fallthrough */
	case 4: bits[3] = lookup[(uint8_t)src[i + 3]]; /* fallthrough */
	case 3: bits[2] = lookup[(uint8_t)src[i + 2]]; /* fallthrough */
	case 2: bits[1] = lookup[(uint8_t)src[i + 1]]; /* fallthrough */
	case 1: bits[0] = lookup[(uint8_t)src[i + 0]];
		return b32blktoa(dst + j, bits);
	}
	return true;
}

bool
b32blktoa(uint8_t *restrict dst, const uint8_t *restrict src)
{
	uint8_t or = src[0] | src[1] | src[2] | src[3]
	           | src[4] | src[5] | src[6] | src[7];
	if (or == 0xFF)
		return false;

	dst[0] = src[0]<<3 | src[1]>>2;
	dst[1] = src[1]<<6 | src[2]<<1 | src[3]>>4;
	dst[2] = src[3]<<4 | src[4]>>1;
	dst[3] = src[4]<<7 | src[5]<<2 | src[6]>>3;
	dst[4] = src[6]<<5 | src[7]>>0;
	return true;
}