#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::linkVideo(ofVideoPlayer * vid){
	cout << vid <<endl;
}

//------------------------------------------------------------------
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, int offset){
	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+offset;
			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;
	}
}

//------------------------------------------------------------------
float pointRecorder::getAngleForTime(float time){
	if(pts.size() <= 1 || bAmRecording) return 0.0f;
	
	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 > 2){
		float timeA = pts[whatPointAmINear].time;
		ofPoint posA = pts[whatPointAmINear].pos;
		float timeB = pts[whatPointAmINear - 1].time;
		ofPoint posB = pts[whatPointAmINear - 1].pos;
		float timeC = pts[whatPointAmINear - 2].time;
		ofPoint posC = pts[whatPointAmINear - 2].pos;
		
		float totalDurationBetweenPoints = timeA - timeB;
		float totalDurationBetweenLastPoints = timeB - timeC;
		
		float myPositionBetweenThesePoints = timeInRange - timeB;
		
		float pct_current = myPositionBetweenThesePoints / totalDurationBetweenPoints;
		float pct_last = myPositionBetweenThesePoints / totalDurationBetweenLastPoints;
		
		ofPoint currentMix;
		currentMix.x = (1-pct_current) * posB.x + (pct_current) * posA.x;
		currentMix.y = (1-pct_current) * posB.y + (pct_current) * posA.y;
		
		ofPoint lastMix;
		lastMix.x = (1-pct_last) * posC.x + (pct_last) * posB.x;
		lastMix.y = (1-pct_last) * posC.y + (pct_last) * posB.y;
		
		float angle = atan2((currentMix.x - lastMix.x),(currentMix.y - lastMix.y));
		
		return angle;
	} else {
		return 0;
	}
}


//------------------------------------------------------------------
float pointRecorder::getRadiusForTime(float time){
	if(pts.size() <= 1 || bAmRecording) return 0.0f;
	
	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 > 2){
		float timeA = pts[whatPointAmINear].time;
		ofPoint posA = pts[whatPointAmINear].pos;
		float timeB = pts[whatPointAmINear - 1].time;
		ofPoint posB = pts[whatPointAmINear - 1].pos;
		float timeC = pts[whatPointAmINear - 2].time;
		ofPoint posC = pts[whatPointAmINear - 2].pos;
		
		float totalDurationBetweenPoints = timeA - timeB;
		float totalDurationBetweenLastPoints = timeB - timeC;
		
		float myPositionBetweenThesePoints = timeInRange - timeB;
		
		float pct_current = myPositionBetweenThesePoints / totalDurationBetweenPoints;
		float pct_last = myPositionBetweenThesePoints / totalDurationBetweenLastPoints;
		
		ofPoint currentMix;
		currentMix.x = (1-pct_current) * posB.x + (pct_current) * posA.x;
		currentMix.y = (1-pct_current) * posB.y + (pct_current) * posA.y;
		
		ofPoint lastMix;
		lastMix.x = (1-pct_last) * posC.x + (pct_last) * posB.x;
		lastMix.y = (1-pct_last) * posC.y + (pct_last) * posB.y;
		
		float radius = sqrt((lastMix.x - currentMix.x) * (lastMix.x - currentMix.x) + (lastMix.y - currentMix.y) * (lastMix.y - currentMix.y)) / 2;
		
		return radius;
	} else {
		return 0;
	}
	
}


//------------------------------------------------------------------
void pointRecorder::draw() {
	
	for (int i = 1; i < pts.size(); i++){
		ofNoFill();
		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_2_1.x,pt_2_1.y,pt_2_2.x,pt_2_2.y);
		ofLine(pts[i].pos.x,pts[i].pos.y,pts[i-1].pos.x,pts[i-1].pos.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, ofVideoPlayer& vid){
	if(pts.size() <= 1 || bAmRecording) return;
	
	float totalTime = getDuration();
	float timeInRange = time;
	while(timeInRange > totalTime){
		timeInRange -= totalTime;
	}
	
	float video_time = vid.getDuration();
	float vid_timeInRange = timeInRange;
	while(vid_timeInRange > video_time){
		vid_timeInRange -= video_time;
	}
	
	cout << vid_timeInRange << endl;
	float vid_pos = vid_timeInRange / video_time;
	
	int whatPointAmINear = 0;
	for(int i = 0; i < pts.size(); i++){
		if(pts[i].time > timeInRange){
			whatPointAmINear = i;
			break;
		}
	}
	
	ofPoint pt = getPointForTime(time);
	
	middlePoint.x = (.5 * pts[whatPointAmINear].pos.x) + (.5 * (pts[whatPointAmINear-1].pos.x));
	middlePoint.y = (.5 * pts[whatPointAmINear].pos.y) + (.5 * (pts[whatPointAmINear-1].pos.y));
	
	float radius = sqrt((pts[whatPointAmINear-1].pos.x - pts[whatPointAmINear].pos.x) * (pts[whatPointAmINear-1].pos.x - pts[whatPointAmINear].pos.x) + (pts[whatPointAmINear-1].pos.y - pts[whatPointAmINear].pos.y) * (pts[whatPointAmINear-1].pos.y - pts[whatPointAmINear].pos.y)) / 2;
	
	angle = atan2((pts[whatPointAmINear].pos.x - pts[whatPointAmINear-1].pos.x),(pts[whatPointAmINear].pos.y - pts[whatPointAmINear-1].pos.y));
	
	ofxPoint2f pt_1_1 = ofxPoint2f(pts[whatPointAmINear].pos.x + radius * cos(angle) , pts[whatPointAmINear].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[whatPointAmINear-1].pos.x + radius * cos(angle+PI) , pts[whatPointAmINear-1].pos.y + radius * sin(angle+PI));
	
	vid.draw(pt.x,pt.y,(pt_3_2.x - pt_1_1.x)*2,(pt_3_2.y - pt_1_1.y)*2);
	
	ofCircle(pt_1_1.x,pt_1_1.y,3);
	ofCircle(pt_3_2.x,pt_3_2.y,3);
}


//------------------------------------------------------------------
void pointRecorder::drawCubeAtTime(float time){
	if(pts.size() <= 1 || bAmRecording) return;
	
	
	ofPoint pt = getPointForTime(time,0);
	ofPoint pt_last = getPointForTime(time,-1); 
	
	float drawAngle = getAngleForTime(time);
	float degAngle = 180/drawAngle*PI;
	
	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;
		}
	}
	
	float radius = getRadiusForTime(time);
	
	
	int cw = 20;
	
	glPushMatrix();
		glTranslatef(pt.x,pt.y,0);
		glRotatef(degAngle,1,1,1);
		glPushMatrix();
	
			glutSolidCube(radius);
	
		glPopMatrix();
	glPopMatrix();
	
	
	
}