// Wiim API ©2006 Eric B.
// http://digitalretrograde.com/projects/wiim/

// May be used and modified freely as long as this message is left intact

// This is a "game" demo of using the Wiimote interface class. It's a partial 
// implementation of "Lunder Lander" that uses SDL for display. Skip down to main(..)
// for the interesting stuff. 

#include "..\SDL_gfx\SDL_gfxPrimitives.h"

#include <vector>


#include "Console.h"
#include "Utils.h"
#include "Wiimote.h"
#include <iostream>
#include <stdio.h>
using namespace std;
#include <assert.h>

#define SCREEN_WIDTH 640
#define SCREEN_HEIGHT 480

SDL_Surface * screen; 
static char temp2[100];
int InitVideo() 
{
	Uint32 flags = SDL_DOUBLEBUF;

	if (SDL_Init(SDL_INIT_VIDEO) != 0) 
	{
		fprintf(stderr, "Unable to initialize SDL: %s\n", SDL_GetError());
		return false;
	}

	screen = SDL_SetVideoMode(SCREEN_WIDTH, SCREEN_HEIGHT, 16, flags);

	if (screen == NULL) 
	{
		fprintf(stderr, "Unable to set video mode: %s\n", SDL_GetError());
		return false;
	}

	return true;
}

P2d_line platform(260, 470, 360, 479);

vector<P2d_line> GetLandscapeLines()
{
	vector<P2d_line> lines;

	lines.push_back(P2d_line(platform.p1, P2d(217, 363)));
	lines.push_back(P2d_line(217, 363, 128, 424));
	lines.push_back(P2d_line(128, 424, 58, 247));
	lines.push_back(P2d_line(58, 247, 0, 268));

	lines.push_back(P2d_line(platform.p2.x, platform.p1.y, 404, 315));
	lines.push_back(P2d_line(404, 315, 463, 369));
	lines.push_back(P2d_line(463, 369, 526, 121));
	lines.push_back(P2d_line(526, 121, 582, 287));
	lines.push_back(P2d_line(582, 287, 640, 260));

	return lines;
}

void DrawLandscape(SDL_Surface * screen)
{
	vector<P2d_line> lines = GetLandscapeLines();
	for (int i = 0; i < lines.size(); i++)
		lineRGBA(screen, (short)lines[i].p1.x, (short)lines[i].p1.y, (short)lines[i].p2.x, (short)lines[i].p2.y, 192, 192, 192, 255);

	rectangleRGBA(screen, (short)platform.p1.x, (short)platform.p1.y, (short)platform.p2.x, (short)platform.p2.y, 255, 255, 0, 255);
}

vector<P2d_line> GetLanderLines(double angle, double x, double y)
{
	vector<P2d_line> lander, rotated_lines;

	lander.push_back(P2d_line(26, 10, 19, 17));
	lander.push_back(P2d_line(19, 17, 19, 26));
	lander.push_back(P2d_line(19, 26, 26, 33));
	lander.push_back(P2d_line(26, 33, 43, 33));
	lander.push_back(P2d_line(43, 33, 50, 26));
	lander.push_back(P2d_line(50, 26, 50, 17));
	lander.push_back(P2d_line(50, 26, 50, 17));
	lander.push_back(P2d_line(50, 17, 43, 10));
	lander.push_back(P2d_line(43, 10, 26, 10));

	lander.push_back(P2d_line(18, 33, 50, 33));
	lander.push_back(P2d_line(50, 33, 50, 47));
	lander.push_back(P2d_line(50, 47, 18, 47));
	lander.push_back(P2d_line(18, 47, 18, 33));

	lander.push_back(P2d_line(18, 40, 13, 54));
	lander.push_back(P2d_line(13, 54, 23, 47));
	lander.push_back(P2d_line(13, 54, 13, 63));
	lander.push_back(P2d_line(7, 63, 18, 63));

	lander.push_back(P2d_line(50, 40, 55, 54));
	lander.push_back(P2d_line(55, 54, 45, 47));
	lander.push_back(P2d_line(55, 54, 55, 63));
	lander.push_back(P2d_line(61, 63, 50, 63));

	lander.push_back(P2d_line(32, 47, 29, 55));
	lander.push_back(P2d_line(29, 55, 40, 55));
	lander.push_back(P2d_line(40, 55, 37, 47));

	for (Uint32 i = 0; i < lander.size(); i++)
	{
		lander[i].p1.offset(-35, -35);
		lander[i].p2.offset(-35, -35);
	}

	for (int j = 0; j < lander.size(); j++)
	{
		P2d p1 = lander[j].p1; P2d p2 = lander[j].p2;
		p1.rotate(angle); p2.rotate(angle);
		p1.offset(x, y); p2.offset(x, y);

		rotated_lines.push_back(P2d_line(p1, p2));
	}

	return rotated_lines;
}

bool TestLandscapeLines(double angle, double x, double y, P2d & velocity)
{
	vector<P2d_line> lines = GetLandscapeLines();
	vector<P2d_line> lander_lines = GetLanderLines(angle, x, y);

	for (int i = 0; i < lines.size(); i++)
	{
		for (int j = 0; j < lander_lines.size(); j++)
		{
			P2d p1 = lander_lines[j].p1; 
			P2d p2 = lander_lines[j].p2;

			P2d_line test1(p1, p1 + velocity);
			P2d_line test2(p2, p2 + velocity);

			if (P2d_line::line_segment_intersection(test1, lines[i]) ||
				P2d_line::line_segment_intersection(test2, lines[i]))
				return true;
		}
	}

	return false;
}

void DrawLander(SDL_Surface * screen, double angle, double x, double y, bool crashed)
{
	vector<P2d_line> lines = GetLanderLines(angle, x, y);

	for (int i = 0; i < lines.size(); i++)
		lineRGBA(screen, (short)lines[i].p1.x, (short)lines[i].p1.y, (short)lines[i].p2.x, (short)lines[i].p2.y, 255, crashed ? 0 : 255, crashed ? 0 : 255, 255);
}

void DrawFlame(SDL_Surface * screen, double angle, double x, double y)
{
	vector<P2d_line> lines;

	lines.push_back(P2d_line(33, 55, 35, 70)); lines[0].p1.offset(-35, -35); lines[0].p2.offset(-35, -35);
	lines.push_back(P2d_line(35, 70, 37, 55)); lines[1].p1.offset(-35, -35); lines[1].p2.offset(-35, -35);

	for (Uint32 i = 0; i < lines.size(); i++)
	{
		P2d p1 = lines[i].p1; P2d p2 = lines[i].p2;
		p1.rotate(angle); p2.rotate(angle);
		p1.offset(x, y); p2.offset(x, y);

		lineRGBA(screen, (short)p1.x, (short)p1.y, (short)p2.x, (short)p2.y, 255, 192, 0, 255);
	}
}

bool TestLander(double angle, double x, double y, P2d_line & test_box)
{
	vector<P2d_line> lines = GetLanderLines(angle, x, y);

	P2d_line box(1000, 1000, -1, -1); // "inside out"

	for (int i = 0; i < lines.size(); i++)
	{
		P2d p1 = lines[i].p1; 
		P2d p2 = lines[i].p2;

		if (p1.x < box.p1.x) box.p1.x = p1.x;
		if (p1.y < box.p1.y) box.p1.y = p1.y;
		if (p2.x > box.p2.x) box.p2.x = p2.x;
		if (p2.y > box.p2.y) box.p2.y = p2.y;
	}

	test_box = box;

	// Make sure we're on the screen
	if (test_box.p1.x < 0.0 || test_box.p1.y < 0.0 || test_box.p2.x > (double)SCREEN_WIDTH || test_box.p2.y > (double)SCREEN_HEIGHT)
		return true;

	return P2d_line::boxes_collide(box, platform);
}

vector<MotionData> last_positions;

// This function takes the motion data and averages it over the last several inputs to
// keep the ship from jumping around because of errant sensor readings.

// It also assumes that the x sensor reading is an angle, which it generally isn't. 
// When the remote is pitched, gravity increases the values. We clamp these to make sure
// the angle doesn't get to be too big (try removing the test and shaking the remote to see).
double GetAveragedAngle(MotionData p)
{
	if (last_positions.size() <= 2)
	{	
		last_positions.push_back(p);
		return p.x;
	}

	last_positions.erase(last_positions.begin());
	
	
	if (p.x > 30) p.x = 30; // Restrict the values to -30 to 30
	if (p.x < -30) p.x = -30;

	last_positions.push_back(p);
	//sprintf(temp2, "X: %d, Y: %d, Z: %d", last_positions[0].x, last_positions[1].x, last_positions[2].x);
	//odprintf(temp2);
	return  (last_positions[0].x + last_positions[1].x + last_positions[2].x) / 3.0;
}

int main( int argc, char* argv[] )
{
	const double gravity = 4.0; // pixels / s * s
	const double rocket = 12.0; // pixels / s * s
	char temp[100];
	
	bool landed = false, crashed = false;
	bool started = false, quit = false;
	double angle = 0.0;
	P2d location = P2d(120.0, 140.0);
	V2d velocity = V2d(0, 0);
	int led_flash = 0;
	
	int updates_per_second = 100;
	int interval = 1000 / updates_per_second;

	vector<Wiimote> wiimotes = HIDDevice::ConnectToHIDDevices<Wiimote>();

	if (wiimotes.size() <= 0) // Need to add a user fri
		return 1;

	wiimotes[0].SetLEDs(0, 1, 1, 0);
	wiimotes[0].StartListening();
	wiimotes[0].RequestMotionData();

	InitVideo();
	
	while (!quit)
	{
		
		SDL_Event sdl_event;
		
		if (SDL_PollEvent( &sdl_event ))
		{
			if (sdl_event.type == SDL_QUIT || sdl_event.key.keysym.sym == SDLK_q || sdl_event.key.keysym.sym == SDLK_ESCAPE)
				quit = true;

			if (sdl_event.key.keysym.sym == SDLK_r)
			{
				angle = 0.0;
				location = P2d(100.0, 100.0);
				velocity = V2d(0, 0);
				landed = crashed = started = false;
			}
		}

		MotionData p = wiimotes[0].GetLastMotionData();
		Button & b = wiimotes[0].GetButton("A");

		double angle = DEG_TO_RAD(GetAveragedAngle(p) * 2.0); 
//sprintf(temp, "%f", 90*angle);
//odprintf(temp);
		if (b.Pressed() && !landed)
		{
			if (!started)
				started = true;

			velocity.x += sin(angle) * rocket / updates_per_second;
			velocity.y -= cos(angle) * rocket / updates_per_second;
			
			wiimotes[0].SetLEDs(led_flash < 5 ? 0 : 1, led_flash < 5 ? 1 : 0, led_flash < 5 ? 1 : 0, led_flash < 5 ? 0 : 1);
			led_flash++;
			if (led_flash > 10) led_flash = 0;
		}
		else
		{
			wiimotes[0].SetLEDs(0,0,0,0);
		}

		if (!landed)
		{
			if (TestLandscapeLines(angle, location.x, location.y, velocity))
				crashed = true;

			P2d_line test_box;
			bool platform_test = TestLander(angle, location.x, location.y, test_box);

			// If the lander is moving down too fast or sideways or at an angle, then you've crashed!
			if (platform_test && velocity.y > 1.2)
				crashed = true;

			if (platform_test && abs(velocity.x) > 0.35)
				crashed = true;

			if (platform_test && abs(angle) > DEG_TO_RAD(5))
				crashed = true;

			landed = (crashed || platform_test);
		}

		if (landed)
		{
			angle = 0;
			cout << "YAY LANDED" <<endl;
		}
		else if (started)
		{
			velocity.y += gravity / updates_per_second;
			location += velocity;
		}

		DrawLandscape(screen);
		DrawLander(screen, angle, location.x, location.y, crashed);

		if (b.Pressed() && !landed)
			DrawFlame(screen, angle, location.x, location.y);

		SDL_Flip(screen);
		SDL_FillRect(screen, &screen->clip_rect, SDL_MapRGB(screen->format, 0x0, 0x0, 0x0));

		SDL_Delay(interval);
	}

	wiimotes[0].StopListening();
	wiimotes[0].Disconnect();
	
	SDL_FreeSurface(screen);

	SDL_Quit();

	return 0;
}