#include <QtCore/QDebug>
#include "endianhelpers.h"
#include "saprotocol.h"

const QBluetoothUuid SAProtocol::dataServiceUuid(QLatin1String("a49eb41e-cb06-495c-9f4f-aa80a90cdf4a"));
const QBluetoothUuid SAProtocol::nudgeServiceUuid(QLatin1String("a49eb41e-cb06-495c-9f4f-bb80a90cdf00"));

const QLatin1String SAProtocol::capabilityDiscoveryProfile("/System/Reserved/ServiceCapabilityDiscovery");

SAProtocol::SAProtocol()
{
}

SAProtocol::PeerDescription SAProtocol::unpackPeerDescription(const QByteArray &data)
{
	PeerDescription desc;
	if (data.size() < 24) {
		qWarning() << "Peer description too small";
		desc.messageType = 0;
		return desc;
	}

	int offset = 0;

	desc.messageType = read<quint8>(data, offset);

	if (desc.messageType != 5 && desc.messageType != 6) {
		qWarning() << "Unknown message type:" << desc.messageType;
		return desc;
	}

	desc.accessoryProtocolVersion = read<quint16>(data, offset);
	desc.accessorySoftwareVersion = read<quint16>(data, offset);

	if (desc.messageType == 6) {
		desc.status = read<quint8>(data, offset);
		desc.errorCode = read<quint8>(data, offset);
	} else {
		// God knows WHYYY.
		desc.status = read<quint8>(data, offset);
	}
	desc.APDUSize = read<quint32>(data, offset);
	desc.SSDUSize = read<quint16>(data, offset);
	desc.sessions = read<quint16>(data, offset);
	desc.timeout = read<quint16>(data, offset);
	desc.unk_1 = read<quint8>(data, offset);
	desc.unk_2 = read<quint16>(data, offset);
	desc.unk_3 = read<quint8>(data, offset);

	int marker = data.indexOf(peerDescriptionSeparator, offset);
	if (marker == -1) {
		qWarning() << "No product in peer description";
		return desc;
	}
	desc.product = QString::fromUtf8(&data.constData()[offset], marker - offset);
	offset = marker + 1;

	marker = data.indexOf(peerDescriptionSeparator, offset);
	if (marker == -1) {
		qWarning() << "No manufacturer in peer description";
		return desc;
	}
	desc.manufacturer = QString::fromUtf8(&data.constData()[offset], marker - offset);
	offset = marker + 1;

	marker = data.indexOf(peerDescriptionSeparator, offset);
	if (marker == -1) {
		qWarning() << "No name in peer description";
		return desc;
	}
	desc.name = QString::fromUtf8(&data.constData()[offset], marker - offset);
	offset = marker + 1;

	marker = data.indexOf(peerDescriptionSeparator, offset);
	if (marker == -1) {
		qWarning() << "No profile in peer description";
		return desc;
	}
	desc.profile = QString::fromUtf8(&data.constData()[offset], marker - offset);
	offset = marker + 1;

	return desc;
}

QByteArray SAProtocol::packPeerDescription(const PeerDescription &desc)
{
	QByteArray data;

	switch (desc.messageType) {
	case 5:
		data.reserve(20 + 4 + desc.product.length() + desc.manufacturer.length() + desc.name.length() + desc.profile.length());
		break;
	case 6:
		data.reserve(21 + 4 + desc.product.length() + desc.manufacturer.length() + desc.name.length() + desc.profile.length());
		break;
	default:
		qWarning() << "Unknown message type:" << desc.messageType;
		return data;
	}

	append<quint8>(data, desc.messageType);

	append<quint16>(data, desc.accessoryProtocolVersion);
	append<quint16>(data, desc.accessorySoftwareVersion);

	if (desc.messageType == 6) {
		append<quint8>(data, desc.status);
		append<quint8>(data, desc.errorCode);
	} else {
		append<quint8>(data, desc.status);
	}

	append<quint32>(data, desc.APDUSize);
	append<quint16>(data, desc.SSDUSize);
	append<quint16>(data, desc.sessions);
	append<quint16>(data, desc.timeout);
	append<quint8>(data, desc.unk_1);
	append<quint16>(data, desc.unk_2);
	append<quint8>(data, desc.unk_3);

	data.append(desc.product.toUtf8());
	data.append(peerDescriptionSeparator);

	data.append(desc.manufacturer.toUtf8());
	data.append(peerDescriptionSeparator);

	data.append(desc.name.toUtf8());
	data.append(peerDescriptionSeparator);

	data.append(desc.profile.toUtf8());
	data.append(peerDescriptionSeparator);

	return data;
}

SAProtocol::SecurityFrame SAProtocol::unpackSecurityFrame(const QByteArray &data)
{
	SecurityFrame sframe;

	int offset = 0;
	sframe.type = static_cast<SecurityFrameType>(read<quint8>(data, offset));
	sframe.authType = read<quint8>(data, offset);

	if (sframe.authType != 0) {
		qWarning() << "TODO Unhandled auth type frame";
		// Those frames may not contain version, data fields from below.
		sframe.version = 0;
		sframe.dataType = 0;
		return sframe;
	}

	sframe.version = read<quint8>(data, offset);
	sframe.dataType = read<quint8>(data, offset);
	int len = read<quint8>(data, offset) - 3;
	sframe.data = data.mid(offset, len);

	if (offset + len < data.size()) {
		qWarning() << "Ignoring trailing data on security frame";
	} else if (offset + len > data.size()) {
		qWarning() << "Security frame too short";
	}

	return sframe;
}

QByteArray SAProtocol::packSecurityFrame(const SAProtocol::SecurityFrame &sframe)
{
	QByteArray data;

	append<quint8>(data, sframe.type);
	append<quint8>(data, sframe.authType);

	switch (sframe.authType) {
	case 0:
		append<quint8>(data, sframe.version);
		append<quint8>(data, sframe.dataType);
		append<quint8>(data, sframe.data.length() + 3); // Includes header size

		data.append(sframe.data);
		break;
	default:
		qWarning() << "TODO Unhandled auth type frame";
		break;
	}

	return data;
}

SAProtocol::Frame SAProtocol::unpackFrame(const QByteArray &data)
{
	Q_ASSERT(data.size() > 2);

	Frame frame;

	frame.protocolVersion = (data[0] & 0xE0) >> 5;
	if (frame.protocolVersion != currentProtocolVersion) {
		qWarning() << "Unknown protocol version: " << frame.protocolVersion;
		return frame;
	}

	frame.type = static_cast<FrameType>((data[0] & 0x10) >> 4);
	frame.sessionId = ((data[0] & 0xF) << 6) | ((data[1] & 0xFC) >> 2);

	frame.data = data.mid(2);

	return frame;
}

QByteArray SAProtocol::packFrame(const Frame &frame)
{
	char header[2];

	header[0] = (frame.protocolVersion << 5) & 0xE0;
	header[0] |= (frame.type << 4) & 0x10;
	header[0] |= ((frame.sessionId & 0x3C0) >> 6) & 0xF;
	header[1] = ((frame.sessionId & 0x3F) << 2) & 0xFC;

	QByteArray data = frame.data;
	return data.prepend(reinterpret_cast<char*>(&header), 2);
}

QByteArray SAProtocol::packFrame(quint16 sessionId, const QByteArray &data, FrameType type)
{
	Frame frame;
	frame.protocolVersion = currentProtocolVersion;
	frame.type = type;
	frame.sessionId = sessionId;
	frame.data = data;
	return packFrame(frame);
}

SAProtocol::DataFrame SAProtocol::unpackDataFrame(const QByteArray &data, bool withSeqNum, bool withFragStatus)
{
	DataFrame frame;
	int offset = 0;
	frame.withSeqNum = withSeqNum;
	frame.withFragStatus = withFragStatus;
	if (withSeqNum) {
		frame.seqNum = read<quint16>(data, offset);
	}
	if (withFragStatus) {
		frame.fragStatus = static_cast<SAProtocol::FragmentStatus>(read<quint8>(data, offset));
	}
	frame.data = data.mid(offset);
	return frame;
}

QByteArray SAProtocol::packDataFrame(const DataFrame &frame)
{
	QByteArray data;
	if (frame.withSeqNum) {
		append<quint16>(data, frame.seqNum);
	}
	if (frame.withFragStatus) {
		append<quint8>(data, frame.fragStatus);
	}
	data.append(frame.data);
	return data;
}

SAProtocol::ControlFrame SAProtocol::unpackControlFrame(const QByteArray &data)
{
	ControlFrame frame;
	int offset = 0;
	int num_ranges;

	frame.type = static_cast<ControlFrameType>(read<quint8>(data, offset));

	switch (frame.type) {
	case ControlFrameImmediateAck:
	case ControlFrameBlockAck:
		frame.seqNum = read<quint16>(data, offset);
		break;
	case ControlFrameNak:
		num_ranges = read<quint8>(data, offset);
		frame.seqNums.reserve(num_ranges);
		for (int i = 0; i < num_ranges; i++) {
			SeqNumRange r;
			r.first = read<quint8>(data, offset);
			r.second = read<quint8>(data, offset);
			frame.seqNums.append(r);
		}
		break;
	default:
		qWarning() << "Unknown frame type";
		break;
	}

	return frame;
}

QByteArray SAProtocol::packControlFrame(const ControlFrame &frame)
{
	QByteArray data;
	append<quint8>(data, frame.type);
	switch (frame.type) {
	case ControlFrameImmediateAck:
	case ControlFrameBlockAck:
		Q_ASSERT(frame.seqNums.empty());
		append<quint16>(data, frame.seqNum);
		break;
	case ControlFrameNak:
		append<quint8>(data, frame.seqNums.size());
		foreach (const SeqNumRange &r, frame.seqNums) {
			append<quint16>(data, r.first);
			append<quint16>(data, r.second);
		}
		break;
	}
	return data;
}

SAProtocol::ServiceConnectionRequestFrame SAProtocol::unpackServiceConnectionRequestFrame(const QByteArray &data)
{
	ServiceConnectionRequestFrame frame;
	int offset = 0;

	frame.messageType = read<quint8>(data, offset);
	frame.acceptorId = read<quint16>(data, offset);
	frame.initiatorId = read<quint16>(data, offset);

	int marker = data.indexOf(';', offset);
	if (marker == -1) {
		qWarning() << "No profile in conn request";
		return frame;
	}

	frame.profile = QString::fromUtf8(&data.constData()[offset], marker - offset);
	offset = marker + 1;

	const int num_sessions = read<quint16>(data, offset);
	frame.sessions.reserve(num_sessions);

	for (int i = 0; i < num_sessions; i++) {
		ServiceConnectionRequestSession session;
		session.sessionId = read<quint16>(data, offset);
		frame.sessions.append(session);
	}

	for (int i = 0; i < num_sessions; i++) {
		ServiceConnectionRequestSession &session = frame.sessions[i];
		session.channelId = read<quint16>(data, offset);
	}

	for (int i = 0; i < num_sessions; i++) {
		ServiceConnectionRequestSession &session = frame.sessions[i];
		session.qosType = read<quint8>(data, offset);
		session.qosDataRate = read<quint8>(data, offset);
		session.qosPriority = read<quint8>(data, offset);
	}

	for (int i = 0; i < num_sessions; i++) {
		ServiceConnectionRequestSession &session = frame.sessions[i];
		session.payloadType = read<quint8>(data, offset);
	}

	return frame;
}

QByteArray SAProtocol::packServiceConnectionRequestFrame(const ServiceConnectionRequestFrame &frame)
{
	QByteArray data;

	append<quint8>(data, frame.messageType);
	append<quint16>(data, frame.acceptorId);
	append<quint16>(data, frame.initiatorId);

	data.append(frame.profile.toUtf8());
	data.append(';'); // Some kind of terminator

	append<quint16>(data, frame.sessions.size());

	foreach (const ServiceConnectionRequestSession &session, frame.sessions) {
		append<quint16>(data, session.sessionId);
	}

	foreach (const ServiceConnectionRequestSession &session, frame.sessions) {
		append<quint16>(data, session.channelId);
	}

	foreach (const ServiceConnectionRequestSession &session, frame.sessions) {
		append<quint8>(data, session.qosType);
		append<quint8>(data, session.qosDataRate);
		append<quint8>(data, session.qosPriority);
	}

	foreach (const ServiceConnectionRequestSession &session, frame.sessions) {
		append<quint8>(data, session.payloadType);
	}

	return data;
}

SAProtocol::ServiceConnectionResponseFrame SAProtocol::unpackServiceConnectionResponseFrame(const QByteArray &data)
{
	ServiceConnectionResponseFrame frame;
	int offset = 0;

	frame.messageType = read<quint8>(data, offset);
	frame.acceptorId = read<quint16>(data, offset);
	frame.initiatorId = read<quint16>(data, offset);

	int marker = data.indexOf(';', offset);
	if (marker == -1) {
		qWarning() << "No profile in conn response";
		return frame;
	}

	frame.profile = QString::fromUtf8(&data.constData()[offset], marker - offset);
	offset = marker + 1;

	frame.statusCode = read<quint8>(data, offset);

	int num_sessions = read<quint16>(data, offset);

	while (num_sessions > 0) {
		int session = read<quint16>(data, offset);
		frame.sessions.append(session);

		num_sessions--;
	}

	return frame;
}

QByteArray SAProtocol::packServiceConnectionResponseFrame(const ServiceConnectionResponseFrame &frame)
{
	QByteArray data;

	append<quint8>(data, frame.messageType);
	append<quint16>(data, frame.acceptorId);
	append<quint16>(data, frame.initiatorId);

	data.append(frame.profile.toUtf8());
	data.append(';');

	append<quint8>(data, frame.statusCode);

	append<quint16>(data, frame.sessions.size());

	foreach (quint16 session, frame.sessions) {
		append<quint16>(data, session);
	}

	return data;
}

SAProtocol::ServiceTerminationRequestFrame SAProtocol::unpackServiceTerminationRequestFrame(const QByteArray &data)
{
	ServiceTerminationRequestFrame frame;
	int offset = 0;

	frame.messageType = read<quint8>(data, offset);
	frame.acceptorId = read<quint16>(data, offset);
	frame.initiatorId = read<quint16>(data, offset);

	int marker = data.indexOf(';', offset);
	if (marker == -1) {
		qWarning() << "No profile in termination request";
		return frame;
	}

	frame.profile = QString::fromUtf8(&data.constData()[offset], marker - offset);

	return frame;
}

QByteArray SAProtocol::packServiceTerminationRequestFrame(const ServiceTerminationRequestFrame &frame)
{
	QByteArray data;

	append<quint8>(data, frame.messageType);
	append<quint16>(data, frame.acceptorId);
	append<quint16>(data, frame.initiatorId);

	data.append(frame.profile.toUtf8());
	data.append(';');

	return data;
}

SAProtocol::ServiceTerminationResponseFrame SAProtocol::unpackServiceTerminationResponseFrame(const QByteArray &data)
{
	ServiceTerminationResponseFrame frame;
	int offset = 0;

	frame.messageType = read<quint8>(data, offset);
	frame.acceptorId = read<quint16>(data, offset);
	frame.initiatorId = read<quint16>(data, offset);

	int marker = data.indexOf(';', offset);
	if (marker == -1) {
		qWarning() << "No profile in termination response";
		return frame;
	}

	frame.profile = QString::fromUtf8(&data.constData()[offset], marker - offset);
	offset = marker + 1;

	frame.statusCode = read<quint8>(data, offset);

	return frame;
}

QByteArray SAProtocol::packServiceTerminationResponseFrame(const ServiceTerminationResponseFrame &frame)
{
	QByteArray data;

	append<quint8>(data, frame.messageType);
	append<quint16>(data, frame.acceptorId);
	append<quint16>(data, frame.initiatorId);

	data.append(frame.profile.toUtf8());
	data.append(';');

	append<quint8>(data, frame.statusCode);

	return data;
}

SAProtocol::CapabilityDiscoveryQuery SAProtocol::unpackCapabilityDiscoveryQuery(const QByteArray &data)
{
	CapabilityDiscoveryQuery msg;
	int offset = 0;

	msg.messageType = static_cast<CapabilityDiscoveryMessageType>(read<quint8>(data, offset));
	msg.queryType = read<quint8>(data, offset);

	msg.checksum = read<quint32>(data, offset);

	int num_records = read<quint8>(data, offset);

	while (num_records > 0) {
		int marker = data.indexOf(';', offset);
		if (marker == -1) {
			qWarning() << "Failure to parse all records!";
			return msg;
		}

		msg.records.append(QString::fromUtf8(&data.constData()[offset], marker - offset));
		offset = marker + 1;

		num_records--;
	}

	return msg;
}

QByteArray SAProtocol::packCapabilityDiscoveryQuery(const CapabilityDiscoveryQuery &msg)
{
	QByteArray data;

	append<quint8>(data, msg.messageType);
	append<quint8>(data, msg.queryType);
	append<quint32>(data, msg.checksum);

	append<quint8>(data, msg.records.size());

	foreach (const QString &record, msg.records) {
		data.append(record.toUtf8());
		data.append(';');
	}

	return data;
}

SAProtocol::CapabilityDiscoveryResponse SAProtocol::unpackCapabilityDiscoveryResponse(const QByteArray &data)
{
	CapabilityDiscoveryResponse msg;
	int offset = 0;

	msg.messageType = static_cast<CapabilityDiscoveryMessageType>(read<quint8>(data, offset));
	msg.queryType = read<quint8>(data, offset);

	msg.checksum = read<quint32>(data, offset);

	int num_records = read<quint16>(data, offset);

	while (num_records > 0) {
		CapabilityDiscoveryProvider provider;

		provider.uuid = read<quint16>(data, offset);

		int marker = data.indexOf(';', offset);
		if (marker == -1) {
			qWarning() << "Failure to parse all providers!";
			return msg;
		}

		provider.name = QString::fromUtf8(&data.constData()[offset], marker - offset);
		offset = marker + 1;

		int num_services = read<quint16>(data, offset);

		while (num_services > 0) {
			CapabilityDiscoveryService service;

			service.componentId = read<quint16>(data, offset);

			marker = data.indexOf(';', offset);
			if (marker == -1) {
				qWarning() << "Failure to parse all services!";
				return msg;
			}

			service.profile = QString::fromUtf8(&data.constData()[offset], marker - offset);
			offset = marker + 1;

			service.aspVersion = read<quint16>(data, offset);
			service.role = read<quint8>(data, offset);
			service.connTimeout = read<quint16>(data, offset);

			provider.services.append(service);
			num_services--;
		}

		msg.providers.append(provider);
		num_records--;
	}

	return msg;
}

QByteArray SAProtocol::packCapabilityDiscoveryResponse(const CapabilityDiscoveryResponse &msg)
{
	QByteArray data;

	append<quint8>(data, msg.messageType);
	append<quint8>(data, msg.queryType);
	append<quint32>(data, msg.checksum);

	append<quint16>(data, msg.providers.size());

	foreach (const CapabilityDiscoveryProvider &provider, msg.providers) {
		append<quint16>(data, provider.uuid);
		data.append(provider.name.toUtf8());
		data.append(';');
		append<quint16>(data, provider.services.size());

		foreach (const CapabilityDiscoveryService &service, provider.services) {
			append<quint16>(data, service.componentId);
			data.append(service.profile.toUtf8());
			data.append(';');
			append<quint16>(data, service.aspVersion);
			append<quint8>(data, service.role);
			append<quint16>(data, service.connTimeout);
		}
	}

	return data;
}