#include "chartGraphicsItem.h"

extern void doFFT(float samples[],float results[]);

//	----------------------------------------------------------------------------
//	macros
//	----------------------------------------------------------------------------

#define _max(a,b)  (((a) > (b)) ? (a) : (b))
#define _min(a,b)  (((a) < (b)) ? (a) : (b))

//	-----------------------------------------------------------
//	lighten
//	-----------------------------------------------------------

QColor lighten(int r, int g, int b)
{
	int gr = 0xe0;
	int gg = 0xe0;
	int gb = 0xe0;

	int frac = 3;

	int rl = (r + frac*gr)/(frac+1);
	int gl = (g + frac*gg)/(frac+1);
	int bl = (b + frac*gb)/(frac+1);

	return QColor(rl, gl, bl);
}

//	----------------------------------------------------------------------------
//	chartGraphicsItem
//	----------------------------------------------------------------------------

chartGraphicsItem::chartGraphicsItem()
{
	QPoint points[CHART_numVersions][CHART_numTypes][CHART_maxPoints];

	for(int iv=0;iv<CHART_numVersions;iv++){
		for(int it=0;it<CHART_numTypes;it++){
			for(int ip=0;ip<CHART_maxPoints;ip++){
				points[iv][it][ip].setX(0);
				points[iv][it][ip].setY(0);
			}
		}
	}
	memset(xpoints,0,sizeof(xpoints));
	memset(ypoints,0,sizeof(ypoints));
	memset(npoints,0,sizeof(npoints));

	npixx = 550;
	npixy = 250;

	xscale = 0;
	yscale = 0;
	xofs = 0;
	yofs = 0;
	minx = 0;
	maxx = 0;
	miny = 0;
	maxy = 0;
	yT = 0;
	yB = 0;
	xL = 0;
	xR = 0;
	yM = 0;
	hT = 10;
	scalings(0,0,1,1);

	ipoint = 0;

	minFreq = 1;
	maxFreq = 2000;
	logscaleX = 0;
	chartType = CHART_GENERIC;
	symmY = 0;
}

//	----------------------------------------------------------------------------
//	boundingRect
//	----------------------------------------------------------------------------

QRectF chartGraphicsItem::boundingRect() const
{
	return QRectF(0,0,npixx,npixy);
}

//	----------------------------------------------------------------------------
//	paint
//	----------------------------------------------------------------------------

void chartGraphicsItem::paint(QPainter *painter, const QStyleOptionGraphicsItem */*option*/, QWidget */*widget*/)
{
	painter->setBrush(Qt::white);
	painter->drawRect(0,0,npixx,npixy);
	switch(chartType){
		case CHART_GENERIC: return drawChart(painter);
		case CHART_BODE_TOP: return drawBodeTop(painter);
		case CHART_BODE_BOT: return drawBodeBot(painter);
	}
}

//	----------------------------------------------------------------------------
//	drawChart
//	----------------------------------------------------------------------------

void chartGraphicsItem::drawChart(QPainter *painter)
{
	int iv = CHART_activeVersion;
	int it = CHART_generic;
	int np = npoints[iv][it];

	if(np>1){
		painter->setPen(QColor(COLOR_FFT));
		painter->drawPolyline(points[iv][it],np-1);
	}
}

//	----------------------------------------------------------------------------
//	drawBodeTop
//	----------------------------------------------------------------------------

void chartGraphicsItem::drawBodeTop(QPainter *painter)
{
	const int NT = 5;

	static int t[NT]={
		CHART_bodeGain,
		CHART_filtGain,
		CHART_openGain,
		CHART_shakeGain,
		CHART_fftAmp
	};

	static QColor c[CHART_numVersions][NT]={
	{
		QColor(COLOR_BODE),
		QColor(COLOR_FILTER),
		QColor(COLOR_OPEN),
		QColor(COLOR_SHAKE),
		QColor(COLOR_FFT)
	},{
		QColor(lighten(COLOR_BODE)),
		QColor(lighten(COLOR_FILTER)),
		QColor(lighten(COLOR_OPEN)),
		QColor(lighten(COLOR_SHAKE)),
		QColor(lighten(COLOR_FFT))
		}
	};

	gridLogX(painter);

	for(int j=0;j<CHART_numVersions;j++){
		int iv = (CHART_numVersions-1)-j;
		for(int i=0;i<NT;i++){
			int it = t[i];
			int np = npoints[iv][it];
			if(np>1){
				painter->setPen(c[iv][i]);
				painter->drawPolyline(points[iv][it],np-1);
			}
		}
	}
}

//	----------------------------------------------------------------------------
//	drawBodeBot
//	----------------------------------------------------------------------------

void chartGraphicsItem::drawBodeBot(QPainter *painter)
{
	const int NT = 6;

	static int t[NT]={
		CHART_bodePhase,
		CHART_filtPhase,
		CHART_openPhase,
		CHART_shakeJitter,
		CHART_stepVel,
		CHART_fftRaw
	};

	static QColor c[CHART_numVersions][NT]={
		{
			QColor(COLOR_BODE),
			QColor(COLOR_FILTER),
			QColor(COLOR_OPEN),
			QColor(COLOR_SHAKE),
			QColor(COLOR_STEP),
			QColor(COLOR_FFT)
		},{
			QColor(lighten(COLOR_BODE)),
			QColor(lighten(COLOR_FILTER)),
			QColor(lighten(COLOR_OPEN)),
			QColor(lighten(COLOR_SHAKE)),
			QColor(lighten(COLOR_STEP)),
			QColor(lighten(COLOR_FFT))
		}
	};

	gridLogX(painter);

	for(int j=0;j<CHART_numVersions;j++){
		int iv = (CHART_numVersions-1)-j;
		for(int i=0;i<NT;i++){
			int it = t[i];
			int np = npoints[iv][it];
			if(np>1){
				painter->setPen(c[iv][i]);
				painter->drawPolyline(points[iv][it],np-1);
			}
		}
	}
}
//	----------------------------------------------------------------------------
//	setPoints
//	----------------------------------------------------------------------------

void chartGraphicsItem::setPoints(float *values,int count, int reset)
{
	float t = 0;
	const float dT = 1.0f/kFreq;

	int iv = CHART_activeVersion;
	int it = CHART_fftRaw;

	if(reset){
		miny = maxy = values[0];
		maxx = 0;
	}
	minx = 0;

	for(int i=0;i<count;i++){
		if(i>=CHART_maxPoints)break;
		points[iv][it][i].setX(scaleX(t));
		points[iv][it][i].setY(scaleY(values[i]));
		scalings(t,values[i]);
		t+= dT;
	}
	npoints[iv][it] = count;
	update();
}

//	----------------------------------------------------------------------------
//	setPoints
//	----------------------------------------------------------------------------

void chartGraphicsItem::setPoints(float *xvalues,float *yvalues,int count)
{
	int iv = CHART_activeVersion;
	int it = CHART_generic;

	minx = maxx = xvalues[0];
	miny = maxy = yvalues[0];
	for(int i=0;i<count;i++){
		if(i>=IMU_BUF_COUNT)break;
		maxx = _max(xvalues[i],maxx);
		maxy = _max(yvalues[i],maxy);
		minx = _min(xvalues[i],minx);
		miny = _min(yvalues[i],miny);
	}
	scalings(xvalues[0],xvalues[0]);

	for(int i=0;i<count;i++){
		if(i>=IMU_BUF_COUNT)break;
		points[iv][it][i].setX(scaleX(xvalues[i]));
		points[iv][it][i].setY(scaleY(yvalues[i]));
	}
	npoints[iv][it] = count;
	update();
}

//	----------------------------------------------------------------------------
//	setBodePoints
//	----------------------------------------------------------------------------

void chartGraphicsItem::setBodeAmp(float *xvalues,float *yvalues,int count, int version, int type)
{
	int iv = version;
	int it = type;

	logscaleX = 1;
	minx = log10(minFreq);
	maxx = log10(maxFreq);

	maxy = 40;
	miny = -40;

	xscale = npixx*(1.0f/(maxx-minx)); xofs = minx;
	yscale = npixy*(1.0f/(miny-maxy)); yofs = maxy;

	xL = 0;
	xR = npixx;
	yB = npixy;
	yM = npixy/2;
	yT = 0;

	int j = 0;
	for(int i=0;i<count;i++){
		if(i>=IMU_BUF_COUNT)break;
		if(xvalues[i]>=minFreq && xvalues[i]<maxFreq){
			points[iv][it][j].setX(scaleX(log10(xvalues[i])));
			points[iv][it][j].setY(scaleY(20*log10(yvalues[i])));
			j++;
		}
	}
	npoints[iv][it] = j;
	update();

}

//	----------------------------------------------------------------------------
//	setBodePhase
//	----------------------------------------------------------------------------

void chartGraphicsItem::setBodePhase(float *xvalues,float *yvalues,int count, int version, int type)
{
	int iv = version;
	int it = type;

	static float x0 = log10(minFreq);
	static float x1 = log10(maxFreq);

	static float y1 = 90;
	static float y0 = -270;

	static float kx = npixx*(1.0f/(x1-x0));
	static float ky = npixy*(1.0f/(y0-y1));

	int j = 0;
	for(int i=0;i<count;i++){
		if(i>=IMU_BUF_COUNT)break;
		if(xvalues[i]>=minFreq && xvalues[i]<maxFreq){
			points[iv][it][j].setX(kx*(log10(xvalues[i])-x0));
			points[iv][it][j].setY(ky*(yvalues[i]-y1));
			j++;
		}
	}
	npoints[iv][it] = j;
	update();

}
//	----------------------------------------------------------------------------
//	scaleX
//	----------------------------------------------------------------------------

int chartGraphicsItem::scaleX(float x){return (x-xofs)*xscale;}
int chartGraphicsItem::scaleY(float y){return (y-yofs)*yscale;}

//	----------------------------------------------------------------------------
//	scalings
//	----------------------------------------------------------------------------

void chartGraphicsItem::scalings(float x, float y, float xrange, float /*yrange*/)
{
	maxx = _max(x,maxx);
	maxy = _max(y,maxy);
	minx = (xrange>0)?(maxx-xrange):(_min(x,minx));
	miny = _min(y,miny);
	if(symmY){
		maxy = _max(abs(maxy),abs(miny));
		miny = -maxy;
	}
	xscale = npixx*(1.0f/(maxx-minx)); xofs = minx;
	yscale = npixy*(1.0f/(miny-maxy)); yofs = maxy;
	xL = scaleX(minx);
	xR = scaleX(maxx);
	yB = scaleY(miny);
	yT = scaleY(maxy);
	if((maxy>0)&&(miny<0)){
		yM = scaleY(0);
	}else{
		yM = scaleY((miny+maxy)/2);
	}
}

//	----------------------------------------------------------------------------
//	addPoint
//	----------------------------------------------------------------------------

void chartGraphicsItem::addPoint(int /*axis*/, float xvalue, float yvalue, int reset)
{
	int iv = CHART_activeVersion;
	int it = CHART_generic;

	const int CCMAX = CHART_maxPoints-1;
	xpoints[ipoint] = xvalue;
	ypoints[ipoint] = yvalue;
	if(npoints[iv][it]<CHART_maxPoints){
		npoints[iv][it]++;
		ipoint = npoints[iv][it];
	}else{
		ipoint = (ipoint<CCMAX)?(ipoint+1):(0);
	}

	if(reset){
		miny = maxy = yvalue;
	}

	scalings(xvalue,yvalue,5);

	int j = ipoint;
	for(int i=0;i<npoints[iv][it]-1;i++){
		points[iv][it][i].setX(scaleX(xpoints[j]));
		points[iv][it][i].setY(scaleY(ypoints[j]));
		j = (j<CCMAX)?(j+1):(0);
	}
	update();

}

//	-----------------------------------------------------------
//	gScaleY
//	-----------------------------------------------------------

int chartGraphicsItem::gScaleY(float gain)
{
	static double s = (-npixy)/4.;
	static int y0 = npixy/2;
	return (int)(y0+s*log10(gain)); // 20*log10(gain)/20
}

//	-----------------------------------------------------------
//	gScaleX
//	-----------------------------------------------------------

int chartGraphicsItem::gScaleX(float freq)
{
	static float s = (float)(npixx/log10((double)(maxFreq/minFreq)));
	return (int)(s*log10(freq));
}

//	----------------------------------------------------------------------------
//	fftPoints
//	----------------------------------------------------------------------------

void chartGraphicsItem::fftPoints(float *values,int count, int reset)
{
	int iv = CHART_activeVersion;
	int it = CHART_fftAmp;

	if(count<IMU_BUF_COUNT)return;
	logscaleX = 1;

	static float results[IMU_BUF_COUNT];
	static float kx = (float)(npixx/log10((double)(maxFreq/minFreq)));
	if(reset)maxy=0;
	if(maxy<0.1)maxy=0.1;
	xofs = 0;
	yofs = npixy/2;

	doFFT(values,results);
	int j = 0;

	if(points[iv][it][IMU_BUF_COUNT/4].ry()==0){
		for(int i=0;i<(IMU_BUF_COUNT/2);i++){
			double f = ((double)i*kFreq)/IMU_BUF_COUNT;
			if(f>=minFreq){
				float x = (float)(log10(f));
				float y = results[i];
				if(y>maxy)maxy = y;
				float ky = (-npixy/2)/maxy;
				points[iv][it][j].setX(kx*x+xofs);
				points[iv][it][j].setY(ky*y+yofs);
				j++;
			}
		}
	}else{
		for(int i=0;i<(IMU_BUF_COUNT/2);i++){
			double f = ((double)i*kFreq)/IMU_BUF_COUNT;
			if(f>=minFreq){
				float y = results[i];
				if(y>maxy)maxy = y;
				float ky = (-npixy/2)/maxy;
				points[iv][it][j].setY(ky*y+yofs);
				j++;
			}
		}
	}
	npoints[iv][it] = j;

	update();
}

//	-----------------------------------------------------------
//	newTuneState
//	-----------------------------------------------------------


void chartGraphicsItem::newTuneState(int axis, tuneState *g, tuneState *r)
{
	switch(chartType){
	case CHART_BODE_TOP:
		setBodeAmp(g->filterfreqs[axis][NiFILT],g->filtergains[axis][NiFILT],FILTBODEMAX,CHART_activeVersion,	CHART_filtGain);
		setBodeAmp(r->filterfreqs[axis][NiFILT],r->filtergains[axis][NiFILT],FILTBODEMAX,CHART_refVersion,		CHART_filtGain);
		break;
	case CHART_BODE_BOT:
		setBodePhase(g->filterfreqs[axis][NiFILT],g->filterphase[axis][NiFILT],FILTBODEMAX,CHART_activeVersion,	CHART_filtPhase);
		setBodePhase(r->filterfreqs[axis][NiFILT],r->filterphase[axis][NiFILT],FILTBODEMAX,CHART_refVersion,	CHART_filtPhase);
		break;
	}

}


//	-----------------------------------------------------------
//	gridLogX
//	-----------------------------------------------------------

void chartGraphicsItem::gridLogX(QPainter *painter)
{
	int frq;
	int x;
	int y;
	int dfrq = 1;
	int dfrq0 = 0;
	static double xMin = log10((double)minFreq);
	static double xMax = log10((double)maxFreq);
	static double xLogScale = npixx/(xMax-xMin);
	int yt = 1;
	int yb = npixy-1;
	int xl = 0;
	int xr = npixx-1;
	int ht = 11;

	for(frq=(int)minFreq;frq<(int)maxFreq;frq+=dfrq){
		x = (int)(xLogScale*(log10((double)frq)-xMin));
		if(frq==10*dfrq)dfrq*=10;
		if(dfrq!=dfrq0){
			dfrq0=dfrq;
			painter->setPen(Qt::gray);
			QString st;
			if(dfrq<1000){
				st.sprintf("%i",dfrq);
			}else{
				st.sprintf("%ik",dfrq/1000);
			}
			painter->drawText(x+2,yt+ht,st);
			painter->setPen(Qt::black);
		} else {
			painter->setPen(Qt::gray);
		}
		painter->drawLine(x,yt,x,yb);
	}
	painter->setPen(Qt::gray);

	const int nlinX = 4;
	const char *legend[2][nlinX]={
		{"20",  "0", "-20", "-40"},
		{ "0","-90","-180","-270"}
	};
	int j = (chartType==CHART_BODE_BOT)?(1):(0);
	for(int i=0;i<nlinX;i++){
		y = ((i+1)*npixy)/4;
		if(y<npixy)painter->drawLine(xl,y,xr,y);
		const char *s = legend[j][i];
		x = xr - 5*strlen(s);
		painter->drawText(x-2,y-2,s);
	}
}

//	-----------------------------------------------------------
//	gridLinX
//	-----------------------------------------------------------

//void chartGraphicsItem::gridLinX(QPainter *painter)
//{
//	int numdivs = 20;
//	int majdivs = 5;

//	int yt = 1;
//	int yb = npixy-1;
//	int xl = 0;
//	int xr = npixx-1;
//	int ht = 11;
//	int x;
//	int y;


//	for(int ii=0;ii<numdivs;ii++){
//		x = xl + (ii*npixx)/numdivs;
//		if(ii%majdivs==0){
//			painter->setPen(Qt::gray);
//			QString st;
//			int dfrq = (int)(ii*(maxFreq/numdivs)+0.5);
//			st.sprintf("%i",dfrq);
//			painter->drawText(x+2,yt+ht,st);
//			painter->setPen(Qt::black);
//		}else{
//			painter->setPen(Qt::darkGray);
//		}
//		painter->drawLine(x,yt,x,yb);
//	}
//	painter->setPen(Qt::darkGray);
//	y = npixy/2;
//	painter->drawLine(xl,y,xr,y);
//}


//	-----------------------------------------------------------
//	gridLinX
//	-----------------------------------------------------------

void chartGraphicsItem::gridLinX(QPainter *painter)
{
	int numdivs = 20;
	int majdivs = 5;
	QString st;

	float xv = minx;

	float dx = (maxx-minx)/numdivs;

	for(int ii=0;ii<numdivs;ii++){
		int x = scaleX(xv);
		if(ii%majdivs==0){
			painter->setPen(Qt::gray);
			st.sprintf("%1.0f",xv);
			painter->drawText(x+2,yT+hT,st);
			painter->setPen(Qt::black);
		}else{
			painter->setPen(Qt::darkGray);
		}
		painter->drawLine(x,yT,x,yB);
		xv += dx;
	}
	painter->setPen(Qt::darkGray);
	painter->drawLine(xL,yM,xR,yM);
	painter->setPen(Qt::black);
	st.sprintf("%1.1f",maxy);	painter->drawText(xR-30,yT+1+hT,st);
	st.sprintf("%1.1f",miny);	painter->drawText(xR-30,yB-1   ,st);

}
