GPS NMEA Data Processing

import json
import time
from datetime import datetime
from typing import Dict, List

class GPSTracker:
    def __init__(self, gps_receiver: GPSReceiver):
        self.gps = gps_receiver
        self.track_log = []
        self.waypoints = []
        self.geofences = []
        
        # Add GPS callbacks
        self.gps.add_callback('position', self._log_position)
        
    def _log_position(self, position_data: Dict):
        """Log position to track"""
        if position_data['latitude'] and position_data['longitude']:
            track_point = {
                'timestamp': datetime.utcnow().isoformat(),
                'latitude': position_data['latitude'],
                'longitude': position_data['longitude'],
                'altitude': position_data['altitude'],
                'speed': position_data['speed'],
                'course': position_data['course'],
                'satellites': position_data['satellites_used']
            }
            
            self.track_log.append(track_point)
            
            # Check geofences
            self._check_geofences(track_point)
            
    def add_waypoint(self, name: str, lat: float, lon: float):
        """Add waypoint"""
        waypoint = {
            'name': name,
            'latitude': lat,
            'longitude': lon,
            'timestamp': datetime.utcnow().isoformat()
        }
        self.waypoints.append(waypoint)
        print(f"📍 Added waypoint: {name} ({lat:.6f}, {lon:.6f})")
        
    def add_geofence(self, name: str, lat: float, lon: float, radius: float):
        """Add circular geofence"""
        geofence = {
            'name': name,
            'latitude': lat,
            'longitude': lon,
            'radius': radius,  # meters
            'active': True
        }
        self.geofences.append(geofence)
        print(f"🚧 Added geofence: {name} (radius: {radius}m)")
        
    def _check_geofences(self, position: Dict):
        """Check if position violates any geofences"""
        for geofence in self.geofences:
            if not geofence['active']:
                continue
                
            distance = self.gps.calculate_distance(
                position['latitude'], position['longitude'],
                geofence['latitude'], geofence['longitude']
            )
            
            if distance <= geofence['radius']:
                print(f"🚨 GEOFENCE ALERT: Entered {geofence['name']} (distance: {distance:.1f}m)")
                
    def get_current_status(self) -> Dict:
        """Get current tracking status"""
        position = self.gps.get_position()
        satellites = self.gps.get_satellites()
        
        status = {
            'timestamp': datetime.utcnow().isoformat(),
            'position': position,
            'satellites_in_view': len(satellites),
            'satellites_with_signal': len([s for s in satellites.values() if s.get('snr')]),
            'track_points': len(self.track_log),
            'waypoints': len(self.waypoints),
            'geofences': len([g for g in self.geofences if g['active']])
        }
        
        return status
        
    def export_track_gpx(self, filename: str):
        """Export track as GPX file"""
        if not self.track_log:
            print("No track data to export")
            return
            
        gpx_content = """<?xml version="1.0" encoding="UTF-8"?>
<gpx version="1.1" creator="GPS Tracker">
  <trk>
    <name>GPS Track</name>
    <trkseg>
"""
        
        for point in self.track_log:
            gpx_content += f"""      <trkpt lat="{point['latitude']}" lon="{point['longitude']}">
        <ele>{point.get('altitude', 0)}</ele>
        <time>{point['timestamp']}</time>
      </trkpt>
"""
        
        gpx_content += """    </trkseg>
  </trk>
</gpx>"""
        
        with open(filename, 'w') as f:
            f.write(gpx_content)
            
        print(f"📄 Track exported to {filename} ({len(self.track_log)} points)")
        
    def export_track_json(self, filename: str):
        """Export track as JSON file"""
        track_data = {
            'metadata': {
                'created': datetime.utcnow().isoformat(),
                'points': len(self.track_log),
                'waypoints': len(self.waypoints)
            },
            'track': self.track_log,
            'waypoints': self.waypoints,
            'geofences': self.geofences
        }
        
        with open(filename, 'w') as f:
            json.dump(track_data, f, indent=2)
            
        print(f"📄 Track exported to {filename}")

# Usage example
tracker = GPSTracker(gps)

# Add some waypoints
tracker.add_waypoint("Start Point", 40.7128, -74.0060)  # New York
tracker.add_waypoint("Destination", 40.7589, -73.9851)  # Times Square

# Add geofence
tracker.add_geofence("Home", 40.7128, -74.0060, 100)  # 100m radius

# Monitor for a while
try:
    while True:
        status = tracker.get_current_status()
        print(f"\n📊 Status: {status['track_points']} points, "
              f"{status['satellites_in_view']} satellites")
        time.sleep(5)
        
except KeyboardInterrupt:
    # Export data
    tracker.export_track_gpx("track.gpx")
    tracker.export_track_json("track.json")

import math
from typing import List, Tuple

class GPSNavigation:
    def __init__(self, gps_receiver: GPSReceiver):
        self.gps = gps_receiver
        self.route = []
        self.current_waypoint_index = 0
        
    def set_route(self, waypoints: List[Tuple[float, float, str]]):
        """Set navigation route"""
        self.route = []
        for lat, lon, name in waypoints:
            self.route.append({
                'latitude': lat,
                'longitude': lon,
                'name': name
            })
        self.current_waypoint_index = 0
        print(f"🗺️  Route set with {len(self.route)} waypoints")
        
    def get_navigation_info(self) -> Dict:
        """Get current navigation information"""
        position = self.gps.get_position()
        
        if not position['latitude'] or not position['longitude']:
            return {'error': 'No GPS fix'}
            
        if not self.route or self.current_waypoint_index >= len(self.route):
            return {'error': 'No route set'}
            
        current_waypoint = self.route[self.current_waypoint_index]
        
        # Calculate distance and bearing to current waypoint
        distance = self.gps.calculate_distance(
            position['latitude'], position['longitude'],
            current_waypoint['latitude'], current_waypoint['longitude']
        )
        
        bearing = self._calculate_bearing(
            position['latitude'], position['longitude'],
            current_waypoint['latitude'], current_waypoint['longitude']
        )
        
        # Calculate course deviation
        current_course = position.get('course', 0) or 0
        course_deviation = self._normalize_angle(bearing - current_course)
        
        # Check if we've reached the waypoint
        if distance < 50:  # 50 meter threshold
            self._advance_waypoint()
            
        nav_info = {
            'current_position': {
                'latitude': position['latitude'],
                'longitude': position['longitude']
            },
            'current_waypoint': current_waypoint,
            'waypoint_index': self.current_waypoint_index + 1,
            'total_waypoints': len(self.route),
            'distance_to_waypoint': distance,
            'bearing_to_waypoint': bearing,
            'current_course': current_course,
            'course_deviation': course_deviation,
            'turn_direction': self._get_turn_direction(course_deviation)
        }
        
        return nav_info
        
    def _calculate_bearing(self, lat1: float, lon1: float, lat2: float, lon2: float) -> float:
        """Calculate bearing between two GPS coordinates"""
        lat1_rad = math.radians(lat1)
        lat2_rad = math.radians(lat2)
        delta_lon_rad = math.radians(lon2 - lon1)
        
        x = math.sin(delta_lon_rad) * math.cos(lat2_rad)
        y = (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(x, y)
        bearing_deg = math.degrees(bearing_rad)
        
        return (bearing_deg + 360) % 360
        
    def _normalize_angle(self, angle: float) -> float:
        """Normalize angle to -180 to +180 degrees"""
        while angle > 180:
            angle -= 360
        while angle < -180:
            angle += 360
        return angle
        
    def _get_turn_direction(self, deviation: float) -> str:
        """Get turn direction based on course deviation"""
        if abs(deviation) < 10:
            return "straight"
        elif deviation > 0:
            return "right"
        else:
            return "left"
            
    def _advance_waypoint(self):
        """Advance to next waypoint"""
        self.current_waypoint_index += 1
        if self.current_waypoint_index < len(self.route):
            waypoint = self.route[self.current_waypoint_index]
            print(f"🎯 Reached waypoint! Next: {waypoint['name']}")
        else:
            print("🏁 Route complete!")
            
    def get_route_progress(self) -> Dict:
        """Get overall route progress"""
        if not self.route:
            return {'error': 'No route set'}
            
        total_distance = 0
        completed_distance = 0
        
        # Calculate total route distance
        for i in range(len(self.route) - 1):
            wp1 = self.route[i]
            wp2 = self.route[i + 1]
            distance = self.gps.calculate_distance(
                wp1['latitude'], wp1['longitude'],
                wp2['latitude'], wp2['longitude']
            )
            total_distance += distance
            
            if i < self.current_waypoint_index:
                completed_distance += distance
                
        # Add distance to current waypoint if available
        position = self.gps.get_position()
        if (position['latitude'] and position['longitude'] and 
            self.current_waypoint_index > 0):
            prev_waypoint = self.route[self.current_waypoint_index - 1]
            completed_distance += self.gps.calculate_distance(
                prev_waypoint['latitude'], prev_waypoint['longitude'],
                position['latitude'], position['longitude']
            )
            
        progress_percent = (completed_distance / total_distance * 100) if total_distance > 0 else 0
        
        return {
            'total_distance': total_distance,
            'completed_distance': completed_distance,
            'remaining_distance': total_distance - completed_distance,
            'progress_percent': progress_percent,
            'current_waypoint': self.current_waypoint_index + 1,
            'total_waypoints': len(self.route)
        }

# Navigation example
nav = GPSNavigation(gps)

# Set a route
route_waypoints = [
    (40.7128, -74.0060, "Start - New York"),
    (40.7589, -73.9851, "Times Square"),
    (40.7614, -73.9776, "Central Park South"),
    (40.7829, -73.9654, "End - Upper East Side")
]

nav.set_route(route_waypoints)

# Navigation loop
try:
    while True:
        nav_info = nav.get_navigation_info()
        
        if 'error' not in nav_info:
            print(f"\n🧭 Navigation:")
            print(f"   To: {nav_info['current_waypoint']['name']}")
            print(f"   Distance: {nav_info['distance_to_waypoint']:.0f}m")
            print(f"   Bearing: {nav_info['bearing_to_waypoint']:.0f}°")
            print(f"   Turn: {nav_info['turn_direction']}")
            
            progress = nav.get_route_progress()
            print(f"   Progress: {progress['progress_percent']:.1f}%")
            
        time.sleep(2)
        
except KeyboardInterrupt:
    print("Navigation stopped")

try:
    import matplotlib.pyplot as plt
    import numpy as np
    
    class GPSVisualizer:
        def __init__(self, gps_receiver: GPSReceiver):
            self.gps = gps_receiver
            
        def plot_track(self, track_data: List[Dict]):
            """Plot GPS track"""
            if not track_data:
                print("No track data to plot")
                return
                
            lats = [p['latitude'] for p in track_data if p['latitude']]
            lons = [p['longitude'] for p in track_data if p['longitude']]
            
            if not lats or not lons:
                print("No valid GPS coordinates to plot")
                return
                
            plt.figure(figsize=(12, 8))
            
            # Plot track
            plt.plot(lons, lats, 'b-', linewidth=2, label='GPS Track')
            plt.plot(lons[0], lats[0], 'go', markersize=10, label='Start')
            plt.plot(lons[-1], lats[-1], 'ro', markersize=10, label='End')
            
            plt.xlabel('Longitude')
            plt.ylabel('Latitude')
            plt.title('GPS Track')
            plt.legend()
            plt.grid(True, alpha=0.3)
            plt.axis('equal')
            
            plt.tight_layout()
            plt.show()
            
        def plot_satellite_sky(self, satellite_data: Dict):
            """Plot satellite positions in sky view"""
            if not satellite_data:
                print("No satellite data to plot")
                return
                
            fig, ax = plt.subplots(figsize=(10, 10), subplot_kw=dict(projection='polar'))
            
            for prn, data in satellite_data.items():
                if data.get('elevation') is not None and data.get('azimuth') is not None:
                    # Convert to polar coordinates
                    elevation = data['elevation']
                    azimuth = math.radians(data['azimuth'])
                    
                    # Plot satellite
                    radius = 90 - elevation  # Invert elevation for polar plot
                    snr = data.get('snr', 0)
                    
                    # Color by SNR
                    if snr > 40:
                        color = 'green'
                    elif snr > 25:
                        color = 'orange'
                    elif snr > 0:
                        color = 'red'
                    else:
                        color = 'gray'
                        
                    ax.plot(azimuth, radius, 'o', color=color, markersize=8)
                    ax.text(azimuth, radius-5, prn, ha='center', va='center', fontsize=8)
                    
            # Configure plot
            ax.set_ylim(0, 90)
            ax.set_yticks([0, 30, 60, 90])
            ax.set_yticklabels(['90°', '60°', '30°', '0°'])  # Elevation labels
            ax.set_theta_zero_location('N')
            ax.set_theta_direction(-1)
            ax.set_title('Satellite Sky View\n(Green=Strong, Orange=Medium, Red=Weak, Gray=No Signal)')
            
            plt.show()
            
        def plot_signal_strength(self, satellite_data: Dict):
            """Plot satellite signal strength"""
            prns = []
            snrs = []
            
            for prn, data in satellite_data.items():
                snr = data.get('snr')
                if snr is not None:
                    prns.append(prn)
                    snrs.append(snr)
                    
            if not prns:
                print("No SNR data to plot")
                return
                
            plt.figure(figsize=(12, 6))
            bars = plt.bar(prns, snrs)
            
            # Color bars by signal strength
            for bar, snr in zip(bars, snrs):
                if snr > 40:
                    bar.set_color('green')
                elif snr > 25:
                    bar.set_color('orange')
                else:
                    bar.set_color('red')
                    
            plt.xlabel('Satellite PRN')
            plt.ylabel('Signal Strength (dB-Hz)')
            plt.title('Satellite Signal Strength')
            plt.grid(True, alpha=0.3)
            
            # Add threshold lines
            plt.axhline(y=25, color='orange', linestyle='--', alpha=0.7, label='Good Signal')
            plt.axhline(y=40, color='green', linestyle='--', alpha=0.7, label='Excellent Signal')
            plt.legend()
            
            plt.tight_layout()
            plt.show()
    
    # Usage
    # visualizer = GPSVisualizer(gps)
    # visualizer.plot_track(tracker.track_log)
    # visualizer.plot_satellite_sky(gps.get_satellites())
    # visualizer.plot_signal_strength(gps.get_satellites())
    
except ImportError:
    print("matplotlib not available for visualization")