#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"

#include "../Settings.h"
#include "../UIHelper.h"

/** helper method to remove old entries */
template <typename Data> void removeOld(Data& data, const Data& dataRef, const Timestamp limit) {
	if (data.size() == 0) {return;}
	if (dataRef.size() == 0) {return;}
	while ( (dataRef.back().key - data.front().key) > limit.ms())  {
		if (data.size() == 0) {return;}
		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 > Settings::SensorDebug::updateEvery);
	}


	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.8;
		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(), line[0].getData(), limit);
		removeOld(line[1].getData(), line[0].getData(), limit);
		removeOld(line[2].getData(), line[0].getData(), limit);
		removeOld(  steps.getData(), line[0].getData(), limit);	// remove steps a little before. prevents errors
	}

};

class PlotGyro : public PlotXLines<3> {

public:

	PlotGyro(QWidget* parent) : PlotXLines(parent) {
		const float s = 1.5;
		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(), line[0].getData(), limit);
		removeOld(line[1].getData(), line[0].getData(), limit);
		removeOld(line[2].getData(), line[0].getData(), limit);
	}

};

class PlotBaro : public PlotXLines<2> {

public:

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

	}

	void add(const Timestamp ts, const BarometerData& data) {

		static int skip = 0;
		if ((++skip % 8) != 0) {return;}

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

	}

	void add(const Timestamp ts, const ActivityData& data) {

		static int skip = 0;
		if ((++skip % 8) != 0) {return;}

		float offset = 0;
		switch(data.curActivity) {
		case ActivityButterPressure::Activity::DOWN:	offset = -0.5; break;
		case ActivityButterPressure::Activity::UP:		offset = +0.5; break;
		case ActivityButterPressure::Activity::STAY:	offset = +0.1; break;
		}

		addLineNode(ts, line[0].getData().front().val + offset, 1);
		if (needsRefresh(ts)) {
			limit();
			refresh(ts);
		}

	}


	void limit() {
		// no limit!
		//removeOld(line[0].getData(), Timestamp::fromMS(15000));		// 15 second values
	}

};

//class PlotTurn : public QWidget {

//};



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

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

	// layout setup
	lay = new QGridLayout(this);
	lay->addWidget(plotGyro,	0, 0, 1, 4);
	lay->addWidget(plotAcc,		1, 0, 1, 4);
	lay->addWidget(plotBaro,	2, 0, 1, 4);
	lay->addWidget(plotTurn,	3, 0, 1, 1);
	lay->addWidget(plotWiFi,	3, 1, 1, 3);

//	lay->setRowStretch(0, 1);
//	lay->setRowStretch(1, 1);
//	lay->setRowStretch(2, 10);
//	lay->setRowStretch(3, 10);
//	lay->setVerticalSpacing(5);
//	lay->setHorizontalSpacing(5);
//	lay->setSizeConstraint(QGridLayout::SetDefaultConstraint);

	// attach as listener to all sensors we want to debug
	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);
	SensorFactory::get().getActivity().addListener(this);

}



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<ActivityData>* sensor, const Timestamp ts, const ActivityData& 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);
}
