#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,
	Obj = 0x1000,
};

struct DisplayControlFlags {
	u16 data;

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

	constexpr DisplayControlFlags() : data(0) {}
	constexpr DisplayControlFlags &set_mode(u16 mode) {
		assert(mode < 6);
		this->data = (this->data & ~0x0007) | mode;
		return *this;
	}
	constexpr DisplayControlFlags &enable_layer(u16 layer) {
		this->data |= layer;
		return *this;
	}
	constexpr DisplayControlFlags &set_1d_object_mapping(u16 enabled) {
		this->data = (this->data & ~0x0040) | ((!!enabled) << 6);
		return *this;
	}
	constexpr operator u16() const { return data; }
};

static inline void set_display_control(const DisplayControlFlags &flags) {
	static volatile u16* const display_control = reinterpret_cast<volatile u16*>(0x04000000);
	*display_control = flags;
}

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 index) {
		assert(index < 1024);
		this->data = (this->data & ~0x03ff) | index;
		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];

struct ObjectAttributes {
	u16 attr0, attr1, attr2, _fill;

	enum ObjectMode : u16 {
		ModeDefault = 0x0000,
		ModeHidden  = 0x0020,
		ModeAffine  = 0x0010,
		ModeDouble  = 0x0030,
	};

	enum GraphicsMode : u16 {
		GraphicsDefault = 0x0000,
		GraphicsBlend   = 0x0400,
		GraphicsWindow  = 0x0800,
	};

	enum Size : u16 {
		Size_8x8   = 0x0000,
		Size_16x16 = 0x0001,
		Size_32x32 = 0x0002,
		Size_64x64 = 0x0003,
		Size_16x8  = 0x0100,
		Size_32x8  = 0x0101,
		Size_32x16 = 0x0102,
		Size_32x64 = 0x0103,
		Size_8x16  = 0x0200,
		Size_8x32  = 0x0201,
		Size_16x32 = 0x0202,
		Size_64x32 = 0x0203,
	};

	constexpr ObjectAttributes() : attr0(0), attr1(0), attr2(0), _fill(0) {}

	constexpr ObjectAttributes &set_x(u16 x) {
		assert(x < 256);
		this->attr1 = (this->attr1 & ~0x000f) | x;
		return *this;
	}

	constexpr ObjectAttributes &set_y(u16 y) {
		assert(y < 256);
		this->attr0 = (this->attr0 & ~0x000f) | y;
		return *this;
	}

	constexpr ObjectAttributes &set_object_mode(ObjectMode mode) {
		this->attr0 = (this->attr0 & ~0x0030) | mode;
		return *this;
	}

	constexpr ObjectAttributes &set_graphics_mode(GraphicsMode mode) {
		this->attr0 = (this->attr0 & ~0x00c0) | mode;
		return *this;
	}

	constexpr ObjectAttributes &set_size(Size size) {
		this->attr0 = (this->attr0 & ~0xc000) | ((size & 0x0300) << 6);
		this->attr1 = (this->attr1 & ~0xc000) | ((size & 0x0003) << 14);
		return *this;
	}

	constexpr ObjectAttributes &set_tile_index(u16 index) {
		assert(index < 1024);
		this->attr2 = (this->attr2 & ~0x003f) | index;
		return *this;
	}

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

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_obj_palette(u32 bank, u32 index, const Color &color) {
	volatile PaletteBank *const banks = reinterpret_cast<volatile PaletteBank*>(0x05000200);
	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);
}

static inline void set_obj_attributes(u32 index, const ObjectAttributes &attr) {
	volatile ObjectAttributes *const attrs = reinterpret_cast<volatile ObjectAttributes*>(0x07000000);
	volatile_copy(&attrs[index], &attr);
}

#endif