/* DS Homebrew demo.
 * Written by mr b0nk 500 <b0nk@b0nk.xyz>.
 */


#include <nds.h>
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <unistd.h>

struct vec2d {
	float x;
	float y;
};

struct player {
	struct vec2d pos;
	struct vec2d delta;
	float max_vel;
	float accel;
	int w;
	int h;
};

void init_player(struct player *p, float x, float y, float max_vel, float accel, int w, int h) {
	p->pos.x = x;
	p->pos.y = y;
	p->delta.x = 0;
	p->delta.y = 0;
	p->max_vel = max_vel;
	p->accel = accel;
	p->w = w;
	p->h = h;
}

int main() {
	touchPosition old_touch;
	touchPosition touch;

	memset(&old_touch, 0, sizeof(old_touch));
	memset(&touch, 0, sizeof(touch));

	videoSetMode(MODE_0_2D);
	videoSetModeSub(MODE_0_2D);

	vramSetBankC(VRAM_C_MAIN_BG);
	vramSetBankD(VRAM_D_SUB_SPRITE);

	/*oamInit(&oamMain, SpriteMapping_1D_32, false);*/
	oamInit(&oamSub, SpriteMapping_1D_32, false);

	/*u16 *main_gfx = oamAllocateGfx(&oamMain, SpriteSize_16x16, SpriteColorFormat_256Color);*/
	u16 *sub_gfx = oamAllocateGfx(&oamSub, SpriteSize_16x16, SpriteColorFormat_256Color);

    	videoSetMode(MODE_0_2D);

	for (int i = 0; i < (16*16)/2; i++) {
		/*main_gfx[i] = 1 | BIT(8);*/
		sub_gfx[i] = 1 | BIT(8);
	}

	/*SPRITE_PALETTE[1] = RGB15(31,0,0);*/
	SPRITE_PALETTE_SUB[1] = RGB15(0,31,0);
	consoleInit(0, 0, BgType_Text4bpp, BgSize_T_256x256, 31, 0, true, true);

	struct player ply;
	struct vec2d drag_speed = {0, 0};
	int max_x = 256;
	int min_x = 0;
	int max_y = 192;
	int min_y = 0;
	int frame_step = 0;
	int pause = 0;
	init_player(&ply, 100, 100, 5.25f, 0.25f, 16, 16);
	min_x -= ply.w;
	min_y -= ply.h;

	for (;;) {
		scanKeys();
		int pressed = keysDown();
		int held = keysHeld();

		frame_step = (pressed & KEY_R) ? !frame_step : frame_step;
		pause = (pressed & KEY_SELECT) ? !pause : pause;
		frame_step = (pressed & KEY_SELECT) ? 0 : frame_step;
		pause = (frame_step) ? 1 : pause;

		if (!pause || frame_step) {
			int up = held & KEY_UP;
			int down = held & KEY_DOWN;
			int left = held & KEY_LEFT;
			int right = held & KEY_RIGHT;
			int is_decel_x = (!left && ply.delta.x < 0) || (!right && ply.delta.x > 0);
			int is_decel_y = (!up && ply.delta.y < 0) || (!down && ply.delta.y > 0);

			if (held & KEY_TOUCH) {
				touchRead(&touch);
				touch.px = (touch.px >= max_x+ply.w) ? max_x+ply.w : touch.px;
				touch.py = (touch.py >= max_y+ply.h) ? max_y+ply.h : touch.py;
				drag_speed.x = (touch.px - old_touch.px)/2;
				drag_speed.y = (touch.py - old_touch.py)/2;
				ply.delta = drag_speed;
				/*ply.delta.x = drag_speed.x;
				ply.delta.y = drag_speed.y;*/
				ply.pos.x = touch.px;
				ply.pos.y = touch.py;
				old_touch = touch;
			}

			if (is_decel_x) {
				int opposite_x = (right && ply.delta.x < 0) || (left && ply.delta.x > 0);
				float tmp_accel = ply.accel*(opposite_x+1);
				ply.delta.x += (ply.delta.x > 0) ? -tmp_accel : tmp_accel;
			} else if (left || right) {
				ply.delta.x += (right) ? ply.accel : -ply.accel;
			}

			if (is_decel_y) {
				int opposite_y = (down && ply.delta.y < 0) || (up && ply.delta.y > 0);
				float tmp_accel = ply.accel*(opposite_y+1);
				ply.delta.y += (ply.delta.y > 0) ? -tmp_accel : tmp_accel;
			} else if (up || down) {
				ply.delta.y += (down) ? ply.accel : -ply.accel;
			}



			ply.delta.x = (ply.delta.x > ply.max_vel) ? ply.max_vel : ply.delta.x;
			ply.delta.y = (ply.delta.y > ply.max_vel) ? ply.max_vel : ply.delta.y;
			ply.delta.x = (ply.delta.x < -ply.max_vel) ? -ply.max_vel : ply.delta.x;
			ply.delta.y = (ply.delta.y < -ply.max_vel) ? -ply.max_vel : ply.delta.y;
			ply.pos.x += ply.delta.x;
			ply.pos.y += ply.delta.y;
			ply.pos.x = (ply.pos.x > max_x) ? min_x+(ply.delta.x-1) : ply.pos.x;
			ply.pos.y = (ply.pos.y > max_y) ? min_y+(ply.delta.y-1) : ply.pos.y;
			ply.pos.x = (ply.pos.x < min_x) ? max_x+(ply.delta.x+1) : ply.pos.x;
			ply.pos.y = (ply.pos.y < min_y) ? max_y+(ply.delta.y+1) : ply.pos.y;
			frame_step = 0;
		}
		if (held & KEY_START) {
			break;
		}

		/*oamSet(&oamMain, 0, x, y, 0, 0, SpriteSize_16x16, SpriteColorFormat_256Color, main_gfx, -1, false, false, false, false, false);*/

		oamSet(&oamSub, 0, (int)ply.pos.x, (int)ply.pos.y, 0, 0, SpriteSize_16x16, SpriteColorFormat_256Color, sub_gfx, -1, false, false, false, false, false);

		swiWaitForVBlank();

		printf("\x1B[5Hx: % 4.5f, y: % 4.5f\n", ply.pos.x, ply.pos.y);
		printf("dx: % 3.5f, dy: % 3.5f\n", ply.delta.x, ply.delta.y);
		iprintf("pause: %i, frame_step: %i\n", pause, frame_step);

		/*oamUpdate(&oamMain);*/
		oamUpdate(&oamSub);
	}

	return 0;
}