#include "metawatch.h"
#include "metawatchbletransport.h"

const GatoUUID MetaWatchBLETransport::ServiceUuid(quint16(0x8880));
const GatoUUID MetaWatchBLETransport::InputCharacteristicUuid(quint16(0x8882));
const GatoUUID MetaWatchBLETransport::OutputCharacteristicUuid(quint16(0x8881));

MetaWatchBLETransport::MetaWatchBLETransport(const GatoAddress &address, QObject *parent) :
	MetaWatchTransport(parent),
	_dev(new GatoPeripheral(address, this))
{
	connect(_dev, SIGNAL(connected()), SLOT(handleDeviceConnected()));
	connect(_dev, SIGNAL(disconnected()), SLOT(handleDeviceDisconnected()));
	connect(_dev, SIGNAL(servicesDiscovered()), SLOT(handleDeviceServices()));
	connect(_dev, SIGNAL(characteristicsDiscovered(GatoService)),
			SLOT(handleDeviceCharacteristics(GatoService)));
	connect(_dev, SIGNAL(valueUpdated(GatoCharacteristic,QByteArray)),
			SLOT(handleDeviceUpdate(GatoCharacteristic,QByteArray)));
}

MetaWatchBLETransport::~MetaWatchBLETransport()
{
	disconnectDevice();
}

bool MetaWatchBLETransport::isDeviceConnected() const
{
	return _dev->state() == GatoPeripheral::StateConnected;
}

void MetaWatchBLETransport::sendMessage(quint8 type, quint8 options, const QByteArray &payload)
{
	QByteArray packet = encode(type, options, payload);
	bool need_ack = type != MetaWatch::MessageWriteLcdBuffer;
	qDebug() << "Send:" << packet.toHex();
	_dev->writeValue(_out, packet,
					 need_ack ? GatoPeripheral::WriteWithResponse : GatoPeripheral::WriteWithoutResponse);
}

void MetaWatchBLETransport::connectDevice()
{
	qDebug() << "Trying to connect to" << _dev->address();
	_dev->connectPeripheral();
}

void MetaWatchBLETransport::disconnectDevice()
{
	_dev->disconnectPeripheral();
}

QByteArray MetaWatchBLETransport::encode(quint8 type, quint8 options, const QByteArray &payload)
{
	QByteArray packet;

	quint8 message_size = 6 + payload.size();
	packet.reserve(message_size);

	packet.append(0x01);
	packet.append(message_size);
	packet.append(type);
	packet.append(options);

	packet.append(payload);
	packet.append("\0\0", 2); // Empty CRC is OK for BLE

	return packet;
}

bool MetaWatchBLETransport::decode(const QByteArray &packet, quint8 *type, quint8 *options, QByteArray *payload)
{
	if (packet.size() < 6) {
		qWarning() << "Message too short";
	}

	if (packet.at(0) != 0x1) {
		qWarning() << "Invalid packet header";
		return false;
	}

	quint8 message_size = packet[1];
	if (message_size < 6 || message_size > 32 || message_size != packet.size()) {
		qWarning() << "Invalid message size:" << message_size;
	}

	*type = packet[2];
	*options = packet[3];

	*payload = packet.mid(4, message_size - 6);

	// Ignore CRC on BLE: it's set to 0 by firmware.

	return true;
}

void MetaWatchBLETransport::handleDeviceConnected()
{
	qDebug() << "MW connected";
	if (_dev->services().isEmpty()) {
		qDebug() << "Trying to discover services";
		QList<GatoUUID> interesting_services;
		interesting_services << ServiceUuid;
		_dev->discoverServices();
	} else {
		// Directly use the services in cache
		handleDeviceServices();
	}
}

void MetaWatchBLETransport::handleDeviceDisconnected()
{
	qDebug() << "MW disconnected";
	emit disconnected();
}

void MetaWatchBLETransport::handleDeviceServices()
{
	QList<GatoService> services = _dev->services();
	qDebug() << "Got" << services.size() << "services";
	foreach (const GatoService &s, services) {
		if (s.uuid() == ServiceUuid) {
			_dev->discoverCharacteristics(s);
		}
	}
}

void MetaWatchBLETransport::handleDeviceCharacteristics(const GatoService &service)
{
	qDebug() << "Got characteristics";
	foreach (const GatoCharacteristic &c, service.characteristics()) {
		if (c.uuid() == InputCharacteristicUuid) {
			_in = c;
			_dev->setNotification(_in, true);
		} else if (c.uuid() == OutputCharacteristicUuid) {
			_out = c;
		}
		if (!_in.isNull() && !_out.isNull()) {
			emit connected();
		}
	}
}

void MetaWatchBLETransport::handleDeviceUpdate(const GatoCharacteristic &characteristic, const QByteArray &value)
{
	qDebug() << "Recv:" << value.toHex();
	quint8 type, options;
	QByteArray payload;

	Q_UNUSED(characteristic);

	if (decode(value, &type, &options, &payload)) {
		emit messageReceived(type, options, payload);
	} else {
		qWarning() << "Failed to decode message from metawatch:" << value.toHex();
	}
}