#include "swimStroke.h"



//--------------------------------------------------------------
swimStroke::swimStroke(){
	bSwim 				= false;
	samp.loadSound("3.wav");
	samp.setLoop(true);
	samp.setSpeed(0.1f);
	samp.play();
}

//--------------------------------------------------------------
void swimStroke::start(){
	lengthSectionTarget 	= 0.5f * lengthSection;
	bSwim 					= true;
	bFirstFrame 			= true;
	overallTargetLength 	= lengthSectionTarget * nPts;
	
}

//--------------------------------------------------------------
void swimStroke::update(){
	
	
	
	//------------------------------------------------ swimming:
	if (bSwim == true){
		
		
		centroid.set(0,0,0);

		lengthSection = 0.98 * lengthSection + 
						0.02f * lengthSectionTarget;
						
		ofVec3f pt = startPosition;
		
		ofVec3f ptPrev = pt;
		glBegin(GL_LINE_STRIP);
		glVertex2f(pt.x, pt.y);
		float angle = 0;
		for (int i = 0; i < nPts; i++){
			ptPrev = pt;
			if (i == 0) angle = angles[i];
			else (angle += angles[i]);
			pt.x += lengthSection * cos(angle);
			pt.y += lengthSection * sin(angle);
			
			centroid += pt;
			
			if (i != 0) angles[i] *= 0.995f;
		}
		glEnd();
		
		//----------------------------------------
		float dist = (startPosition - pt).length();
		float pct =  1 - (dist / overallTargetLength);
		samp.setSpeed(0.6f + pct * 10.5);
		//----------------------------------------
		
		centroid /=  nPts;
		
		float backAngle = returnAngle(ptPrev, pt);
		angle = 0;
		for (int i = nPts-1; i >= 0; i--){
			ptPrev = pt;
			if (i == nPts-1) angle = backAngle;
			else (angle -= angles[i+1]);
			pt.x -= lengthSection * cos(angle);
			pt.y -= lengthSection * sin(angle);
			if (i != 0) angles[i] *= 0.995f;
		}
		glEnd();
		
		float diff = returnAngle(pt,ptPrev) - angles[0];
		while (diff > PI){ diff -= TWO_PI; printf(".\n");};
		while (diff < -PI){ diff += TWO_PI; printf("-\n"); };
		diff /= 2.0f;
		angles[0] += diff;
		
		
		while (angles[0] > PI){ angles[0] -= TWO_PI; printf(".\n");};
		while (angles[0] < -PI){ angles[0] += TWO_PI; printf("-\n"); };
		
		startPosition += (pt-startPosition)/2.0f;
	}
	
	
	if (bFirstFrame == true){
		prevCentroid = centroid;
		bFirstFrame = false;
	} else {
		ofVec3f diff 	= centroid - prevCentroid;
		startPosition 	-= diff * 0.05f;
		//prevCentroid  	= centroid;
	}
	
	
	
}

//--------------------------------------------------------------
void swimStroke::draw(){
	
	
	ofSetColor(0x000000);
	ofVec3f pt = startPosition;
	
	glBegin(GL_LINE_STRIP);
	glVertex2f(pt.x, pt.y);
	float angle = 0;
	for (int i = 0; i < nPts; i++){
		if (i == 0) angle = angles[i];
		else (angle += angles[i]);
		pt.x += lengthSection * cos(angle);
		pt.y += lengthSection * sin(angle);
		glVertex2f(pt.x, pt.y);
	}
	glEnd();
	
	if (bSwim == true){
		ofNoFill();
		ofRect(boundingBoxBottom.x , boundingBoxBottom.y, 
			   boundingBoxTop.x - boundingBoxBottom.x,
			   boundingBoxTop.y - boundingBoxBottom.y);
	
		ofRect(centroid.x, centroid.y,5,5);
	}

	
}

