RTT/Manager.cpp

#include "Manager.h"

#ifdef ANDROID
	#include <QtAndroidExtras/QAndroidJniObject>
#endif

#include <iostream>
#include <QDebug>
#include <sstream>
#include <chrono>
#include <iomanip>
#include <thread>>

#include <QStandardPaths>
#include <QtGlobal>
#include <QDir>
#include <QTextStream>


#include "uwb.h"

const std::string NUC1 = "38:de:ad:6d:77:25";
const std::string NUC2 = "38:de:ad:6d:60:ff";
const std::string NUC3 = "1c:1b:b5:ef:a2:9a";
const std::string NUC4 = "1c:1b:b5:ec:d1:82";

static long long startTime = 0;

static QString GetCurrentTimeForFileName()
{
	auto time = std::time(nullptr);
	std::stringstream ss;
    ss << std::put_time(std::localtime(&time), "%Y%m%d_%H%M%S");
	auto s = ss.str();
	std::replace(s.begin(), s.end(), ':', '-');
	return QString::fromStdString(s);
}

static long long nowInMsec()
{
    auto now = std::chrono::system_clock::now();
    auto duration = now.time_since_epoch();
    auto millis = std::chrono::duration_cast<std::chrono::milliseconds>(duration).count();
    return millis;
}

Manager::Manager() {


}


void Manager::trigger() {

    QString folder = QStandardPaths::writableLocation(QStandardPaths::DownloadLocation) + "/ftm/";

    QString prefix = GetCurrentTimeForFileName();

    dataLogger = std::make_shared<QFile>(folder+"/"+prefix+"_ftm.txt");
    uwbLogger  = std::make_shared<QFile>(folder+"/"+prefix+"_uwb.txt");
    gtLogger   = std::make_shared<QFile>(folder+"/"+prefix+"_gt.txt");

	if (!dataLogger->exists()) {
		// create the folder, if necessary
		QDir dir(folder);
		if (!dir.exists()) {
			qWarning("creating new folder");
			if (!dir.mkpath(".")) {
				qWarning() << "Failed to create new folder";
			}
		}
	}

	if (!dataLogger->open(QIODevice::ReadWrite)) {
		qWarning() << "Failed to create data logger file" << dataLogger->fileName();
		dataLogger = nullptr;
	}

    if (!uwbLogger->open(QIODevice::ReadWrite)) {
        qWarning() << "Failed to create uwb data logger file" << uwbLogger->fileName();
        uwbLogger = nullptr;
    }

    if (!gtLogger->open(QIODevice::ReadWrite)) {
        qWarning() << "Failed to create gt data logger file" << gtLogger->fileName();
        gtLogger = nullptr;
    }

    startTime = nowInMsec();

#ifdef ANDROID

    QAndroidJniObject::callStaticMethod<int>("android/net/wifi/UWB", "start", "()I");
	QAndroidJniObject::callStaticMethod<int>("android/net/wifi/RTT", "start", "()I");


#else

	//onData("38:de:ad:6d:77:25;FAILED");
	onData("1337;"+NUC1+";6230;1231");
	onData("1337;"+NUC2+";3430;3423");
	onData("1337;"+NUC3+";5630;2341");
	onData("1337;"+NUC4+";8830;2241");

#endif
}

void Manager::stop() {
#ifdef ANDROID
	QAndroidJniObject::callStaticMethod<int>("android/net/wifi/RTT", "stop", "()I");
    QAndroidJniObject::callStaticMethod<int>("android/net/wifi/UWB", "stop", "()I");
#endif

	dataLogger->flush();
	dataLogger->close();

    uwbLogger->flush();
    uwbLogger->close();

    gtLogger->flush();
    gtLogger->close();
}


void Manager::manualCheckpoint() {
    qDebug() << "Manual checkpoint";

    long long timestamp = nowInMsec();
    timestamp -= startTime;

    QTextStream out(gtLogger.get());

    out << timestamp << endl;
}

void Manager::onData(std::string str) {

	qDebug() << QString(str.c_str());

    long long timestamp = nowInMsec();
    timestamp -= startTime;

    bool successfullMeas = true;
	std::stringstream lineStream(str);
	std::string cell;

	std::string mac;

	int distIndex = 0;
	int i = 0;
	const float alpha = 0.7f;

    QTextStream out(dataLogger.get());

    out << timestamp << ";";

	while (std::getline(lineStream, cell, ';')) {

		switch(i) {

            case 0: {
                // success flag
                successfullMeas = (cell == "1");

                out << (successfullMeas ? 1 : 0) << ";";
                break;
            }
            case 1: {
                // timestamp; ignore;
				break;
			}
            case 2: {
				if(NUC1 == cell) {distIndex = 0;}
				if(NUC2 == cell) {distIndex = 1;}
				if(NUC3 == cell) {distIndex = 2;}
				if(NUC4 == cell) {distIndex = 3;}

                out << QString(cell.c_str()) << ";";
				break;
			}

            case 3: {
                if (successfullMeas) {
                    _dist[distIndex] = std::stoi(cell) + _offset;
                    //_dist[distIndex] = _dist[distIndex] * alpha + atoi(cell.c_str()) * (1-alpha);
				} else {
                    _dist[distIndex] = 0;
				}

                out << _dist[distIndex] << ";";
				break;
			}

            case 4: {
                if (successfullMeas) {
                    _stdDev[distIndex] = atoi(cell.c_str());
                } else {
                    _stdDev[distIndex] = 0;
                }

                out << _stdDev[distIndex] << ";";
				break;
			}

            case 5: {
				// RSSI

                out << cell.c_str() << ";";
                break;
			}
		}
		++i;
	}

    out << endl;


//    if (dataLogger && successfullMeas) {
//        dataLogger->write(str.c_str());
//        dataLogger->write("\n");
//    }

	emit distChanged();
}

void Manager::onUWBData(std::vector<uchar> data) {
  qDebug() << "Received uwb data " << data;

  long long timestamp = nowInMsec();
  timestamp -= startTime;

  UwbResult uwbResult = uwb_parse(data.data());

  _uwbDist[0] = 0;
  _uwbDist[1] = 0;
  _uwbDist[2] = 0;
  _uwbDist[3] = 0;

  for (UwbDistance& dist : uwbResult.distances) {
    qDebug() << dist.nodeID;

    switch (dist.nodeID) {
        case 0x1D8C: _uwbDist[0] = static_cast<float>(dist.distance); break;
        case 0x47A7: _uwbDist[1] = static_cast<float>(dist.distance); break;
        case 0x863B: _uwbDist[2] = static_cast<float>(dist.distance); break;
        case 0x58B4: _uwbDist[3] = static_cast<float>(dist.distance); break;
    }

    if (uwbLogger) {

        QTextStream out(uwbLogger.get());

        out << timestamp << ";" << "1;" << "DW" << hex << dist.nodeID << dec << ";" << dist.distance << endl;
    }
  }

  emit distChanged();
}

Manager mgmt;

#ifdef ANDROID
extern "C" {

	JNIEXPORT void JNICALL Java_android_net_wifi_RTT_onRTTComplete(JNIEnv* env, jobject jobj, jbyteArray arrayID) {
		(void) env; (void) jobj;
		jsize length = env->GetArrayLength(arrayID);
		jbyte* data = env->GetByteArrayElements(arrayID, 0);
		std::string str((char*)data, length);
		env->ReleaseByteArrayElements(arrayID, data, JNI_ABORT);
		mgmt.onData(str);
	}

    JNIEXPORT void JNICALL Java_android_net_wifi_UWB_onUWBComplete(JNIEnv* env, jobject jobj, jbyteArray arrayID) {
        (void) env; (void) jobj;
        jsize length = env->GetArrayLength(arrayID);
        jbyte* data = env->GetByteArrayElements(arrayID, 0);
        std::vector<uchar> c_data;
        c_data.assign(reinterpret_cast<char*>(data), reinterpret_cast<char*>(data)+length);
        env->ReleaseByteArrayElements(arrayID, data, JNI_ABORT);

        mgmt.onUWBData(c_data);
    }

}
#endif