#include <nds.h>
#include <stdlib.h>
#include <string.h>

#include "engine.h"

DTCM_DATA pixel pixels[NPIXELS];

void pixel_init(pixel *p) {
	p->x = rand() % (SCREEN_WIDTH << FRACBITS);
	p->y = rand() % (SCREEN_WIDTH << FRACBITS);
	p->vx = (rand() % 10 - 5) << FRACBITS;
	p->vy = (rand() % 10 - 5) << FRACBITS;
}

void pixel_accel(pixel *p, s16 x, s16 y) {
	p->vx += x;
	if (p->vx > VELOCITY_THRESHOLD)
		p->vx = VELOCITY_THRESHOLD;
	else if (p->vx < -VELOCITY_THRESHOLD)
		p->vx = -VELOCITY_THRESHOLD;

	p->vy += y;
	if (p->vy > VELOCITY_THRESHOLD)
		p->vy = VELOCITY_THRESHOLD;
	else if (p->vy < -VELOCITY_THRESHOLD)
		p->vy = -VELOCITY_THRESHOLD;
}

void pixel_update(pixel *p) {
	p->x += p->vx;
	p->y += p->vy;
}

void pixel_draw(pixel *p) {
	if (p->x < 0 || p->x > SCREEN_WIDTH << FRACBITS ||
	    p->y < 0 || p->y > SCREEN_HEIGHT << FRACBITS)
		return;

	glVertex3v16((p->x - p->vx) >> FRACBITS, (p->y - p->vy) >> FRACBITS, 0);
	glVertex3v16(p->x >> FRACBITS, p->y >> FRACBITS, 0);
	glVertex3v16(p->x >> FRACBITS, p->y >> FRACBITS, 0);
}

DTCM_DATA u16 mx = 128;
DTCM_DATA u16 my = 96;

void engine() {
	touchPosition touch;
	int i;

	while(1) {
		// Input handling
		scanKeys();
		if (keysHeld() & KEY_TOUCH) {
			touch = touchReadXY();
			mx = touch.px;
			my = touch.py;
			if (keysDown() & KEY_TOUCH) {	// just pressed
				for (i = 0; i < NPIXELS; i++) {
					s32 dx = pixels[i].x - (mx << FRACBITS);
					s32 dy = pixels[i].y - (my << FRACBITS);
					u16 mag = swiSqrt(dx*dx + dy*dy);
					// Shift for the following division,
					// and keep the sign!
					dx = (dx & BIT(31)) | ((dx << FRACBITS) & ~BIT(31));
					dy = (dy & BIT(31)) | ((dy << FRACBITS) & ~BIT(31));
					dx = (dx / mag) * 10;
					dy = (dy / mag) * 10;

					pixel_accel(&pixels[i], dx, dy);
				}
			}
		}

		for (i = 0; i < NPIXELS; i++) {
			// Get distance
			s16 dx = (mx << FRACBITS) - pixels[i].x;
			s16 dy = (my << FRACBITS) - pixels[i].y;
			// This works because the multiplication shifts
			// the decimal place outwards, then the sqrt shifts it
			// back inwards. It's bizarre, but it all works out
			// somehow.
			u16 d = swiSqrt((u32)dx*(u32)dx + (u32)dy*(u32)dy);

			// Apply acceleration to pixels towards the cursor
			s16 ax = dx / 50;
			s16 ay = dy / 50;

			// Apply a drag proportional to the distance from the
			// cursor
			if (d > 25 << FRACBITS) {
				ax += -pixels[i].vx * (d - (25 << FRACBITS)) / 5000;
				ay += -pixels[i].vy * (d - (25 << FRACBITS)) / 5000;
			}

			// And a "chaotic" acceleration inversely proportional
			// to the distance from the cursor
			ax += (rand() % (20 << (FRACBITS*2)) - (10 << (FRACBITS*2))) / d;
			ay += (rand() % (20 << (FRACBITS*2)) - (10 << (FRACBITS*2))) / d;

			pixel_accel(&pixels[i], ax, ay);
			pixel_update(&pixels[i]);
		}

		glBegin(GL_TRIANGLES);
		for (i = 0; i < NPIXELS; i++) {
			pixel_draw(&pixels[i]);
		}
		//glEnd();

		glBegin(GL_QUADS);
		// Draw rect around the cursor
		glVertex3v16(mx - 5, my - 5, 0);
		glVertex3v16(mx + 5, my - 5, 0);
		glVertex3v16(mx + 5, my + 5, 0);
		glVertex3v16(mx - 5, my + 5, 0);
		//glEnd();

		swiWaitForVBlank();

		glFlush(0);
	}

}