# Copyright 2025 DanaK and Claude Sonnet 4 AI
# License: MIT License
'''
GPS Logger with OLED Display and CSV Logging
This script logs GPS data to a CSV file written to an SD card,
displays it on an OLED screen, and provides real-time updates on speed, distance, and bearing.
This script is designed to run on a CircuitPython-compatible microcontroller
with GPS and OLED display support. Original hardware Adafruit Feather Adalogger RP2040
with GPS FeatherWing and OLED FeatherWing.
'''

import board
import os
import time
import supervisor
import math
import rtc
import displayio
import terminalio
import digitalio
import busio
import adafruit_gps
import keypad
import storage
import adafruit_sdcard
from adafruit_display_text import label
from i2cdisplaybus import I2CDisplayBus
import adafruit_displayio_ssd1306
import neopixel as neopixel

def meters_to_feet(meters):
    """Convert meters to feet."""
    return meters * 3.28084 # 1 meter = 3.28084 feet

def knots_to_mph(knots):
    """Convert knots to miles per hour."""
    return knots * 1.15078  # 1 knot = 1.15078 mph

def knots_to_fps(knots):
    """Convert knots to feet per second."""
    return knots * 1.68781  # 1 knot = 1.68781 feet/second

def calculate_bearing(lat1, lon1, lat2, lon2):
    """Calculate the bearing between two GPS coordinates in degrees."""
    if lat1 is None or lon1 is None or lat2 is None or lon2 is None:
        return None

    # Convert to radians
    lat1_rad = math.radians(lat1)
    lat2_rad = math.radians(lat2)
    delta_lon_rad = math.radians(lon2 - lon1)

    # Calculate bearing
    y = math.sin(delta_lon_rad) * math.cos(lat2_rad)
    x = math.cos(lat1_rad) * math.sin(lat2_rad) - math.sin(lat1_rad) * math.cos(lat2_rad) * math.cos(delta_lon_rad)

    bearing_rad = math.atan2(y, x)
    bearing_deg = math.degrees(bearing_rad)

    # Normalize to 0-360 degrees
    bearing_deg = (bearing_deg + 360) % 360

    return bearing_deg

def bearing_to_compass(bearing):
    """Convert bearing in degrees to compass direction."""
    if bearing is None:
        return "N/A"

    directions = ["N", "NNE", "NE", "ENE", "E", "ESE", "SE", "SSE",
                 "S", "SSW", "SW", "WSW", "W", "WNW", "NW", "NNW"]

    index = round(bearing / 22.5) % 16
    return directions[index]

def calculate_distance_feet(lat1, lon1, lat2, lon2):
    """Calculate the distance between two GPS coordinates in feet using Haversine formula."""
    if lat1 is None or lon1 is None or lat2 is None or lon2 is None:
        return None

    # Earth's radius in feet (approximately 20,902,231 feet)
    earth_radius_feet = 20902231.0

    # Convert latitude and longitude from degrees to radians
    lat1_rad = math.radians(lat1)
    lat2_rad = math.radians(lat2)
    delta_lat_rad = math.radians(lat2 - lat1)
    delta_lon_rad = math.radians(lon2 - lon1)

    # Haversine formula
    a = (math.sin(delta_lat_rad / 2) ** 2 +
         math.cos(lat1_rad) * math.cos(lat2_rad) *
         math.sin(delta_lon_rad / 2) ** 2)
    c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))

    # Distance in feet
    distance_feet = earth_radius_feet * c

    return distance_feet

def calculate_origin_metrics(current_lat, current_lon):
    """Calculate cached bearing and distance to origin point."""
    global cached_distance_to_origin, cached_bearing_to_origin, last_calc_lat, last_calc_lon
    
    # Check if we need to recalculate (coordinates changed)
    if (last_calc_lat != current_lat or last_calc_lon != current_lon or 
        cached_distance_to_origin is None or cached_bearing_to_origin is None):
        
        if origin_lat is not None and origin_lon is not None:
            cached_bearing_to_origin = calculate_bearing(current_lat, current_lon, origin_lat, origin_lon)
            cached_distance_to_origin = calculate_distance_feet(current_lat, current_lon, origin_lat, origin_lon)
            last_calc_lat = current_lat
            last_calc_lon = current_lon
        else:
            cached_bearing_to_origin = None
            cached_distance_to_origin = None
    
    return cached_bearing_to_origin, cached_distance_to_origin

def calculate_setpoint_metrics(current_lat, current_lon):
    """Calculate cached bearing and distance to set point."""
    global cached_distance_to_setpoint, cached_bearing_to_setpoint
    
    if last_setpoint_lat is not None and last_setpoint_lon is not None:
        cached_bearing_to_setpoint = calculate_bearing(current_lat, current_lon, last_setpoint_lat, last_setpoint_lon)
        cached_distance_to_setpoint = calculate_distance_feet(current_lat, current_lon, last_setpoint_lat, last_setpoint_lon)
    else:
        cached_bearing_to_setpoint = None
        cached_distance_to_setpoint = None
    
    return cached_bearing_to_setpoint, cached_distance_to_setpoint

def create_new_csv_file(mode=None):
    """Create a new CSV file with current timestamp and mode name, write header."""
    global csv_filename, header_written, origin_lat, origin_lon
    global cached_distance_to_origin, cached_bearing_to_origin, last_calc_lat, last_calc_lon

    # Get current time from RTC
    r = rtc.RTC()
    current_time = r.datetime

    # Determine mode suffix for filename
    mode_suffix = ""
    if mode == 1:
        mode_suffix = "_Interval_Log"
    elif mode == 2:
        mode_suffix = "_Set_Point"
    elif mode == 3:
        mode_suffix = "_Origin_Point"

    # Generate new filename based on current RTC time with mode suffix
    csv_filename = f"/sd/{current_time.tm_mon:02d}{current_time.tm_mday:02d}{current_time.tm_hour:02d}{current_time.tm_min:02d}{current_time.tm_sec:02d}{mode_suffix}.csv"

    try:
        # Write CSV header to new file
        with open(csv_filename, "w") as file:
            file.write("Timestamp,Latitude,Longitude,Speed_ft_s,Bearing_deg,Distance_ft\n")
        header_written = True
        # Reset origin point and clear cache when new file is created
        origin_lat = None
        origin_lon = None
        cached_distance_to_origin = None
        cached_bearing_to_origin = None
        last_calc_lat = None
        last_calc_lon = None
        print(f"New GPS data file created: {csv_filename}")
        print("Origin point reset for new file")
        return True
    except Exception as e:
        print(f"Error creating new CSV file: {e}")
        return False

colors = {
    "red": 0xFF0000,
    "green": 0x00FF00,
    "blue": 0x0000FF,
    "yellow": 0xFFFF00,
    "cyan": 0x00FFFF,
    "magenta": 0xFF00FF,
    "white": 0xFFFFFF,
    "black": 0x333333
}

pixel = neopixel.NeoPixel(board.NEOPIXEL, 1, brightness=0.2, auto_write=True)
pixel.fill(colors["red"])


print(len(colors))
cs = digitalio.DigitalInOut(board.SD_CS)
sd_spi = busio.SPI(board.SD_CLK, board.SD_MOSI, board.SD_MISO)
sdcard = adafruit_sdcard.SDCard(sd_spi, cs)
vfs = storage.VfsFat(sdcard)
storage.mount(vfs, "/sd")

# CSV filename will be generated after GPS fix is obtained
csv_filename = "/sd/gps_data_temp.csv"  # Temporary filename until GPS time is available

displayio.release_displays()

i2c = board.I2C()  # uses board.SCL and board.SDA
# i2c = board.STEMMA_I2C()  # For using the built-in STEMMA QT connector on a microcontroller
display_bus = I2CDisplayBus(i2c, device_address=0x3C)
display = adafruit_displayio_ssd1306.SSD1306(display_bus, width=128, height=32)

# Make the display context
splash = displayio.Group()
display.root_group = splash

intervals = [2.5, 5, 10, 15, 30, 60]
current_interval_index = 0  # Start with the first interval (2.5s)

# Variables to track previous GPS coordinates for bearing calculation
prev_lat = None
prev_lon = None
current_bearing = None
filename_set = False  # Flag to track if we've set the filename using GPS time
header_written = False  # Flag to track if CSV header has been written
reading_interval = intervals[current_interval_index]  # Initialize with first interval
new_file_notification = False  # Flag to show "New File" notification
notification_start_time = 0  # Time when notification started

# LED flashing variables
led_state = False  # Current LED state (False = off, True = on)
last_led_toggle = time.monotonic()  # Last time LED was toggled
led_interval = 0.5  # LED toggle interval in seconds

# Mode variables
current_mode = 1  # 1 = Interval Logging, 2 = Set-Point, 3 = Origin Point
mode_notification = False  # Flag to show mode change notification
mode_notification_start_time = 0  # Time when mode notification started

# Set-point mode variables
last_setpoint_lat = None
last_setpoint_lon = None

# Origin point mode variables
origin_lat = None
origin_lon = None

# Cache variables for optimization
cached_distance_to_origin = None
cached_bearing_to_origin = None
cached_distance_to_setpoint = None
cached_bearing_to_setpoint = None
last_calc_lat = None
last_calc_lon = None

# Help screen variables
help_screen_active = False
help_screen_start_time = 0

# Initialize keypad for non-blocking button handling
button_pins = (board.D9, board.D6, board.D5, board.BUTTON)
keys = keypad.Keys(button_pins, value_when_pressed=False, pull=True)

led = digitalio.DigitalInOut(board.LED)
led.direction = digitalio.Direction.OUTPUT

uart = busio.UART(board.TX, board.RX, baudrate=9600, timeout=10)
gps = adafruit_gps.GPS(uart, debug=False)  # Use UART/pyserial

# Turn on the basic GGA and RMC info (what you typically want)
gps.send_command(b"PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0")
# Turn on the basic GGA and RMC info + VTG for speed in km/h
# gps.send_command(b"PMTK314,0,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0")
# Turn on just minimum info (RMC only, location):
# gps.send_command(b'PMTK314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0')
# Turn off everything:
# gps.send_command(b'PMTK314,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0')
# Turn on everything (not all of it is parsed!)
# gps.send_command(b'PMTK314,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0')

# Set update rate to once a second (1hz) which is what you typically want.
gps.send_command(b"PMTK220,1000")
# Or decrease to once every two seconds by doubling the millisecond value.
# Be sure to also increase your UART timeout above!
# gps.send_command(b'PMTK220,2000')
# You can also speed up the rate, but don't go too fast or else you can lose
# data during parsing.  This would be twice a second (2hz, 500ms delay):
# gps.send_command(b'PMTK220,500')

# Draw labels
speed_text = "Spd: 0.0"
speed_area = label.Label(terminalio.FONT, text=speed_text, color=0xFFFFFF, x=2, y=6)
splash.append(speed_area)

bearing_text = "Dir: N/A"
bearing_area = label.Label(terminalio.FONT, text=bearing_text, color=0xFFFFFF, x=2, y=17)
splash.append(bearing_area)

lat_text = "0.000000"
lat_area = label.Label(terminalio.FONT, text=lat_text, color=0xFFFFFF, x=2, y=28)
splash.append(lat_area)

lon_text = "0.000000"
lon_area = label.Label(terminalio.FONT, text=lon_text, color=0xFFFFFF, x=62, y=28)
splash.append(lon_area)

# Add satellite count display in upper right corner
sat_text = "Sat: 0"
sat_area = label.Label(terminalio.FONT, text=sat_text, color=0xFFFFFF, x=90, y=6)
splash.append(sat_area)

# Add reading interval display in upper right corner (below satellites)
interval_text = f"Int:{reading_interval}s"
interval_area = label.Label(terminalio.FONT, text=interval_text, color=0xFFFFFF, x=80, y=17)
splash.append(interval_area)

last_print = time.monotonic()
while True:
    # Check for button presses using keypad (non-blocking)
    event = keys.events.get()
    if event:
        if event.pressed:
            if event.key_number == 0:  # Button A
                if current_mode == 1:  # Interval Logging Mode - cycle interval
                    current_interval_index = (current_interval_index + 1) % len(intervals)
                    reading_interval = intervals[current_interval_index]
                    interval_area.text = f"Int:{reading_interval}s"
                    print(f"Button A pressed - Reading interval changed to {reading_interval} seconds (index {current_interval_index})")
                elif current_mode == 2:  # Set-Point Mode - record data point
                    if gps.has_fix and gps.latitude is not None and gps.longitude is not None:
                        print("Button A pressed - Recording set-point data")
                        # Use cached calculations if available, otherwise calculate
                        bearing_to_setpoint, distance_to_setpoint = calculate_setpoint_metrics(gps.latitude, gps.longitude)

                        # Save set-point data to file
                        try:
                            if header_written:
                                r = rtc.RTC()
                                current_rtc_time = r.datetime
                                rtc_timestamp = f"{current_rtc_time.tm_mon:02d}{current_rtc_time.tm_mday:02d}{current_rtc_time.tm_hour:02d}{current_rtc_time.tm_min:02d}{current_rtc_time.tm_sec:02d}"
                                speed_fps = knots_to_fps(gps.speed_knots) if gps.speed_knots is not None else 0.0
                                bearing_value = bearing_to_setpoint if bearing_to_setpoint is not None else 0.0
                                distance_value = distance_to_setpoint if distance_to_setpoint is not None else 0.0
                                data_point = f"{rtc_timestamp},{gps.latitude:.6f},{gps.longitude:.6f},{speed_fps:.1f},{bearing_value:.1f},{distance_value:.1f}\n"
                                with open(csv_filename, "a") as file:
                                    file.write(data_point)
                                print(f"Set-point data recorded: Lat {gps.latitude:.6f}, Lon {gps.longitude:.6f}")
                        except Exception as e:
                            print(f"Error saving set-point data: {e}")

                        # Update last set-point location and clear cache
                        last_setpoint_lat = gps.latitude
                        last_setpoint_lon = gps.longitude
                        cached_distance_to_setpoint = None
                        cached_bearing_to_setpoint = None
                    else:
                        print("Button A pressed - No GPS fix available for set-point")
                elif current_mode == 3:  # Origin Point Mode - cycle interval or record data point
                    # If origin point is not set, pressing A sets it
                    if origin_lat is None or origin_lon is None:
                        if gps.has_fix and gps.latitude is not None and gps.longitude is not None:
                            origin_lat = gps.latitude
                            origin_lon = gps.longitude
                            print(f"Button A pressed - Origin point set: Lat {origin_lat:.6f}, Lon {origin_lon:.6f}")
                        else:
                            print("Button A pressed - No GPS fix available to set origin point")
                    else:
                        # Origin point is set, so cycle the logging interval
                        current_interval_index = (current_interval_index + 1) % len(intervals)
                        reading_interval = intervals[current_interval_index]
                        interval_area.text = f"Int:{reading_interval}s"
                        print(f"Button A pressed - Reading interval changed to {reading_interval} seconds (index {current_interval_index})")

            elif event.key_number == 1:  # Button B - Mode switch
                current_mode = current_mode + 1
                if current_mode > 3:
                    current_mode = 1
                mode_notification = True
                mode_notification_start_time = time.monotonic()
                if current_mode == 1:
                    mode_text = "Interval Log"
                elif current_mode == 2:
                    mode_text = "Set-Point"
                else:  # current_mode == 3
                    mode_text = "Origin Point"
                print(f"Button B pressed - Switched to Mode {current_mode}: {mode_text}")
                
            elif event.key_number == 2:  # Button C
                print("Button C pressed - Creating new data file")
                if create_new_csv_file(current_mode):
                    new_file_notification = True
                    notification_start_time = time.monotonic()
            elif event.key_number == 3:  # Board Button - Show help
                help_screen_active = True
                help_screen_start_time = time.monotonic()
                print("Board Button pressed - Showing help screen")

    # Make sure to call gps.update() every loop iteration and at least twice
    # as fast as data comes from the GPS unit (usually every second).
    # This returns a bool that's true if it parsed new data (you can ignore it
    # though if you don't care and instead look at the has_fix property).
    gps.update()
    
    # Set origin point on first GPS fix after new file creation (or if not set) - works in any mode
    if origin_lat is None and origin_lon is None and gps.has_fix and gps.latitude is not None and gps.longitude is not None:
        origin_lat = gps.latitude
        origin_lon = gps.longitude
        print(f"Origin point automatically set: Lat {origin_lat:.6f}, Lon {origin_lon:.6f}")
    
    # Handle LED flashing (non-blocking)
    current_time = time.monotonic()
    if current_time - last_led_toggle >= led_interval:
        led_state = not led_state  # Toggle LED state
        led.value = led_state
        last_led_toggle = current_time

    # Handle Mode notification display
    if mode_notification:
        if time.monotonic() - mode_notification_start_time >= 2.0:
            # Stop showing mode notification after 2 seconds
            mode_notification = False
        else:
            # Show mode notification
            if current_mode == 1:
                speed_area.text = "Mode 1"
                bearing_area.text = "Int Log"
            elif current_mode == 2:
                speed_area.text = "Mode 2"
                bearing_area.text = "Set-Point"
            else:  # current_mode == 3
                speed_area.text = "Mode 3"
                bearing_area.text = "Origin Pt"

    # Handle Help screen display
    if help_screen_active:
        if time.monotonic() - help_screen_start_time >= 5.0:
            # Stop showing help screen after 5 seconds
            help_screen_active = False
        else:
            # Show help screen based on current mode
            if current_mode == 1:  # Interval Logging Mode
                speed_area.text = "A: Timed Log"
                bearing_area.text = "B: Chg Mode"
                lat_area.text = "C: New File"
                lon_area.text = ""
            elif current_mode == 2:  # Set-Point Mode
                speed_area.text = "A: Create Set Point"
                bearing_area.text = "B: Chg Mode"
                lat_area.text = "C: New File"
                lon_area.text = ""
            else:  # current_mode == 3  # Origin Point Mode
                speed_area.text = "A: Set/Chg Int"
                bearing_area.text = "B: Chg Mode"
                lat_area.text = "C: New File"
                lon_area.text = ""

    # Handle "New File" notification display
    if new_file_notification:
        if time.monotonic() - notification_start_time >= 2.0:
            # Stop showing notification after 2 seconds
            new_file_notification = False
        else:
            # Show "New File" notification (only if not showing mode or help notification)
            if not mode_notification and not help_screen_active:
                speed_area.text = "New File"
                bearing_area.text = "Created!"

    # Update display continuously (when no notifications are showing)
    if not new_file_notification and not mode_notification and not help_screen_active and gps.has_fix:
        if current_mode == 1:  # Interval Logging Mode
            speed_fps = knots_to_fps(gps.speed_knots) if gps.speed_knots is not None else 0.0
            speed_area.text = f"Spd: {speed_fps:.1f}"
            if current_bearing is not None:
                bearing_area.text = f"Dir: {current_bearing:.0f}° {bearing_to_compass(current_bearing)}"
            else:
                bearing_area.text = "Dir: N/A"
        elif current_mode == 2:  # Set-Point Mode
            if last_setpoint_lat is not None and last_setpoint_lon is not None and gps.latitude is not None and gps.longitude is not None:
                bearing_to_setpoint, distance_to_setpoint = calculate_setpoint_metrics(gps.latitude, gps.longitude)
                speed_area.text = f"Dist: {distance_to_setpoint:.1f}ft"
                if bearing_to_setpoint is not None:
                    bearing_area.text = f"To: {bearing_to_setpoint:.0f}° {bearing_to_compass(bearing_to_setpoint)}"
                else:
                    bearing_area.text = "To: N/A"
            else:
                speed_area.text = "No SetPt"
                bearing_area.text = "Press A"
        elif current_mode == 3:  # Origin Point Mode
            if origin_lat is not None and origin_lon is not None and gps.latitude is not None and gps.longitude is not None:
                bearing_to_origin, distance_to_origin = calculate_origin_metrics(gps.latitude, gps.longitude)
                
                # Display distance in miles if over 2500 feet, otherwise in feet
                if distance_to_origin > 2500:
                    distance_miles = distance_to_origin / 5280  # Convert feet to miles
                    speed_area.text = f"Dist: {distance_miles:.1f}mi"
                else:
                    speed_area.text = f"Dist: {distance_to_origin:.1f}ft"
                
                if bearing_to_origin is not None:
                    bearing_area.text = f"To: {bearing_to_origin:.0f}° {bearing_to_compass(bearing_to_origin)}"
                else:
                    bearing_area.text = "To: N/A"
            else:
                speed_area.text = "No Origin"
                bearing_area.text = "Press A"

        # Update coordinates and satellite count (same for both modes)
        lat_area.text = f"{gps.latitude:.6f}" if gps.latitude is not None else "Lat: N/A"
        lon_area.text = f"{gps.longitude:.6f}" if gps.longitude is not None else "Lon: N/A"
        sat_count = gps.satellites if gps.satellites is not None else 0
        sat_area.text = f"S:{sat_count}"

    # Check if it's time for the next reading based on current interval (Mode 1 and Mode 3)
    current = time.monotonic()
    if (current_mode == 1 or current_mode == 3) and current - last_print >= reading_interval:

        last_print = current

        # Set neopixel color based on compass bearing
        if current_bearing is not None:
            if 350 <= current_bearing or current_bearing <= 10:  # Due North (350-360, 0-10)
                pixel.fill(colors["green"])
            elif 170 <= current_bearing <= 190:  # Due South (170-190)
                pixel.fill(colors["red"])
            elif 10 < current_bearing < 170:  # East of North/South (10-170)
                pixel.fill(colors["yellow"])
            elif 190 < current_bearing < 350:  # West of North/South (190-350)
                pixel.fill(colors["magenta"])
        else:
            # No bearing available, use default color
            pixel.fill(colors["red"])

        if not gps.has_fix:
            # Try again if we don't have a fix yet.
            print("Waiting for fix...")
            continue
        # We have a fix! (gps.has_fix is true)
        # Print out details about the fix like location, date, etc.
        print("=" * 40)  # Print a separator line.
        print(
            "Fix timestamp: {}/{}/{} {:02}:{:02}:{:02}".format(  # noqa: UP032
                gps.timestamp_utc.tm_mon,  # Grab parts of the time from the
                gps.timestamp_utc.tm_mday,  # struct_time object that holds
                gps.timestamp_utc.tm_year,  # the fix time.  Note you might
                gps.timestamp_utc.tm_hour,  # not get all data like year, day,
                gps.timestamp_utc.tm_min,  # month!
                gps.timestamp_utc.tm_sec,
            )
        )
        print(f"Fix quality: {gps.fix_quality}")
        # Some attributes beyond latitude, longitude and timestamp are optional
        # and might not be present.  Check if they're None before trying to use!
        if gps.satellites is not None:
            print(f"# satellites: {gps.satellites}")
        print(time.monotonic())
        print(f"Latitude: {gps.latitude:.6f} degrees")
        print(f"Longitude: {gps.longitude:.6f} degrees")
        # print(f"Precise Latitude: {gps.latitude_degrees} degs, {gps.latitude_minutes:2.4f} mins")
        # print(f"Precise Longitude: {gps.longitude_degrees} degs, {gps.longitude_minutes:2.4f} mins")
        # if gps.altitude_m is not None:
        #     print(f"Altitude: {meters_to_feet(gps.altitude_m)} feet")
        if gps.speed_knots is not None:
            print(f"Speed: {knots_to_fps(gps.speed_knots)} feet/second")
        # if gps.speed_kmh is not None:
        #     print(f"Speed: {gps.speed_kmh} km/h")
        # if gps.track_angle_deg is not None:
        #     print(f"Track angle: {gps.track_angle_deg} degrees")
        # if gps.horizontal_dilution is not None:
        #     print(f"Horizontal dilution: {gps.horizontal_dilution}")
        # if gps.height_geoid is not None:
        #     print(f"Height geoid: {gps.height_geoid} meters")

        # Update OLED display
        speed_fps = knots_to_fps(gps.speed_knots) if gps.speed_knots is not None else 0.0

        # Set RTC and generate filename from GPS time on first fix
        if not filename_set:
            # Convert GPS UTC time to US Central Time (UTC-6)
            # Note: This doesn't account for daylight saving time automatically
            utc_time = gps.timestamp_utc
            central_offset_hours = -5  # US Central Standard Time is UTC-5

            # Calculate Central Time by adjusting hours
            central_hour = utc_time.tm_hour + central_offset_hours
            central_day = utc_time.tm_mday
            central_mon = utc_time.tm_mon
            central_year = utc_time.tm_year

            # Handle day rollover for negative hours
            if central_hour < 0:
                central_hour += 24
                central_day -= 1
                if central_day < 1:
                    # Handle month rollover (simplified)
                    central_mon -= 1
                    if central_mon < 1:
                        central_mon = 12
                        central_year -= 1
                    # Set day to last day of previous month (simplified to 30)
                    central_day = 30

            # Handle day rollover for hours >= 24
            elif central_hour >= 24:
                central_hour -= 24
                central_day += 1
                # Simplified month rollover (assuming 31 days max)
                if central_day > 31:
                    central_day = 1
                    central_mon += 1
                    if central_mon > 12:
                        central_mon = 1
                        central_year += 1

            # Create Central Time struct_time
            central_time = time.struct_time((
                central_year, central_mon, central_day,
                central_hour, utc_time.tm_min, utc_time.tm_sec,
                utc_time.tm_wday, utc_time.tm_yday, utc_time.tm_isdst
            ))

            # Set RTC using Central Time
            r = rtc.RTC()
            r.datetime = central_time
            print(f"RTC set from GPS timestamp (converted to Central Time: {central_mon:02d}/{central_day:02d}/{central_year} {central_hour:02d}:{utc_time.tm_min:02d}:{utc_time.tm_sec:02d})")

            # Generate CSV filename based on Central Time (mmddhhmm.csv) with mode suffix
            mode_suffix = ""
            if current_mode == 1:
                mode_suffix = "_IntLog"
            elif current_mode == 2:
                mode_suffix = "_SetPt"
            elif current_mode == 3:
                mode_suffix = "_Origin"
            
            csv_filename = f"/sd/{central_mon:02d}{central_day:02d}{central_hour:02d}{utc_time.tm_min:02d}{mode_suffix}.csv"
            print(f"GPS data will be saved to: {csv_filename}")
            filename_set = True

            # Write CSV header to new file
            try:
                with open(csv_filename, "w") as file:
                    file.write("Timestamp,Latitude,Longitude,Speed_ft_s,Bearing_deg,Distance_ft\n")
                header_written = True
                print("CSV header written to file")
            except Exception as e:
                print(f"Error writing CSV header: {e}")

        # Calculate bearing and distance based on current mode
        distance_feet = 0.0
        bearing_value = 0.0
        
        if current_mode == 1:  # Interval Logging Mode - calculate from previous coordinates
            if prev_lat is not None and prev_lon is not None and gps.latitude is not None and gps.longitude is not None:
                current_bearing = calculate_bearing(prev_lat, prev_lon, gps.latitude, gps.longitude)
                distance_feet = calculate_distance_feet(prev_lat, prev_lon, gps.latitude, gps.longitude)
                bearing_value = current_bearing if current_bearing is not None else 0.0
        elif current_mode == 3:  # Origin Point Mode - calculate from origin
            if origin_lat is not None and origin_lon is not None and gps.latitude is not None and gps.longitude is not None:
                bearing_to_origin, distance_to_origin = calculate_origin_metrics(gps.latitude, gps.longitude)
                bearing_value = bearing_to_origin if bearing_to_origin is not None else 0.0
                distance_feet = distance_to_origin if distance_to_origin is not None else 0.0

        # Update display (only if not showing "New File" notification)
        if not new_file_notification and not mode_notification:
            # Display updates are handled in the continuous section above
            pass

        # Always update coordinates and satellite count during GPS readings
        lat_area.text = f"{gps.latitude:.6f}" if gps.latitude is not None else "Lat: N/A"
        lon_area.text = f"{gps.longitude:.6f}" if gps.longitude is not None else "Lon: N/A"
        sat_count = gps.satellites if gps.satellites is not None else 0
        sat_area.text = f"S:{sat_count}"

        try:
            # Save data to file with bearing and distance using RTC timestamp
            # Only write data if header has been written
            if header_written:
                r = rtc.RTC()
                current_rtc_time = r.datetime
                rtc_timestamp = f"{current_rtc_time.tm_mon:02d}{current_rtc_time.tm_mday:02d}{current_rtc_time.tm_hour:02d}{current_rtc_time.tm_min:02d}{current_rtc_time.tm_sec:02d}"
                distance_value = distance_feet if distance_feet is not None else 0.0
                data_point = f"{rtc_timestamp},{gps.latitude:.6f},{gps.longitude:.6f},{speed_fps:.1f},{bearing_value:.1f},{distance_value:.1f}\n"
                with open(csv_filename, "a") as file:
                    file.write(data_point)
        except Exception as e:
            print(f"Error saving data: {e}")

        # Display bearing and distance in console based on mode
        if current_mode == 1:  # Interval Logging Mode
            if prev_lat is not None and prev_lon is not None:
                if current_bearing is not None:
                    print(f"Bearing: {current_bearing:.2f} degrees")
                    compass_direction = bearing_to_compass(current_bearing)
                    print(f"Compass Direction: {compass_direction}")
                if distance_feet is not None:
                    print(f"Distance traveled: {distance_feet:.1f} feet")
        elif current_mode == 3:  # Origin Point Mode
            if origin_lat is not None and origin_lon is not None:
                if bearing_value != 0.0:
                    print(f"Bearing to origin: {bearing_value:.2f} degrees")
                    compass_direction = bearing_to_compass(bearing_value)
                    print(f"Compass Direction to origin: {compass_direction}")
                if distance_feet is not None:
                    print(f"Distance to origin: {distance_feet:.1f} feet")

        # Update previous coordinates for next calculation
        if gps.latitude is not None and gps.longitude is not None:
            prev_lat = gps.latitude
            prev_lon = gps.longitude