#include "physics.h"
#include <math.h>

	//When called this updates the position of
	//the ball, using a pointer to that object
	void physics::ApplyTime(float vel, vertex_t dir, vertex_t *position)
	{
		//it takes the current position and adds velocity + dir onto it
		position->x = position->x + vel*dir.x;
		position->y = position->y + vel*dir.y;
		position->z = position->z + vel*dir.z;
	};
	//This takes in the normal of the wall its hitting, the direction of an object,
	//the pointer to xzDirection of an object, and the pointer to the direction of an object
	//and calculates the new directiont the object has
	void physics::HitWall(vertex_t theNormal, vertex_t dir, vertex_t *xzDir, vertex_t *pDir)
	{
		vertex_t temp = theNormal;
		float wallNormalDotOldDir = DotProduct(theNormal, dir) * 2; 
		temp.x = temp.x * wallNormalDotOldDir;
		temp.y = temp.y *wallNormalDotOldDir;
		temp.z = temp.z *wallNormalDotOldDir;
		dir = subtract(dir, temp);
		normalize(&dir);
		xzDir->x = dir.x;
		xzDir->y = dir.y;
		xzDir->z = dir.z;
		xzDir->y = 0;
		lastTile = -1;
		pDir->x = dir.x;
		pDir->y = dir.y;
		pDir->z = dir.z;
	};
	//creates a ray and checks how close an object is to hitting a plane.  It takes the plane of the wall its hitting
	//a point on that wall, the origin of the object thats moving, and the direction of the object and checks if it
	//the time its going to hit is between 0 and 0.15
	bool physics::rayPlaneIntersection( vertex_t planeNormal, vertex_t pointOnPlane, vertex_t orig, vertex_t dir)
	{
		float d = DotProduct(planeNormal, pointOnPlane);
		if(DotProduct(planeNormal, dir) == 0) return false;
		float t = (d-(DotProduct(planeNormal, orig) )) / (DotProduct(planeNormal, dir));
		
		if (t >= 0 && t < 0.15)
		{
			return true;
		}
		else return false;
	}
	//creates a ray and checks how close an object is to hitting a plane.  It takes the plane of the wall its hitting
	//a point on that wall, the origin of the object thats moving, and the direction of the object and checks if it
	//the time its going to hit is between 0 and -0.15
	//This is used to see if the ball has moved into a new tile
	bool physics::rayTilePlaneIntersection( vertex_t planeNormal, vertex_t pointOnPlane, vertex_t orig, vertex_t dir)
	{
		float d = DotProduct(planeNormal, pointOnPlane);
		if(DotProduct(planeNormal, dir) == 0) return false;
		float t = (d-(DotProduct(planeNormal, orig) )) / (DotProduct(planeNormal, dir));
		
		if (t < 0 && t >= -0.15)
		{
			return true;
		}
		else return false;
	}
	//This takes the radius of an object, its origin, and the radius of an object its trying to hit and its origin
	//and checks to see if it hits
	bool physics::collisionDetection(vertex_t targetTransform, float targetRadius, vertex_t pos, float rad)
	{
		vertex_t temp;
		float distance;

		temp.x = targetTransform.x - pos.x;
		temp.y = targetTransform.y - pos.y;
		temp.z = targetTransform.z - pos.z;
		distance = sqrt(temp.x*temp.x + temp.y*temp.y + temp.z*temp.z);

		if (rad + targetRadius >= distance)
			return true;
		return false;
	};

	bool physics::CircleCollisionCheck(vertex_t ballPos1, vertex_t ballPos2, float size)
	{
		double d = sqrt(pow(ballPos1.x - ballPos2.x, 2) + pow(ballPos1.z - ballPos2.z, 2));
		if(d <= size *2)
			return true;
		else return false;
	};

	std::vector<vertex_t> physics::CircleCollisionResponse(vertex_t ballPos1, vertex_t ballPos2, vertex_t ballDir1, vertex_t ballDir2, float vel1, float vel2, float size)
	{
		std::vector<vertex_t> newBallVel;
		float d = sqrt(pow(ballPos1.x - ballPos2.x, 2) + pow(ballPos1.z - ballPos2.z, 2));
		float nx = (ballPos2.x - ballPos1.x) / d;
		float ny = (ballPos2.z -ballPos1.z) / d;
		float p = 2 * ((ballDir1.x) * nx + (ballDir1.z) * ny - (ballDir2.x) * nx - (ballDir2.z) * ny) / (2);//circle1.mass + circle2.mass);
		float vx1 = (ballDir1.x) - p /* circle1.mass*/ * nx;
		float vy1 =(ballDir1.z) - p /* circle1.mass */* ny;
		float vx2 = (ballDir2.x) + p /* circle2.mass */* nx;
		float vy2 = (ballDir2.z) + p /* circle2.mass */* ny;
		vertex_t temp;

		temp.x = vx1;
		temp.y = 0;
		temp.z = vy1;
		normalize(&temp);
		newBallVel.push_back(temp);
		temp.x = vx2;
		temp.z = vy2;
		normalize(&temp);
		newBallVel.push_back(temp);
		return newBallVel;
	};