#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>
#include <3ds.h>
#include <sf2d.h>
//#include "physobject.cpp";

#define CONFIG_3D_SLIDERSTATE (*(float *)0x1FF81080)
#define debug

#define rw 50
#define rh 50
#define count 10

//forward declarations
char* get_mod_val_id(u8);

float gravity_y = -10.0f;
float gravity_x = 0.0f;

int sel = 0;

char* f[10] = { "0", "1", "2", "3", "4", "5", "6", "7", "8", "9" };

bool global = true;

class physobject {
public:
	float rect_x = 320 / 2;
	float rect_y = 240 / 2;
	u8 mov_rect = 0.0f;
	float vel_x = 0.0f;
	float vel_y = 0.0f;
	float elasticity = .5f;
	float friction = .5f;
	int kind = 0; //0 = rectangle 1 = circle 2 = triangle
	u32 color;
};

int main()
{
	// Set the random seed based on the time
	srand(time(NULL));

	//init screen background to white
	sf2d_init();
	sf2d_set_clear_color(RGBA8(0xFF, 0xFF, 0xFF, 0xFF));
	sf2d_set_3D(0);
	sf2d_set_vblank_wait(1);
	
	physobject arr[count];

	for (size_t i = 0; i < count; i++)
	{
		arr[i].rect_x = rand() % 320;
		arr[i].rect_y = rand() % 240;
		arr[i].color = RGBA8(rand() % 255, rand() % 255, rand() % 255, 255);
		arr[i].kind = rand() % 3;
	}

	//Variables

	//Represents the current modifiable value: gravity_y, gravity_x, friction, elasticity
	u8 mod_val = 0;

	//coordinates of screen touch
	u16 touch_x = 320/2;
	u16 touch_y = 240/2;

	//increment - determines how much is added or subtracted to mod_val - is always power of 10 
	double inc = 1.0f;
	
	//boolean to determine if touch screen is currently dictating the square's location
	u8 mov_rect = 0.0f;

	//records where the user tapped on the square e.g. if the top right corner is tapped, it will stay at top right corner
	s8 offset_x = 0.0f;
	s8 offset_y = 0.0f;

	//HID variables
	touchPosition touch;
	circlePosition circle;
	u32 held;
	u32 pressed;
	u32 released;

	//physobject objects;

	consoleInit(GFX_TOP, NULL);

	printf("\x1b[0;0HPhysics Sandbox v0.1 by PieFace");
	printf("\x1b[23;0HControls:");
	printf("\x1b[24;0HTap and drag square to move it");
	printf("\x1b[25;0HX - Reset Square");
	printf("\x1b[26;0HDPad L&R - Change between mod_vals");
	printf("\x1b[27;0HDPad U&D - Increment and decrement mod_val");
	printf("\x1b[28;0HL&R - Change increment (pow of 10)");	
	printf("\x1b[29;0HStart - Exit");
	while (aptMainLoop()) {

		//gather inputs
		hidScanInput();
		hidCircleRead(&circle);
		held = hidKeysHeld();
		pressed = hidKeysDown();
		released = hidKeysUp();
		

		//check button presses

		//Press Start to Exit
		if (held & KEY_START) 
		{
			break;
		}
		
		//Read touch values if the screen is pressed
		if (held & KEY_TOUCH) 
		{
			hidTouchRead(&touch);
			touch_x = touch.px;
			touch_y = touch.py;
			if(mov_rect)
			{
				arr[sel].vel_x = (touch_x + offset_x - arr[sel].rect_x)*60.0f;
				arr[sel].vel_y = (touch_y + offset_y - arr[sel].rect_y)*60.0f;
			}
		}

		//DPAD up - Increment current mod_val
		if(pressed & KEY_DUP)
		{
			if(mod_val == 0)
			{
				gravity_y += inc;
			}
			
			if(mod_val == 1)
			{
				gravity_x += inc;
			}

			if(mod_val == 2)
			{
				if (global)
				{
					for (size_t i = 0; i < count; i++)
					{
						arr[i].elasticity += inc;
						if (arr[i].elasticity > 1) arr[i].elasticity = 1;
					}
				}
				else
				{
					arr[sel].elasticity += inc;
					if (arr[sel].elasticity > 1) arr[sel].elasticity = 1;
				}
			}

			if(mod_val == 3)
			{
				if (global)
				{
					for (size_t i = 0; i < count; i++)
					{
						arr[i].friction += inc;
						if (arr[i].friction > 1) arr[i].friction = 0;
					}
				}
				else
				{
					arr[sel].friction += inc;
					if (arr[sel].friction > 1) arr[sel].friction = 0;
				}
			}
		}
		
		//DPAD down - decrement current mod_val
		if(pressed & KEY_DDOWN)
		{
			if(mod_val == 0)
			{
				gravity_y -= inc;
			}
			
			if(mod_val == 1)
			{
				gravity_x -= inc;
			}

			if(mod_val == 2)
			{
				if (global)
				{
					for (size_t i = 0; i < count; i++)
					{
						arr[i].elasticity -= inc;
						if (arr[i].elasticity < 0) arr[i].elasticity = 0;
					}
				}
				else
				{
					arr[sel].elasticity -= inc;
					if (arr[sel].elasticity < 0) arr[sel].elasticity = 0;
				}
			}

			if(mod_val == 3)
			{
				if (global)
				{
					for (size_t i = 0; i < count; i++)
					{
						arr[i].friction -= inc;
						if (arr[i].friction < 0) arr[i].friction = 0;
					}
				}
				else
				{
					arr[sel].friction -= inc;
					if (arr[sel].friction < 0) arr[sel].friction = 0;
				}
			}
		}

		//DPAD Left and Right - Change mod_val
		if(pressed & KEY_DLEFT)
		{
			mod_val--;
			if (mod_val > 3) mod_val = 3;
		}
		
		if(pressed & KEY_DRIGHT)
		{
			mod_val++;
			if (mod_val > 3) mod_val = 0;
		}
		

		//L and R - Multiply and divide increment by 10
		if(pressed & KEY_L) inc *= 10;
		if(pressed & KEY_R) inc /= 10;		


		//Collisions and Friction
		//Detects collision with edge of the screen
		//Inverts velocity perpendicular to collision and gradually reduces velocity parallel to collision
		for (size_t i = 0; i < count; i++)
		{
			//Collision and friction with top or bottom edge
			if (arr[i].rect_x - rw / 2 <= 1 || arr[i].rect_x + rw / 2 >= 319)
			{
				arr[i].vel_x = -arr[i].vel_x * arr[i].elasticity;
				
				if (arr[i].rect_x < rw / 2) arr[i].rect_x = rw / 2;
				if (arr[i].rect_x > 320 - rw / 2) arr[i].rect_x = 320 - rw / 2;
				
				arr[i].vel_y += (arr[i].vel_y > 0 ? -fmin(arr[i].friction, arr[i].vel_y) : -fmax(-arr[i].friction, arr[i].vel_y));
			}

			//Collision and friction with left and right edge
			if (arr[i].rect_y - rh / 2 <= 1 || arr[i].rect_y + rh / 2 >= 239)
			{
				arr[i].vel_y = -arr[i].vel_y * arr[i].elasticity;

				if (arr[i].rect_y < rh / 2) arr[i].rect_y = rh / 2;
				if (arr[i].rect_y > 240 - rh / 2) arr[i].rect_y = 240 - rh / 2;

				arr[i].vel_x += (arr[i].vel_x > 0 ? -fmin(arr[i].friction, arr[i].vel_x) : -fmax(-arr[i].friction, arr[i].vel_x));
			}
		}

		//If the coordinates of the screen tap are inside the square, allow movement of the square
		if (pressed & KEY_TOUCH)
		{
			for (size_t i = 0; i < count; i++)
			{
				if ((touch_x > arr[i].rect_x - rw / 2) && (touch_x < arr[i].rect_x + rw / 2) && (touch_y > arr[i].rect_y - rh / 2) && (touch_y < arr[i].rect_y + rh / 2))
				{
					sel = i;
					mov_rect = 1;
					offset_x = arr[i].rect_x - touch_x;
					offset_y = arr[i].rect_y - touch_y;
					break;
				}
			}
		}

		//If the screen is let go, stop allowing screen to control square
		if ((released & KEY_TOUCH))
		{
			mov_rect = 0;
			offset_x = 0;
			offset_y = 0;
		}

		//Press x to reset all values
		if(pressed & KEY_X)
		{
			for (size_t i = 0; i < count; i++)
			{
				arr[i].rect_x = rand() % 320;
				arr[i].rect_y = rand() % 240;
				arr[i].color = RGBA8(rand() % 255, rand() % 255, rand() % 255, 255);
				arr[i].vel_x = 0.0f;
				arr[i].vel_y = 0.0f;
				arr[i].elasticity = .5f;
				arr[i].friction = .5f;
			}
		}

		if (pressed & KEY_Y)
		{
			global = !global;
		}

		//Determines how to move the square
		//If the touch screen is not dictating the square's location, apply gravity and velocity
		//Else move square to where screen is touched
		for (size_t i = 0; i < count; i++)
		{
			if (!mov_rect)
			{
				arr[i].vel_y -= gravity_y;
				arr[i].vel_x += gravity_x;

				arr[i].rect_x += arr[i].vel_x / 60.0f;
				arr[i].rect_y += arr[i].vel_y / 60.0f;
			}
			else if (i == sel)
			{
				arr[sel].rect_x = touch_x + offset_x;
				arr[sel].rect_y = touch_y + offset_y;
			}
		}

		//Live printing of important values
		#ifdef debug
		printf("\x1b[0;41H%s:%5.2f","fps",sf2d_get_fps());
		printf("\x1b[1;0H%s:%03d,%03d","touch_coord",touch_x,touch_y);
		printf("\x1b[2;0H%s:%03.0f,%03.0f","rect_coord", arr[sel].rect_x, arr[sel].rect_y);
		printf("\x1b[3;0H%s:%03d,%03d","offset",offset_x,offset_y);
		printf("\x1b[4;0H%s:%010.3f,%010.3f","velocity", arr[sel].vel_x, -arr[sel].vel_y);
		printf("\x1b[5;0H%s:%f,%f","gravity", gravity_x, gravity_y);
		printf("\x1b[6;0H%s:%f","elasticity", arr[sel].elasticity);
		printf("\x1b[7;0H%s:%f","friction", arr[sel].friction);
		printf("\x1b[8;0H%s:%f","increment", inc);		
		printf("\x1b[9;0H%s:%s","mod_val", get_mod_val_id(mod_val));
		printf("\x1b[10;0H%s:%s", "sel", f[sel]);
		printf("\x1b[11;0H%s:%s", "global", global ? "true" : "false");
		#endif

		//draw frame
		sf2d_start_frame(GFX_BOTTOM, GFX_LEFT);

		for (size_t i = count; i-- > 0;)
		{
			if (arr[i].kind == 0)
			{
				sf2d_draw_rectangle((int)arr[i].rect_x - rw / 2, (int)arr[i].rect_y - rh / 2, 50, 50, arr[i].color);
			}
			else if (arr[i].kind == 1)
			{
				sf2d_draw_fill_circle((int)arr[i].rect_x, (int)arr[i].rect_y, rw / 2, arr[i].color);
			}
			else if (arr[i].kind == 2)
			{
				int x = (int)arr[i].rect_x;
				int y = (int)arr[i].rect_y;
				sf2d_draw_triangle(x, y - rh / 2, x - rw / 2, y + rh / 2, x + rw / 2, y + rh / 2, arr[i].color);
			}
		}

		sf2d_end_frame();

		sf2d_swapbuffers();
	}

	
	sf2d_fini();
	return 0;
}

//returns string of mod_val from id
char* get_mod_val_id(u8 id)
{
	if(id==0) return "gravity_y ";
	if(id==1) return "gravity_x ";
	if(id==2) return "elasticity";
	if(id==3) return "friction  ";
	return "err_bad_id";
}