#ifndef UTIL_HPP
#define UTIL_HPP

#include "types.hpp"
#include <assert.h>
#include <type_traits>

constexpr u32 SCREEN_WIDTH  = 240;
constexpr u32 SCREEN_HEIGHT = 160;

vu16* const REG_VCOUNT = reinterpret_cast<vu16*>(0x04000006);

enum DisplayControlMode : u16 {
	Mode0 = 0x0000,
	Mode1 = 0x0001,
	Mode2 = 0x0002,
	Mode3 = 0x0003,
	Mode4 = 0x0004,
	Mode5 = 0x0005,
};

enum DisplayControlLayer : u16 {
	BG0 = 0x0100,
	BG1 = 0x0200,
	BG2 = 0x0400,
	BG3 = 0x0800,
	BG4 = 0x1000,
};

typedef u16 DisplayControlFlags;

constexpr DisplayControlFlags operator|(DisplayControlMode mode, DisplayControlLayer layer) {
	return static_cast<DisplayControlFlags>(static_cast<u16>(mode) | static_cast<u16>(layer));
}

static inline void set_display_control(DisplayControlMode mode, DisplayControlLayer layer) {
	static volatile DisplayControlFlags* const display_control = reinterpret_cast<DisplayControlFlags*>(0x04000000);
	*display_control = mode | layer;
}

enum BGControlColorMode : u16 {
	Mode_4bpp = 0x0000,
	Mode_8bpp = 0x0080,
};

enum BGControlSize : u16 {
	Regular_32x32  = 0x0000,
	Regular_64x32  = 0x4000,
	Regular_32x64  = 0x8000,
	Regular_64x64  = 0xc000,
	Affine_16x16   = 0x0000,
	Affine_32x32   = 0x4000,
	Affine_64x64   = 0x8000,
	Affine_128x128 = 0xc000,
};

struct BGControlFlags {
	u16 flags;

	constexpr BGControlFlags() : flags(0) {}

	constexpr BGControlFlags &set_priority(u16 priority) {
		assert(priority < 4);
		this->flags = (this->flags & 0xfffc) | priority;
		return *this;
	}

	constexpr BGControlFlags &set_charblock_base(u16 block) {
		assert(block < 4);
		this->flags = (this->flags & 0x000c) | (block << 2);
		return *this;
	}

	constexpr BGControlFlags &set_mosaic(u16 enabled) {
		this->flags = (this->flags & 0xffbf) | (!!enabled << 6);
		return *this;
	}

	constexpr BGControlFlags &set_color_mode(BGControlColorMode mode) {
		this->flags = (this->flags & 0xff7f) | mode;
		return *this;
	}

	constexpr BGControlFlags &set_screenblock_base(u16 block) {
		assert(block < 32);
		this->flags = (this->flags & ~0x1f00) | (block << 8);
		return *this;
	}

	constexpr BGControlFlags &set_affine_wrapping(u16 enabled) {
		this->flags = (this->flags & 0xdfff) | (!!enabled << 13);
		return *this;
	}

	constexpr BGControlFlags &set_size(BGControlSize size) {
		this->flags = (this->flags & ~0xc000) | size;
		return *this;
	}

	constexpr operator u16() const { return this->flags; }
};

static inline void set_bg_control(u16 bg, BGControlFlags flags) {
	assert(bg < 4);

	vu16* const reg = reinterpret_cast<vu16*>(0x04000008 + 2*bg);
	*reg = flags;
}

static inline void set_bg_offsets(u16 bg, u16 h_offset, u16 v_offset) {
	vu16* const h_reg = reinterpret_cast<vu16*>(0x04000010 + 4*bg);
	vu16* const v_reg = reinterpret_cast<vu16*>(0x04000012 + 4*bg);

	*h_reg = h_offset;
	*v_reg = v_offset;
}

struct Color {
	u16 data;

	static constexpr Color from_rgb(u16 r, u16 g, u16 b) {
		assert(r < 32 && g < 32 && b < 32);
		return Color { .data = u16(r | (g << 5) | (b << 10)) };
	}
};

typedef Color PaletteBank[16];

struct Tile4 { u32 data[8]; };
typedef Tile4 CharBlock4[512];

struct ScreenEntry {
	u16 data;

	constexpr ScreenEntry() : data(0) {}

	constexpr ScreenEntry &set_tile_index(u16 tid) {
		assert(tid < 1024);
		this->data = (this->data & ~0x03ff) | tid;
		return *this;
	}

	constexpr ScreenEntry &set_hflip(u16 flip) {
		this->data = (this->data & ~0x0400) | (!!flip << 10);
		return *this;
	}

	constexpr ScreenEntry &set_vflip(u16 flip) {
		this->data = (this->data & ~0x0800) | (!!flip << 11);
		return *this;
	}

	constexpr ScreenEntry &set_palette_bank(u16 bank) {
		assert(bank < 16);
		this->data = (this->data & ~0xf000) | (bank << 12);
		return *this;
	}

	constexpr operator u16() const { return this->data; }
};

typedef ScreenEntry ScreenBlock[1024];

template <typename T>
static inline void volatile_copy(volatile T* dst, const T* src) {
	using U = std::conditional_t<(sizeof(T) % sizeof(u32) == 0) && (alignof(T) % alignof(u32) == 0), u32, u16>;
	static_assert(sizeof(T) % sizeof(U) == 0);
	static_assert(alignof(T) % alignof(U) == 0);
	auto *d = reinterpret_cast<volatile U*>(dst);
	auto *s = reinterpret_cast<const U*>(src);
	for (unsigned i = 0; i < sizeof(T) / sizeof(U); i++) {
		d[i] = s[i];
	}
}

static inline void set_tile(u16 block, u16 index, const Tile4 &tile) {
	// TODO bounds check
	volatile CharBlock4 *const blocks = reinterpret_cast<volatile CharBlock4*>(0x06000000);
	volatile_copy(&blocks[block][index], &tile);
}

static inline void set_bg_palette(u32 bank, u32 index, const Color &color) {
	volatile PaletteBank *const banks = reinterpret_cast<volatile PaletteBank*>(0x05000000);
	volatile_copy(&banks[bank][index], &color);
}


static inline void set_screen_entry(u32 block, u32 index, const ScreenEntry &entry) {
	volatile ScreenBlock *const blocks = reinterpret_cast<volatile ScreenBlock*>(0x06000000);
	volatile_copy(&blocks[block][index], &entry);
}

#endif