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 <random>

#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";
const std::string NUC5 = "d0:c6:37:bc:5c:41";

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::test() {

}

bool Manager::trigger() {
    if (_isRunning) {
        stop();
    } else {
        start();
    }

    return _isRunning;
}

void Manager::start() {
    if (_logToDisk) {
        QString folder = QStandardPaths::writableLocation(QStandardPaths::DownloadLocation) + "/ftm/";
        QString prefix = getCurrentTimeForFileName();

        ftmLogger = std::make_shared<QFile>(folder+"/"+prefix+"_ftm.txt");

        if (!ftmLogger->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 (!ftmLogger->open(QIODevice::ReadWrite)) {
            qWarning() << "Failed to create data logger file" << ftmLogger->fileName();
            ftmLogger = nullptr;
        }

        // write header
        if (ftmLogger) {
            QTextStream out(ftmLogger.get());
            out << "timeInMS;" << "MAC;" << "NucIdx;" << "Success;"
                << "DistMM;" << "DistStdDevMM;" << "Rssi;" << "NumSuccessfullMeas;" << "NumAttemptedMeas"
                << endl;
        }

    } else {
        ftmLogger = nullptr;
    }

    startTime = nowInMsec();


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

    _isRunning = true;
    emit isRunningChanged();
#else
    std::random_device rd;
    std::mt19937 gen{rd()};
    std::normal_distribution<float> d{5000,1000};
    //std::uniform_real<float> d{0, 500};

    WifiRttResult res;
    res.success = 1;
    res.mac = NUC1;
    res.numAttemptedMeas = 8;
    res.numSuccessfullMeas = 8;

    for(int n=0; n<1000; ++n) {
        res.timeMS = nowInMsec();
        res.distMM = static_cast<int>(std::round(d(gen)));

        onWifiData(res);
    }

    stop();
#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

    if (ftmLogger) {
        ftmLogger->flush();
        ftmLogger->close();
        ftmLogger = nullptr;
    }

    _isRunning = false;
    emit isRunningChanged();
}

int Manager::runTimeInMs() const {
    return static_cast<int>(nowInMsec() - startTime);
}

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

void Manager::onWifiData(WifiRttResult result) {
    qDebug() << result;

    if (result.success == 1 && result.numSuccessfullMeas > 3) {
        int nucIdx = 0;

        if(NUC1 == result.mac) {nucIdx = 0;}
        if(NUC2 == result.mac) {nucIdx = 1;}
        if(NUC3 == result.mac) {nucIdx = 2;}
        if(NUC4 == result.mac) {nucIdx = 3;}
        if(NUC5 == result.mac) {nucIdx = 4;}

        emit newDistMeas(nucIdx, result.distMM);

        if (ftmLogger) {
            QTextStream out(ftmLogger.get());

            out << result.timeMS  << ";"
                << QString::fromStdString(result.mac) << ";"
                << nucIdx         << ";"
                << result.success << ";"
                << result.distMM  << ";"
                << result.distStdDevMM << ";"
                << result.rssi << ";"
                << result.numSuccessfullMeas << ";"
                << result.numAttemptedMeas
                << endl;
        }
    }

    if (_maxRuntime != 0 && runTimeInMs() > _maxRuntime) {
        stop();
    }
}

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;
    }
  }

  emit uwbDistChanged();
}

Manager mgmt;

#ifdef ANDROID
extern "C" {

    JNIEXPORT void JNICALL Java_android_net_wifi_RTT_onRttData(JNIEnv* env, jobject jobj, jint success, jbyteArray macString, jlong timeMS, jint distMM, jint distStdDevMM, jint numMeas, jint numSuccessfullMeas, jint rssi) {
        (void) env;
        (void) jobj;

        jsize length = env->GetArrayLength(macString);
        jbyte* data = env->GetByteArrayElements(macString, nullptr);
        std::string str(reinterpret_cast<char*>(data), static_cast<std::string::size_type>(length));
        env->ReleaseByteArrayElements(macString, data, JNI_ABORT);

        WifiRttResult result;
        result.success = success;
        result.mac = str;
        result.timeMS = timeMS;
        result.distMM = distMM;
        result.distStdDevMM = distStdDevMM;
        result.numAttemptedMeas = numMeas;
        result.numSuccessfullMeas = numSuccessfullMeas;
        result.rssi = rssi;

        mgmt.onWifiData(result);
    }

    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, nullptr);
        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