#include "grafRecorder.h"

grafRecorder::grafRecorder()
{
    z_const     = 18;
    lastPoint   = 0;
    lastDist    = 0;
    lastAngle   = 0;
    pointForTime.set(0,0,0);

    max.set(0,0,0);
    min.set(0,0,0);

    timeCounter			= 0;
	timeOfLastFrame		= ofGetElapsedTimef();
	frameRateCounter    = 0;
	fps                 = 1/30.f;

	bAmRecording        = false;
	bReset              = false;
	drawMode            = DRAW_NORM;

	width  = 1280;
	height = 800;

	ptStart = 0;
	ptEnd   = 0;

	font.loadFont("fonts/frabk.ttf", 24, true, true);

}

grafRecorder::~grafRecorder()
{
    //dtor
}

void grafRecorder::setup( float w, float h)
{
    width  = w;
	height = h;
}

void grafRecorder::clear()
{
    angles.clear();
    dists.clear();
    leftPts.clear();
    rightPts.clear();

    pointRecorder::clear();
    pts_orig.clear();

    ptStart = 0;
	ptEnd   = 0;

}

bool grafRecorder::addLoadedPoint(ofPoint pt, float dist, float angle, float time, bool bAverage){

    //---------------- ignore 0,0 error
    if( pt.x == 0 && pt.y == 0) return false;


    //-------------- try to add the point

    // on the first point, grab the start time
	if (pts.size() == 0){
		startTime = ofGetElapsedTimef();
	}

    // combine the position and the time here:
	timePt	myPoint;
	myPoint.pos			= pt;

	if( time == -1 )    myPoint.time	= ofGetElapsedTimef() - startTime;
    else                myPoint.time    = time;

    // check distance and return if too small
    if( pts.size() > 0 && dist == -1 )          dist = ((ofxVec2f)(pts[pts.size()-1].pos-pt)).length();
    else if ( pts.size() == 0 && dist == -1 )   dist = 0;

    if( pts.size() == 0 || dist > 0.0001 ) dists.push_back(dist);// myPoint.dist = dist;
    else return false;

	if( pts.size() > 0 && angle == -1) angle = atan2(pt.y - pts[pts.size()-1].pos.y, pt.x - pts[pts.size()-1].pos.x);
	else if(angle == -1) angle = 0;
	angles.push_back(angle);//myPoint.angle = angle;


	// add and check if point is misplaced
	int inPt = -1;
	for( int i = 0; i < pts.size(); i++)
	{
        if( pts[i].time > time )
        {
            inPt = i;
            break;
        }
    }

    if( inPt == -1 )
    {
        pts.push_back(myPoint);
        pts_orig.push_back(myPoint);
    }
    else{
         pts.insert(pts.begin()+inPt,myPoint);
         pts_orig.insert(pts_orig.begin()+inPt,myPoint);
    }

	// delete if overrun limit
	if (pts.size() > maxNumPts)
	{
		pts.erase(pts.begin());
	}


    int numPoints = pts.size()-1;

    //------------- average
    if( numPoints > 1 && bAverage)
    {
        pts[numPoints-1].pos.x = (0.2f * pts[numPoints-2].pos.x) + (0.60f * pts[numPoints-1].pos.x) + (0.2f * pts[numPoints].pos.x);
        pts[numPoints-1].pos.y = (0.2f * pts[numPoints-2].pos.y) + (0.60f * pts[numPoints-1].pos.y) + (0.2f * pts[numPoints].pos.y);
        dists[numPoints-1]=(0.25f * dists[numPoints-2]) + (0.50f * dists[numPoints-1]) + (0.25f * dists[numPoints]);
    }

    ptEnd =  pts.size()-1;


    // save originals
	if(pts_orig.size() > maxNumPts)
	{
		pts_orig.erase(pts_orig.begin());
	}

    return true;
}

bool grafRecorder::addNewPoint(ofPoint pt)
{

    //---------------- ignore 0,0 error
    if( pt.x == 0 && pt.y == 0) return false;

    pt.x /= width;
    pt.y /= width;

    //--------------- check distances and record
    // if distance is 0, dont record it
    if(dists.size() == 0) dists.push_back(0);
    else{
        float dist = ((ofxVec2f)(pts[pts.size()-1].pos-pt)).length();
        if( dist > 0.00001 )
            dists.push_back(dist);
        else
            return false;
    }


    //-------------- add the point
    pointRecorder::addPoint(pt);

    // -- save originals
    pts_orig.push_back( pts[pts.size()-1] );
    if (pts_orig.size() > maxNumPts)
	{
		pts_orig.erase(pts.begin());
	}


    int numPoints = pts.size()-1;

    //------------ record angles
    if(angles.size() == 0) angles.push_back(0);
    else{

        float angle = atan2(pts[numPoints].pos.y - pts[numPoints-1].pos.y, pts[numPoints].pos.x - pts[numPoints-1].pos.x);
        angles.push_back(angle);
    }

    //------------- average
    if( numPoints > 1 )
    {
        pts[numPoints-1].pos.x = (0.25f * pts[numPoints-2].pos.x) + (0.50f * pts[numPoints-1].pos.x) + (0.25f * pts[numPoints].pos.x);
        pts[numPoints-1].pos.y = (0.25f * pts[numPoints-2].pos.y) + (0.50f * pts[numPoints-1].pos.y) + (0.25f * pts[numPoints].pos.y);
        dists[numPoints-1]=(0.25f * dists[numPoints-2]) + (0.50f * dists[numPoints-1]) + (0.25f * dists[numPoints]);
    }

     updateMinMax(pts.size()-1);

    return true;
}

void grafRecorder::update(ofPoint pt)
{


    float diffTime		        = ofGetElapsedTimef() - timeOfLastFrame;
	timeOfLastFrame		        = ofGetElapsedTimef();
	if(!bPause) timeCounter		+= diffTime;


     if(bAmRecording && (ofGetElapsedTimef()-frameRateCounter) >= fps )
     {

         if( addNewPoint(pt) ) frameRateCounter += fps;
     }

    if( pts.size() == 0 ) return;

    if(bAmRecording){
         lastPoint      = pts.size()-1;
         pointForTime   = pts[lastPoint].pos;
         lastDist       = dists[lastPoint];
         lastAngle      = angles[lastPoint];
         timeInRange    = pts[lastPoint].time;
         ptEnd          = lastPoint;
    }
    else{

        //if( drawMode == DRAW_EDIT ) timeCounter = pts[ptEnd].time;
        pointForTime = getPointForTime(timeCounter,lastPoint, lastDist, lastAngle);
        if( lastPoint > ptEnd )
        {
            pointForTime = getPointForTime(pts[ptEnd].time,lastPoint, lastDist, lastAngle);
        }

        //cout << "lastPoint " << lastPoint << endl;
    }

    if( !bAmRecording && lastPoint == ptEnd  ) bReset = true;


}

void grafRecorder::updateMinMax( int i )
{
    //int i = pts.size()-1;

    if( i == 0 || pts[i].pos.x > max.x ) max.x = pts[i].pos.x;
    if( i == 0 || pts[i].pos.y > max.y ) max.y = pts[i].pos.y;
    if( i == 0 || pts[i].pos.y > max.z ) max.z = pts[i].pos.z;

    if( i == 0 || pts[i].pos.x < min.x ) min.x = pts[i].pos.x;
    if( i == 0 || pts[i].pos.y < min.y ) min.y = pts[i].pos.y;
    if( i == 0 || pts[i].pos.y < min.z ) min.z = pts[i].pos.z;

}

void grafRecorder::toggleRecording()
{
    if(!bAmRecording){
        clear();
        bAmRecording= true;
        bReset      = false;

    }else{
        bAmRecording= false;
        timeCounter = 0;
    }
}

void grafRecorder::togglePaused()
{
    bPause = !bPause;
}


float grafRecorder::getDuration() {
	float totalDuration = 0;
	if (pts.size() > 0)
	{
		totalDuration = pts[ptEnd].time;
	}
	return totalDuration;
}

ofPoint grafRecorder::getPointForTime(float time, int & whatPointAmINear, float & dist, float & angle)
{

	if (pts.size() <= 1 || bAmRecording) return ofPoint(0,0,0);

	float totalTime     = getDuration();
    timeInRange   = time;

	while (timeInRange > totalTime){
		timeInRange -= totalTime;
	}

	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 pta = pts[whatPointAmINear - 1].pos;
		float timeb = pts[whatPointAmINear    ].time;
		ofPoint ptb = pts[whatPointAmINear    ].pos;

		float totalDurationBetweenThesePts	= timeb - timea;
		float myPositionBetweenThesePts		= timeInRange - timea;
		float pct = myPositionBetweenThesePts / totalDurationBetweenThesePts;

		ofPoint mix(0,0,0);
		mix.x = (1-pct) * pta.x + (pct) * ptb.x;
		mix.y = (1-pct) * pta.y + (pct) * ptb.y;
		mix.z = (1-pct) * pta.z + (pct) * ptb.z;

		dist  = (1-pct) * dists[whatPointAmINear - 1] + (pct) * dists[whatPointAmINear];
		angle = (1-pct) * angles[whatPointAmINear - 1] + (pct) * angles[whatPointAmINear];

		return mix;

	} else {

		dist    = dists[whatPointAmINear];
		angle   = angles[whatPointAmINear];
		return pts[whatPointAmINear].pos;
	}
}

ofPoint grafRecorder::getVelocityForTime(float time){


	// ok the trick is to do the same as above (find the time in the gesture)
	// and to find a time slightly less... (prev position and current position).

	if (pts.size() <= 1 || bAmRecording) return ofPoint(0,0,0);

	float totalTime = getDuration();
	float timeInRange = time;

	while (timeInRange > totalTime){
		timeInRange -= totalTime;
	}

	float prevTime = MAX(0, timeInRange - 0.016666f); // time minus 1/60 of a second....
	//cout << timeInRange << " " << prevTime << endl;

	ofPoint curPoint	= pointRecorder::getPointForTime(timeInRange);
	ofPoint prevPoint	= pointRecorder::getPointForTime(prevTime);


	ofPoint velocity;

	velocity.x			= curPoint.x - prevPoint.x;
	velocity.y			= curPoint.y - prevPoint.y;

	return velocity;
}

void grafRecorder::draw(float scale)
{



    if( pts.size() == 0 ) return;

    drawWireframe();
    drawTimeStroked();
    drawTimeline(scale);
    if( drawMode != DRAW_EDIT ) drawArrow();
}

void grafRecorder::drawEdit()
{
    ofNoFill();
    ofPushMatrix();
        glTranslatef(0,0, (1000*pts[ptStart].time) / z_const);
        ofRect(min.x,min.y,max.x-min.x,max.y-min.y);
    ofPopMatrix();

     ofPushMatrix();
        glTranslatef(0,0, (1000*pts[ptEnd].time) / z_const);
        ofRect(min.x,min.y,max.x-min.x,max.y-min.y);
    ofPopMatrix();
}

void grafRecorder::drawTangentPoint( ofPoint pt, float  time, float dist, float angle )
{
    float left_x,left_y,right_x,right_y;

   /* left_x 	= pt.x-dist;//(cos(angle)*dist);
    left_y 	= pt.y+dist;//(sin(angle)*dist);
    right_x = pt.x+dist;//(cos(angle)*dist);
    right_y = pt.y-dist;//(sin(angle)*dist);*/

    left_x 	= pt.x-(sin(angle)*dist);
    left_y 	= pt.y+(cos(angle)*dist);
    right_x 	= pt.x+(sin(angle)*dist);
    right_y 	= pt.y-(cos(angle)*dist);

    float time_num = (1000*time) / z_const;

    glVertex3f(left_x, left_y, time_num);
    glVertex3f(right_x, right_y, time_num);


    leftPts.push_back( ofPoint(left_x,left_y,time_num) );
    rightPts.push_back( ofPoint(right_x,right_y,time_num) );
}

void grafRecorder::drawTimeStroked()
{


    leftPts.clear();
    rightPts.clear();


    glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
    glEnable(GL_BLEND);
    glColor4f(1,1,1, .8);



    glBegin(GL_QUAD_STRIP);
    drawTangentPoint(pts[ptStart].pos,pts[ptStart].time,0,0);

    float dist, angle;

    for (int i = ptStart + 1; i < lastPoint; i++)
    {

        dist = (dists[i] * .5f) ;//+ .5f;
        angle = angles[i];

        drawTangentPoint(pts[i].pos,pts[i].time,dist,angle);

	}

    if(!bAmRecording) drawTangentPoint(pointForTime,timeInRange,lastDist,lastAngle);

    glEnd();


	glDisable(GL_BLEND);

    // make the stroke
    ofNoFill();
    glBegin(GL_LINE_STRIP);
        for( int i=0; i<leftPts.size()-1; i++)
            glVertex3f(leftPts[i].x, leftPts[i].y, leftPts[i].z);
    glEnd();



   glBegin(GL_LINE_STRIP);
        for( int i=0; i<rightPts.size()-1; i++)
            glVertex3f(rightPts[i].x, rightPts[i].y, leftPts[i].z);

    glEnd();



}

void grafRecorder::drawCurved()
{
    ofSetColor(255,0,0);
    ofBeginShape();
        for( int i=0; i<leftPts.size()-1; i++)
            ofCurveVertex(leftPts[i].x, leftPts[i].y);//, leftPts[i].z);
    ofEndShape();


    ofBeginShape();
        for( int i=0; i<rightPts.size()-1; i++)
            ofCurveVertex(rightPts[i].x, rightPts[i].y);//, rightPts[i].z);
    ofEndShape();
}

void grafRecorder::drawTimeline(float scale)
{
    if( pts.size() == 0 ) return;

    float xp = min.x + (max.x-min.x)/2;
    float yp = max.y + .050;

    float timeStart = (1000 * (pts[ ptStart ].time) )/ z_const;
    float timeEnd = (1000 * timeInRange )/ z_const;

    // draw full line always
    glBegin( GL_LINES);
        glVertex3f(xp,yp,timeStart);
        glVertex3f(xp,yp,timeEnd);
    glEnd();


    // draw markers
    glBegin( GL_LINES);

    float h = .010;
    int count = 0;

    for( float t = timeStart; t < timeEnd; t+=(300/ z_const))
    {

            if( count % 2 == 0) h = .020;
            else h = .010;

            glVertex3f(xp,yp,t);
            glVertex3f(xp,yp-h,t);

            count++;
    }


    h = .030;

    glVertex3f(xp,yp,timeStart);
    glVertex3f(xp,yp-h,timeStart);

    glVertex3f(xp,yp,timeEnd);
    glVertex3f(xp,yp-h,timeEnd);


    glEnd();


    ofSetColor(255,255,255,255);

//    cout << "width" << width << endl;
    glPushMatrix();
        glTranslatef(xp,yp,timeEnd);
        glScalef(1/scale,1/scale,0);
        font.drawString( ofToString( timeInRange, 2 ) , 2,0);
    glPopMatrix();

}

void grafRecorder::drawArrow()
{

        float time_time = (1000*timeInRange) / z_const;
        float size = .012;

        ofPoint vel       = getVelocityForTime(timeCounter);
        float lengthOfVel = sqrt(vel.x * vel.x + vel.y * vel.y);
		float angle       = atan2(vel.y, vel.x);

        if(bAmRecording) angle = lastAngle;

        glColor4f(1,1,1, .7);

        glPushMatrix();

        glTranslatef(pointForTime.x, pointForTime.y, time_time);
		glRotatef(angle * RAD_TO_DEG, 0,0,1);
        glScalef(1+.05*lengthOfVel,1+.05*lengthOfVel,1);

            glBegin(GL_TRIANGLES);
                    glVertex3f(size, size*2, 0);
                    glVertex3f(size*2, 0, 0);
                    glVertex3f(size, -size*2, 0);
            glEnd();

            glBegin(GL_QUADS);
                    glVertex3f(-size,-size, 0);
                    glVertex3f( size,-size, 0);
                    glVertex3f( size, size, 0);
                    glVertex3f(-size, size, 0);
            glEnd();

        glPopMatrix();

}

void grafRecorder::drawWireframe()
{
    glColor3f(.5,.5,.5);

    glBegin(GL_LINE_STRIP);
    for (int i = 1; i < pts.size()-1; i++)
    {
        glVertex3f( pts[i].pos.x, pts[i].pos.y, (1000*pts[i].time) / z_const);
    }
    glEnd();

   // ofSetPolyMode(int mode);
   /* ofBeginShape( );
    for (int i = 1; i < pts.size()-1; i++)
    {
        glPushMatrix();
        glTranslatef(0,0, (1000*pts[i].time) / z_const);
        ofCurveVertex( pts[i].pos.x, pts[i].pos.y);//, (1000*pts[i].time) / z_const);
        glPopMatrix();
    }
    ofEndShape();*/
}

void grafRecorder::saveToXML( string filename )
{
    ofxXmlSettings xml;


    for( int i=0; i<pts.size(); i++) xml.addTag("pt");

    for( int i=0; i<pts.size(); i++)
    {
        xml.pushTag("pt",i);
        xml.setValue("x",pts[i].pos.x);
        xml.setValue("y",pts[i].pos.y);
        xml.setValue("time",pts[i].time);
        xml.setValue("angle",angles[i]);
        xml.setValue("dist",dists[i]);
        xml.popTag();

    }

    xml.saveFile(filename);
}

void grafRecorder::loadFromXML( string filename )
{
    clear();

    ofxXmlSettings xml;

    xml.loadFile(filename);

    int ntags = xml.getNumTags("pt");

    for( int i=0; i<ntags; i++)
    {
        timePt pt;
        xml.pushTag("pt",i);

        pt.pos.x = xml.getValue("x",0.0f);
        pt.pos.y = xml.getValue("y",0.0f);
        pt.pos.z = 0;
        pt.time  = xml.getValue("time",0.0f);

        pts.push_back(pt);
        angles.push_back(xml.getValue("angle",0.0f));
        dists.push_back(xml.getValue("dist",0.0f));

        xml.popTag();

    }

    bAmRecording = false;
    timeCounter  = 0;

}

void grafRecorder::setStartPt(int val)
{
    if( val < pts.size() && val >= 0 ){
        ptStart = val;

        float timeChange = pts_orig[0].time-pts_orig[ptStart].time;
        for( int i = 0; i < pts.size(); i++)
        {   pts[i].time = pts_orig[i].time-pts_orig[ptStart].time;
            updateMinMax(i);
        }
    }

};

void grafRecorder::setEndPt(int val)
{
     if( val < pts.size() && val >= 0 ) ptEnd = val;
     for( int i = 0; i < pts.size(); i++)
        updateMinMax(i);

};

 void  grafRecorder::resetForEditing()
 {
    if( pts.size() == 0 ) return;

    timeCounter = pts[ptEnd-1].time;
    timeOfLastFrame = ofGetElapsedTimef();

    //timeCounter = pts[ptEnd].time;
    lastPoint   = ptEnd-1;
    bPause      = true;
    drawMode    = DRAW_EDIT;

    for( int i = 0; i < pts.size(); i++)
        updateMinMax(i);
 }


float grafRecorder::getCurentZDepth()
{
    return (timeInRange*1000) / z_const;
}

ofPoint grafRecorder::getCurrentPoint()
{
    return pointForTime;
}


void grafRecorder::toggleEditing(bool bEditing)
{
    if(bEditing)
        resetForEditing();
    else{
        bPause      = false;
        drawMode    = DRAW_NORM;
    }

}

void grafRecorder::calcMinMax()
{

    max.set(0,0,0);
    min.set(0,0,0);

    for( int i = 0; i < pts.size(); i++)
    {
        if( i == 0 || pts[i].pos.x > max.x ) max.x = pts[i].pos.x;
        if( i == 0 || pts[i].pos.y > max.y ) max.y = pts[i].pos.y;
        if( i == 0 || pts[i].time  > max.z ) max.z = pts[i].time;

        if( i == 0 || pts[i].pos.x < min.x ) min.x = pts[i].pos.x;
        if( i == 0 || pts[i].pos.y < min.y ) min.y = pts[i].pos.y;
        if( i == 0 || pts[i].time  < min.z ) min.z = pts[i].time;


    }



}
