#include "grafTag.h"

grafTag::grafTag()
{

    bPaused         = false;
    bReset          = false;

    startTime       = 0.f;
	maxNumPts       = 5000;
	ptStart         = 0;
	ptEnd           = 0;

    timeCounter     = 0.f;
    timeOfLastFrame = 0.f;
    timeInRange     = 0.f;

	nowIndex        = 0;
    nowDist         = 0.f;
    nowAngle        = 0.f;
    nowPointForTime.set(0,0,0);

    rotation.set(0,0,0);
    position.set(0,0,0);
    z_const     = 16;

    bPlayed = false;
}

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


void grafTag::clear()
{
    pts.clear();
	pts_orig.clear();
}


void grafTag::update()
{

    // don't do it if there are no points
    if( pts.size() == 0 ) return;

    // check time change to update animation
    float diffTime		        = ofGetElapsedTimef() - timeOfLastFrame;
	timeOfLastFrame		        = ofGetElapsedTimef();
	if(!bPaused) timeCounter	+= diffTime;


    // set current point and time
    nowPointForTime = getPointForTime( timeCounter, nowIndex, nowDist, nowAngle);

    // check in case time jumps over end pt
    if( nowIndex > ptEnd )
        nowPointForTime = getPointForTime(pts[ptEnd-1].time,nowIndex, nowDist, nowAngle);


    // test for reset -- check last point and first in case time jump goes over
    // could be problematic with a fast framerate
    if( nowIndex == ptEnd  || (nowIndex == 0 && bPlayed) ){
         bReset = true;
         nowPointForTime = getPointForTime(pts[ptEnd-1].time,nowIndex, nowDist, nowAngle);
    }

    // tag has been played
    bPlayed = true;

}

bool grafTag::addNewPoint(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 ) 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;
	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);
        pts[numPoints-1].dist  = (0.25f * pts[numPoints-2].dist)  + (0.50f * pts[numPoints-1].dist)  + (0.25f * pts[numPoints].dist);
    }

    ptEnd =  pts.size()-1;


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

    return true;
}

void grafTag::averagePts()
{
    for( int i = 2; i < pts.size(); i++)
    {

       // pts[i-1].pos.x = (0.3f * pts[i-2].pos.x) + (0.40f * pts[i-1].pos.x) + (0.3f * pts[i].pos.x);
       // pts[i-1].pos.y = (0.3f * pts[i-2].pos.y) + (0.40f * pts[i-1].pos.y) + (0.3f * pts[i].pos.y);
       // pts[i-1].dist  = (0.2f * pts[i-2].dist)  + (0.60f * pts[i-1].dist)  + (0.2f * pts[i].dist);
        pts[i-1].dist  = (0.25f * pts[i-2].dist)  + (0.50f * pts[i-1].dist)  + (0.25f * pts[i].dist);


    }

}

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

	if (pts.size() <= 1) 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) * pts[whatPointAmINear - 1].dist + (pct) * pts[whatPointAmINear].dist;
		angle = (1-pct) * pts[whatPointAmINear - 1].angle + (pct) * pts[whatPointAmINear].angle;
		//angle = atan2(pts[whatPointAmINear].pos.y - pts[whatPointAmINear - 1].pos.y, pts[whatPointAmINear].pos.x - pts[whatPointAmINear-1].pos.x);

		return mix;

	} else {

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

ofPoint grafTag::getPointForTime( float time)
{

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

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

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

	float whatAmINear = 0;
	for (int i = 0; i < pts.size(); i++){

		if (pts[i].time > myTimeInRange){
			whatAmINear = i;
			break;
		}
	}

	if (whatAmINear > 0){

		float timea = pts[whatAmINear - 1].time;
		ofPoint pta = pts[whatAmINear - 1].pos;
		float timeb = pts[whatAmINear    ].time;
		ofPoint ptb = pts[whatAmINear    ].pos;

		float totalDurationBetweenThesePts	= timeb - timea;
		float myPositionBetweenThesePts		= myTimeInRange - 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;

		return mix;

	} else {
		return pts[whatAmINear].pos;
	}
}

ofPoint grafTag::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() <= 0) 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	= getPointForTime(timeInRange);
	ofPoint prevPoint	= getPointForTime(prevTime);


	ofPoint velocity;

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

	return velocity;
}

ofPoint grafTag::getCurrentPoint()
{
    return nowPointForTime;
}

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

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

void grafTag::resetToStart()
{
    nowIndex           = ptStart;
    timeCounter         = pts[ptStart].time;
    timeOfLastFrame     = ofGetElapsedTimef();
    bPlayed                 = false;

}

void grafTag::calcMinMax()
{

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

    distMin = 0;
    distMax = 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;

        if( i == 0 || pts[i].dist  > distMax  ) distMax = pts[i].dist;
        if( i == 0 || pts[i].dist  < distMin  ) distMin = pts[i].dist;
    }



}

float grafTag::getPctDone()
{
    if (pts.size() == 0 ) return 0;
    else return (pts[ptEnd].time - pts[nowIndex].time) / pts[ptEnd].time;
}
/*
void grafTag::averageTail()
{
    if( pts.size() > 10 )
    {

        for( int i = pts.size()-5; i  < pts.size(); i++)
        {
            float pct = ( ( i-( pts.size()-6 ) ) / 4.f );
            float diff = distMax-pts[i].dist;
            pts[i].dist  += diff*powf( pct,.95);;//.75 ;//* ntimes;//(0.25f * pts[i+2].dist)  + (0.50f * pts[i+1].dist)  + (0.25f * pts[i].dist);
        }
        pts[pts.size()-1].dist = distMax;

        for( int i = 0; i  < 5; i++)
        {
            float pct = 1-(i / 4.f);
            float diff = distMax-pts[i].dist;
            pts[i].dist  += diff*powf( pct,1.1);;//.75 ;//* ntimes;//(0.25f * pts[i+2].dist)  + (0.50f * pts[i+1].dist)  + (0.25f * pts[i].dist);
        }
        pts[0].dist = distMax;
    }
}
*/
grafTag::grafTag( const grafTag &  mom )
{
    pts.clear();
	pts_orig.clear();

	pts.assign( mom.pts.begin(), mom.pts.end() );
	pts_orig.assign( mom.pts_orig.begin(), mom.pts_orig.end() );


	bPaused     = mom.bPaused;
	bReset      = mom.bReset;

	rotation    = mom.rotation;
	position    = mom.position;
	z_const     = mom.z_const;

    startTime   = mom.startTime;
	maxNumPts   = mom.maxNumPts;
	ptStart     = mom.ptStart;
	ptEnd       = mom.ptEnd;

    timeCounter             = mom.timeCounter;
    timeOfLastFrame         = mom.timeOfLastFrame;
    timeInRange             = mom.timeInRange;

	nowIndex                = mom.nowIndex;
    nowDist                 = mom.nowDist;
    nowAngle                = mom.nowAngle;
    nowPointForTime         = mom.nowPointForTime;

    max                     = mom.max;
    min                     = mom.min;
    distMin                 = mom.distMin;
    distMax                 = mom.distMax;

    bPlayed                 = mom.bPlayed;

 }
