#include "pointRecorder.h"


//------------------------------------------------------------------
pointRecorder::pointRecorder(){
	middlePoint = ofxPoint2f(0,0);
	angle = 0;
	maxNumPts = 100;
}

//------------------------------------------------------------------
void pointRecorder::addPoint(ofPoint pt) {
	if(pts.size() == 0){
		startTime = ofGetElapsedTimef();
	}
	
	timePoint new_point;
	new_point.pos = pt;
	new_point.time = ofGetElapsedTimef() - startTime;
	
	pts.push_back(new_point);
	if (pts.size() > maxNumPts){
		pts.erase(pts.begin());
	}
}

//------------------------------------------------------------------
void pointRecorder::update(){

}

//------------------------------------------------------------------
void pointRecorder::clear(){
	pts.clear();
}

//------------------------------------------------------------------
float pointRecorder::getDuration(){
	float totalDuration = 0.0f;
	if(pts.size() >= 1){
		totalDuration = pts[pts.size() - 1].time;
	}
	return totalDuration;
}

//------------------------------------------------------------------
ofPoint pointRecorder::getPointForTime(float time){
	if(pts.size() <= 1 || bAmRecording) return ofPoint(0,0,0);
	
	float totalTime = getDuration();
	float timeInRange = time;
	while(timeInRange > totalTime){
		timeInRange -= totalTime;
	}
	
	int whatPointAmINear = 0;
	for(int i = 0; i < pts.size(); i++){
		if(pts[i].time > timeInRange){
			whatPointAmINear = i;
			break;
		}
	}
	
	if(whatPointAmINear > 0){
		float timeA = pts[whatPointAmINear - 1].time;
		ofPoint posA = pts[whatPointAmINear - 1].pos;
		float timeB = pts[whatPointAmINear ].time;
		ofPoint posB = pts[whatPointAmINear ].pos;
		
		float totalDurationBetweenPoints = timeB - timeA;
		float myPositionBetweenThesePoints = timeInRange - timeA;
		float pct = myPositionBetweenThesePoints / totalDurationBetweenPoints;
		
		ofPoint mix(0,0,0);
		mix.x = (1-pct) * posA.x + (pct) * posB.x;
		mix.y = (1-pct) * posA.y + (pct) * posB.y;
		
		return mix;
	} else {
		return pts[whatPointAmINear].pos;
	}
}


//------------------------------------------------------------------
void pointRecorder::draw() {
	
	for (int i = 1; i < pts.size(); i++){
		ofFill();
		middlePoint.x = (.5 * pts[i].pos.x) + (.5 * (pts[i-1].pos.x));
		middlePoint.y = (.5 * pts[i].pos.y) + (.5 * (pts[i-1].pos.y));
		
		float radius = sqrt((pts[i-1].pos.x - pts[i].pos.x) * (pts[i-1].pos.x - pts[i].pos.x) + (pts[i-1].pos.y - pts[i].pos.y) * (pts[i-1].pos.y - pts[i].pos.y)) / 2;
		//ofCircle(middlePoint.x,middlePoint.y,radius);
		
		angle = atan2((pts[i].pos.x - pts[i-1].pos.x),(pts[i].pos.y - pts[i-1].pos.y));
		//ofDrawBitmapString(ofToString(angle), 5,5);
		
		ofxPoint2f pt_1_1 = ofxPoint2f(pts[i].pos.x + radius * cos(angle) , pts[i].pos.y + radius * sin(angle));
		ofxPoint2f pt_1_2 = ofxPoint2f(pts[i].pos.x + radius * cos(angle+PI) , pts[i].pos.y + radius * sin(angle+PI));
		
		ofxPoint2f pt_2_1 = ofxPoint2f(middlePoint.x + radius * cos(angle) , middlePoint.y + radius * sin(angle));
		ofxPoint2f pt_2_2 = ofxPoint2f(middlePoint.x + radius * cos(angle+PI) , middlePoint.y + radius * sin(angle+PI));
		
		ofxPoint2f pt_3_1 = ofxPoint2f(pts[i-1].pos.x + radius * cos(angle) , pts[i-1].pos.y + radius * sin(angle));
		ofxPoint2f pt_3_2 = ofxPoint2f(pts[i-1].pos.x + radius * cos(angle+PI) , pts[i-1].pos.y + radius * sin(angle+PI));
		
		//ofLine(pt_1.x,pt_1.y,pt_2.x,pt_2.y);
		//ofLine(pts[i].x,pts[i].y,pts[i-1].x,pts[i-1].y);
		ofBeginShape();
			ofVertex(pt_1_1.x,pt_1_1.y);
			ofVertex(pt_2_1.x,pt_2_1.y);
			ofVertex(pt_3_1.x,pt_3_1.y);
			ofVertex(pt_3_2.x,pt_3_2.y);
			ofVertex(pt_2_2.x,pt_2_2.y);
			ofVertex(pt_1_2.x,pt_1_2.y);
		ofEndShape(true);
	}
	ofFill();	
}

//------------------------------------------------------------------
void pointRecorder::drawAtTime(float time,int length, int spacing){
	if(pts.size() <= 1 || bAmRecording) return;
	
	float totalTime = getDuration();
	float timeInRange = time;
	while(timeInRange > totalTime){
		timeInRange -= totalTime;
	}
	
	int whatPointAmINear = 0;
	for(int i = 0; i < pts.size(); i++){
		if(pts[i].time > timeInRange){
			whatPointAmINear = i;
			break;
		}
	}
	
	if(whatPointAmINear > length*spacing){
		for (int i = whatPointAmINear; i > whatPointAmINear - length; i-=spacing){
			ofFill();
			int step_num = (whatPointAmINear - i) + 1;
			int opacity = 255 - ((step_num)*(255/length));
			//cout << opacity << endl;
			ofSetColor(255,255,255,opacity);
			ofEnableAlphaBlending();
			middlePoint.x = (.5 * pts[i].pos.x) + (.5 * (pts[i-1].pos.x));
			middlePoint.y = (.5 * pts[i].pos.y) + (.5 * (pts[i-1].pos.y));
			
			float radius = sqrt((pts[i-1].pos.x - pts[i].pos.x) * (pts[i-1].pos.x - pts[i].pos.x) + (pts[i-1].pos.y - pts[i].pos.y) * (pts[i-1].pos.y - pts[i].pos.y)) / 2;
			
			angle = atan2((pts[i].pos.x - pts[i-1].pos.x),(pts[i].pos.y - pts[i-1].pos.y));
			
			ofxPoint2f pt_1_1 = ofxPoint2f(pts[i].pos.x + radius * cos(angle) , pts[i].pos.y + radius * sin(angle));
			ofxPoint2f pt_1_2 = ofxPoint2f(pts[i].pos.x + radius * cos(angle+PI) , pts[i].pos.y + radius * sin(angle+PI));
			
			ofxPoint2f pt_2_1 = ofxPoint2f(middlePoint.x + radius * cos(angle) , middlePoint.y + radius * sin(angle));
			ofxPoint2f pt_2_2 = ofxPoint2f(middlePoint.x + radius * cos(angle+PI) , middlePoint.y + radius * sin(angle+PI));
			
			ofxPoint2f pt_3_1 = ofxPoint2f(pts[i-1].pos.x + radius * cos(angle) , pts[i-1].pos.y + radius * sin(angle));
			ofxPoint2f pt_3_2 = ofxPoint2f(pts[i-1].pos.x + radius * cos(angle+PI) , pts[i-1].pos.y + radius * sin(angle+PI));

			ofBeginShape();
			ofVertex(pt_1_1.x,pt_1_1.y);
			ofVertex(pt_2_1.x,pt_2_1.y);
			ofVertex(pt_3_1.x,pt_3_1.y);
			ofVertex(pt_3_2.x,pt_3_2.y);
			ofVertex(pt_2_2.x,pt_2_2.y);
			ofVertex(pt_1_2.x,pt_1_2.y);
			ofEndShape(true);
		}
	}
	ofFill();
}