#pragma once
#include <stdint.h>
#include <stddef.h>

namespace gp {
	namespace math {
		
		template<typename word_t>
		size_t log2(word_t v);

		/**
		Sean Eron Anderson
		seander@cs.stanford.edu
		**/
		template<>
		constexpr size_t log2<uint32_t>(uint32_t v)
		{
			constexpr int MultiplyDeBruijnBitPosition[32] = 
			{
				0, 9, 1, 10, 13, 21, 2, 29, 11, 14, 16, 18, 22, 25, 3, 30,
				8, 12, 20, 28, 15, 17, 24, 7, 19, 27, 23, 6, 26, 5, 4, 31
			};

			v |= v >> 1;
			v |= v >> 2;
			v |= v >> 4;
			v |= v >> 8;
			v |= v >> 16;

			return MultiplyDeBruijnBitPosition[(uint32_t)(v * 0x07C4ACDDU) >> 27];
		}
		
		template<>
		constexpr size_t log2<uint64_t>(uint64_t v)
		{
			constexpr int MultiplyDeBruijnBitPosition[64] = 
			{
				0, 58, 1, 59, 47, 53, 2, 60, 39, 48, 27, 54, 33, 42, 3, 61,
				51, 37, 40, 49, 18, 28, 20, 55, 30, 34, 11, 43, 14, 22, 4, 62,
				57, 46, 52, 38, 26, 32, 41, 50, 36, 17, 19, 29, 10, 13, 21, 56,
				45, 25, 31, 35, 16, 9, 12, 44, 24, 15, 8, 23, 7, 6, 5, 63
			};

			v |= v >> 1;
			v |= v >> 2;
			v |= v >> 4;
			v |= v >> 8;
			v |= v >> 16;
			v |= v >> 32;

			return MultiplyDeBruijnBitPosition[(uint64_t)(v * 0x03f6eaf2cd271461) >> 58];
		}

		static_assert(log2<uint32_t>(7) == 2, "bad log2");
		static_assert(log2<uint32_t>(8) == 3, "bad log2");

		template<typename word_t>
		constexpr size_t msb(word_t v);

		template<>
		constexpr size_t msb<uint32_t>(uint32_t v)
		{
			auto l = log2(v);
			return l + (((1 << l) ^ v) != 0);
		}
		template<>
		constexpr size_t msb<uint64_t>(uint64_t v)
		{
			auto l = log2(v);
			return l + (((1 << l) ^ v) != 0);
		}

		static_assert(msb<uint32_t>(7) == 3, "bad msb");
		static_assert(msb<uint32_t>(8) == 3, "bad msb");
	}
}