tercom.py

# /tercom.py
import numpy as np
import datetime
import json
import math

# --- Data Loading & Simulation ---

def load_terrain_data(map_file="data/terrain_map.npy", metadata_file="data/map_metadata.json"):
    """
    Loads the terrain map array and its metadata.
    Returns:
        tuple: (terrain_map (2D np.array), metadata (dict))
    """
    try:
        terrain_map = np.load(map_file)
        with open(metadata_file, 'r') as f:
            metadata = json.load(f)
        
        # Basic validation of metadata
        required_keys = ['lat_top', 'lon_left', 'lat_bottom', 'lon_right', 'height', 'width']
        if not all(key in metadata for key in required_keys):
            raise ValueError("Metadata file is missing required keys.")
        if terrain_map.shape != (metadata['height'], metadata['width']):
             raise ValueError(f"Map shape {terrain_map.shape} doesn't match metadata ({metadata['height']}, {metadata['width']})")

        print(f"Loaded terrain map '{map_file}' ({terrain_map.shape})")
        print(f"Map boundaries: Lat ({metadata['lat_top']}, {metadata['lat_bottom']}), Lon ({metadata['lon_left']}, {metadata['lon_right']})")
        return terrain_map, metadata
    except FileNotFoundError:
        print(f"Error: Map file '{map_file}' or metadata file '{metadata_file}' not found.")
        return None, None
    except Exception as e:
        print(f"Error loading terrain data: {e}")
        return None, None

def simulate_observed_profile(terrain_map, metadata, true_y, true_x, length, heading_deg, noise_stddev=1.0):
    """
    Simulates a sensor reading by extracting a profile from the map
    at a 'true' location and adding noise.

    Args:
        terrain_map (np.array): The ground truth map.
        metadata (dict): Map metadata.
        true_y (int): True starting row index on the map.
        true_x (int): True starting column index on the map.
        length (int): Number of points in the profile.
        heading_deg (float): Direction of flight path in degrees (0=North, 90=East).
        noise_stddev (float): Standard deviation of Gaussian noise to add.

    Returns:
        np.array: 1D array of observed altitudes, or None if path goes off map.
    """
    map_height, map_width = terrain_map.shape
    profile = []
    rad_heading = math.radians(90 - heading_deg) # Convert compass heading to math angle (0=East)

    # Calculate grid spacing (approximate)
    # meters_per_lat = 111132.954 - 559.822 * math.cos(2 * math.radians((metadata['lat_top'] + metadata['lat_bottom'])/2)) # Simplified
    # meters_per_lon = 111319.488 * math.cos(math.radians((metadata['lat_top'] + metadata['lat_bottom'])/2)) # Simplified
    # dy_meters = (metadata['lat_top'] - metadata['lat_bottom']) * meters_per_lat / map_height
    # dx_meters = (metadata['lon_right'] - metadata['lon_left']) * meters_per_lon / map_width
    # For simplicity in demo, assume grid cells are roughly square or step size is 1 grid cell per point
    step_size = 1.0 # Sample one map point per profile point

    current_y, current_x = float(true_y), float(true_x)

    for i in range(length):
        y_idx, x_idx = int(round(current_y)), int(round(current_x))

        # Check boundaries
        if not (0 <= y_idx < map_height and 0 <= x_idx < map_width):
            print(f"Warning: Simulated path went off map at step {i} ({y_idx}, {x_idx}). Returning partial profile.")
            break # Or return None if strict boundary checking is needed

        profile.append(terrain_map[y_idx, x_idx])

        # Move to the next point along the heading
        current_y -= step_size * math.sin(rad_heading) # Y decreases upwards in image/array coords
        current_x += step_size * math.cos(rad_heading)

    if not profile:
        return None

    observed = np.array(profile)
    noise = np.random.normal(0, noise_stddev, observed.shape)
    return observed + noise

# --- TERCOM Matching ---

def normalize_profile(profile):
    """ Subtract mean to make matching robust to absolute altitude errors. """
    mean_alt = np.mean(profile)
    return profile - mean_alt

def calculate_mse(profile1, profile2):
    """ Calculates Mean Squared Error between two profiles. """
    if profile1.shape != profile2.shape:
        raise ValueError("Profiles must have the same shape for MSE calculation.")
    return np.mean((profile1 - profile2)**2)

def find_best_match(terrain_map, observed_profile, possible_headings_deg=None):
    """
    Searches the terrain map for the best match of the observed profile.

    Args:
        terrain_map (np.array): The 2D ground truth altitude map.
        observed_profile (np.array): The 1D observed altitude profile.
        possible_headings_deg (list, optional): List of headings (degrees) to check.
                                                 If None, assumes heading is implicitly known
                                                 and matches only along map axes (0, 90, 180, 270).
                                                 For a full search, provide a range like np.arange(0, 360, 10).

    Returns:
        tuple: (best_y, best_x, best_heading, min_error) or (None, None, None, float('inf')) if no match found.
               y, x are the *start* indices of the best matching profile on the map.
    """
    map_height, map_width = terrain_map.shape
    profile_len = len(observed_profile)

    if profile_len == 0:
        print("Error: Observed profile is empty.")
        return None, None, None, float('inf')

    if profile_len > min(map_height, map_width): # Basic check, more complex for diagonal paths
         print("Warning: Profile length might be too large for the map dimensions.")
         # Consider adding more robust checks based on headings

    # Normalize the observed profile once
    norm_observed = normalize_profile(observed_profile)

    min_error = float('inf')
    best_y, best_x, best_heading = None, None, None

    if possible_headings_deg is None:
        # Simplified: Assume we only check horizontal/vertical paths for demo
        possible_headings_deg = [0, 90, 180, 270]
        print("Performing simplified search (headings: 0, 90, 180, 270)")
    else:
        print(f"Searching headings: {possible_headings_deg}")


    # Iterate through possible start positions and headings
    for heading_deg in possible_headings_deg:
        rad_heading = math.radians(90 - heading_deg) # Math angle
        dy = -math.sin(rad_heading) # Step change in y per profile point
        dx = math.cos(rad_heading)  # Step change in x per profile point

        # Determine search bounds to keep the *entire* profile on the map
        # These are approximate bounds, could be tighter for diagonal paths
        max_y_offset = abs(int(round((profile_len - 1) * dy)))
        max_x_offset = abs(int(round((profile_len - 1) * dx)))

        # Iterate through possible starting cells (y, x)
        for y_start in range(map_height):
            for x_start in range(map_width):
                # Pre-check if the path *might* go off bounds (quick check)
                y_end_approx = y_start + (profile_len - 1) * dy
                x_end_approx = x_start + (profile_len - 1) * dx
                if not (0 <= y_end_approx < map_height and 0 <= x_end_approx < map_width):
                    continue # Skip if the end point seems out of bounds

                # Extract the profile from the map for this position and heading
                map_profile_points = []
                valid_path = True
                for i in range(profile_len):
                    current_y = y_start + i * dy
                    current_x = x_start + i * dx
                    y_idx, x_idx = int(round(current_y)), int(round(current_x))

                    if not (0 <= y_idx < map_height and 0 <= x_idx < map_width):
                        valid_path = False
                        break # This path goes off map
                    map_profile_points.append(terrain_map[y_idx, x_idx])
                
                if not valid_path or len(map_profile_points) != profile_len:
                    continue # Skip incomplete paths

                map_profile = np.array(map_profile_points)
                norm_map_profile = normalize_profile(map_profile)

                # Calculate error (MSE)
                error = calculate_mse(norm_observed, norm_map_profile)

                # Update best match if this one is better
                if error < min_error:
                    min_error = error
                    best_y, best_x = y_start, x_start
                    best_heading = heading_deg
                    # print(f"New best match: ({best_y}, {best_x}), Head: {best_heading}, MSE: {min_error:.4f}") # Debugging

    if best_y is not None:
         print(f"Best match found: Start Index=({best_y}, {best_x}), Heading={best_heading} deg, MSE={min_error:.4f}")
    else:
         print("No suitable match found within map boundaries.")

    return best_y, best_x, best_heading, min_error


# --- Coordinate Conversion ---

def indices_to_latlon(y, x, metadata):
    """
    Convert map grid indices (y - row, x - column) to geographic coordinates
    using linear interpolation based on map metadata.
    """
    map_height = metadata['height']
    map_width = metadata['width']
    lat_top = metadata['lat_top']
    lon_left = metadata['lon_left']
    lat_bottom = metadata['lat_bottom']
    lon_right = metadata['lon_right']

    # Calculate latitude: Linearly interpolate between lat_top and lat_bottom
    # Note: y=0 is top, y=map_height-1 is bottom
    lat_range = lat_top - lat_bottom
    lat = lat_top - (y / (map_height - 1)) * lat_range if map_height > 1 else lat_top

    # Calculate longitude: Linearly interpolate between lon_left and lon_right
    # Note: x=0 is left, x=map_width-1 is right
    lon_range = lon_right - lon_left
    lon = lon_left + (x / (map_width - 1)) * lon_range if map_width > 1 else lon_left

    return lat, lon

# --- NMEA Sentence Generation (Reused from dsmac.py) ---

def decimal_to_nmea_lat(lat):
    direction = 'N' if lat >= 0 else 'S'
    abs_lat = abs(lat)
    degrees = int(abs_lat)
    minutes = (abs_lat - degrees) * 60
    return f"{degrees:02d}{minutes:07.4f}", direction

def decimal_to_nmea_lon(lon):
    direction = 'E' if lon >= 0 else 'W'
    abs_lon = abs(lon)
    degrees = int(abs_lon)
    minutes = (abs_lon - degrees) * 60
    return f"{degrees:03d}{minutes:07.4f}", direction

def calculate_nmea_checksum(nmea_str):
    checksum = 0
    for char in nmea_str:
        checksum ^= ord(char)
    return f"{checksum:02X}"

def create_gpgga_sentence(lat, lon, altitude=0):
    now = datetime.datetime.utcnow()
    time_str = now.strftime("%H%M%S.%f")[:9]  # hhmmss.ss
    lat_str, lat_dir = decimal_to_nmea_lat(lat)
    lon_str, lon_dir = decimal_to_nmea_lon(lon)
    fix_quality = "1"  # Simulation fix
    num_sat = "08"     # Dummy value
    hdop = "1.0"       # Dummy value
    altitude_str = f"{altitude:.1f}"
    geoid_sep = "0.0"  # Dummy value

    nmea_body = f"GPGGA,{time_str},{lat_str},{lat_dir},{lon_str},{lon_dir},{fix_quality},{num_sat},{hdop},{altitude_str},M,{geoid_sep},M,,"
    checksum = calculate_nmea_checksum(nmea_body)
    nmea_sentence = f"${nmea_body}*{checksum}"
    return nmea_sentence