/*
 *  verlet.h
 *  openFrameworks
 *
 *  Created by amanda mcdonald crowley on 5/09/08.
 *  Copyright 2008 __MyCompanyName__. All rights reserved.
 *
 */

#include "pointmass.h"
#include "stick.h"
#include "joint.h"

class verlet {
	
	public:
	
		float NUM_PARTICLES;
		float resting;
		
		vector<pointMass *> active;
		vector<joint *>		joints;
		vector<stick *>		sticks;
		
		ofxVec2f gravity;
		
		verlet() {
			gravity.set(0,1);
		}
		
		~verlet() {
			delete & active;
			delete & joints;
			delete & sticks;
		}
		
		void addPoint(float x, float y, float forcex, float forcey, float aMass) {
			pointMass * tempPM = new pointMass(x, y, aMass);
			tempPM->AddForce(ofxVec2f(forcex, forcey));
			active.push_back(tempPM);
		}
		
		void addStick(pointMass* a, pointMass* b) {
			stick * tempS = new stick(*a, *b);
			sticks.push_back(tempS);
		}
		
		void addJoint(pointMass* a, pointMass* b, float min, float max) {
			joint * tempJ = new joint(*a, *b, min,max);
			joints.push_back(tempJ);
		}
		
		void addBlob(float x, float y) {
			float as = active.size();
			float r = 80 + 50 * sin(50);
			int nSteps = 8;
			float angleChange = TWO_PI / (float)(nSteps - 1);
			float angle = 0;
			for (int i = 0; i < nSteps-1; i++){
				float px = x + r * cos(angle);
				float py = y + r * sin(angle);
				
				pointMass * tempPM = new pointMass(px, py,1);
				active.push_back(tempPM);
				
				if (i > 0) {
					stick * tempS = new stick(*active[i-1], *active[i]);
					sticks.push_back(tempS);
				}
				
				angle += angleChange;
			}
			
			stick * tempS = new stick(*active[0], *active[nSteps-2]);
			sticks.push_back(tempS);
			
			int counter = 3;
			
			for(int i = 0; i < nSteps- 4; i++) {
				for(int j = 0; j < 3; j++) {
					int tempj = j;
					
					if(j + i +3 > 6) j -= 4;
					joint * tempJ = new joint(*active[i], *active[i+3+j], 1, 1.1);
					joints.push_back(tempJ);
					j = tempj;

				}
			}

				counter ++;
		}
		
		void draw() {
			if(active.size() > 0) {
				for(int i = 0; i < active.size(); i++) {
					ofSetColor(255,255,255);
					ofCircle(active[i]->currPos.x, active[i]->currPos.y, 4);
				}
			}
			if(sticks.size() > 0 ) {
				for(int i = 0; i < sticks.size(); i++) {
					ofSetColor(128,128,128);
					ofLine(sticks[i]->a->currPos.x, sticks[i]->a->currPos.y, 
						   sticks[i]->b->currPos.x, sticks[i]->b->currPos.y);
				}
			}
			if(joints.size() > 0 ) {
				for(int i = 0; i < joints.size(); i++) {
					ofSetColor(180,13,13);
					ofLine(joints[i]->a->currPos.x, joints[i]->a->currPos.y, 
						   joints[i]->b->currPos.x, joints[i]->b->currPos.y);
				}
			}
		}
		
		void tick() {
			NUM_PARTICLES = active.size();
			if(NUM_PARTICLES > 0) {
				resting = 0;
				for(int i=0; i<NUM_PARTICLES; i++) {
					pointMass &p = *active[i];
					
					float speed = 0.0f, epsilon;
					
					epsilon = 0.001;
					speed = (p.currPos.x - p.prevPos.x)*(p.currPos.x - p.prevPos.x) + 
							(p.currPos.y - p.prevPos.y)*(p.currPos.y - p.prevPos.y);
					if(speed > epsilon || p.step < 10) {
						p.Move(0.1);
						printf("active[%i] moving at %f\n", speed);
						resting++;
					}
				}
			}
		}

		void setGravity(float g) {
			gravity.set(0, g);
		}
		
		void setGravity(ofxVec2f g) {
			gravity.set(g);
		}
		// This function should accumulate forces for each particle
		void AccumulateForces()
		{    
			// All particles are influenced by gravity
			for(int i=0; i<NUM_PARTICLES; i++)  active[i]->AddForce(gravity);
		}
		
		// Here constraints should be satisfied
		void SatisfyConstraints() {
			for(int j = 0; j < 15; j++) {
				if(sticks.size() > 0) {
					for(int i = 0; i < sticks.size();i++) sticks[i]->Satisfy();
				}
				if(joints.size() >0) {
					for(int i = 0; i < joints.size();i++) joints[i]->Satisfy();
				}
			}
		}
		
		void step() {
			AccumulateForces();
			tick();
			SatisfyConstraints();
		}
};