#include "spring.h"

int Spring::S_muscle_flow = 1;
float Spring::S_t = 0.0;
float Spring::S_k = SPRING_K;

Spring::Spring(float nx1, float ny1, float nx2, float ny2, int fj) {
		x[0] = nx1;		x[1] = nx2;
		y[0] = ny1;		y[1] = ny2;	
		freejoints = fj;
		length = sqrt((ny2-ny1)*(ny2-ny1)+(nx2-nx1)*(nx2-nx1));
		muscle_offset = 0;
		muscle_frequency = 0;
		muscle_amplitude = 0;
}

Spring::Spring(float nx1, float ny1, int fj) {
		x[0] = nx1;		x[1] = 0;
		y[0] = ny1;		y[1] = 0;
		freejoints = fj;
		muscle_offset = 0;
		muscle_frequency = 0;
		muscle_amplitude = 0;
}

Spring::Spring() {
		x[0] = 0;		x[1] = 0;
		y[0] = 0;		y[1] = 0;
		freejoints = 0;
		muscle_offset = 0;
		muscle_frequency = 0;
		muscle_amplitude = 0;
}

void Spring::AddMuscle(float f, float a, float dt) {
		muscle_offset = dt;
		muscle_amplitude = a;
		muscle_frequency = f;
}

void Spring::AddJoint(float nx, float ny, int fj) {
		x[1]=nx; y[1]=ny; freejoints+=fj;
		length = sqrt((ny-y[0])*(ny-y[0])+(nx-x[0])*(nx-x[0]));
}

Vector Spring::ResultantForce(int which, float dt) {
	float angle,magnitude;
	float dy,dx;
	float l;
	float t;

	if (!freejoints) {
			return Vector();
	}

#if DEBUG_MATH
	cout << "DEBUG_MATH: which="<<which<<" y[!which]="<<y[!which]<<" - y[which]="<<y[which]<<" x[!which]="<<x[!which]<<" - x[which]="<<x[which]<<"\n";
	cout << "DEBUG_MATH: t="<<t<<"\n";
#endif 

	dy = y[!which]-y[which];
	dx = x[!which]-x[which];

#if DEBUG_SPRINGS
	cout<<"DEBUG_SPRINGS: dx,dy = "<<dx<<","<<dy<<"\n";
#endif
	t = Spring::T();	

	l = sqrt(dy*dy+dx*dx);
	a = length - l;

	if (muscle_amplitude) {
		muscle = cos(2*PI*muscle_frequency*(t+muscle_offset)) ;
		a += muscle * muscle_amplitude * length;
		#if DEBUG_MUSCLES
		cout << "DEBUG_MUSCLES: muscle="<<muscle << " t="<<t<<"\n";
		#endif
	}

#if DEBUG_SPRINGS
	cout << "DEBUG_SPRINGS: A = "<<a<<" K = "<<Spring::K()<<"\n";
#endif
	
	angle = atan2(dy,dx);
#if DEBUG_SPRINGS
	cout << "DEBUG_SPRINGS: freejoints = "<<freejoints<<"\n";
#endif 

	magnitude = -Spring::K()*a/freejoints;

	if (magnitude > MAX_SPRING) magnitude = MAX_SPRING;
	else if (magnitude < MIN_SPRING) magnitude = MIN_SPRING;


	return Vector(angle,magnitude);
}