#include "config.h"
#include <iostream>
#include <cmath>
#include "joint.h"

float Joint::S_gravity = GRAVITY;
float Joint::S_mass = JOINT_MASS;
float Joint::S_world_width = SIZE_WORLD_X;

void Joint::OnTimeSlice(float ndt) {
	//float dx,dy;
	Spring *s;
	Vector F;
	
	// Move time
	dt = ndt;
	t += dt;

	if (!fixed) { 
	
		// Apply forces from attached springs
		for(int i=0;i<springcount;i++) {
			s=springs[i];
			F = s->ResultantForce(springwhich[i],dt);
			#if DEBUG_FORCES
			cout << "DEBUG_FORCES: Spring #"<< i <<" applies " << F << "N\n";
			#endif 		
			this->ApplyForce(F);
		}

		// Apply gravity
		F = Vector(270,Joint::Gravity())*Joint::Mass();
		this->ApplyForce(F);

		// Nix the force if starting friction applies


		if (y==0.0) {
			float a = force.angle();
			if (((a>PI && a<2*PI) || (a<0 && a>-2*PI)) && 
				force.Xvector().magnitude()<FRICTION_START) {
				#if DEBUG_FORCES
				cout<< "DEBUG_FORCES: Applying starting friction to " <<
					force << "\n";
				#endif
				force = Vector();
			}		
		}

		force.roundto(ROUND_TO);	

		#if DEBUG_FORCES
		cout << "DEBUG_FORCES: Resultant Force is "<< force <<" N\n";
		#endif


		// Force becomes Acceleration
		acceleration = force / Joint::Mass();

		#if DEBUG_MATH
		cout << "DEBUG_MATH: Accleration is " << acceleration << "m/s^2\n";
		#endif
		
		// Reset force for next timeslice
		force = Vector();
	
		// Acceleration becomes Velocity
		velocity += acceleration*dt;

		velocity *= FRICTION;

#if DEBUG_MATH
		cout<<"DEBUG_MATH: Velocity is "<<velocity<<"m/s\n";
#endif 
		// Velocity->Translation
		dx = velocity.x() * dt;
		dy = velocity.y() * dt;

#if DEBUG_MATH
		cout<<"DEBUG_MATH: dx,dy="<<dx<<","<<dy<<"\n";
#endif 
	}
}

void Joint::BounceY() {
	if (y<0) {
		y=0;
		if (dy<-0.0175) 
			velocity = velocity.absY() * ELASTICITY ;
		else
			velocity = Vector();
	}
}

void Joint::BounceX() {
	if (x<=0) {
		x=0;
		velocity = velocity.absX() * ELASTICITY;
		Spring::MuscleFlow(1);
	}
	else if (x>=Joint::WorldWidth()) {
		x=Joint::WorldWidth();
		velocity = velocity.nabsX();
		Spring::MuscleFlow(-1);
	}
}

void Joint::ApplyForce(float na, float nf) {
	force += Vector(na,nf);	
}

void Joint::ApplyForce(Vector F) {
	force += F;
}

void Joint::ConnectSpring(Spring &s, int i) {
	if (springcount == JOINT_MAX_SPRINGS) {
		throw xTooManySprings();
	}
	else {
		springs[springcount] = &s;
		springwhich[springcount++] = i;
	}
}

Spring *Joint::DisconnectSpring(int s) {
	Spring *S;
	if (springcount == 0) {
		throw xTooManySprings();
	}
	else {
		S=springs[s];
		for(int i=s+1;i<springcount;i++) {
			springs[i-1] = springs[i];
		}
		springcount--;
	}

	return S;
}

Spring* Joint::ConnectJoint(Joint &j) {
	Spring* s = new Spring(x,y,!fixed);

	this->ConnectSpring(*s,0);
	s->AddJoint(j.x,j.y,!j.fixed);
	j.ConnectSpring(*s,1);

	return s;	
}	

void Joint::MoveTo(float nx, float ny) {
	 x=nx; 
	 y=ny;
	 for (int i=0; i<springcount; i++) {
		springs[i]->Set(springwhich[i], x, y);
	 }
}

void Joint::Fix() {
	if (!fixed) 
		for (int i=0;i<springcount; i++) {
			springs[i]->Fix();
		}
	fixed = 1;
}

void Joint::Unfix() {
	if (fixed) 
		for (int i=0;i<springcount; i++) {
			springs[i]->Unfix();
		}
	fixed = 0;
}

Spring *Joint::ConnectedTo(Joint *J) {
	Spring *S;
	for(int x=0;x<this->Springs();x++) {
		S = this->GetSpring(x);
		if (S->ConnectedJoint(1-this->SpringEnd(x)) == J->index) 
			return S;
	}

	return NULL;
}
/*		for (int y=0;y<J->Springs();y++) 
			if (this->GetSpring(x) == J->GetSpring(y)) 
				return this->GetSpring(x);
*/