#include "world.h"
#include "globals.h"
#include "BadEntity.h"
#include "GoodEntity.h"
#include <stdlib.h>
#include <time.h>
#include <assert.h>

int BAD_ENTITY_COUNT = 20;
int GOOD_ENTITY_COUNT = 5;

const int CONSTRAINT_ITERATIONS = 3;

const float CENTRE_FORCE = 0;//6e-5f * TOTAL_ENTITY_COUNT*TOTAL_ENTITY_COUNT;

World::World(scene::ISceneManager* smgr) : smgr(smgr), level(1), clicked(false), play(false), playerCanDie(false)
{
	lifeImg = device->getVideoDriver()->getTexture("../data/img/life.png");
	levelImg = device->getVideoDriver()->getTexture("../data/img/level.png");
	numImg = device->getVideoDriver()->getTexture("../data/img/numbers.png");

	player = new Player(smgr, NULL);

	createLevel();
}

void World::createLevel()
{
	srand(clock());

	BAD_ENTITY_COUNT = 15 + level*5;
	GOOD_ENTITY_COUNT = level+4;

	// Bad entities
	for (int i = 0; i < BAD_ENTITY_COUNT; ++i)
	{
		add(new BadEntity(smgr, core::vector2df((f32)(rand()%SCREEN_WIDTH), (f32)(rand()%SCREEN_HEIGHT))));
	}
	// Good entities
	Entity* startEnt = NULL;
	for (int i = 0; i < GOOD_ENTITY_COUNT; ++i)
	{
		startEnt = new GoodEntity(smgr, core::vector2df((f32)(rand()%SCREEN_WIDTH), (f32)(rand()%SCREEN_HEIGHT)));
		add(startEnt);
	}
	player->setTarget(startEnt);

	// Run an update step and clear velocities, to stabilise starting position
	playerCanDie = false;
	update(1.0f / 60.0f);
	// clear vel
	for (u32 i=0; i < entities.size(); ++i)
	{
		Entity* ent = entities[i];
		ent->calcNewPos(1.0f / 60.0f);
	}
	playerCanDie = true;
}

void World::add(Entity* entity)
{
	entities.push_back(entity);
}

void World::update(float deltaTime)
{
	// Calculate inter-entity forces
	for (u32 i=0; i < entities.size(); ++i)
	{
		Entity* ent = entities[i];
		ent->clearForce();
	}
	for (u32 i=0; i < entities.size(); ++i)
	{
		for (u32 j=0; j < i; ++j)
		{
			// Every entity has 8 "ghost" copies arrayed in a rectangle
			// which play their part in physics
			for (int ghost1x = -1; ghost1x <= 1; ++ghost1x)
			for (int ghost1y = -1; ghost1y <= 1; ++ghost1y)
			for (int ghost2x = -1; ghost2x <= 1; ++ghost2x)
			for (int ghost2y = -1; ghost2y <= 1; ++ghost2y)
			{
				Entity* ent1 = entities[i];
				Entity* ent2 = entities[j];
				core::vector2df pos1 = ent1->getPos() + core::vector2df((f32)SCREEN_WIDTH*ghost1x, (f32)SCREEN_HEIGHT*ghost1y);
				core::vector2df pos2 = ent2->getPos() + core::vector2df((f32)SCREEN_WIDTH*ghost2x, (f32)SCREEN_HEIGHT*ghost2y);
				core::vector2df f = pos1 - pos2;
				f32 magnitude = 1.0f / (1.0f + f.getLengthSQ());
				f.normalize();
				f *= magnitude * ent1->repulsiveForce * ent2->repulsiveForce;
				if (ent1->attracts && ent2->attracts)
				{
					f *= -1;
				}
				ent1->addForce(f);
				ent2->addForce(-f);
			}
		}
	}

	/*// Pull towards centre
	core::vector2df centreOfScreen(0.5f*SCREEN_WIDTH, 0.5f*SCREEN_HEIGHT);
	for (u32 i=0; i < entities.size(); ++i)
	{
		Entity* ent = entities[i];
		core::vector2df f = centreOfScreen - ent->getPos();
		ent->addForce(CENTRE_FORCE * f);
	}*/

	// Integration step
	for (u32 i=0; i < entities.size(); ++i)
	{
		Entity* ent = entities[i];
		ent->setPos(ent->calcNewPos(deltaTime));
	}

	// Apply constraints
	for (int iter=0; iter < CONSTRAINT_ITERATIONS; ++iter)
	{
		// Collisions
		for (u32 i=0; i < entities.size(); ++i)
		{
			for (u32 j=0; j < i; ++j)
			{
				// Calc actual separation between entities
				Entity* ent1 = entities[i];
				Entity* ent2 = entities[j];
				core::vector2df separation = ent1->getPos() - ent2->getPos();

				// Calc minimum separation between entities
				float minSeparationSQ = (ent1->radius + ent2->radius);
				minSeparationSQ *= minSeparationSQ;

				if (separation.getLengthSQ() < minSeparationSQ)
				{
					// Collision. Push entities away from each other.
					separation.normalize();
					separation *= (ent1->radius + ent2->radius);
					core::vector2df midpoint = 0.5f * (ent1->getPos() + ent2->getPos());
					ent1->setPos(midpoint + separation * 0.5f);
					ent2->setPos(midpoint - separation * 0.5f);
				}
			}
		}

		// Remove duplicate connections
		for (u32 i=0; i < connections.size(); ++i)
		{
			if (connections[i]->potentialDuplicate)
			{
				for (u32 j=0; j < connections.size(); ++j)
				{
					if (i != j && connections[i]->duplicateOf(connections[j]))
					{
						printf("Removed duplicate %d (same as %d)\n", j, i);
						Connection* removed = connections[j];
						connections.erase(j);
						delete removed;
						if (i >= j) --i;
						--j;
					}
				}
				connections[i]->potentialDuplicate = false;
			}
		}

		// Connection constraints
		for (u32 i=0; i < connections.size(); ++i)
		{
			Connection* conn = connections[i];
			conn->update();
		}
		
		// Constrain to screen edges
		for (u32 i=0; i < entities.size(); ++i)
		{
			Entity* ent = entities[i];
			core::vector2df newPos = ent->getPos();
			if (newPos.X < 0) newPos.X = 0;
			if (newPos.Y < 0) newPos.Y = 0;
			if (newPos.X > SCREEN_WIDTH) newPos.X = SCREEN_WIDTH;
			if (newPos.Y > SCREEN_HEIGHT) newPos.Y = SCREEN_HEIGHT;
			ent->setPos(newPos);
		}
	}

	// Kill player on collision
	if (playerCanDie)
	for (u32 i=0; i < entities.size(); ++i)
	{
		Entity* ent = entities[i];
		if (!ent->canLandOn)
		{
			if (ent->getPos().getDistanceFromSQ(player->getPos()) < 40*40)
			{
				// Ouch!
				player->die();

				// Destroy connection being dragged
				s32 connIdx = connections.linear_search(player->draggingConnection);
				if (connIdx != -1)
				{
					connections.erase(connIdx);
				}
				delete player->draggingConnection;
				player->draggingConnection = NULL;

				// Destroy thing we collided with
				delete entities[i];
				entities.erase(i);
				break;
			}
		}
	}
	
	// Move player
	player->update(deltaTime);

	// Player deaded?
	if (playerCanDie && player->lives < 0)
	{
		// TODO: Display "level over" graphic
		level = 1;
		player->reset();
		destroyLevel();
		createLevel();
	}
	else
	{
		// Check if all good entities are connected
		// Clear all "connected" flags
		for (u32 i=0; i < entities.size(); ++i)
		{
			Entity* ent = entities[i];
			ent->isConnected = false;
		}
		// Set all of them again
		for (u32 i=0; i < connections.size(); ++i)
		{
			Connection* conn = connections[i];
			conn->from->isConnected = true;
			conn->to->isConnected = true;
		}
		// Check if any are not set
		bool allEntitiesConnected = true;
		for (u32 i=0; i < entities.size(); ++i)
		{
			Entity* ent = entities[i];
			if (ent->canLandOn && !ent->isConnected)
			{
				allEntitiesConnected = false;
				break;
			}
		}
		if (allEntitiesConnected)
		{
			printf("LEVEL WON!\n");
			++level;
			destroyLevel();
			createLevel();
			// TODO: Display "level won" graphic
		}
	}
}

bool World::OnEvent(const SEvent& e)
{
	if (e.EventType == irr::EET_MOUSE_INPUT_EVENT)
	{
		if (e.MouseInput.Event == irr::EMIE_LMOUSE_LEFT_UP)
		{
			clicked = true;
			if (!play) return true;

			scene::ISceneNode* node = smgr->getSceneCollisionManager()->
				getSceneNodeFromScreenCoordinatesBB(core::vector2di(e.MouseInput.X, e.MouseInput.Y));
			if (node)
			{	
				for (u32 i=0; i < entities.size(); ++i)
				{
					if (entities[i]->ownsSceneNode(node) && entities[i]->canLandOn)
					{
						Connection* newConnection = player->setTarget(entities[i]);
						if (newConnection)
						{
							connections.push_back(newConnection);
						}
						break;
					}
				}
			}
		}
	}
	return false;
}

void draw(video::ITexture* img, const core::position2di& pos)
{
	device->getVideoDriver()->draw2DImage(img, pos,
		core::recti(core::vector2di(0,0), img->getSize()), 0, video::SColor(255,255,255,255), true);
}

// TODO: Support numbers above 9
void World::drawNum(int& X, int Y, int num)
{
	num %= 10;
	device->getVideoDriver()->draw2DImage(numImg, core::position2d<s32>(X, Y),
		core::recti(num*32,0, num*32+32,32), 0, video::SColor(255,255,255,255), true);
	X += 32;
}

void World::drawUI()
{
	// level
	int X = 10;
	const int Y = 10;
	video::IVideoDriver* driver = device->getVideoDriver();
	draw(levelImg, core::position2d<s32>(X, Y));
	X += levelImg->getSize().Width + 10;

	// level number
	drawNum(X, Y, level);
	X += 20;

	// lives counter
	for (int i = 0; i < player->lives; ++i)
	{
		draw(lifeImg, core::position2d<s32>(X, Y));
		X += 42;
		//device->getVideoDriver()->draw2DRectangle(video::SColor(200, 32, 32, 192),
		//	core::recti(10 + i*42, 10, 42 + i*42, 42));
	}
}