#include "RemoteGui.h"
#include <BulletCollision/CollisionShapes/btShapeHull.h>

RemoteGui::RemoteGui(Environment *env)
: Gui(env), m_addCounter(0)
{
	m_dynamicsWorld = env->getDynamicsWorld();
}

RemoteGui::~RemoteGui()
{
}

void RemoteGui::init(int port)
{
	printf("Initializing remote gui...\n");

	struct sockaddr_in serv_addr;

	sockfd = socket(AF_INET, SOCK_STREAM, 0);

	memset((char *) &serv_addr, 0, sizeof(serv_addr));
	serv_addr.sin_family = AF_INET;
	serv_addr.sin_addr.s_addr = INADDR_ANY;	// foloseste adresa IP a masinii
	serv_addr.sin_port = htons(port);

	if (bind(sockfd, (struct sockaddr *) &serv_addr, sizeof(struct sockaddr)) < 0) {
			printf("Error binding!\n");
			exit(0);
	 }
}

int RemoteGui::equal(guiUpdateData *e1, guiUpdateData *e2)
{
	for (int i = 0; i < 16; ++ i) {
		if (e1->m[i] != e2->m[i])
			return 0;
	}

	if (e1->aabbMin != e2->aabbMin)
		return 0;

	if (e1->aabbMax != e2->aabbMax)
		return 0;

	for (int i = 0; i < 3; ++ i) {
		for (int j = 0; j < 3; ++ j) {
			if (e1->rot[i][j] != e2->rot[i][j])
				return 0;
		}
	}

//	if (e1->rot != e2->rot)
//		return 0;

	return 1;
}

//int RemoteGui::getGuiData(guiData **gd_out)
int RemoteGui::getGuiData(map<void *, guiAddData *> &addObjects, map<void *, guiUpdateData *> &updateObjects, vector<int> &removeObjects)
{
	int numObjects = m_dynamicsWorld->getNumCollisionObjects();

//	guiData *gd = new guiData[numObjects];

	// temp map - created to test for removed objects
	map<void *, guiUpdateData *> temp;

//	getchar();
	for (int i = 0; i < numObjects; ++ i)
	{
		guiUpdateData *elem = new guiUpdateData;

		btCollisionObject *colObj = m_dynamicsWorld->getCollisionObjectArray()[i];
		btRigidBody *body = btRigidBody::upcast(colObj);
		if(body && body->getMotionState())
		{
			btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState();
			myMotionState->m_graphicsWorldTrans.getOpenGLMatrix(elem->m);
			elem->rot = myMotionState->m_graphicsWorldTrans.getBasis();
		}
		else
		{
			colObj->getWorldTransform().getOpenGLMatrix(elem->m);
			elem->rot = colObj->getWorldTransform().getBasis();
		}

		m_dynamicsWorld->getBroadphase()->getBroadphaseAabb(elem->aabbMin, elem->aabbMax);

		map<void *, guiUpdateData *>::iterator it = m_cache.find((void *) colObj);

		// check for new or updated objects
		if (it == m_cache.end()) {
			// add new object
			guiAddData *addElem = new guiAddData;

			addElem->id = m_addCounter;
			elem->id = m_addCounter ++;

//			printf("[%d %d]\n", addElem->id, elem->id);

			btShapeHull *hull = new btShapeHull((btConvexShape *) colObj->getCollisionShape());
			hull->buildHull(colObj->getCollisionShape()->getMargin());
			addElem->numIndices = hull->numIndices();
			addElem->numVertices = hull->numVertices();
			addElem->indexPointer = (int *) hull->getIndexPointer();
			addElem->vertexPointer = (btVector3 *) hull->getVertexPointer();

//			printf("[%d %d %d %d %f]\n", addElem->id, addElem->numIndices, addElem->indexPointer[0], addElem->numIndices, addElem->vertexPointer[0]);
//			printf("[%d %d %d]\n", addElem->id, addElem->numIndices, addElem->numIndices);
			printf("[%d %d %d]\n", addElem->id, hull->numIndices(), hull->numVertices());

			addObjects[colObj] = addElem;

			m_cache[colObj] = elem;

			updateObjects[colObj] = elem;
//			printf("%p\n", m_dynamicsWorld->getCollisionObjectArray()[i]);
		} else {
			// update existing object
			if (!equal(it->second, elem)) {
				updateObjects[colObj] = elem;
				delete m_cache[colObj];
				m_cache[colObj] = elem;
			}
		}

		temp[colObj] = elem;
	}

	// check for removed objects
	map<void *, guiUpdateData *>::iterator it = m_cache.begin();
	while (it != m_cache.end()) {
		if (temp.find(it->first) == temp.end()) {
			removeObjects.push_back((*it).second->id);
		}
		++ it;
	}

//	*gd_out = gd;

	return numObjects;
}

void RemoteGui::sendData(int sock, const void *buffer, size_t size)
{
	size_t total;
	int ret;

	for (total = 0; total < size; total += ret) {
		ret = send(sock, (char *) buffer + total, size - total, 0);
//		printf("ret = %d\n", ret);
		if (ret <= 0) {
			perror("send");
			exit(-1);
		}
	}
}

float* RemoteGui::packScalar(btScalar s, void *buffer)
{
	float *buff = (float *) buffer;

	*buff ++ = s;

	return buff;
}

float* RemoteGui::packVector3(btVector3 v, void *buffer)
{
	float *buff = (float *) buffer;

	buff = packScalar(v.getX(), buff);
	buff = packScalar(v.getY(), buff);
	buff = packScalar(v.getZ(), buff);

	return buff;
}

float* RemoteGui::packMatrix3x3(btMatrix3x3 m, void *buffer)
{
	float *buff = (float *) buffer;

	for (int i = 0; i < 3; ++ i) {
		buff = packVector3(m[i], buff);
	}

	return buff;
}

int * RemoteGui::packInt(int i, void *buffer)
{
	int *buff = (int *) buffer;

	*buff ++ = i;

	return buff;
}

void* RemoteGui::packGuiData(guiUpdateData *gd, void *buff)
{
	// add id
	buff = packInt(gd->id, buff);

	// add btScalar vector
	char *p = (char *) buff;
	for (int i = 0; i < 16; ++ i) {
		buff = packScalar(gd->m[i], buff);
	}
	for (int i = 0; i < 16; i++) {
		printf("ZZZZZZZZZZ: i=%d: %f\n", i, *(float*)(p+sizeof(float)*i));
	}

	// add rot
	buff = packMatrix3x3(gd->rot, buff);

	// add aabbMin
	buff = packVector3(gd->aabbMin, buff);

	// add aabbMax
	buff = packVector3(gd->aabbMax, buff);

	return buff;
}

int RemoteGui::packAddData(map<void *, guiAddData *> addObjects, void **buffer)
{
//	printf("Packing add data!\n");
	void *temp, *buff;
	int i;

	map<void *, guiAddData *>::iterator it = addObjects.begin();

	int size = 0;

	for ( ; it != addObjects.end(); it ++) {
		guiAddData *obj = (*it).second;

		size += sizeof(int);
		size += sizeof(int);
		size += sizeof(int) * obj->numIndices;
		size += sizeof(int);
		size += sizeof(float) * 3 * obj->numVertices;
	}

	buff = new char [size];
	temp = buff;

	it = addObjects.begin();
	for ( ; it != addObjects.end(); it ++) {
		guiAddData *obj = (*it).second;

		// add id
		temp = packInt(obj->id, temp);
//		printf("packing: obj->id = %d\n", obj->id);

		// add indexPointer
		temp = packInt(obj->numIndices, temp);
		for (i = 0; i < obj->numIndices; ++ i) {
			temp = packInt(obj->indexPointer[i], temp);
		}

		// add vertexPointer
		temp = packInt(obj->numVertices, temp);
		for (i = 0; i < obj->numVertices; ++ i) {
			temp = packVector3(obj->vertexPointer[i], temp);
		}
	}

	*buffer = buff;

	return size;
}

int RemoteGui::packUpdateData(map<void *, guiUpdateData *> updateObjects, void **buffer)
{
	void *temp, *buff;

	map<void *, guiUpdateData *>::iterator it = updateObjects.begin();

	int size = 0;

	for ( ; it != updateObjects.end(); it ++) {
		size += sizeof(int) + sizeof(float) * (16 + 9 + 3 + 3);
	}

	buff = new char [size];
	temp = buff;

	it = updateObjects.begin();
	for ( ; it != updateObjects.end(); it ++) {
		guiUpdateData *obj = (*it).second;
		temp = packGuiData(obj, temp);
	}

	*buffer = buff;

	return size;
}

int RemoteGui::packRemoveData(vector<int> removeObjects, void **buffer)
{
	void *temp, *buff;

	vector<int>::iterator it = removeObjects.begin();

	int size = sizeof(int) * removeObjects.size();

	buff = new int[size];
	temp = buff;

	for ( ; it != removeObjects.end(); it ++) {
		temp = packInt((*it), temp);
	}

	*buffer = buff;

	return size;
}

void RemoteGui::run()
{
	printf("Running remote gui...\n");

	int n;
//	char buffer[256];
	map<void *, guiAddData *> addObjects;
	map<void *, guiUpdateData *> updateObjects;
	vector<int> removeObjects;

	struct sockaddr_in cli_addr;

	listen(sockfd, MAX_CLIENTS);

     int clilen = sizeof(struct sockaddr_in);
     int newsockfd = accept(sockfd,
                 (struct sockaddr *) &cli_addr,
                 (socklen_t *) &clilen);

	if (newsockfd < 0) {
		printf("Error accepting connection!\n");
		exit(0);
	}

	n = send(newsockfd, SERVER_BANNER, strlen(SERVER_BANNER), 0);
	if (n < 0) {
		printf("Error writing to socket\n");
		exit(0);
	}

     while (1) {
		char q;

		n = recv(newsockfd, (void*) &q, 1, 0);
     	if (n < 0) {
			printf("Error reading from socket\n");
			exit(0);
		}

     	if (n == 0) {
			printf("Client left \n");
     	    break;
		}

		addObjects.clear();
		updateObjects.clear();
		removeObjects.clear();
		n = getGuiData(addObjects, updateObjects, removeObjects);

		void *buff;

		n = addObjects.size();
		sendData(newsockfd, (const void *) &n, sizeof(int));
		int sizeAddData = packAddData(addObjects, &buff);
		sendData(newsockfd, (const void *) &sizeAddData, sizeof(int));
		sendData(newsockfd, (const void *) buff, sizeAddData);
		delete [] (char *) buff;

		n = updateObjects.size();
		sendData(newsockfd, (const void *) &n, sizeof(int));
		int sizeUpdateData = packUpdateData(updateObjects, &buff);
		sendData(newsockfd, (const void *) &sizeUpdateData, sizeof(int));
		sendData(newsockfd, (const void *) buff, sizeUpdateData);
		delete [] (char *) buff;

		n = removeObjects.size();
		sendData(newsockfd, (const void *) &n, sizeof(int));
		int sizeRemoveData = packRemoveData(removeObjects, &buff);
		sendData(newsockfd, (const void *) &sizeRemoveData, sizeof(int));
		sendData(newsockfd, (const void *) buff, sizeRemoveData);
		delete [] (int *) buff;

		printf("Sent data!\n");

		m_dynamicsWorld->stepSimulation(0.1);

     }

     close(sockfd);
     close(newsockfd);
}