main.py

# /main.py
import numpy as np
import tercom
import os
import json
import random

def generate_example_data(map_file="data/terrain_map.npy", meta_file="data/map_metadata.json"):
    """Creates simple example terrain map and metadata if they don't exist."""
    if os.path.exists(map_file) and os.path.exists(meta_file):
        return # Data already exists

    print("Generating example terrain data...")
    os.makedirs("data", exist_ok=True)

    # Create a simple map (e.g., a slope with some bumps)
    height, width = 100, 150
    # Base slope
    y_coords, x_coords = np.mgrid[0:height, 0:width]
    terrain = 50 + 0.5 * y_coords + 0.2 * x_coords
    # Add some random hills/valleys
    for _ in range(15):
        hill_y, hill_x = random.randint(0, height-1), random.randint(0, width-1)
        hill_rad = random.uniform(5, 15)
        hill_h = random.uniform(-20, 30)
        dist_sq = (y_coords - hill_y)**2 + (x_coords - hill_x)**2
        terrain += hill_h * np.exp(-dist_sq / (2 * hill_rad**2))
    
    # Add noise
    terrain += np.random.normal(0, 0.5, terrain.shape)

    np.save(map_file, terrain)

    # Create metadata (example coordinates)
    metadata = {
        "lat_top": 40.0,
        "lon_left": -74.0,
        "lat_bottom": 39.8, # Approx 0.2 degree latitude range
        "lon_right": -73.7, # Approx 0.3 degree longitude range
        "height": height,
        "width": width,
        "description": "Example terrain map generated by main.py"
    }
    with open(meta_file, 'w') as f:
        json.dump(metadata, f, indent=4)
    print(f"Saved example map to {map_file}")
    print(f"Saved example metadata to {meta_file}")


def main():
    """
    Main function to demonstrate TERCOM:
      1. Load terrain map and metadata.
      2. Simulate an observed profile from a known location.
      3. Use TERCOM to find the best match for the observed profile.
      4. Convert the best match indices to lat/lon.
      5. Create and output an NMEA GPGGA sentence.
    """
    print("--- TERCOM Python Demo ---")

    # Ensure example data exists
    generate_example_data()

    # 1. Load Data
    terrain_map, metadata = tercom.load_terrain_data()
    if terrain_map is None:
        return

    # 2. Simulate Sensor Data
    # Define a 'true' starting point and path for simulation
    map_h, map_w = terrain_map.shape
    true_start_y = map_h // 3        # Example start Y index
    true_start_x = map_w // 4        # Example start X index
    profile_length = 25              # How many points in the measurement
    true_heading = 45                # Fly northeast (degrees)
    sensor_noise = 2.0               # Altitude noise standard deviation (meters)

    print(f"\nSimulating observed profile from true start ({true_start_y}, {true_start_x}), heading {true_heading} deg...")
    observed_profile = tercom.simulate_observed_profile(
        terrain_map, metadata, true_start_y, true_start_x,
        profile_length, true_heading, noise_stddev=sensor_noise
    )

    if observed_profile is None:
        print("Failed to simulate observed profile (path likely went off map).")
        return

    print(f"Generated observed profile with {len(observed_profile)} points.")

    # 3. Find Best Match using TERCOM
    print("\nSearching for best match using TERCOM...")
    # Define headings to search (can be more granular for better accuracy)
    # Use None for simplified search, or a list/range e.g., np.arange(0, 360, 15)
    search_headings = np.arange(0, 360, 30) # Check every 30 degrees

    best_y, best_x, best_heading, min_error = tercom.find_best_match(
        terrain_map, observed_profile, possible_headings_deg=search_headings
    )

    if best_y is None:
        print("\nTERCOM failed to find a match.")
        return

    # 4. Convert Indices to Coordinates
    est_lat, est_lon = tercom.indices_to_latlon(best_y, best_x, metadata)

    # Also calculate the 'true' lat/lon for comparison
    true_lat, true_lon = tercom.indices_to_latlon(true_start_y, true_start_x, metadata)

    print("\n--- Results ---")
    print(f"True Start Location (Simulation): Index=({true_start_y}, {true_start_x}), Lat={true_lat:.6f}, Lon={true_lon:.6f}, Heading={true_heading} deg")
    print(f"Estimated Start Location (TERCOM): Index=({best_y}, {best_x}), Lat={est_lat:.6f}, Lon={est_lon:.6f}, Heading={best_heading} deg")
    print(f"Minimum Matching Error (MSE): {min_error:.4f}")

    # 5. Generate NMEA Output
    # Use the estimated altitude from the *start* of the matched profile on the map
    # More advanced: use average altitude or altitude from a specific point in the profile
    estimated_altitude = terrain_map[best_y, best_x]
    nmea_sentence = tercom.create_gpgga_sentence(est_lat, est_lon, altitude=estimated_altitude)

    print("\nMock NMEA GPGGA sentence:")
    print(nmea_sentence)

    # Save NMEA sentence
    output_file = "nmea_tercom_output.csv"
    try:
        with open(output_file, "a") as csvfile:
            csvfile.write(nmea_sentence + "\n")
        print(f"NMEA sentence appended to {output_file}")
    except IOError as e:
        print(f"Error writing to {output_file}: {e}")


if __name__ == "__main__":
    main()