#!/usr/bin/env python3
"""
Space Navigation AI Controller
3-Step Mission to Stars Implementation
Author: Global Byte Shop Thailand
"""

import numpy as np
import cv2
import time
import serial
import threading
import json
from datetime import datetime, timezone
import math
import sqlite3
from collections import deque
import logging

# AI and ML imports
import torch
import torch.nn as nn
from ultralytics import YOLO
from sklearn.ensemble import RandomForestRegressor

# GPS and Navigation
import pynmea2
import pyproj

# Star tracking
from astropy.coordinates import SkyCoord, EarthLocation, AltAz
from astropy.time import Time
import astropy.units as u

class SpaceNavigationController:
    def __init__(self):
        """Initialize the 3-step space navigation system"""
        
        # System configuration
        self.config = {
            'gps_port': '/dev/ttyUSB0',
            'arduino_port': '/dev/ttyACM0',
            'camera_id': 0,
            'lora_port': '/dev/ttyUSB1',
            'database': 'space_mission.db',
            'star_catalog': 'hipparcos.dat'
        }
        
        # Initialize logging
        logging.basicConfig(level=logging.INFO)
        self.logger = logging.getLogger(__name__)
        
        # Step 1: Satellite Tracking System
        self.gps_data = {}
        self.imu_data = {}
        self.position_history = deque(maxlen=1000)
        
        # Step 2: AI Navigation System
        self.ai_model = self.load_navigation_model()
        self.vision_model = YOLO('yolov8n.pt')
        self.obstacle_map = np.zeros((1000, 1000))
        
        # Step 3: Star Tracking System
        self.star_catalog = self.load_star_catalog()
        self.telescope_position = {'az': 0, 'alt': 0}
        self.tracked_stars = []
        
        # Communication and data storage
        self.database = self.init_database()
        self.telemetry_buffer = deque(maxlen=100)
        
        # System state
        self.mission_state = 'INIT'  # INIT, TRACKING, NAVIGATING, STAR_SEARCH
        self.current_waypoint = None
        self.target_coordinates = None
        
        self.logger.info("🚀 Space Navigation Controller initialized")
        
    def init_database(self):
        """Initialize SQLite database for mission data"""
        conn = sqlite3.connect(self.config['database'])
        cursor = conn.cursor()
        
        # Create tables for mission data
        cursor.execute('''
            CREATE TABLE IF NOT EXISTS gps_data (
                timestamp REAL,
                latitude REAL,
                longitude REAL,
                altitude REAL,
                speed REAL,
                heading REAL
            )
        ''')
        
        cursor.execute('''
            CREATE TABLE IF NOT EXISTS star_observations (
                timestamp REAL,
                star_id TEXT,
                azimuth REAL,
                altitude REAL,
                magnitude REAL,
                calculated_position TEXT
            )
        ''')
        
        cursor.execute('''
            CREATE TABLE IF NOT EXISTS navigation_log (
                timestamp REAL,
                mission_state TEXT,
                current_position TEXT,
                target_position TEXT,
                distance_to_target REAL,
                ai_confidence REAL
            )
        ''')
        
        conn.commit()
        return conn
    
    def load_navigation_model(self):
        """Load AI navigation model"""
        class NavigationNet(nn.Module):
            def __init__(self, input_size=20, hidden_size=128, output_size=4):
                super(NavigationNet, self).__init__()
                self.fc1 = nn.Linear(input_size, hidden_size)
                self.fc2 = nn.Linear(hidden_size, hidden_size)
                self.fc3 = nn.Linear(hidden_size, output_size)
                self.relu = nn.ReLU()
                self.dropout = nn.Dropout(0.2)
                
            def forward(self, x):
                x = self.relu(self.fc1(x))
                x = self.dropout(x)
                x = self.relu(self.fc2(x))
                x = self.dropout(x)
                x = torch.tanh(self.fc3(x))  # Output: [heading, speed, confidence, risk]
                return x
        
        model = NavigationNet()
        try:
            model.load_state_dict(torch.load('space_navigation_model.pth', map_location='cpu'))
            model.eval()
            self.logger.info("✅ Navigation AI model loaded")
        except FileNotFoundError:
            self.logger.warning("⚠️ Navigation model not found, using random initialization")
        
        return model
    
    def load_star_catalog(self):
        """Load star catalog for celestial navigation"""
        # Simplified star catalog (in real implementation, use Hipparcos catalog)
        stars = {
            'Polaris': {'ra': 37.95, 'dec': 89.26, 'mag': 1.97},
            'Sirius': {'ra': 101.29, 'dec': -16.72, 'mag': -1.44},
            'Vega': {'ra': 279.23, 'dec': 38.78, 'mag': 0.03},
            'Arcturus': {'ra': 213.92, 'dec': 19.18, 'mag': -0.05},
            'Capella': {'ra': 79.17, 'dec': 45.99, 'mag': 0.08}
        }
        
        self.logger.info(f"✅ Star catalog loaded: {len(stars)} stars")
        return stars
    
    # ==================== STEP 1: SATELLITE TRACKING ====================
    
    def start_gps_tracking(self):
        """Start GPS satellite tracking thread"""
        def gps_reader():
            try:
                gps_serial = serial.Serial(self.config['gps_port'], 9600, timeout=1)
                self.logger.info("📡 GPS tracking started")
                
                while True:
                    line = gps_serial.readline().decode('ascii', errors='replace')
                    if line.startswith('$GPGGA'):
                        self.parse_gps_data(line)
                    time.sleep(0.1)
                    
            except Exception as e:
                self.logger.error(f"❌ GPS tracking error: {e}")
        
        gps_thread = threading.Thread(target=gps_reader, daemon=True)
        gps_thread.start()
    
    def parse_gps_data(self, nmea_sentence):
        """Parse NMEA GPS data"""
        try:
            msg = pynmea2.parse(nmea_sentence)
            if msg.gps_qual > 0:  # Valid GPS fix
                self.gps_data = {
                    'timestamp': time.time(),
                    'latitude': float(msg.latitude),
                    'longitude': float(msg.longitude),
                    'altitude': float(msg.altitude),
                    'satellites': int(msg.num_sats),
                    'hdop': float(msg.horizontal_dil)
                }
                
                # Store in database
                cursor = self.database.cursor()
                cursor.execute('''
                    INSERT INTO gps_data VALUES (?, ?, ?, ?, ?, ?)
                ''', (
                    self.gps_data['timestamp'],
                    self.gps_data['latitude'],
                    self.gps_data['longitude'],
                    self.gps_data['altitude'],
                    0,  # speed (calculated separately)
                    0   # heading (calculated separately)
                ))
                self.database.commit()
                
                # Add to position history
                self.position_history.append({
                    'lat': self.gps_data['latitude'],
                    'lon': self.gps_data['longitude'],
                    'time': self.gps_data['timestamp']
                })
                
        except Exception as e:
            self.logger.error(f"❌ GPS parsing error: {e}")
    
    def read_imu_data(self):
        """Read IMU sensor data from Arduino"""
        try:
            arduino = serial.Serial(self.config['arduino_port'], 115200, timeout=1)
            arduino.write(b'GET_IMU\n')
            response = arduino.readline().decode().strip()
            
            if response.startswith('IMU:'):
                values = response[4:].split(',')
                self.imu_data = {
                    'timestamp': time.time(),
                    'accel_x': float(values[0]),
                    'accel_y': float(values[1]),
                    'accel_z': float(values[2]),
                    'gyro_x': float(values[3]),
                    'gyro_y': float(values[4]),
                    'gyro_z': float(values[5]),
                    'mag_x': float(values[6]) if len(values) > 6 else 0,
                    'mag_y': float(values[7]) if len(values) > 7 else 0,
                    'mag_z': float(values[8]) if len(values) > 8 else 0
                }
                
                # Calculate heading from magnetometer
                heading = math.atan2(self.imu_data['mag_y'], self.imu_data['mag_x'])
                self.imu_data['heading'] = math.degrees(heading)
                
        except Exception as e:
            self.logger.error(f"❌ IMU reading error: {e}")
    
    # ==================== STEP 2: AI NAVIGATION ====================
    
    def process_camera_feed(self):
        """Process camera feed for obstacle detection"""
        cap = cv2.VideoCapture(self.config['camera_id'])
        
        while True:
            ret, frame = cap.read()
            if not ret:
                continue
                
            # Object detection using YOLO
            results = self.vision_model(frame, verbose=False)
            
            obstacles = []
            for result in results:
                boxes = result.boxes
                if boxes is not None:
                    for box in boxes:
                        x1, y1, x2, y2 = box.xyxy[0].cpu().numpy()
                        conf = box.conf[0].cpu().numpy()
                        cls = int(box.cls[0].cpu().numpy())
                        
                        if conf > 0.5:  # Confidence threshold
                            obstacles.append({
                                'class': cls,
                                'confidence': conf,
                                'bbox': [x1, y1, x2, y2],
                                'distance': self.estimate_distance(y2 - y1)  # Rough distance estimation
                            })
            
            # Update obstacle map
            self.update_obstacle_map(obstacles)
            
            # Display results (optional)
            self.draw_navigation_overlay(frame, obstacles)
            cv2.imshow('Space Navigation', frame)
            
            if cv2.waitKey(1) & 0xFF == ord('q'):
                break
        
        cap.release()
        cv2.destroyAllWindows()
    
    def estimate_distance(self, object_height_pixels):
        """Estimate distance to object based on pixel height"""
        # Simplified distance estimation
        # In real implementation, use stereo vision or LiDAR
        focal_length = 800  # Camera focal length in pixels
        real_height = 1.7   # Assumed average object height in meters
        
        if object_height_pixels > 0:
            distance = (real_height * focal_length) / object_height_pixels
            return min(distance, 100)  # Cap at 100 meters
        return 100
    
    def update_obstacle_map(self, obstacles):
        """Update local obstacle map"""
        # Clear previous obstacles (simple approach)
        self.obstacle_map *= 0.9  # Fade old obstacles
        
        # Add new obstacles
        for obstacle in obstacles:
            if obstacle['distance'] < 50:  # Only consider nearby obstacles
                # Convert to map coordinates (simplified)
                map_x = int(500 + obstacle['bbox'][0] / 10)  # Center map at 500,500
                map_y = int(500 + obstacle['bbox'][1] / 10)
                
                if 0 <= map_x < 1000 and 0 <= map_y < 1000:
                    self.obstacle_map[map_y, map_x] = obstacle['confidence']
    
    def calculate_navigation_command(self):
        """Use AI to calculate navigation command"""
        if not self.gps_data or not self.target_coordinates:
            return {'heading': 0, 'speed': 0, 'confidence': 0}
        
        # Prepare input features for AI model
        features = np.zeros(20)
        
        # Current position and target
        features[0] = self.gps_data['latitude']
        features[1] = self.gps_data['longitude']
        features[2] = self.target_coordinates['lat']
        features[3] = self.target_coordinates['lon']
        
        # Distance and bearing to target
        distance = self.calculate_distance(
            self.gps_data['latitude'], self.gps_data['longitude'],
            self.target_coordinates['lat'], self.target_coordinates['lon']
        )
        bearing = self.calculate_bearing(
            self.gps_data['latitude'], self.gps_data['longitude'],
            self.target_coordinates['lat'], self.target_coordinates['lon']
        )
        
        features[4] = distance / 1000  # Normalize to km
        features[5] = bearing / 360    # Normalize to [0,1]
        
        # IMU data
        if self.imu_data:
            features[6] = self.imu_data['accel_x'] / 10
            features[7] = self.imu_data['accel_y'] / 10
            features[8] = self.imu_data['accel_z'] / 10
            features[9] = self.imu_data['gyro_x'] / 100
            features[10] = self.imu_data['gyro_y'] / 100
            features[11] = self.imu_data['gyro_z'] / 100
            features[12] = self.imu_data['heading'] / 360
        
        # Obstacle information
        obstacle_density = np.mean(self.obstacle_map)
        features[13] = obstacle_density
        
        # Historical movement
        if len(self.position_history) > 1:
            recent_speed = self.calculate_speed()
            features[14] = recent_speed / 50  # Normalize
        
        # Time and environmental factors
        features[15] = (time.time() % 86400) / 86400  # Time of day
        features[16] = self.gps_data.get('satellites', 0) / 12  # GPS quality
        
        # AI prediction
        try:
            with torch.no_grad():
                input_tensor = torch.FloatTensor(features).unsqueeze(0)
                output = self.ai_model(input_tensor)
                
                navigation_command = {
                    'heading': float(output[0][0]) * 360,  # Convert to degrees
                    'speed': float(output[0][1]) * 50,     # Convert to km/h
                    'confidence': float(output[0][2]),
                    'risk_assessment': float(output[0][3])
                }
                
                return navigation_command
                
        except Exception as e:
            self.logger.error(f"❌ AI navigation error: {e}")
            return {'heading': bearing, 'speed': 10, 'confidence': 0.5}
    
    # ==================== STEP 3: STAR TRACKING ====================
    
    def start_star_tracking(self):
        """Start star tracking system"""
        def star_tracker():
            while True:
                try:
                    # Capture image from telescope camera
                    star_image = self.capture_star_field()
                    
                    # Detect stars in image
                    detected_stars = self.detect_stars(star_image)
                    
                    # Match with star catalog
                    matched_stars = self.match_star_patterns(detected_stars)
                    
                    # Calculate position from star observations
                    if len(matched_stars) >= 3:
                        calculated_position = self.celestial_navigation(matched_stars)
                        self.validate_gps_with_stars(calculated_position)
                    
                    # Update telescope pointing
                    self.update_telescope_pointing()
                    
                    time.sleep(1)  # 1 Hz update rate
                    
                except Exception as e:
                    self.logger.error(f"❌ Star tracking error: {e}")
                    time.sleep(5)
        
        star_thread = threading.Thread(target=star_tracker, daemon=True)
        star_thread.start()
        self.logger.info("⭐ Star tracking started")
    
    def capture_star_field(self):
        """Capture image from star tracking camera"""
        # In real implementation, use specialized astronomy camera
        cap = cv2.VideoCapture(1)  # Assume second camera is star tracker
        ret, frame = cap.read()
        cap.release()
        
        if ret:
            # Convert to grayscale for star detection
            gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
            return gray
        return None
    
    def detect_stars(self, image):
        """Detect stars in the captured image"""
        if image is None:
            return []
        
        # Apply Gaussian blur to reduce noise
        blurred = cv2.GaussianBlur(image, (5, 5), 0)
        
        # Threshold to find bright objects (stars)
        _, thresh = cv2.threshold(blurred, 200, 255, cv2.THRESH_BINARY)
        
        # Find contours (star candidates)
        contours, _ = cv2.findContours(thresh, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        
        stars = []
        for contour in contours:
            # Calculate centroid and brightness
            M = cv2.moments(contour)
            if M["m00"] != 0:
                cx = int(M["m10"] / M["m00"])
                cy = int(M["m01"] / M["m00"])
                
                # Estimate magnitude from contour area
                area = cv2.contourArea(contour)
                magnitude = 6 - math.log10(area + 1)  # Rough magnitude estimation
                
                stars.append({
                    'x': cx,
                    'y': cy,
                    'magnitude': magnitude,
                    'area': area
                })
        
        # Sort by brightness (magnitude)
        stars.sort(key=lambda s: s['magnitude'])
        return stars[:20]  # Return top 20 brightest stars
    
    def match_star_patterns(self, detected_stars):
        """Match detected stars with catalog using pattern recognition"""
        matched_stars = []
        
        if len(detected_stars) < 3:
            return matched_stars
        
        # Simple pattern matching (in real implementation, use more sophisticated algorithms)
        for star_name, star_data in self.star_catalog.items():
            # Calculate expected position based on current time and location
            if self.gps_data:
                expected_pos = self.calculate_star_position(star_data, 
                                                          self.gps_data['latitude'], 
                                                          self.gps_data['longitude'])
                
                # Find closest detected star
                min_distance = float('inf')
                best_match = None
                
                for detected in detected_stars:
                    # Convert pixel coordinates to angular coordinates
                    az, alt = self.pixel_to_angular(detected['x'], detected['y'])
                    
                    # Calculate angular distance
                    distance = math.sqrt((az - expected_pos['azimuth'])**2 + 
                                       (alt - expected_pos['altitude'])**2)
                    
                    if distance < min_distance and distance < 5:  # 5 degree tolerance
                        min_distance = distance
                        best_match = detected
                
                if best_match:
                    matched_stars.append({
                        'name': star_name,
                        'catalog_data': star_data,
                        'detected_data': best_match,
                        'expected_position': expected_pos,
                        'match_confidence': 1.0 / (1.0 + min_distance)
                    })
        
        return matched_stars
    
    def calculate_star_position(self, star_data, latitude, longitude):
        """Calculate expected star position for given location and time"""
        try:
            # Create star coordinate
            star_coord = SkyCoord(ra=star_data['ra']*u.degree, 
                                dec=star_data['dec']*u.degree, 
                                frame='icrs')
            
            # Observer location
            location = EarthLocation(lat=latitude*u.deg, lon=longitude*u.deg)
            
            # Current time
            obs_time = Time.now()
            
            # Transform to horizontal coordinates
            altaz_frame = AltAz(obstime=obs_time, location=location)
            star_altaz = star_coord.transform_to(altaz_frame)
            
            return {
                'azimuth': star_altaz.az.degree,
                'altitude': star_altaz.alt.degree
            }
            
        except Exception as e:
            self.logger.error(f"❌ Star position calculation error: {e}")
            return {'azimuth': 0, 'altitude': 0}
    
    def pixel_to_angular(self, x, y):
        """Convert pixel coordinates to angular coordinates"""
        # Camera field of view (degrees)
        fov_horizontal = 15.0
        fov_vertical = 10.0
        
        # Image dimensions (assume 640x480)
        img_width = 640
        img_height = 480
        
        # Convert to angular coordinates
        azimuth = (x - img_width/2) * fov_horizontal / img_width
        altitude = (img_height/2 - y) * fov_vertical / img_height
        
        return azimuth, altitude
    
    def celestial_navigation(self, matched_stars):
        """Calculate position using celestial navigation"""
        if len(matched_stars) < 3:
            return None
        
        # Use spherical trigonometry to calculate position
        # This is a simplified implementation
        
        total_lat = 0
        total_lon = 0
        total_weight = 0
        
        for star in matched_stars:
            weight = star['match_confidence']
            
            # Simplified position calculation
            # In real implementation, use proper celestial navigation algorithms
            estimated_lat = self.gps_data['latitude'] + (star['detected_data']['y'] - 240) * 0.01
            estimated_lon = self.gps_data['longitude'] + (star['detected_data']['x'] - 320) * 0.01
            
            total_lat += estimated_lat * weight
            total_lon += estimated_lon * weight
            total_weight += weight
        
        if total_weight > 0:
            calculated_position = {
                'latitude': total_lat / total_weight,
                'longitude': total_lon / total_weight,
                'confidence': total_weight / len(matched_stars),
                'method': 'celestial_navigation',
                'stars_used': len(matched_stars)
            }
            
            # Store in database
            cursor = self.database.cursor()
            cursor.execute('''
                INSERT INTO star_observations VALUES (?, ?, ?, ?, ?, ?)
            ''', (
                time.time(),
                ','.join([s['name'] for s in matched_stars]),
                calculated_position['latitude'],
                calculated_position['longitude'],
                0,  # magnitude
                json.dumps(calculated_position)
            ))
            self.database.commit()
            
            return calculated_position
        
        return None
    
    def validate_gps_with_stars(self, star_position):
        """Validate GPS position with star-based navigation"""
        if not star_position or not self.gps_data:
            return
        
        # Calculate difference between GPS and star-based position
        lat_diff = abs(self.gps_data['latitude'] - star_position['latitude'])
        lon_diff = abs(self.gps_data['longitude'] - star_position['longitude'])
        
        distance_diff = self.calculate_distance(
            self.gps_data['latitude'], self.gps_data['longitude'],
            star_position['latitude'], star_position['longitude']
        )
        
        if distance_diff > 1000:  # More than 1km difference
            self.logger.warning(f"⚠️ GPS/Star position mismatch: {distance_diff:.1f}m")
            
            # Use star position if confidence is high
            if star_position['confidence'] > 0.8:
                self.logger.info("🌟 Using star-based navigation as primary")
                self.gps_data.update({
                    'latitude': star_position['latitude'],
                    'longitude': star_position['longitude'],
                    'source': 'celestial'
                })
        else:
            self.logger.info(f"✅ GPS/Star position validated: {distance_diff:.1f}m difference")
    
    def update_telescope_pointing(self):
        """Update telescope servo positions for star tracking"""
        try:
            arduino = serial.Serial(self.config['arduino_port'], 115200, timeout=1)
            
            # Calculate servo positions for current target
            if self.tracked_stars:
                target_star = self.tracked_stars[0]  # Track brightest star
                
                # Convert to servo angles (0-180 degrees)
                az_servo = int((target_star['expected_position']['azimuth'] + 180) / 2)
                alt_servo = int((target_star['expected_position']['altitude'] + 90) / 2)
                
                # Constrain to servo limits
                az_servo = max(0, min(180, az_servo))
                alt_servo = max(0, min(180, alt_servo))
                
                # Send servo commands
                cmd = f"TELESCOPE:{az_servo},{alt_servo}\n"
                arduino.write(cmd.encode())
                
                self.telescope_position = {'az': az_servo, 'alt': alt_servo}
                
        except Exception as e:
            self.logger.error(f"❌ Telescope control error: {e}")
    
    # ==================== UTILITY FUNCTIONS ====================
    
    def calculate_distance(self, lat1, lon1, lat2, lon2):
        """Calculate distance between two GPS coordinates (Haversine formula)"""
        R = 6371000  # Earth radius in meters
        
        lat1_rad = math.radians(lat1)
        lat2_rad = math.radians(lat2)
        delta_lat = math.radians(lat2 - lat1)
        delta_lon = math.radians(lon2 - lon1)
        
        a = (math.sin(delta_lat/2)**2 + 
             math.cos(lat1_rad) * math.cos(lat2_rad) * math.sin(delta_lon/2)**2)
        c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
        
        return R * c
    
    def calculate_bearing(self, lat1, lon1, lat2, lon2):
        """Calculate bearing from point 1 to point 2"""
        lat1_rad = math.radians(lat1)
        lat2_rad = math.radians(lat2)
        delta_lon = math.radians(lon2 - lon1)
        
        y = math.sin(delta_lon) * 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))
        
        bearing = math.atan2(y, x)
        return (math.degrees(bearing) + 360) % 360
    
    def calculate_speed(self):
        """Calculate current speed from position history"""
        if len(self.position_history) < 2:
            return 0
        
        recent = self.position_history[-1]
        previous = self.position_history[-2]
        
        distance = self.calculate_distance(
            previous['lat'], previous['lon'],
            recent['lat'], recent['lon']
        )
        
        time_diff = recent['time'] - previous['time']
        
        if time_diff > 0:
            speed = distance / time_diff  # m/s
            return speed * 3.6  # Convert to km/h
        
        return 0
    
    def draw_navigation_overlay(self, frame, obstacles):
        """Draw navigation information on camera frame"""
        # Draw obstacles
        for obstacle in obstacles:
            x1, y1, x2, y2 = map(int, obstacle['bbox'])
            cv2.rectangle(frame, (x1, y1), (x2, y2), (0, 0, 255), 2)
            
            # Distance label
            distance_text = f"{obstacle['distance']:.1f}m"
            cv2.putText(frame, distance_text, (x1, y1-10), 
                       cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)
        
        # Draw navigation info
        if self.gps_data:
            info_text = f"GPS: {self.gps_data['latitude']:.6f}, {self.gps_data['longitude']:.6f}"
            cv2.putText(frame, info_text, (10, 30), 
                       cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
        
        if self.target_coordinates:
            distance = self.calculate_distance(
                self.gps_data['latitude'], self.gps_data['longitude'],
                self.target_coordinates['lat'], self.target_coordinates['lon']
            )
            target_text = f"Target: {distance:.0f}m"
            cv2.putText(frame, target_text, (10, 60), 
                       cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 2)
        
        # Mission state
        state_text = f"State: {self.mission_state}"
        cv2.putText(frame, state_text, (10, 90), 
                   cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 0), 2)
    
    def send_telemetry(self):
        """Send telemetry data via LoRa"""
        try:
            lora = serial.Serial(self.config['lora_port'], 9600, timeout=1)
            
            telemetry = {
                'timestamp': time.time(),
                'mission_state': self.mission_state,
                'gps': self.gps_data,
                'imu': self.imu_data,
                'stars_tracked': len(self.tracked_stars),
                'telescope_position': self.telescope_position
            }
            
            # Send compressed telemetry
            telemetry_json = json.dumps(telemetry, separators=(',', ':'))
            lora.write(f"TELEM:{telemetry_json}\n".encode())
            
            self.telemetry_buffer.append(telemetry)
            
        except Exception as e:
            self.logger.error(f"❌ Telemetry error: {e}")
    
    def run_mission(self, target_lat, target_lon):
        """Run complete 3-step space navigation mission"""
        self.logger.info("🚀 Starting space navigation mission")
        
        # Set target coordinates
        self.target_coordinates = {'lat': target_lat, 'lon': target_lon}
        
        # Step 1: Start satellite tracking
        self.mission_state = 'SATELLITE_TRACKING'
        self.start_gps_tracking()
        
        # Wait for GPS lock
        while not self.gps_data:
            self.logger.info("📡 Waiting for GPS lock...")
            time.sleep(2)
        
        self.logger.info("✅ GPS lock acquired")
        
        # Step 2: Start AI navigation
        self.mission_state = 'AI_NAVIGATION'
        
        # Start camera processing thread
        camera_thread = threading.Thread(target=self.process_camera_feed, daemon=True)
        camera_thread.start()
        
        # Step 3: Start star tracking
        self.mission_state = 'STAR_TRACKING'
        self.start_star_tracking()
        
        # Main mission loop
        try:
            while True:
                # Read sensors
                self.read_imu_data()
                
                # Calculate navigation command
                nav_command = self.calculate_navigation_command()
                
                # Log navigation data
                cursor = self.database.cursor()
                cursor.execute('''
                    INSERT INTO navigation_log VALUES (?, ?, ?, ?, ?, ?)
                ''', (
                    time.time(),
                    self.mission_state,
                    json.dumps(self.gps_data),
                    json.dumps(self.target_coordinates),
                    self.calculate_distance(
                        self.gps_data['latitude'], self.gps_data['longitude'],
                        self.target_coordinates['lat'], self.target_coordinates['lon']
                    ),
                    nav_command['confidence']
                ))
                self.database.commit()
                
                # Send telemetry
                self.send_telemetry()
                
                # Check if target reached
                distance_to_target = self.calculate_distance(
                    self.gps_data['latitude'], self.gps_data['longitude'],
                    self.target_coordinates['lat'], self.target_coordinates['lon']
                )
                
                if distance_to_target < 10:  # Within 10 meters
                    self.logger.info("🎯 Target reached!")
                    self.mission_state = 'TARGET_REACHED'
                    break
                
                # Display status
                self.logger.info(f"📍 Distance to target: {distance_to_target:.1f}m")
                self.logger.info(f"🧭 Navigation: {nav_command['heading']:.1f}° @ {nav_command['speed']:.1f}km/h")
                
                time.sleep(1)
                
        except KeyboardInterrupt:
            self.logger.info("🛑 Mission interrupted by user")
        except Exception as e:
            self.logger.error(f"❌ Mission error: {e}")
        finally:
            self.database.close()
            self.logger.info("📊 Mission data saved to database")

def main():
    """Main function to run space navigation mission"""
    try:
        # Initialize navigation controller
        navigator = SpaceNavigationController()
        
        # Example mission: Navigate to specific coordinates
        target_latitude = 13.7563  # Bangkok, Thailand
        target_longitude = 100.5018
        
        # Run the mission
        navigator.run_mission(target_latitude, target_longitude)
        
    except Exception as e:
        print(f"❌ Fatal error: {e}")

if __name__ == '__main__':
    main()