#ifndef POINTMASS_H
#define POINTMASS_H

//completed to fit into ofxVec2f
//3D later?

#include "additionalFunctions.h"

class pointMass
{
	public:

		pointMass()
		{
			mass = 0.0f;
			currPos.set(0.0f, 0.0f);
			prevPos.set(0.0f, 0.0f);
			force.set(0.0f, 0.0f);
			friction = 0.0f;
			atRest = false;
			Move(1);
			Move(1);
		}

		pointMass(float xPos, float yPos, float aMass)
		{
			mass = aMass;
			currPos.set(xPos, yPos);
			prevPos.set(xPos, yPos);
			force.set(0.0f, 0.0f);
			friction = 0.99f;
			atRest = false;
			step = 0.0f;
			prevdt = 0.0f;
			Move(1);
			Move(1);
		}

		pointMass(pointMass& that)
		{
			mass = that.mass;
			currPos.set(that.currPos);
			prevPos.set(that.prevPos);
			force.set(that.force);
			friction = that.friction;
			atRest = that.atRest;
			Move(1);
			Move(1);
		}

		void bind() {
			currPos.x = MAX( MIN(currPos.x,600), 0);
			currPos.y = MAX( MIN(currPos.y,600), 0);
		}

		void Move(const float dt)
		{
			float dtdt, f1, f2;

			dtdt = dt * dt;
			f1 = friction;
			f2 = 1.0f + f1;
			
			ofxVec2f t;
			ofxVec2f a = force / mass;
			ofxVec2f c = currPos;
			ofxVec2f p = prevPos;
			t = f2 * c - f1 * prevPos + a * dtdt;
			//t = (c + (c - p) + a * dtdt); //correct verlet (some assumptions), also simplifies with ofxVec2f
			
			prevPos = c;
			currPos = t;

			bind();
			
			step++;
		  
		}

		float SquareVelocity() const
		{
			float t1 = currPos.x - prevPos.x;
			float t2 = currPos.y - prevPos.y;
			return t1 * t1 + t2 * t2;
		}

		static float Dist(pointMass& a, pointMass& b)
		{
			return sqrt(ofxVec2fDistance(a.currPos, b.currPos));
		}

		void SetFriction(const float aFriction)
		{
			friction = 1.0f - aFriction;
		}

		void SetForce(const ofxVec2f& aForce)
		{
			force = aForce;
		}

		void AddForce(const ofxVec2f& aForce)
		{
			force += aForce;
		}

		ofxVec2f currPos;
		ofxVec2f prevPos;
		ofxVec2f nextPos;
		ofxVec2f force;
		float mass;
		float friction;
		float prevdt;
		//vector<connection *> connections;
		bool atRest;
		float step;
};

#endif
