From a5d6ff9be734007a065ebbf056fe1466ea23f6fe Mon Sep 17 00:00:00 2001
From: Luca Jost <luca-jost@hotmail.com>
Date: Sun, 22 Sep 2024 22:23:58 +1200
Subject: [PATCH] [GS] Generate flight statistics

---
 .../src/logging/flightStatistics.hpp          | 170 ++++++++++++++++++
 1 file changed, 170 insertions(+)
 create mode 100644 ground_station/src/logging/flightStatistics.hpp

diff --git a/ground_station/src/logging/flightStatistics.hpp b/ground_station/src/logging/flightStatistics.hpp
new file mode 100644
index 00000000..8f445eef
--- /dev/null
+++ b/ground_station/src/logging/flightStatistics.hpp
@@ -0,0 +1,170 @@
+/// Copyright (C) 2020, 2024 Control and Telemetry Systems GmbH
+///
+/// SPDX-License-Identifier: GPL-3.0-or-later
+
+#pragma once
+
+#include "console.hpp"
+#include "utils.hpp"
+
+#include <cstring>
+#include <string>
+#include <vector>
+
+struct UnPackedData {
+  uint8_t state;
+  uint16_t timestamp;
+  uint8_t errors;
+  int32_t lat;
+  int32_t lon;
+  int32_t altitude;
+  int16_t velocity;
+  uint16_t voltage;
+  uint8_t pyro_continuity;
+  bool testing_mode;
+};
+
+class FlightStatistics {
+ public:
+  FlightStatistics() = default;
+
+  void parseFlightLog(const char* directory, const char* name, uint8_t linkSource) {
+    if (!fatfs.chdir(directory)) {
+      console.warning.print("[REC] Open directory failed");
+      console.warning.println(directory);
+      return;
+    }
+
+    if ((linkSource < 1U) || (linkSource > 2U)) {
+      console.warning.println("Invalid link source");
+      return;
+    }
+
+    // NOLINTNEXTLINE(cppcoreguidelines-init-variables) something is wrong with fatfs
+    File file = fatfs.open(name);
+
+    uint16_t count = 0;
+    char line[128];
+    file.open(&fatfs, name, FILE_READ);
+
+    while (file.available()) {
+      size_t size = file.readBytesUntil('\n', line, 128);
+      if (size > 0 && count > 0) {
+        line[size] = '\0';
+
+        std::string tokens[10] = {};
+        if (!parseLine(line, tokens)) {
+          console.warning.println("Invalid line format");
+          continue;
+        }
+
+        if (std::stoi(tokens[0]) == linkSource) {
+          data.timestamp = std::stoi(tokens[1]);
+          data.state = std::stoi(tokens[2]);
+          data.errors = std::stoi(tokens[3]);
+          data.lat = std::stof(tokens[4]);
+          data.lon = std::stof(tokens[5]);
+          data.altitude = std::stof(tokens[6]);
+          data.velocity = std::stof(tokens[7]);
+          data.voltage = std::stof(tokens[8]);
+
+          if (stateChanged(data.state)) {
+            // Liftoff
+            if (data.state == 3) {
+              liftoffTimestamp = data.timestamp;
+            }
+            // Apogee
+            if (data.state == 5) {
+              timeToApogeeSeconds = static_cast<float>(data.timestamp - liftoffTimestamp) / 10.0F;
+            }
+            // Main
+            if (data.state == 6) {
+              timeToMainSeconds = static_cast<float>(data.timestamp - liftoffTimestamp) / 10.0F;
+              mainDeployAltitude = data.altitude;
+            }
+          }
+          if (data.altitude > maxAltitude) {
+            maxAltitude = data.altitude;
+          }
+          if (data.velocity > maxVelocity) {
+            maxVelocity = data.velocity;
+          }
+
+          flightTimeRaw = data.timestamp - liftoffTimestamp;
+
+          lastLatitude = data.lat;
+          lastLongitude = data.lon;
+        }
+      }
+      count++;
+    }
+    flightTimeSeconds = static_cast<float>(flightTimeRaw) / 10.0F;
+    lastReportedAltitude = data.altitude;
+    file.close();
+  }
+
+  int32_t getMaxAltitude() const { return maxAltitude; }
+
+  float getTimeToApogee() const { return timeToApogeeSeconds; }
+
+  int32_t getMaxVelocity() const { return maxVelocity; }
+
+  float getLastLatitude() const { return static_cast<float>(lastLatitude) / 10000.0F; }
+
+  float getLastLongitude() const { return static_cast<float>(lastLongitude) / 10000.0F; }
+
+  float getFlightTime() const { return flightTimeSeconds; }
+
+  float getDrogueDescentRate() const {
+    return static_cast<float>(maxAltitude - mainDeployAltitude) / (timeToMainSeconds - timeToApogeeSeconds);
+  }
+
+  float getMainDescentRate() const {
+    return static_cast<float>(mainDeployAltitude - lastReportedAltitude) / (flightTimeSeconds - timeToMainSeconds);
+  }
+
+ private:
+  bool parseLine(const char* line, std::string tokens[10]) {
+    const char* start = line;
+    const char* end = strchr(start, ',');
+    int tokenIndex = 0;
+
+    while (end != nullptr && tokenIndex < 10) {
+      tokens[tokenIndex] = std::string(start, end - start);
+      tokenIndex++;
+      start = end + 1;
+      end = strchr(start, ',');
+    }
+
+    console.log.println(tokenIndex);
+    if (tokenIndex != 10) {
+      return false;
+    }
+
+    return true;
+  }
+
+  bool stateChanged(uint8_t state) {
+    if (state != previousState) {
+      previousState = state;
+      return true;
+    }
+    return false;
+  }
+
+  UnPackedData data{};
+
+  uint8_t previousState = 0;
+
+  uint32_t liftoffTimestamp = 0;
+  uint32_t flightTimeRaw = 0;
+  float timeToApogeeSeconds = 0.0F;
+  float timeToMainSeconds = 0.0F;
+  float flightTimeSeconds = 0.0F;
+  int32_t maxAltitude = 0;
+  int32_t lastReportedAltitude = 0;
+  int32_t maxVelocity = 0;
+  int32_t lastLatitude = 0;
+  int32_t lastLongitude = 0;
+  int32_t mainDeployAltitude = 0;
+};