#include <QDataStream>
#include <gato/gato.h>
#include "stylus.h"

static const QString stylus_name(QString::fromLatin1("JN104FE9"));
static const GatoUUID stylus_service(QString::fromLatin1("dcd68980-aadc-11e1-a22a-0002a5d5c51b"));
static const GatoUUID agg_char_uuid(QString::fromLatin1("00002a5a-0000-1000-8000-00805f9b34fb"));

Stylus::Stylus(QObject *parent) :
	QObject(parent),
	_manager(new GatoCentralManager(this)),
	_peripheral(0),
	_p(0), _x(0), _y(0), _z(0)
{
	connect(_manager, SIGNAL(discoveredPeripheral(GatoPeripheral*,int)),
			SLOT(handleDiscoveredPeripheral(GatoPeripheral*,int)));
}

Stylus::~Stylus()
{
	if (_peripheral) {
		disconnect(_peripheral, 0, this, 0);
		_peripheral->disconnectPeripheral();
	}
}

qreal Stylus::pressure() const
{
	return _p;
}

qreal Stylus::x() const
{
	return _x;
}

qreal Stylus::y() const
{
	return _y;
}

qreal Stylus::z() const
{
	return _z;
}

void Stylus::connectToAnyDevice()
{
	_manager->scanForPeripherals();
}

void Stylus::connectDevice(const QString &addr)
{
	connectToPeripheral(new GatoPeripheral(GatoAddress(addr), this));
}

void Stylus::disconnectDevice()
{
	if (_peripheral) {
		_peripheral->disconnectPeripheral();
		_peripheral->deleteLater();
		_peripheral = 0;
	}
}

void Stylus::connectToPeripheral(GatoPeripheral *peripheral)
{
	if (_peripheral) {
		disconnectDevice();
	}

	_peripheral = peripheral;

	qDebug() << "Connecting to peripheral" << peripheral->name() << peripheral->address().toString();

	connect(_peripheral, SIGNAL(connected()), SLOT(handleConnected()));
	connect(_peripheral, SIGNAL(disconnected()), SLOT(handleDisconnected()));
	connect(_peripheral, SIGNAL(servicesDiscovered()), SLOT(handleServices()));
	connect(_peripheral, SIGNAL(characteristicsDiscovered(GatoService)), SLOT(handleCharacteristics(GatoService)));
	connect(_peripheral, SIGNAL(valueUpdated(GatoCharacteristic,QByteArray)), SLOT(handleValueUpdated(GatoCharacteristic,QByteArray)));

	_peripheral->connectPeripheral();
}

void Stylus::handleDiscoveredPeripheral(GatoPeripheral *peripheral, int rssi)
{
	qDebug() << "Found peripheral" << peripheral->address().toString() << peripheral->name() << rssi;
	if (peripheral->name() == stylus_name) {
		_manager->stopScan();
		connectToPeripheral(peripheral);
	}
}
void Stylus::handleConnected()
{
	qDebug() << "Connected to" << _peripheral->name();
	_peripheral->discoverServices();
}

void Stylus::handleDisconnected()
{
	qDebug() << "Peripheral disconnected";
}

void Stylus::handleServices()
{
	qDebug() << "Services found";
	foreach (const GatoService &service, _peripheral->services()) {
		qDebug() << service.uuid();
		if (service.uuid() == stylus_service) {
			// Found the service we want
			qDebug() << "Found stylus service";
			_peripheral->discoverCharacteristics(service);
		}
	}
}

void Stylus::handleCharacteristics(const GatoService &service)
{
	foreach (const GatoCharacteristic &c, service.characteristics()) {
		if (c.uuid() == agg_char_uuid) {
			qDebug() << c.uuid();
			_peripheral->setNotification(c, true);
		}
	}
}

void Stylus::handleValueUpdated(const GatoCharacteristic &characteristic, const QByteArray &value)
{
	if (characteristic.uuid() == agg_char_uuid) {
		QDataStream s(value);
		s.setByteOrder(QDataStream::BigEndian);
		qint16 p, x, y, z;
		s >> p >> x >> y >> z;
		handleReport(p, x, y, z);
	}
}

void Stylus::handleReport(int p, int x, int y, int z)
{
	qreal newp = p / 7000.0;
	if (newp != _p) {
		_p = newp;
		emit pressureChanged();
	}
	qreal newx = x / 268.0;
	if (newx != _x) {
		_x = newx;
		emit xChanged();
	}
	qreal newy = y / 268.0;
	if (newy != _y) {
		_y = newy;
		emit yChanged();
	}
	qreal newz = z / 268.0;
	if (newz != _z) {
		_z = newz;
		emit zChanged();
	}
}