#include "../misc/fixc11.h"
#include "SensorDataWidget.h"

#include "plot/PlottWidget.h"
#include <QGridLayout>
#include <QColor>

#include "../sensors/SensorFactory.h"
#include "PlotTurns.h"
#include "PlotWiFiScan.h"


template <typename Data> void removeOld(Data& data, const Timestamp limit) {
	if (data.size() < 2) {return;}
	while ( (data.back().key - data.front().key) > limit.ms())  {
		data.remove(0);
	}
}

template <int num> class PlotXLines : public PlotWidget {

protected:

	QColor colors[4] = {QColor(255,0,0), QColor(0,192,0), QColor(0,0,255), QColor(0,0,0)};
	LinePlot line[num];

public:

	PlotXLines(QWidget* parent) : PlotWidget(parent) {
		for (int i = 0; i < num; ++i) {
			pc.addPlot(&line[i]);
			line[i].setColor(colors[i]);
		}
	}

	void addLineNode(const Timestamp ts, const float y, const int idx) {
		LinePlot& lp = line[idx];
		lp.getData().add(ts.ms(), y);
	}

	Timestamp lastRefresh;
	bool needsRefresh(const Timestamp ts) {
		const Timestamp diff = ts - lastRefresh;
		return (diff > Timestamp::fromMS(100));
	}


	void refresh(const Timestamp ts) {

		// ensure event from main-thread using queued-connection
		QMetaObject::invokeMethod(this, "update", Qt::QueuedConnection);

		lastRefresh = ts;

	}

};

class PlotAcc : public PlotXLines<3> {

protected:

	PointPlot steps;

public:

	PlotAcc(QWidget* parent) : PlotXLines(parent) {
		steps.setColor(colors[2]);
		steps.setPointSize(8);
		pc.addPlot(&steps);
		const float s = 4.2;
		const float ref = 9.81;
		pc.setValRange(Range(ref-s, ref+s));
	}

	void addStep(const Timestamp ts) {
		steps.getData().add(ts.ms(), 9.81);
	}

	void add(const Timestamp ts, const AccelerometerData& data) {
		addLineNode(ts, data.x, 0);
		addLineNode(ts, data.y, 1);
		addLineNode(ts, data.z, 2);
		if (needsRefresh(ts)) {
			limit();
			refresh(ts);
		}
	}

	void limit() {
		const Timestamp limit = Timestamp::fromMS(3000);
		removeOld(line[0].getData(), limit);
		removeOld(line[1].getData(), limit);
		removeOld(line[2].getData(), limit);
		removeOld(steps.getData(), limit - Timestamp::fromMS(100));	// remove steps a little before. prevents errors
	}

};

class PlotGyro : public PlotXLines<3> {

public:

	PlotGyro(QWidget* parent) : PlotXLines(parent) {
		const float s = 1;
		const float ref = 0;
		pc.setValRange(Range(ref-s, ref+s));
	}

	void add(const Timestamp ts, const GyroscopeData& data) {
		addLineNode(ts, data.x, 0);
		addLineNode(ts, data.y, 1);
		addLineNode(ts, data.z, 2);
		if (needsRefresh(ts)) {
			limit();
			refresh(ts);
		}
	}

	void limit() {
		const Timestamp limit = Timestamp::fromMS(3000);
		removeOld(line[0].getData(), limit);
		removeOld(line[1].getData(), limit);
		removeOld(line[2].getData(), limit);
	}

};

class PlotBaro : public PlotXLines<1> {

public:

	PlotBaro(QWidget* parent) : PlotXLines(parent) {

	}

	void add(const Timestamp ts, const BarometerData& data) {
		addLineNode(ts, data.hPa, 0);
		if (needsRefresh(ts)) {
			limit();
			refresh(ts);
		}
		const float s = 0.5;
		const float ref = line[0].getData().front().val;
		pc.setValRange(Range(ref-s, ref+s));
	}

	void limit() {
		removeOld(line[0].getData(), Timestamp::fromMS(8000));
	}

};

class PlotTurn : public QWidget {

};



SensorDataWidget::SensorDataWidget(QWidget* parent) : QWidget(parent) {

	QGridLayout* lay = new QGridLayout(this);

	plotGyro = new PlotGyro(this);
	plotAcc = new PlotAcc(this);
	plotBaro = new PlotBaro(this);
	plotTurn = new PlotTurns(this);
	plotWiFi = new PlotWiFiScan(this);

	lay->addWidget(plotGyro,	0, 0, 1, 4, Qt::AlignTop);
	lay->addWidget(plotAcc,		1, 0, 1, 4, Qt::AlignTop);
	lay->addWidget(plotBaro,	2, 0, 1, 4, Qt::AlignTop);
	lay->addWidget(plotTurn,	3, 0, 1, 1, Qt::AlignTop);
	lay->addWidget(plotWiFi,	3, 1, 1, 3, Qt::AlignTop);

	SensorFactory::get().getAccelerometer().addListener(this);
	SensorFactory::get().getGyroscope().addListener(this);
	SensorFactory::get().getBarometer().addListener(this);
	SensorFactory::get().getSteps().addListener(this);
	SensorFactory::get().getTurns().addListener(this);
	SensorFactory::get().getWiFi().addListener(this);

	//setAutoFillBackground(false);

}

void SensorDataWidget::onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) {
	(void) sensor;
	((PlotAcc*)plotAcc)->add(ts, data);
}

void SensorDataWidget::onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) {
	(void) sensor;
	(void) data;
	((PlotAcc*)plotAcc)->addStep(ts);
}

void SensorDataWidget::onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) {
	(void) sensor;
	((PlotGyro*)plotGyro)->add(ts, data);
}

void SensorDataWidget::onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) {
	(void) sensor;
	((PlotBaro*)plotBaro)->add(ts, data);
}

void SensorDataWidget::onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) {
	(void) sensor;
	((PlotTurns*)plotTurn)->add(ts, data);
}

void SensorDataWidget::onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) {
	(void) sensor;
	((PlotWiFiScan*)plotWiFi)->add(ts, data);
}