#include "CollisionTutorialBullet2.h"
#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
#include "../CommonInterfaces/CommonRenderInterface.h"

#include "../CommonInterfaces/CommonExampleInterface.h"
#include "LinearMath/btTransform.h"

#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "../RenderingExamples/TimeSeriesCanvas.h"
#include "stb_image/stb_image.h"
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3Matrix3x3.h"
#include "../CommonInterfaces/CommonParameterInterface.h"

#include "LinearMath/btAlignedObjectArray.h"
#include "CollisionSdkC_Api.h"
#include "LinearMath/btQuickprof.h"

///Not Invented Here link reminder http://www.joelonsoftware.com/articles/fog0000000007.html

///todo: use the 'userData' to prevent this use of global variables
static int gTotalPoints = 0;
const int sPointCapacity = 10000;
const int sNumCompounds = 10;
const int sNumSpheres = 10;

lwContactPoint pointsOut[sPointCapacity];
int numNearCallbacks = 0;
static btVector4 sColors[4] =
	{
		btVector4(1, 0.7, 0.7, 1),
		btVector4(1, 1, 0.7, 1),
		btVector4(0.7, 1, 0.7, 1),
		btVector4(0.7, 1, 1, 1),
};

void myNearCallback(plCollisionSdkHandle sdkHandle, plCollisionWorldHandle worldHandle, void* userData, plCollisionObjectHandle objA, plCollisionObjectHandle objB)
{
	numNearCallbacks++;
	int remainingCapacity = sPointCapacity - gTotalPoints;
	btAssert(remainingCapacity > 0);

	if (remainingCapacity > 0)
	{
		lwContactPoint* pointPtr = &pointsOut[gTotalPoints];
		int numNewPoints = plCollide(sdkHandle, worldHandle, objA, objB, pointPtr, remainingCapacity);
		btAssert(numNewPoints <= remainingCapacity);
		gTotalPoints += numNewPoints;
	}
}

class CollisionTutorialBullet2 : public CommonExampleInterface
{
	CommonGraphicsApp* m_app;
	GUIHelperInterface* m_guiHelper;
	int m_tutorialIndex;

	TimeSeriesCanvas* m_timeSeriesCanvas0;

	plCollisionSdkHandle m_collisionSdkHandle;
	plCollisionWorldHandle m_collisionWorldHandle;

	//	int m_stage;
	//	int m_counter;

public:
	CollisionTutorialBullet2(GUIHelperInterface* guiHelper, int tutorialIndex)
		: m_app(guiHelper->getAppInterface()),
		  m_guiHelper(guiHelper),
		  m_tutorialIndex(tutorialIndex),
		  m_timeSeriesCanvas0(0),
		  m_collisionSdkHandle(0),
		  m_collisionWorldHandle(0)
	//	m_stage(0),
	//	m_counter(0)
	{
		gTotalPoints = 0;
		m_app->setUpAxis(1);

		switch (m_tutorialIndex)
		{
			case TUT_SPHERE_PLANE_RTB3:
			case TUT_SPHERE_PLANE_BULLET2:
			{
				if (m_tutorialIndex == TUT_SPHERE_PLANE_BULLET2)
				{
					m_collisionSdkHandle = plCreateBullet2CollisionSdk();
				}
				else
				{
#ifndef DISABLE_REAL_TIME_BULLET3_COLLISION_SDK
					m_collisionSdkHandle = plCreateRealTimeBullet3CollisionSdk();
#endif  //DISABLE_REAL_TIME_BULLET3_COLLISION_SDK
				}
				if (m_collisionSdkHandle)
				{
					int maxNumObjsCapacity = 1024;
					int maxNumShapesCapacity = 1024;
					int maxNumPairsCapacity = 16384;
					btAlignedObjectArray<plCollisionObjectHandle> colliders;
					m_collisionWorldHandle = plCreateCollisionWorld(m_collisionSdkHandle, maxNumObjsCapacity, maxNumShapesCapacity, maxNumPairsCapacity);
					//create objects, do query etc
					{
						float radius = 1.f;

						void* userPointer = 0;
						{
							for (int j = 0; j < sNumCompounds; j++)
							{
								plCollisionShapeHandle compoundShape = plCreateCompoundShape(m_collisionSdkHandle, m_collisionWorldHandle);

								for (int i = 0; i < sNumSpheres; i++)
								{
									btVector3 childPos(i * 1.5, 0, 0);
									btQuaternion childOrn(0, 0, 0, 1);

									btVector3 scaling(radius, radius, radius);

									plCollisionShapeHandle childShape = plCreateSphereShape(m_collisionSdkHandle, m_collisionWorldHandle, radius);
									plAddChildShape(m_collisionSdkHandle, m_collisionWorldHandle, compoundShape, childShape, childPos, childOrn);

									//m_guiHelper->createCollisionObjectGraphicsObject(colObj,color);
								}
								if (m_tutorialIndex == TUT_SPHERE_PLANE_BULLET2)
								{
									btCollisionShape* colShape = (btCollisionShape*)compoundShape;
									m_guiHelper->createCollisionShapeGraphicsObject(colShape);
								}
								else
								{
								}

								{
									btVector3 pos(j * sNumSpheres * 1.5, -2.4, 0);
									btQuaternion orn(0, 0, 0, 1);
									plCollisionObjectHandle colObjHandle = plCreateCollisionObject(m_collisionSdkHandle, m_collisionWorldHandle, userPointer, -1, compoundShape, pos, orn);
									if (m_tutorialIndex == TUT_SPHERE_PLANE_BULLET2)
									{
										btCollisionObject* colObj = (btCollisionObject*)colObjHandle;
										btVector4 color = sColors[j & 3];
										m_guiHelper->createCollisionObjectGraphicsObject(colObj, color);
										colliders.push_back(colObjHandle);
										plAddCollisionObject(m_collisionSdkHandle, m_collisionWorldHandle, colObjHandle);
									}
								}
							}
						}
					}

					{
						plCollisionShapeHandle colShape = plCreatePlaneShape(m_collisionSdkHandle, m_collisionWorldHandle, 0, 1, 0, -3.5);
						btVector3 pos(0, 0, 0);
						btQuaternion orn(0, 0, 0, 1);
						void* userPointer = 0;
						plCollisionObjectHandle colObj = plCreateCollisionObject(m_collisionSdkHandle, m_collisionWorldHandle, userPointer, 0, colShape, pos, orn);
						colliders.push_back(colObj);
						plAddCollisionObject(m_collisionSdkHandle, m_collisionWorldHandle, colObj);
					}

					int numContacts = plCollide(m_collisionSdkHandle, m_collisionWorldHandle, colliders[0], colliders[1], pointsOut, sPointCapacity);
					printf("numContacts = %d\n", numContacts);
					void* myUserPtr = 0;

					plWorldCollide(m_collisionSdkHandle, m_collisionWorldHandle, myNearCallback, myUserPtr);
					printf("total points=%d\n", gTotalPoints);

					//plRemoveCollisionObject(m_collisionSdkHandle,m_collisionWorldHandle,colObj);
					//plDeleteCollisionObject(m_collisionSdkHandle,colObj);
					//plDeleteShape(m_collisionSdkHandle,colShape);
				}

				/*
				m_timeSeriesCanvas0 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,256,"Constant Velocity");
				
				m_timeSeriesCanvas0 ->setupTimeSeries(2,60, 0);
				m_timeSeriesCanvas0->addDataSource("X position (m)", 255,0,0);
				m_timeSeriesCanvas0->addDataSource("X velocity (m/s)", 0,0,255);
				m_timeSeriesCanvas0->addDataSource("dX/dt (m/s)", 0,0,0);
				 */
				break;
			}

			default:
			{
				m_timeSeriesCanvas0 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface, 512, 256, "Unknown");
				m_timeSeriesCanvas0->setupTimeSeries(1, 60, 0);
			}
		};

		{
			int boxId = m_app->registerCubeShape(100, 0.01, 100);
			b3Vector3 pos = b3MakeVector3(0, -3.5, 0);
			b3Quaternion orn(0, 0, 0, 1);
			b3Vector4 color = b3MakeVector4(1, 1, 1, 1);
			b3Vector3 scaling = b3MakeVector3(1, 1, 1);
			m_app->m_renderer->registerGraphicsInstance(boxId, pos, orn, color, scaling);
		}

		{
			int textureIndex = -1;

			if (1)
			{
				int width, height, n;

				const char* filename = "data/cube.png";
				const unsigned char* image = 0;

				const char* prefix[] = {"./", "../", "../../", "../../../", "../../../../"};
				int numprefix = sizeof(prefix) / sizeof(const char*);

				for (int i = 0; !image && i < numprefix; i++)
				{
					char relativeFileName[1024];
					sprintf(relativeFileName, "%s%s", prefix[i], filename);
					image = stbi_load(relativeFileName, &width, &height, &n, 3);
				}

				b3Assert(image);
				if (image)
				{
					textureIndex = m_app->m_renderer->registerTexture(image, width, height);
				}
			}
		}

		m_app->m_renderer->writeTransforms();
	}
	virtual ~CollisionTutorialBullet2()
	{
		delete m_timeSeriesCanvas0;

		plDeleteCollisionWorld(m_collisionSdkHandle, m_collisionWorldHandle);

		plDeleteCollisionSdk(m_collisionSdkHandle);

		m_timeSeriesCanvas0 = 0;
	}

	virtual void initPhysics()
	{
	}
	virtual void exitPhysics()
	{
	}

	virtual void stepSimulation(float deltaTime)
	{
#ifndef BT_NO_PROFILE
		CProfileManager::Reset();
#endif

		void* myUserPtr = 0;

		gTotalPoints = 0;
		numNearCallbacks = 0;
		{
			BT_PROFILE("plWorldCollide");
			if (m_collisionSdkHandle && m_collisionWorldHandle)
			{
				plWorldCollide(m_collisionSdkHandle, m_collisionWorldHandle, myNearCallback, myUserPtr);
			}
		}

#if 0
		switch (m_tutorialIndex)
		{
			case TUT_SPHERE_SPHERE:
			{
				if (m_timeSeriesCanvas0)
				{
					float xPos = 0.f;
					float xVel = 1.f;
					m_timeSeriesCanvas0->insertDataAtCurrentTime(xPos,0,true);
					m_timeSeriesCanvas0->insertDataAtCurrentTime(xVel,1,true);
				}
				break;
			}
			

			default:
			{
			}
			
		};
#endif

		if (m_timeSeriesCanvas0)
			m_timeSeriesCanvas0->nextTick();

		//	m_app->m_renderer->writeSingleInstanceTransformToCPU(m_bodies[i]->m_worldPose.m_position, m_bodies[i]->m_worldPose.m_orientation, m_bodies[i]->m_graphicsIndex);

		m_app->m_renderer->writeTransforms();
#ifndef BT_NO_PROFILE
		CProfileManager::Increment_Frame_Counter();
#endif
	}
	virtual void renderScene()
	{
		if (m_app && m_app->m_renderer)
		{
			m_app->m_renderer->renderScene();

			m_app->m_renderer->clearZBuffer();

			m_app->drawText3D("X", 1, 0, 0, 1);
			m_app->drawText3D("Y", 0, 1, 0, 1);
			m_app->drawText3D("Z", 0, 0, 1, 1);

			for (int i = 0; i < gTotalPoints; i++)
			{
				const lwContactPoint& contact = pointsOut[i];
				btVector3 color(1, 1, 0);
				btScalar lineWidth = 3;
				if (contact.m_distance < 0)
				{
					color.setValue(1, 0, 0);
				}
				m_app->m_renderer->drawLine(contact.m_ptOnAWorld, contact.m_ptOnBWorld, color, lineWidth);
			}
		}
	}

	virtual void physicsDebugDraw(int debugDrawFlags)
	{
	}
	virtual bool mouseMoveCallback(float x, float y)
	{
		return false;
	}
	virtual bool mouseButtonCallback(int button, int state, float x, float y)
	{
		return false;
	}
	virtual bool keyboardCallback(int key, int state)
	{
		return false;
	}

	virtual void resetCamera()
	{
		float dist = 10.5;
		float pitch = -32;
		float yaw = 136;
		float targetPos[3] = {0, 0, 0};
		if (m_app->m_renderer && m_app->m_renderer->getActiveCamera())
		{
			m_app->m_renderer->getActiveCamera()->setCameraDistance(dist);
			m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch);
			m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw);
			m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0], targetPos[1], targetPos[2]);
		}
	}
};

class CommonExampleInterface* CollisionTutorialBullet2CreateFunc(struct CommonExampleOptions& options)
{
	return new CollisionTutorialBullet2(options.m_guiHelper, options.m_option);
}