#include "grafCurveSmoother.h"


grafCurveSmoother::grafCurveSmoother()
{
    //ctor
}

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


void grafCurveSmoother::smoothTag( int resolution, grafTag * src,  grafTag * dst)
{
    if( src->pts.size() < 4 ) return;

    pts.clear();

    for( int j = 4;j < src->pts.size(); j+=1)
    {
        int startPos = (int)j- 3;

        if( j < 4 )
            ;//dst->addNewPoint(src->pts[startPos + 0].pos,-1,src->pts[startPos + 0].angle,src->pts[startPos + 0].time);//dst->addNewPoint(src->pts[j].pos,src->pts[j].time); //pts.push_back( src->pts[j] );
        else{

            //cout << "dist " << src->pts[startPos + 0].dist << endl;

            float x0 = src->pts[startPos + 0].pos.x;//[0];
            float y0 = src->pts[startPos + 0].pos.y;//[1];
            float z0 = src->pts[startPos + 0].time;
            float a0 = src->pts[startPos + 0].dist;

            float x1 = src->pts[startPos + 1].pos.x;//[0];
            float y1 = src->pts[startPos + 1].pos.y;//[1];
            float z1 = src->pts[startPos + 1].time;
            float a1 = src->pts[startPos + 1].dist;

            float x2 = src->pts[startPos + 2].pos.x;//[0];
            float y2 = src->pts[startPos + 2].pos.y;//[1];
            float z2 = src->pts[startPos + 2].time;
            float a2 = src->pts[startPos + 2].dist;

            float x3 = src->pts[startPos + 3].pos.x;//[0];
            float y3 = src->pts[startPos + 3].pos.y;//[1];
            float z3 = src->pts[startPos + 3].time;
            float a3 = src->pts[startPos + 3].dist;

            float t,t2,t3;
            float x,y,z,a,d;

            for (int i = 1; i < resolution; i++){

                t 	=  (float)i / (float)(resolution-1);
                t2 	= t * t;
                t3 	= t2 * t;

                x = 0.5f * ( ( 2.0f * x1 ) +
                ( -x0 + x2 ) * t +
                ( 2.0f * x0 - 5.0f * x1 + 4 * x2 - x3 ) * t2 +
                ( -x0 + 3.0f * x1 - 3.0f * x2 + x3 ) * t3 );

                y = 0.5f * ( ( 2.0f * y1 ) +
                ( -y0 + y2 ) * t +
                ( 2.0f * y0 - 5.0f * y1 + 4 * y2 - y3 ) * t2 +
                ( -y0 + 3.0f * y1 - 3.0f * y2 + y3 ) * t3 );

                z = 0.5f * ( ( 2.0f * z1 ) +
                ( -z0 + z2 ) * t +
                ( 2.0f * z0 - 5.0f * z1 + 4 * z2 - z3 ) * t2 +
                ( -z0 + 3.0f * z1 - 3.0f * z2 + z3 ) * t3 );

                a = 0.5f * ( ( 2.0f * a1 ) +
                ( -a0 + a2 ) * t +
                ( 2.0f * a0 - 5.0f * a1 + 4 * a2 - a3 ) * t2 +
                ( -a0 + 3.0f * a1 - 3.0f * a2 + a3 ) * t3 );

                timePt newPoint;
                newPoint.pos.x = x;//*src->width;
                newPoint.pos.y = y;//*src->width;
                newPoint.pos.z = 0;
                newPoint.time = z;

                //if( i == 0 ) a = a0;
                //else if( i == resolution-1 ) a = a3;
                //a = .01;
                dst->addNewPoint(newPoint.pos,a,-1,z);

            }

        }

    }

    timePt newPoint;
    newPoint.pos.x = src->pts[src->pts.size()-1].pos.x;//*src->width;
    newPoint.pos.y = src->pts[src->pts.size()-1].pos.y;//*src->width;
    newPoint.pos.z = 0;

   // dst->addNewPoint(newPoint.pos,-1,-1,src->pts[src->pts.size()-1].time);

    dst->rotation = src->rotation;
    dst->position = src->position;
    dst->z_const = src->z_const;

    //cout << "dst pz " << dst->position.z <<  endl;
    //cout << "src pz " << src->position.z <<  endl;
    //cout << src->pts[src->pts.size()-1].time << " " <<  dst->pts[dst->pts.size()-1].time <<endl;

}


void grafCurveSmoother::draw(float z_const)
{
    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();

}
/*
if (curveVertices.size() >= 4){

 		int startPos = (int)curveVertices.size() - 4;

 		float x0 = curveVertices[startPos + 0][0];
	   	float y0 = curveVertices[startPos + 0][1];
 		float x1 = curveVertices[startPos + 1][0];
	   	float y1 = curveVertices[startPos + 1][1];
 		float x2 = curveVertices[startPos + 2][0];
	   	float y2 = curveVertices[startPos + 2][1];
 		float x3 = curveVertices[startPos + 3][0];
	   	float y3 = curveVertices[startPos + 3][1];

 		int resolution = 20;

		float t,t2,t3;
		float x,y;

		for (int i = 0; i < resolution; i++){

			t 	=  (float)i / (float)(resolution-1);
			t2 	= t * t;
			t3 	= t2 * t;

			x = 0.5f * ( ( 2.0f * x1 ) +
			( -x0 + x2 ) * t +
			( 2.0f * x0 - 5.0f * x1 + 4 * x2 - x3 ) * t2 +
			( -x0 + 3.0f * x1 - 3.0f * x2 + x3 ) * t3 );

			y = 0.5f * ( ( 2.0f * y1 ) +
			( -y0 + y2 ) * t +
			( 2.0f * y0 - 5.0f * y1 + 4 * y2 - y3 ) * t2 +
			( -y0 + 3.0f * y1 - 3.0f * y2 + y3 ) * t3 );

			double* newPoint = new double[3];
			newPoint[0] = x;
			newPoint[1] = y;
			newPoint[2] = 0;
			polyVertices.push_back(newPoint);
		}
 	}

*/
