#include <cmath>
#include <QtCore/QDebug>
#include <QtCore/QDir>
#include <QtCore/QFile>
#include <QtPositioning/QNmeaPositionInfoSource>

#include "nmeasource.h"

NmeaSource::NmeaSource(QObject *parent) :
	QObject(parent)
{
	m_possrc = QGeoPositionInfoSource::createDefaultSource(this);
	m_satsrc = QGeoSatelliteInfoSource::createDefaultSource(this);
	if (m_possrc) {
		connect(m_possrc, SIGNAL(positionUpdated(QGeoPositionInfo)),
				this, SLOT(handlePosition(QGeoPositionInfo)));
	} else {
#ifdef QT_DEBUG
		qDebug() << "Using test GPS source";
		QNmeaPositionInfoSource* nmea = new QNmeaPositionInfoSource(QNmeaPositionInfoSource::SimulationMode, this);
		QFile* file = new QFile(QDir::home().absoluteFilePath("nmea.txt"), this);
		if (file->open(QIODevice::ReadOnly | QIODevice::Text)) {
			nmea->setDevice(file);
			m_possrc = nmea;
			connect(m_possrc, SIGNAL(positionUpdated(QGeoPositionInfo)),
					this, SLOT(handlePosition(QGeoPositionInfo)));
		} else {
			qWarning() << "Could not open test GPS source";
			delete file;
			delete nmea;
		}
#else
		qWarning() << "No valid GPS position source!";
#endif
	}
	if (m_satsrc) {
		connect(m_satsrc, SIGNAL(satellitesInUseUpdated(QList<QGeoSatelliteInfo>)),
				this, SLOT(handleSatsInUse(QList<QGeoSatelliteInfo>)));
		connect(m_satsrc, SIGNAL(satellitesInViewUpdated(QList<QGeoSatelliteInfo>)),
				this, SLOT(handleSatsInView(QList<QGeoSatelliteInfo>)));
	}
}

NmeaSource::~NmeaSource()
{
}

void NmeaSource::start()
{
	m_last_satsinview = 0;
	if (m_possrc) {
		m_possrc->startUpdates();
	}
	if (m_satsrc) {
		m_satsrc->startUpdates();
	}
}

void NmeaSource::stop()
{
	if (m_possrc) {
		m_possrc->stopUpdates();
	}
	if (m_satsrc) {
		m_satsrc->stopUpdates();
	}
}

QString NmeaSource::formatFixMode(const QGeoCoordinate &coord)
{
	switch (coord.type()) {
	case QGeoCoordinate::Coordinate3D:
		return QString::fromLatin1("3");
	case QGeoCoordinate::Coordinate2D:
		return QString::fromLatin1("2");
	default:
		return QString::fromLatin1("1");
	}
}

QString NmeaSource::formatTimestamp(const QTime& time)
{
	return time.toString("HHmmss.zzz");
}

QString NmeaSource::formatDatestamp(const QDate& date)
{
	return date.toString("ddMMyy");
}

QString NmeaSource::formatLatitude(const QGeoCoordinate& coord)
{
	if (coord.isValid()) {
		QLatin1Char fill('0');
		double degrees;
		double minutes = modf(fabs(coord.latitude()), &degrees) * 60;
		return QString::fromLatin1("%1%2")
				.arg(static_cast<int>(degrees), 2, 10, fill)
				.arg(minutes, 7, 'f', 4, fill);
	} else {
		return QString();
	}
}

QString NmeaSource::formatLatitudeNS(const QGeoCoordinate& coord)
{
	if (coord.isValid()) {
		if (coord.latitude() >= 0.0) {
			return QLatin1String("N");
		} else {
			return QLatin1String("S");
		}
	} else {
		return QString();
	}
}

QString NmeaSource::formatLongitude(const QGeoCoordinate& coord)
{
	if (coord.isValid()) {
		QLatin1Char fill('0');
		double degrees;
		double minutes = modf(fabs(coord.longitude()), &degrees) * 60;
		return QString::fromLatin1("%1%2")
				.arg(static_cast<int>(degrees), 3, 10, fill)
				.arg(minutes, 7, 'f', 4, fill);
	} else {
		return QString();
	}
}

QString NmeaSource::formatLongitudeEW(const QGeoCoordinate& coord)
{
	if (coord.isValid()) {
		if (coord.longitude() >= 0.0) {
			return QString::fromLatin1("E");
		} else {
			return QString::fromLatin1("W");
		}
	} else {
		return QString();
	}
}

QString NmeaSource::formatAltitude(const QGeoCoordinate& coord)
{
	if (coord.type() == QGeoCoordinate::Coordinate3D) {
		return QString::number(coord.altitude(), 'f', 1);
	} else {
		return QString();
	}
}

QString NmeaSource::formatAltitudeUnits(const QGeoCoordinate& coord)
{
	if (coord.type() == QGeoCoordinate::Coordinate3D) {
		return QString::fromLatin1("M");
	} else {
		return QString();
	}
}

QString NmeaSource::formatSpeedKnots(const QGeoPositionInfo& pos)
{
	if (pos.hasAttribute(QGeoPositionInfo::GroundSpeed)) {
		double speed = pos.attribute(QGeoPositionInfo::GroundSpeed); // [m/s]
		speed *= 1.943844; // [knots]
		return QString::number(speed, 'f', 1);
	} else {
		return QString();
	}
}

QString NmeaSource::formatSpeedKmH(const QGeoPositionInfo& pos)
{
	if (pos.hasAttribute(QGeoPositionInfo::GroundSpeed)) {
		double speed = pos.attribute(QGeoPositionInfo::GroundSpeed); // [m/s]
		speed *= 3.6; // [km/h]
		return QString::number(speed, 'f', 1);
	} else {
		return QString();
	}
}

QString NmeaSource::formatTrueCourse(const QGeoPositionInfo& pos)
{
	if (pos.hasAttribute(QGeoPositionInfo::Direction)) {
		double course = pos.attribute(QGeoPositionInfo::Direction); // [deg]
		return QString::number(course, 'f', 1);
	} else {
		return QString();
	}
}

QString NmeaSource::formatMagCourse(const QGeoPositionInfo& pos)
{
	if (pos.hasAttribute(QGeoPositionInfo::Direction)
		&& pos.hasAttribute(QGeoPositionInfo::MagneticVariation)) {
		double course = pos.attribute(QGeoPositionInfo::Direction); // [deg]
		double magvar = pos.attribute(QGeoPositionInfo::MagneticVariation); // [+/-deg]
		return QString::number(course + magvar, 'f', 1);
	} else {
		return QString();
	}
}

QString NmeaSource::formatMagVariation(const QGeoPositionInfo& pos)
{
	if (pos.hasAttribute(QGeoPositionInfo::MagneticVariation)) {
		double var = pos.attribute(QGeoPositionInfo::MagneticVariation); // [+/-deg]
		return QString::number(fabs(var), 'f', 1);
	} else {
		return QString();
	}
}

QString NmeaSource::formatMagVariationEW(const QGeoPositionInfo& pos)
{
	if (pos.hasAttribute(QGeoPositionInfo::MagneticVariation)) {
		if (pos.attribute(QGeoPositionInfo::MagneticVariation) >= 0.0) {
			return QString::fromLatin1("E");
		} else {
			return QString::fromLatin1("W");
		}
	} else {
		return QString();
	}
}

QString NmeaSource::formatHDOP(const QGeoPositionInfo& pos)
{
	if (pos.hasAttribute(QGeoPositionInfo::HorizontalAccuracy)) {
		double eph = pos.attribute(QGeoPositionInfo::HorizontalAccuracy); // [m]
		return QString::number(eph / H_UERE, 'f', 1);
	} else {
		return QString();
	}
}

QString NmeaSource::formatVDOP(const QGeoPositionInfo& pos)
{
	if (pos.hasAttribute(QGeoPositionInfo::VerticalAccuracy)) {
		double epv = pos.attribute(QGeoPositionInfo::VerticalAccuracy); // [m]
		return QString::number(epv / V_UERE, 'f', 1);
	} else {
		return QString();
	}
}

QString NmeaSource::formatPDOP(const QGeoPositionInfo& pos)
{

	if (pos.hasAttribute(QGeoPositionInfo::HorizontalAccuracy)
		&& pos.hasAttribute(QGeoPositionInfo::VerticalAccuracy)) {
		double eph = pos.attribute(QGeoPositionInfo::HorizontalAccuracy); // [m]
		double epv = pos.attribute(QGeoPositionInfo::VerticalAccuracy); // [m]
		double epe = sqrt(eph * eph + epv * epv);
		return QString::number(epe / P_UERE, 'f', 1);
	} else {
		return QString();
	}
}

QString NmeaSource::formatError(const QGeoPositionInfo& pos,
						   PositionAccuracyDirection which)
{
	switch (which) {
	case Spherical:
		if (pos.hasAttribute(QGeoPositionInfo::HorizontalAccuracy)
			&& pos.hasAttribute(QGeoPositionInfo::VerticalAccuracy)) {
			double eph = pos.attribute(QGeoPositionInfo::HorizontalAccuracy); // [m]
			double epv = pos.attribute(QGeoPositionInfo::VerticalAccuracy); // [m]
			double epe = sqrt(eph * eph + epv * epv);
			return QString::number(epe, 'f', 1);
		} else {
			return QString();
		}
		break;
	case Horizontal:
		if (pos.hasAttribute(QGeoPositionInfo::HorizontalAccuracy)) {
			double eph = pos.attribute(QGeoPositionInfo::HorizontalAccuracy); // [m]
			return QString::number(eph, 'f', 1);
		} else {
			return QString();
		}
		break;
	case Vertical:
		if (pos.hasAttribute(QGeoPositionInfo::VerticalAccuracy)) {
			double epv = pos.attribute(QGeoPositionInfo::VerticalAccuracy); // [m]
			return QString::number(epv, 'f', 1);
		} else {
			return QString();
		}
		break;
	}
	return QString();
}

QString NmeaSource::formatErrorUnits(const QGeoPositionInfo& pos,
								PositionAccuracyDirection which)
{
	switch (which) {
	case Spherical:
		if (pos.hasAttribute(QGeoPositionInfo::HorizontalAccuracy)
			&& pos.hasAttribute(QGeoPositionInfo::VerticalAccuracy)) {
			return QString::fromLatin1("M");
		} else {
			return QString();
		}
		break;
	case Horizontal:
		if (pos.hasAttribute(QGeoPositionInfo::HorizontalAccuracy)) {
			return QString::fromLatin1("M");
		} else {
			return QString();
		}
		break;
	case Vertical:
		if (pos.hasAttribute(QGeoPositionInfo::VerticalAccuracy)) {
			return QString::fromLatin1("M");
		} else {
			return QString();
		}
		break;
	}
	return QString();
}

QString NmeaSource::formatSatPrn(const QGeoSatelliteInfo &sat)
{
	return QString::fromLatin1("%1").arg(sat.satelliteIdentifier(), 2, 10, QLatin1Char('0'));
}

QString NmeaSource::formatSatElev(const QGeoSatelliteInfo &sat)
{
	if (sat.hasAttribute(QGeoSatelliteInfo::Elevation)) {
		int elv = lrint(sat.attribute(QGeoSatelliteInfo::Elevation));
		return QString::fromLatin1("%1").arg(elv, 2, 10, QLatin1Char('0'));
	} else {
		return QString();
	}
}

QString NmeaSource::formatSatAz(const QGeoSatelliteInfo &sat)
{
	if (sat.hasAttribute(QGeoSatelliteInfo::Azimuth)) {
		int az = lrint(sat.attribute(QGeoSatelliteInfo::Azimuth));
		return QString::fromLatin1("%1").arg(az, 3, 10, QLatin1Char('0'));
	} else {
		return QString();
	}
}
QString NmeaSource::formatSatSnr(const QGeoSatelliteInfo &sat)
{
	int snr = sat.signalStrength();
	if (snr >= 0) {
		return QString::fromLatin1("%1").arg(snr, 2, 10, QLatin1Char('0'));
	} else {
		return QString();
	}
	return QString::fromLatin1("%1").arg(sat.signalStrength(), 2, 10, QLatin1Char('0'));
}

void NmeaSource::emitSentence(const QString &sentence)
{
	QByteArray data = sentence.toLatin1();
	uint checksum = 0;
	foreach (const char& c, data) {
		checksum ^= c;
	}

#if 0
	qDebug("%s", qPrintable(QString::fromLatin1("$%1*%2").arg(sentence).
					   arg(checksum, 2, 16, QLatin1Char('0'))));
#endif

	emit dataReady(QString::fromLatin1("$%1*%2\r\n")
				   .arg(sentence)
				   .arg(checksum, 2, 16, QLatin1Char('0')));
}

void NmeaSource::generateGPRMC(const QGeoPositionInfo &pos)
{
	QString sentence =
		QString::fromLatin1("GPRMC,%1,%2,%3,%4,%5,%6,%7,%8,%9,%10,%11,%12")
			// 1 = UTC of position fix
			.arg(formatTimestamp(pos.timestamp().time()))
			// 2 = Data status (A = Autonomous V = Warning)
			.arg(pos.isValid() ? QLatin1Char('A') : QLatin1Char('V'))
			// 3 = Latitude of fix
			.arg(formatLatitude(pos.coordinate()))
			// 4 = N or S
			.arg(formatLatitudeNS(pos.coordinate()))
			// 5 = Longitude of fix
			.arg(formatLongitude(pos.coordinate()))
			// 6 = E or W
			.arg(formatLongitudeEW(pos.coordinate()))
			// 7 = Speed over ground in knots
			.arg(formatSpeedKnots(pos))
			// 8 = True track made good in degrees
			.arg(formatTrueCourse(pos))
			// 9 = Date Stamp
			.arg(formatDatestamp(pos.timestamp().date()))
			// 10 = Magnetic variation in degrees
			.arg(formatMagVariation(pos))
			// 11 = E or W
			.arg(formatMagVariationEW(pos))
			// 12 = FAA mode indicator (A = Autonomous, N = Not valid)
			.arg(pos.isValid() ? QLatin1Char('A') : QLatin1Char('N'))
			;
	emitSentence(sentence);
}

void NmeaSource::generateGPGGA(const QGeoPositionInfo &pos)
{
	QString sentence =
		QString::fromLatin1("GPGGA,%1,%2,%3,%4,%5,%6,%7,%8,%9,%10,%11,%12,%13,%14")
			// 1 = UTC of position fix
			.arg(formatTimestamp(pos.timestamp().time()))
			// 2 = Latitude of fix
			.arg(formatLatitude(pos.coordinate()))
			// 3 = N or S
			.arg(formatLatitudeNS(pos.coordinate()))
			// 4 = Longitude of fix
			.arg(formatLongitude(pos.coordinate()))
			// 5 = E or W
			.arg(formatLongitudeEW(pos.coordinate()))
			// 6 = Fix quality (0 = invalid, 1 = GPS)
			.arg(pos.isValid() ? QLatin1Char('1') : QLatin1Char('0'))
			// 7 = Number of satellites being tracked
			.arg(m_last_satsinview, 2, 10, QLatin1Char('0'))
			// 8 = Horizontal dillusion of position
			.arg(formatHDOP(pos))
			// 9 = Altitude
			.arg(formatAltitude(pos.coordinate()))
			// 10 = Altitude units
			.arg(formatAltitudeUnits(pos.coordinate()))
			// 11 = Height of geoid (mean sea level) about WGS 84
			.arg(QString()) // TODO We could calculate it. Usefulness debatable.
			// 12 = Units
			.arg(QString())
			// 13 = Time in seconds since last DGPS update
			.arg(QString())
			// 14 = DGPS station ID number
			.arg(QString())
			;
	emitSentence(sentence);
}

void NmeaSource::generateGPGLL(const QGeoPositionInfo &pos)
{
	QString sentence =
		QString::fromLatin1("GPGLL,%1,%2,%3,%4,%5,%6,%7")
			// 1 = Latitude of fix
			.arg(formatLatitude(pos.coordinate()))
			// 2 = N or S
			.arg(formatLatitudeNS(pos.coordinate()))
			// 3 = Longitude of fix
			.arg(formatLongitude(pos.coordinate()))
			// 4 = E or W
			.arg(formatLongitudeEW(pos.coordinate()))
			// 5 = UTC of fix
			.arg(formatTimestamp(pos.timestamp().time()))
			// 6 = Data valid (A = Active, V = Void)
			.arg(pos.isValid() ? QLatin1Char('A') : QLatin1Char('V'))
			// 7 = Mode indicator (A = Autonomous, N = Not valid)
			.arg(pos.isValid() ? QLatin1Char('A') : QLatin1Char('N'))
			;
	emitSentence(sentence);
}

void NmeaSource::generateGPVTG(const QGeoPositionInfo &pos)
{
	QString sentence =
		QString::fromLatin1("GPVTG,%1,%2,%3,%4,%5,%6,%7,%8")
			// 1 = True track made good in degrees
			.arg(formatTrueCourse(pos))
			// 2 = T
			.arg(pos.hasAttribute(QGeoPositionInfo::Direction)
				 ? QString::fromLatin1("T") : QString())
			// 3 = Magnetic track made good
			.arg(formatMagCourse(pos))
			// 4 = M
			.arg((pos.hasAttribute(QGeoPositionInfo::Direction)
				 && pos.hasAttribute(QGeoPositionInfo::MagneticVariation))
					  ? QString::fromLatin1("M") : QString())
			// 5 = Speed over ground in knots
			.arg(formatSpeedKnots(pos))
			// 6 = N
			.arg(pos.hasAttribute(QGeoPositionInfo::GroundSpeed)
				 ? QString::fromLatin1("N") : QString())
			// 7 = Mode indicator (A = Autonomous, N = Not valid)
			.arg(formatSpeedKmH(pos))
			// 8 = K
			.arg(pos.hasAttribute(QGeoPositionInfo::GroundSpeed)
				 ? QString::fromLatin1("K") : QString())
			;
	emitSentence(sentence);
}

void NmeaSource::generatePGRME(const QGeoPositionInfo &pos)
{
	QString sentence =
		QString::fromLatin1("PGRME,%1,%2,%3,%4,%5,%6")
			// 1 = Horizontal error estimate
			.arg(formatError(pos, Horizontal))
			// 2 = Units
			.arg(formatErrorUnits(pos, Horizontal))
			// 3 = Vertical error estimate
			.arg(formatError(pos, Vertical))
			// 4 = Units
			.arg(formatErrorUnits(pos, Vertical))
			// 5 = Spherical error estimate
			.arg(formatError(pos, Spherical))
			// 6 = Units
			.arg(formatErrorUnits(pos, Spherical))
			;
	emitSentence(sentence);
}

void NmeaSource::generateGPGSA(const QGeoPositionInfo& pos, const QList<QGeoSatelliteInfo>& info)
{
	const int n = info.size();
	QString sentence =
		QString::fromLatin1("GPGSA,%1,%2,%3,%4,%5,%6,%7,%8,%9,%10,%11,%12,%13,%14,%15,%16,%17")
			// 1 = Mode (A = Automatic)
			.arg(QLatin1Char('A'))
			// 2 = Fix mode
			.arg(formatFixMode(pos.coordinate()))
			// 3 - 14 = IDs of SVs used in position fix
			.arg(n > 0 ? formatSatPrn(info[0]) : QString())
			.arg(n > 1 ? formatSatPrn(info[1]) : QString())
			.arg(n > 2 ? formatSatPrn(info[2]) : QString())
			.arg(n > 3 ? formatSatPrn(info[3]) : QString())
			.arg(n > 4 ? formatSatPrn(info[4]) : QString())
			.arg(n > 5 ? formatSatPrn(info[5]) : QString())
			.arg(n > 6 ? formatSatPrn(info[6]) : QString())
			.arg(n > 7 ? formatSatPrn(info[7]) : QString())
			.arg(n > 8 ? formatSatPrn(info[8]) : QString())
			.arg(n > 9 ? formatSatPrn(info[9]) : QString())
			.arg(n > 10 ? formatSatPrn(info[10]) : QString())
			.arg(n > 11 ? formatSatPrn(info[11]) : QString())
			// 15 = PDOP
			.arg(formatPDOP(pos))
			// 16 = HDOP
			.arg(formatHDOP(pos))
			// 17 = VDOP
			.arg(formatVDOP(pos))
			;
	emitSentence(sentence);
}

void NmeaSource::generateGPGSV(const QGeoPositionInfo& pos, const QList<QGeoSatelliteInfo>& info)
{
	const int nsats = info.size();
	const int nmsgs = nsats / 4;
	int i;

	Q_UNUSED(pos);

	for (i = 0; i < nmsgs; i++) {
		int s = i * 4; // Start from this satellite index)
		QString sentence =
			QString::fromLatin1("GPGSV,%1,%2,%3,%4,%5,%6,%7,%8,%9,%10,%11,%12,%13,%14,%15,%16,%17,%18,%19")
				// 1 = Number of sentences for full data
				.arg(nmsgs)
				// 2 = Sentence number
				.arg(i + 1)
				// 3 = Total number of satellites in view
				.arg(nsats)
				// 4 - 14 = IDs of SVs used in position fix
				.arg(nsats > s + 0 ? formatSatPrn(info[s + 0]) : QString())
				.arg(nsats > s + 0 ? formatSatElev(info[s + 0]) : QString())
				.arg(nsats > s + 0 ? formatSatAz(info[s + 0]) : QString())
				.arg(nsats > s + 0 ? formatSatSnr(info[s + 0]) : QString())
				.arg(nsats > s + 1 ? formatSatPrn(info[s + 1]) : QString())
				.arg(nsats > s + 1 ? formatSatElev(info[s + 1]) : QString())
				.arg(nsats > s + 1 ? formatSatAz(info[s + 1]) : QString())
				.arg(nsats > s + 1 ? formatSatSnr(info[s + 1]) : QString())
				.arg(nsats > s + 2 ? formatSatPrn(info[s + 2]) : QString())
				.arg(nsats > s + 2 ? formatSatElev(info[s + 2]) : QString())
				.arg(nsats > s + 2 ? formatSatAz(info[s + 2]) : QString())
				.arg(nsats > s + 2 ? formatSatSnr(info[s + 2]) : QString())
				.arg(nsats > s + 3 ? formatSatPrn(info[s + 3]) : QString())
				.arg(nsats > s + 3 ? formatSatElev(info[s + 3]) : QString())
				.arg(nsats > s + 3 ? formatSatAz(info[s + 3]) : QString())
				.arg(nsats > s + 3 ? formatSatSnr(info[s + 3]) : QString())
				;
		emitSentence(sentence);
	}
}

void NmeaSource::handlePosition(const QGeoPositionInfo &pos)
{
	generateGPRMC(pos);

	generateGPGGA(pos);
	generateGPGLL(pos);
	generateGPVTG(pos);
	generatePGRME(pos); //Estimated Position Error

	//PGRMZ = Altitude info, PGRMM = Map datum?
	//generateGPRMB?
	//generateGPBOD?
	//generateGPRTE?

	m_last_pos = pos;
}

void NmeaSource::handleSatsInUse(const QList<QGeoSatelliteInfo> &info)
{
	generateGPGSA(m_last_pos, info);
}

void NmeaSource::handleSatsInView(const QList<QGeoSatelliteInfo> &info)
{
	generateGPGSV(m_last_pos, info);
	m_last_satsinview = info.count();
}