range-testing/helper_functions.py

import json
import math
import re


def calculate_distance(point1: dict, point2: dict) -> float:
    """
    Calculate the distance in meters between two geographic points with altitude.

    :param point1: A dictionary with 'latitude', 'longitude', and optional 'altitude'.
    :param point2: A dictionary with 'latitude', 'longitude', and optional 'altitude'.
    :return: Distance in meters as a float.
    """
    try:
        # Extract latitude, longitude, and altitude
        lat1, lon1, alt1 = (
            point1["latitude"],
            point1["longitude"],
            point1.get("altitude", 0),
        )
        lat2, lon2, alt2 = (
            point2["latitude"],
            point2["longitude"],
            point2.get("altitude", 0),
        )
    except KeyError:
        return "Points were not properly set"

    # Convert latitude and longitude from degrees to radians
    lat1, lon1, lat2, lon2 = map(math.radians, [lat1, lon1, lat2, lon2])

    # Radius of the Earth in meters
    R = 6369500

    # Compute deltas
    delta_lat = lat2 - lat1
    delta_lon = lon2 - lon1

    # Haversine formula for horizontal distance
    a = (
        math.sin(delta_lat / 2) ** 2
        + math.cos(lat1) * math.cos(lat2) * math.sin(delta_lon / 2) ** 2
    )
    c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
    horizontal_distance = R * c

    # Altitude difference
    delta_alt = alt2 - alt1

    # Total 3D distance
    distance = math.sqrt(horizontal_distance**2 + delta_alt**2)

    return distance


# Split string by commas, if there are commas
def comma_split(string: str):
    if "," in string:
        return string.split(",")
    else:
        return string


# Extract stats from strings
def extract_numbers(string):
    pattern = r"-?\d+\.\d+|-?\d+"
    numbers = re.findall(pattern, string)
    numbers = [float(num) if "." in num else int(num) for num in numbers]
    return numbers


# Parse the latitude or longitude from the NMEA format
def parse_lat_lon(value: str, direction: str) -> float:
    """
    Parses latitude or longitude from NMEA format.

    Args:
        value (str): The raw NMEA latitude or longitude value.
        direction (str): Direction indicator ('N', 'S', 'E', 'W').

    Returns:
        float: The parsed latitude or longitude as a decimal degree.
    """
    if not value or not direction:
        return None

    # Latitude has 2° digits, longitude has 3° digits
    if direction in ["N", "S"]:
        deg_len = 2
    elif direction in ["E", "W"]:
        deg_len = 3
    else:
        return None  # Invalid direction

    degrees = int(value[:deg_len])
    minutes = float(value[deg_len:])
    decimal = degrees + minutes / 60

    if direction in ["S", "W"]:
        decimal = -decimal
    return decimal


# Convert RSSI to DBM
def rssi_to_dbm(rssi):
    if rssi == 99:
        return "Unknown"
    else:
        # Convert RSSI to dBm
        return -113 + 2 * rssi


def normalize(value: float, unit: str) -> float:
    """Convert all rates to Mbits/sec."""
    if unit.lower().startswith("kbit"):
        return value / 1000.0
    return value


# Save data to JSON file
def save_data_to_json(data, filename):
    try:
        # Load existing data
        try:
            with open(filename, "r") as file:
                existing_data = json.load(file)
        except FileNotFoundError:
            # If file doesn't exist, start with an empty list
            existing_data = []

        # Append new data
        existing_data.append(data)

        # Save updated data back to the file
        with open(filename, "w") as file:
            json.dump(existing_data, file, indent=4)
    except Exception as e:
        print(f"Error saving data to JSON: {e}")


def fix_long(bad_long):
    """
    Fix longitude values incorrectly parsed from NMEA (leading zero dropped degrees).

    Assumes all values should be around -79.x based on recording location.
    Removes the spurious leading digit from minutes.
    """
    broken_degrees = 7
    # Recover "minutes" from the broken decimal
    extended_minutes = (abs(bad_long) - broken_degrees) * 60

    # Remove the extra leading digit (e.g. 920 -> 20)
    minutes = extended_minutes % 100

    corrected_degrees = 79
    decimal = corrected_degrees + minutes / 60

    if bad_long < 0:
        decimal = -decimal

    return decimal


def add_distance_after(filename: str, base_location: dict):
    try:
        with open(filename, "r") as file:
            data = json.load(file)
    except FileNotFoundError:
        print("No file by that name")
        return None

    distance = "Unknown"

    for dictionary in data:
        if "latitude" in dictionary and type(dictionary["latitude"]) == float:
            distance = calculate_distance(base_location, dictionary)
            dictionary["distance"] = distance
        elif "iperf_full" in dictionary:
            dictionary["start_distance"] = distance

    # Save updated data back to the file
    with open(filename, "w") as file:
        json.dump(data, file, indent=4)


if __name__ == "__main__":
    filenames = [
        "/home/madrigal/repos/range-testing/data/boat_relay_sept_18/test_1758215714_copy.json",
        "/home/madrigal/repos/range-testing/data/boat_relay_sept_18/test_1758217711_copy.json",
        "/home/madrigal/repos/range-testing/data/boat_relay_sept_18/test_1758219350_copy.json",
    ]
    base_location = {
        "latitude": 43.656328,
        "longitude": -79.307884,
        "altitude": 80,
    }
    for filename in filenames:
        add_distance_after(filename=filename, base_location=base_location)