# main_com_testes.py - Sistema com Testes Iniciais Completos
# Nave Mãe: 8 servos | Planador: 4 servos | Relatório de Testes

import machine
import time
import math
import struct
from machine import Pin, PWM, I2C

# =================================================================
# CONFIGURAÇÃO DE HARDWARE
# =================================================================

# Servos da NAVE MÃE (8 servos)
NAVE_MAE_SERVOS = {
    'aileron_left': 13,
    'aileron_right': 12,
    'flaps_left': 25,
    'flaps_right': 26,
    'airbrake': 33,
    'elevator_left': 14,
    'elevator_right': 32,
    'rudder': 27
}

# Servos do PLANADOR (4 servos)
PLANADOR_SERVOS = {
    'flaps_left': 25,
    'flaps_right': 26,
    'elevator': 14,
    'rudder': 27
}

# LEDs do sistema
LED_SYSTEM_ACTIVE = 15    # Verde - Sistema ativo/inativo
LED_AIRCRAFT_TYPE = 2     # Azul - Tipo de aeronave
LED_MODE = 4              # Amarelo - Modo de voo
LED_ALERT = 19            # Laranja - Alertas

# CONTROLES (2 botões + 1 chave)
BUTTON_MODE_PIN = 34      # Botão MODO (azul)
BUTTON_POWER_PIN = 18     # Botão POWER (vermelho)
SWITCH_AIRCRAFT_PIN = 35  # Chave aeronave

# Sensores
I2C_SDA = 21
I2C_SCL = 22
MPU6050_ADDR = 0x68

# =================================================================
# CONFIGURAÇÕES DAS AERONAVES
# =================================================================

AIRCRAFT_CONFIGS = {
    'PLANADOR': {
        'name': 'PLANADOR',
        'description': 'Planador - 2 Flaps + 1 Elevator + 1 Leme',
        'active_servos': ['flaps_left', 'flaps_right', 'elevator', 'rudder'],
        'total_servos': 4,
        'flight_modes': {
            'THERMAL': {
                'name': 'TÉRMICAS',
                'description': 'Voo em térmicas - flaps neutros',
                'pid_gains': {
                    'roll_kp': 1.5, 'roll_ki': 0.1, 'roll_kd': 0.8,
                    'pitch_kp': 2.0, 'pitch_ki': 0.12, 'pitch_kd': 1.0,
                    'yaw_kp': 1.0, 'yaw_ki': 0.05, 'yaw_kd': 0.4
                },
                'max_deflection': {'roll': 20, 'pitch': 25, 'yaw': 15},
                'target_attitude': {'roll': 0, 'pitch': 3},
                'flaps_position': 0
            },
            'CRUISE': {
                'name': 'CRUZEIRO',
                'description': 'Voo de cruzeiro - flaps baixos',
                'pid_gains': {
                    'roll_kp': 2.0, 'roll_ki': 0.15, 'roll_kd': 1.0,
                    'pitch_kp': 2.5, 'pitch_ki': 0.18, 'pitch_kd': 1.2,
                    'yaw_kp': 1.2, 'yaw_ki': 0.06, 'yaw_kd': 0.5
                },
                'max_deflection': {'roll': 25, 'pitch': 30, 'yaw': 18},
                'target_attitude': {'roll': 0, 'pitch': 0},
                'flaps_position': 10
            },
            'LANDING': {
                'name': 'POUSO',
                'description': 'Aproximação - flaps máximos',
                'pid_gains': {
                    'roll_kp': 2.5, 'roll_ki': 0.18, 'roll_kd': 1.2,
                    'pitch_kp': 3.0, 'pitch_ki': 0.2, 'pitch_kd': 1.5,
                    'yaw_kp': 1.8, 'yaw_ki': 0.1, 'yaw_kd': 0.8
                },
                'max_deflection': {'roll': 30, 'pitch': 35, 'yaw': 20},
                'target_attitude': {'roll': 0, 'pitch': -2},
                'flaps_position': 25
            }
        }
    },

    'NAVE_MAE': {
        'name': 'NAVE MÃE',
        'description': 'Nave Mãe - 2 Ailerons + 2 Flaps + 1 Freio + 2 Elevators + 1 Leme',
        'active_servos': ['aileron_left', 'aileron_right', 'flaps_left', 'flaps_right', 'airbrake', 'elevator_left', 'elevator_right', 'rudder'],
        'total_servos': 8,
        'flight_modes': {
            'CRUISE': {
                'name': 'CRUZEIRO',
                'description': 'Voo de cruzeiro - configuração limpa',
                'pid_gains': {
                    'roll_kp': 1.8, 'roll_ki': 0.12, 'roll_kd': 0.9,
                    'pitch_kp': 1.5, 'pitch_ki': 0.1, 'pitch_kd': 0.8,
                    'yaw_kp': 1.0, 'yaw_ki': 0.05, 'yaw_kd': 0.4
                },
                'max_deflection': {'roll': 20, 'pitch': 20, 'yaw': 15},
                'target_attitude': {'roll': 0, 'pitch': 2},
                'flaps_position': 0,
                'airbrake_position': 0
            },
            'APPROACH': {
                'name': 'APROXIMAÇÃO',
                'description': 'Aproximação - flaps parciais',
                'pid_gains': {
                    'roll_kp': 2.2, 'roll_ki': 0.16, 'roll_kd': 1.1,
                    'pitch_kp': 2.0, 'pitch_ki': 0.14, 'pitch_kd': 1.0,
                    'yaw_kp': 1.4, 'yaw_ki': 0.08, 'yaw_kd': 0.6
                },
                'max_deflection': {'roll': 25, 'pitch': 25, 'yaw': 18},
                'target_attitude': {'roll': 0, 'pitch': -1},
                'flaps_position': 15,
                'airbrake_position': 0
            },
            'LANDING': {
                'name': 'POUSO',
                'description': 'Pouso - configuração completa',
                'pid_gains': {
                    'roll_kp': 2.8, 'roll_ki': 0.2, 'roll_kd': 1.4,
                    'pitch_kp': 2.5, 'pitch_ki': 0.18, 'pitch_kd': 1.2,
                    'yaw_kp': 2.0, 'yaw_ki': 0.12, 'yaw_kd': 1.0
                },
                'max_deflection': {'roll': 30, 'pitch': 30, 'yaw': 20},
                'target_attitude': {'roll': 0, 'pitch': -3},
                'flaps_position': 30,
                'airbrake_position': 20
            }
        }
    }
}

# =================================================================
# SISTEMA DE TESTES COMPLETO
# =================================================================

class TestReport:
    """Classe para gerar relatórios de teste"""
    def __init__(self):
        self.tests = []
        self.start_time = time.ticks_ms()
        
    def add_test(self, category, name, status, details="", duration=0):
        test = {
            'category': category,
            'name': name,
            'status': status,  # 'PASS', 'FAIL', 'SKIP', 'WARN'
            'details': details,
            'duration': duration,
            'timestamp': time.ticks_ms()
        }
        self.tests.append(test)
        
    def get_summary(self):
        total = len(self.tests)
        passed = sum(1 for t in self.tests if t['status'] == 'PASS')
        failed = sum(1 for t in self.tests if t['status'] == 'FAIL')
        warned = sum(1 for t in self.tests if t['status'] == 'WARN')
        skipped = sum(1 for t in self.tests if t['status'] == 'SKIP')
        
        total_time = time.ticks_diff(time.ticks_ms(), self.start_time) / 1000.0
        
        return {
            'total': total,
            'passed': passed,
            'failed': failed,
            'warned': warned,
            'skipped': skipped,
            'success_rate': (passed / total * 100) if total > 0 else 0,
            'total_time': total_time
        }
        
    def print_report(self):
        summary = self.get_summary()
        
        print("\n" + "=" * 80)
        print("📋 RELATÓRIO DE TESTES COMPLETO")
        print("=" * 80)
        
        # Resumo geral
        print(f"\n📊 RESUMO GERAL:")
        print(f"   Total de testes: {summary['total']}")
        print(f"   ✅ Passou: {summary['passed']}")
        print(f"   ❌ Falhou: {summary['failed']}")
        print(f"   ⚠️  Avisos: {summary['warned']}")
        print(f"   ⏭️  Pulados: {summary['skipped']}")
        print(f"   📈 Taxa de sucesso: {summary['success_rate']:.1f}%")
        print(f"   ⏱️  Tempo total: {summary['total_time']:.1f}s")
        
        # Status geral
        if summary['failed'] == 0:
            if summary['warned'] == 0:
                print(f"\n🎉 RESULTADO: TODOS OS TESTES PASSARAM!")
            else:
                print(f"\n⚠️  RESULTADO: TESTES OK COM AVISOS")
        else:
            print(f"\n❌ RESULTADO: FALHAS DETECTADAS - REVISAR")
        
        # Detalhes por categoria
        categories = {}
        for test in self.tests:
            cat = test['category']
            if cat not in categories:
                categories[cat] = []
            categories[cat].append(test)
        
        print(f"\n📋 DETALHES POR CATEGORIA:")
        for category, tests in categories.items():
            print(f"\n📁 {category}:")
            for test in tests:
                status_icon = {
                    'PASS': '✅',
                    'FAIL': '❌',
                    'WARN': '⚠️',
                    'SKIP': '⏭️'
                }.get(test['status'], '❓')
                
                print(f"   {status_icon} {test['name']}")
                if test['details']:
                    print(f"      └─ {test['details']}")
        
        print("\n" + "=" * 80)

class SistemaComTestes:
    def __init__(self):
        print("=" * 80)
        print("🧪 SISTEMA COM TESTES INICIAIS COMPLETOS")
        print("=" * 80)

        self.aircraft_type = 'PLANADOR'
        self.flight_mode = 'THERMAL'
        self.system_active = False
        self.loop_count = 0
        self.last_telemetry_time = 0

        # Relatório de testes
        self.test_report = TestReport()

        # Estados dos controles
        self.mode_button_last = False
        self.power_button_last = False
        self.switch_last = False
        self.last_button_time = 0

        # Estados PID
        self.pid_state = {
            'roll': {'error_prev': 0, 'integral': 0},
            'pitch': {'error_prev': 0, 'integral': 0},
            'yaw': {'error_prev': 0, 'integral': 0}
        }

        # Dados de voo
        self.flight_data = {
            'roll': 0,
            'pitch': 0,
            'yaw_rate': 0,
            'commands': {},
            'pid_outputs': {}
        }

        # Executar testes iniciais
        self.run_initial_tests()

    def run_initial_tests(self):
        """Executa todos os testes iniciais"""
        print("\n🚀 INICIANDO TESTES INICIAIS...")
        print("Este processo verificará todos os componentes do sistema")
        
        # 1. Testes de hardware
        self.test_hardware()
        
        # 2. Testes de sensores
        self.test_sensors()
        
        # 3. Testes de servos
        self.test_all_servos()
        
        # 4. Testes de controles
        self.test_controls()
        
        # 5. Testes de PID e estabilização
        self.test_stabilization_system()
        
        # 6. Testes específicos por aeronave
        self.test_aircraft_specific()
        
        # 7. Gerar relatório final
        self.test_report.print_report()

    def test_hardware(self):
        """Testa componentes de hardware básico"""
        print("\n🔧 TESTANDO HARDWARE BÁSICO...")
        
        # Teste LEDs
        start_time = time.ticks_ms()
        try:
            self.led_system_active = Pin(LED_SYSTEM_ACTIVE, Pin.OUT)
            self.led_aircraft_type = Pin(LED_AIRCRAFT_TYPE, Pin.OUT)
            self.led_mode = Pin(LED_MODE, Pin.OUT)
            self.led_alert = Pin(LED_ALERT, Pin.OUT)
            
            # Teste sequencial dos LEDs
            leds = [
                (self.led_system_active, "Sistema (Verde)"),
                (self.led_aircraft_type, "Aeronave (Azul)"),
                (self.led_mode, "Modo (Amarelo)"),
                (self.led_alert, "Alerta (Laranja)")
            ]
            
            all_leds_ok = True
            for led, name in leds:
                try:
                    led.value(1)
                    time.sleep_ms(200)
                    led.value(0)
                    time.sleep_ms(100)
                    self.test_report.add_test("HARDWARE", f"LED {name}", "PASS")
                except Exception as e:
                    self.test_report.add_test("HARDWARE", f"LED {name}", "FAIL", str(e))
                    all_leds_ok = False
                    
            duration = time.ticks_diff(time.ticks_ms(), start_time)
            if all_leds_ok:
                self.test_report.add_test("HARDWARE", "Sistema de LEDs", "PASS", "4 LEDs funcionando", duration)
            else:
                self.test_report.add_test("HARDWARE", "Sistema de LEDs", "WARN", "Alguns LEDs com problemas", duration)
                
        except Exception as e:
            duration = time.ticks_diff(time.ticks_ms(), start_time)
            self.test_report.add_test("HARDWARE", "Sistema de LEDs", "FAIL", str(e), duration)

        # Teste Controles
        start_time = time.ticks_ms()
        controls_ok = 0
        total_controls = 3
        
        # Botão Modo
        try:
            self.mode_button = Pin(BUTTON_MODE_PIN, Pin.IN, Pin.PULL_UP)
            initial_state = self.mode_button.value()
            self.test_report.add_test("HARDWARE", "Botão MODO (GPIO 34)", "PASS", f"Estado: {initial_state}")
            controls_ok += 1
        except Exception as e:
            self.test_report.add_test("HARDWARE", "Botão MODO (GPIO 34)", "FAIL", str(e))
            
        # Botão Power
        try:
            self.power_button = Pin(BUTTON_POWER_PIN, Pin.IN, Pin.PULL_UP)
            initial_state = self.power_button.value()
            self.test_report.add_test("HARDWARE", "Botão POWER (GPIO 18)", "PASS", f"Estado: {initial_state}")
            controls_ok += 1
        except Exception as e:
            self.test_report.add_test("HARDWARE", "Botão POWER (GPIO 18)", "FAIL", str(e))
            self.power_button = None
            
        # Chave Aeronave
        try:
            self.switch = Pin(SWITCH_AIRCRAFT_PIN, Pin.IN, Pin.PULL_UP)
            initial_state = self.switch.value()
            aircraft_selected = "NAVE MÃE" if not initial_state else "PLANADOR"
            self.test_report.add_test("HARDWARE", "Chave Aeronave (GPIO 35)", "PASS", f"Posição: {aircraft_selected}")
            controls_ok += 1
        except Exception as e:
            self.test_report.add_test("HARDWARE", "Chave Aeronave (GPIO 35)", "FAIL", str(e))
        
        duration = time.ticks_diff(time.ticks_ms(), start_time)
        if controls_ok == total_controls:
            self.test_report.add_test("HARDWARE", "Sistema de Controles", "PASS", f"{controls_ok}/{total_controls} controles OK", duration)
        elif controls_ok > 0:
            self.test_report.add_test("HARDWARE", "Sistema de Controles", "WARN", f"{controls_ok}/{total_controls} controles OK", duration)
        else:
            self.test_report.add_test("HARDWARE", "Sistema de Controles", "FAIL", "Nenhum controle funcionando", duration)

    def test_sensors(self):
        """Testa sensores do sistema"""
        print("\n🔬 TESTANDO SENSORES...")
        
        start_time = time.ticks_ms()
        try:
            # Inicializar I2C
            self.i2c = I2C(0, sda=Pin(I2C_SDA), scl=Pin(I2C_SCL), freq=100000)
            self.test_report.add_test("SENSORES", "Barramento I2C", "PASS", f"SDA: GPIO{I2C_SDA}, SCL: GPIO{I2C_SCL}")
            
            # Testar MPU6050
            try:
                # Wake up
                self.i2c.writeto_mem(MPU6050_ADDR, 0x6B, b'\x00')
                time.sleep_ms(100)
                
                # Verificar WHO_AM_I
                who_am_i = self.i2c.readfrom_mem(MPU6050_ADDR, 0x75, 1)[0]
                if who_am_i == 0x68:
                    self.test_report.add_test("SENSORES", "MPU6050 Comunicação", "PASS", f"WHO_AM_I: 0x{who_am_i:02X}")
                    
                    # Testar leitura de dados
                    accel_data = self.i2c.readfrom_mem(MPU6050_ADDR, 0x3B, 6)
                    gyro_data = self.i2c.readfrom_mem(MPU6050_ADDR, 0x43, 6)
                    
                    # Processar dados
                    ax = struct.unpack('>h', accel_data[0:2])[0] / 16384.0
                    ay = struct.unpack('>h', accel_data[2:4])[0] / 16384.0
                    az = struct.unpack('>h', accel_data[4:6])[0] / 16384.0
                    
                    gx = struct.unpack('>h', gyro_data[0:2])[0] / 131.0
                    gy = struct.unpack('>h', gyro_data[2:4])[0] / 131.0
                    gz = struct.unpack('>h', gyro_data[4:6])[0] / 131.0
                    
                    # Calcular atitude
                    roll = math.atan2(ay, az) * 180 / math.pi
                    pitch = math.atan2(-ax, math.sqrt(ay**2 + az**2)) * 180 / math.pi
                    
                    # Verificar se os dados são razoáveis
                    accel_magnitude = math.sqrt(ax**2 + ay**2 + az**2)
                    
                    if 0.8 < accel_magnitude < 1.2:  # Próximo de 1g
                        self.test_report.add_test("SENSORES", "MPU6050 Dados", "PASS", 
                                                f"Roll: {roll:.1f}°, Pitch: {pitch:.1f}°, |a|: {accel_magnitude:.2f}g")
                        self.sensors_available = True
                    else:
                        self.test_report.add_test("SENSORES", "MPU6050 Dados", "WARN", 
                                                f"Aceleração anômala: {accel_magnitude:.2f}g")
                        self.sensors_available = True
                        
                else:
                    self.test_report.add_test("SENSORES", "MPU6050 Comunicação", "FAIL", f"WHO_AM_I inválido: 0x{who_am_i:02X}")
                    self.sensors_available = False
                    
            except Exception as e:
                self.test_report.add_test("SENSORES", "MPU6050", "FAIL", str(e))
                self.sensors_available = False
                
        except Exception as e:
            self.test_report.add_test("SENSORES", "Barramento I2C", "FAIL", str(e))
            self.i2c = None
            self.sensors_available = False
            
        duration = time.ticks_diff(time.ticks_ms(), start_time)
        
        if self.sensors_available:
            self.test_report.add_test("SENSORES", "Sistema de Sensores", "PASS", "MPU6050 funcionando", duration)
        else:
            self.test_report.add_test("SENSORES", "Sistema de Sensores", "WARN", "Modo simulação ativado", duration)

    def test_all_servos(self):
        """Testa todos os servos do sistema"""
        print("\n🎛️ TESTANDO SERVOS...")
        
        # Configurar todos os servos
        self.servos = {}
        
        # Teste servos da Nave Mãe
        print("   📍 Testando servos da NAVE MÃE...")
        nave_servos_ok = 0
        for name, pin in NAVE_MAE_SERVOS.items():
            start_time = time.ticks_ms()
            try:
                servo = PWM(Pin(pin), freq=50)
                
                # Teste de movimento básico
                positions = [90, 60, 120, 90]  # Centro, mín, máx, centro
                for pos in positions:
                    servo.duty(self.angle_to_duty(pos))
                    time.sleep_ms(200)
                
                self.servos[name] = servo
                duration = time.ticks_diff(time.ticks_ms(), start_time)
                self.test_report.add_test("SERVOS_NAVE", f"{name} (GPIO {pin})", "PASS", "Movimento OK", duration)
                nave_servos_ok += 1
                
            except Exception as e:
                duration = time.ticks_diff(time.ticks_ms(), start_time)
                self.test_report.add_test("SERVOS_NAVE", f"{name} (GPIO {pin})", "FAIL", str(e), duration)
        
        # Teste servos do Planador (apenas os que não foram testados)
        print("   📍 Testando servos exclusivos do PLANADOR...")
        planador_servos_ok = 0
        for name, pin in PLANADOR_SERVOS.items():
            if name not in self.servos:  # Servo exclusivo do planador
                start_time = time.ticks_ms()
                try:
                    servo = PWM(Pin(pin), freq=50)
                    
                    # Teste de movimento
                    positions = [90, 60, 120, 90]
                    for pos in positions:
                        servo.duty(self.angle_to_duty(pos))
                        time.sleep_ms(200)
                    
                    self.servos[name] = servo
                    duration = time.ticks_diff(time.ticks_ms(), start_time)
                    self.test_report.add_test("SERVOS_PLANADOR", f"{name} (GPIO {pin})", "PASS", "Movimento OK", duration)
                    planador_servos_ok += 1
                    
                except Exception as e:
                    duration = time.ticks_diff(time.ticks_ms(), start_time)
                    self.test_report.add_test("SERVOS_PLANADOR", f"{name} (GPIO {pin})", "FAIL", str(e), duration)
            else:
                # Servo compartilhado
                self.test_report.add_test("SERVOS_PLANADOR", f"{name} (GPIO {pin})", "PASS", "Compartilhado com Nave Mãe")
                planador_servos_ok += 1
        
        # Resumo dos servos
        total_servos = len(self.servos)
        expected_nave = len(NAVE_MAE_SERVOS)
        expected_planador = len(PLANADOR_SERVOS)
        
        self.test_report.add_test("SERVOS", "Total de Servos", "PASS" if total_servos > 0 else "FAIL", 
                                f"{total_servos} servos configurados")
        
        print(f"   ✅ {total_servos} servos configurados com sucesso")

    def test_controls(self):
        """Testa sistema de controles"""
        print("\n🎮 TESTANDO CONTROLES...")
        
        # Teste de leitura dos controles
        start_time = time.ticks_ms()
        
        if hasattr(self, 'mode_button'):
            button_state = not self.mode_button.value()
            self.test_report.add_test("CONTROLES", "Leitura Botão MODO", "PASS", f"Pressionado: {button_state}")
        
        if hasattr(self, 'power_button') and self.power_button:
            button_state = not self.power_button.value()
            self.test_report.add_test("CONTROLES", "Leitura Botão POWER", "PASS", f"Pressionado: {button_state}")
        
        if hasattr(self, 'switch'):
            switch_state = not self.switch.value()
            aircraft = "NAVE MÃE" if switch_state else "PLANADOR"
            self.test_report.add_test("CONTROLES", "Leitura Chave Aeronave", "PASS", f"Selecionado: {aircraft}")
        
        duration = time.ticks_diff(time.ticks_ms(), start_time)
        self.test_report.add_test("CONTROLES", "Sistema de Controles", "PASS", "Leitura funcionando", duration)

    def test_stabilization_system(self):
        """Testa sistema de estabilização"""
        print("\n🧠 TESTANDO SISTEMA DE ESTABILIZAÇÃO...")
        
        # Teste do sistema PID
        start_time = time.ticks_ms()
        
        try:
            # Simular dados de sensor
            test_attitude = {'roll': 10, 'pitch': -5, 'yaw_rate': 2}
            test_target = {'roll': 0, 'pitch': 0}
            test_config = AIRCRAFT_CONFIGS['PLANADOR']['flight_modes']['THERMAL']
            
            # Testar cálculo PID
            pid_output = self.calculate_pid_control(test_attitude, test_target, test_config)
            
            # Verificar se saídas são razoáveis
            if all(-50 < pid_output[axis] < 50 for axis in ['roll', 'pitch', 'yaw']):
                self.test_report.add_test("ESTABILIZAÇÃO", "Cálculo PID", "PASS", 
                                        f"Roll: {pid_output['roll']:.1f}, Pitch: {pid_output['pitch']:.1f}, Yaw: {pid_output['yaw']:.1f}")
            else:
                self.test_report.add_test("ESTABILIZAÇÃO", "Cálculo PID", "WARN", "Valores PID extremos")
            
            # Testar cálculo de comandos de servo
            sensor_data = {'roll': 5, 'pitch': -3, 'yaw_rate': 1, 'sensors_valid': False}
            commands = self.calculate_servo_commands(sensor_data)
            
            if commands and all(30 <= commands[servo] <= 150 for servo in commands):
                self.test_report.add_test("ESTABILIZAÇÃO", "Comandos de Servo", "PASS", 
                                        f"{len(commands)} comandos válidos")
            else:
                self.test_report.add_test("ESTABILIZAÇÃO", "Comandos de Servo", "WARN", "Comandos fora dos limites")
                
        except Exception as e:
            self.test_report.add_test("ESTABILIZAÇÃO", "Sistema PID", "FAIL", str(e))
        
        duration = time.ticks_diff(time.ticks_ms(), start_time)
        self.test_report.add_test("ESTABILIZAÇÃO", "Sistema de Estabilização", "PASS", "Cálculos funcionando", duration)

    def test_aircraft_specific(self):
        """Testa funcionalidades específicas de cada aeronave"""
        print("\n✈️ TESTANDO SISTEMAS ESPECÍFICOS...")
        
        # Teste Planador
        self.test_planador_systems()
        
        # Teste Nave Mãe
        self.test_nave_mae_systems()

    def test_planador_systems(self):
        """Testa sistemas específicos do planador"""
        print("   📍 Testando sistemas do PLANADOR...")
        
        start_time = time.ticks_ms()
        config = AIRCRAFT_CONFIGS['PLANADOR']
        
        # Verificar servos ativos
        active_servos = config['active_servos']
        servos_available = sum(1 for servo in active_servos if servo in self.servos)
        
        if servos_available == len(active_servos):
            self.test_report.add_test("PLANADOR", "Servos Ativos", "PASS", f"{servos_available}/4 servos disponíveis")
        else:
            self.test_report.add_test("PLANADOR", "Servos Ativos", "WARN", f"{servos_available}/4 servos disponíveis")
        
        # Testar elevator único
        if 'elevator' in self.servos:
            try:
                # Teste específico do elevator
                elevator_positions = [70, 90, 110, 90]  # Up, center, down, center
                for pos in elevator_positions:
                    self.servos['elevator'].duty(self.angle_to_duty(pos))
                    time.sleep_ms(300)
                
                self.test_report.add_test("PLANADOR", "Elevator Único", "PASS", "Movimento completo testado")
            except Exception as e:
                self.test_report.add_test("PLANADOR", "Elevator Único", "FAIL", str(e))
        else:
            self.test_report.add_test("PLANADOR", "Elevator Único", "FAIL", "Servo elevator não encontrado")
        
        # Testar flaps diferenciais
        if 'flaps_left' in self.servos and 'flaps_right' in self.servos:
            try:
                # Teste movimento diferencial (simulando roll)
                test_sequences = [
                    (80, 100),  # Flap esquerdo up, direito down
                    (90, 90),   # Neutro
                    (100, 80),  # Flap esquerdo down, direito up
                    (90, 90)    # Neutro
                ]
                
                for left_pos, right_pos in test_sequences:
                    self.servos['flaps_left'].duty(self.angle_to_duty(left_pos))
                    self.servos['flaps_right'].duty(self.angle_to_duty(right_pos))
                    time.sleep_ms(400)
                
                self.test_report.add_test("PLANADOR", "Flaps Diferenciais", "PASS", "Movimento roll testado")
            except Exception as e:
                self.test_report.add_test("PLANADOR", "Flaps Diferenciais", "FAIL", str(e))
        
        # Testar modos de voo
        modes_tested = 0
        for mode_name, mode_config in config['flight_modes'].items():
            try:
                # Simular dados para o modo
                test_data = {'roll': 0, 'pitch': 0, 'yaw_rate': 0, 'sensors_valid': False}
                
                # Temporariamente alterar modo
                old_mode = getattr(self, 'flight_mode', 'THERMAL')
                self.flight_mode = mode_name
                
                commands = self.calculate_servo_commands(test_data)
                
                # Restaurar modo
                self.flight_mode = old_mode
                
                if commands:
                    flaps_pos = mode_config.get('flaps_position', 0)
                    self.test_report.add_test("PLANADOR", f"Modo {mode_name}", "PASS", 
                                            f"Flaps: {flaps_pos}°, Target pitch: {mode_config['target_attitude']['pitch']}°")
                    modes_tested += 1
                else:
                    self.test_report.add_test("PLANADOR", f"Modo {mode_name}", "FAIL", "Comandos inválidos")
                    
            except Exception as e:
                self.test_report.add_test("PLANADOR", f"Modo {mode_name}", "FAIL", str(e))
        
        duration = time.ticks_diff(time.ticks_ms(), start_time)
        if modes_tested == len(config['flight_modes']):
            self.test_report.add_test("PLANADOR", "Sistema Planador", "PASS", f"{modes_tested} modos testados", duration)
        else:
            self.test_report.add_test("PLANADOR", "Sistema Planador", "WARN", f"{modes_tested} modos OK", duration)

    def test_nave_mae_systems(self):
        """Testa sistemas específicos da nave mãe"""
        print("   📍 Testando sistemas da NAVE MÃE...")
        
        start_time = time.ticks_ms()
        config = AIRCRAFT_CONFIGS['NAVE_MAE']
        
        # Verificar servos ativos
        active_servos = config['active_servos']
        servos_available = sum(1 for servo in active_servos if servo in self.servos)
        
        if servos_available == len(active_servos):
            self.test_report.add_test("NAVE_MAE", "Servos Ativos", "PASS", f"{servos_available}/8 servos disponíveis")
        else:
            self.test_report.add_test("NAVE_MAE", "Servos Ativos", "WARN", f"{servos_available}/8 servos disponíveis")
        
        # Testar ailerons
        if 'aileron_left' in self.servos and 'aileron_right' in self.servos:
            try:
                # Teste movimento oposto dos ailerons
                test_sequences = [
                    (80, 100),  # Aileron esquerdo up, direito down
                    (90, 90),   # Neutro
                    (100, 80),  # Aileron esquerdo down, direito up
                    (90, 90)    # Neutro
                ]
                
                for left_pos, right_pos in test_sequences:
                    self.servos['aileron_left'].duty(self.angle_to_duty(left_pos))
                    self.servos['aileron_right'].duty(self.angle_to_duty(right_pos))
                    time.sleep_ms(300)
                
                self.test_report.add_test("NAVE_MAE", "Ailerons", "PASS", "Movimento diferencial testado")
            except Exception as e:
                self.test_report.add_test("NAVE_MAE", "Ailerons", "FAIL", str(e))
        
        # Testar elevators duplos
        if 'elevator_left' in self.servos and 'elevator_right' in self.servos:
            try:
                # Teste movimento sincronizado dos elevators
                test_positions = [70, 90, 110, 90]  # Up, center, down, center
                
                for pos in test_positions:
                    self.servos['elevator_left'].duty(self.angle_to_duty(pos))
                    self.servos['elevator_right'].duty(self.angle_to_duty(pos))
                    time.sleep_ms(400)
                
                self.test_report.add_test("NAVE_MAE", "Elevators Duplos", "PASS", "Movimento sincronizado testado")
            except Exception as e:
                self.test_report.add_test("NAVE_MAE", "Elevators Duplos", "FAIL", str(e))
        
        # Testar freio aerodinâmico
        if 'airbrake' in self.servos:
            try:
                # Teste do freio
                brake_positions = [90, 110, 130, 90]  # Neutro, parcial, máximo, neutro
                
                for pos in brake_positions:
                    self.servos['airbrake'].duty(self.angle_to_duty(pos))
                    time.sleep_ms(500)
                
                self.test_report.add_test("NAVE_MAE", "Freio Aerodinâmico", "PASS", "Teste de abertura completo")
            except Exception as e:
                self.test_report.add_test("NAVE_MAE", "Freio Aerodinâmico", "FAIL", str(e))
        
        # Testar modos de voo
        modes_tested = 0
        for mode_name, mode_config in config['flight_modes'].items():
            try:
                # Simular dados para o modo
                test_data = {'roll': 0, 'pitch': 0, 'yaw_rate': 0, 'sensors_valid': False}
                
                # Temporariamente alterar aeronave e modo
                old_aircraft = getattr(self, 'aircraft_type', 'PLANADOR')
                old_mode = getattr(self, 'flight_mode', 'THERMAL')
                
                self.aircraft_type = 'NAVE_MAE'
                self.flight_mode = mode_name
                
                commands = self.calculate_servo_commands(test_data)
                
                # Restaurar configuração
                self.aircraft_type = old_aircraft
                self.flight_mode = old_mode
                
                if commands:
                    flaps_pos = mode_config.get('flaps_position', 0)
                    airbrake_pos = mode_config.get('airbrake_position', 0)
                    self.test_report.add_test("NAVE_MAE", f"Modo {mode_name}", "PASS", 
                                            f"Flaps: {flaps_pos}°, Freio: {airbrake_pos}°")
                    modes_tested += 1
                else:
                    self.test_report.add_test("NAVE_MAE", f"Modo {mode_name}", "FAIL", "Comandos inválidos")
                    
            except Exception as e:
                self.test_report.add_test("NAVE_MAE", f"Modo {mode_name}", "FAIL", str(e))
        
        duration = time.ticks_diff(time.ticks_ms(), start_time)
        if modes_tested == len(config['flight_modes']):
            self.test_report.add_test("NAVE_MAE", "Sistema Nave Mãe", "PASS", f"{modes_tested} modos testados", duration)
        else:
            self.test_report.add_test("NAVE_MAE", "Sistema Nave Mãe", "WARN", f"{modes_tested} modos OK", duration)

    def angle_to_duty(self, angle):
        """Converte ângulo para duty cycle PWM"""
        return int(40 + (angle / 180) * 75)

    def read_sensors(self):
        """Lê dados do MPU6050 ou gera simulação"""
        if not self.sensors_available or not self.i2c:
            return self.get_simulation_data()

        try:
            accel_x = struct.unpack('>h', self.i2c.readfrom_mem(MPU6050_ADDR, 0x3B, 2))[0]
            accel_y = struct.unpack('>h', self.i2c.readfrom_mem(MPU6050_ADDR, 0x3D, 2))[0]
            accel_z = struct.unpack('>h', self.i2c.readfrom_mem(MPU6050_ADDR, 0x3F, 2))[0]
            gyro_z = struct.unpack('>h', self.i2c.readfrom_mem(MPU6050_ADDR, 0x47, 2))[0]

            ax = accel_x / 16384.0
            ay = accel_y / 16384.0
            az = accel_z / 16384.0
            gz = gyro_z / 131.0

            roll = math.atan2(ay, az) * 180 / math.pi
            pitch = math.atan2(-ax, math.sqrt(ay**2 + az**2)) * 180 / math.pi

            return {
                'roll': roll,
                'pitch': pitch,
                'yaw_rate': gz,
                'sensors_valid': True
            }
        except:
            return self.get_simulation_data()

    def get_simulation_data(self):
        """Dados simulados"""
        t = time.ticks_ms() / 1000.0
        return {
            'roll': math.sin(t * 0.3) * 8 + math.sin(t * 2.5) * 2,
            'pitch': math.cos(t * 0.2) * 5 + math.cos(t * 2.1) * 1.5,
            'yaw_rate': math.sin(t * 0.15) * 3 + math.sin(t * 1.8) * 1,
            'sensors_valid': False
        }

    def calculate_pid_control(self, current_attitude, target_attitude, mode_config):
        """Controle PID"""
        dt = 0.1
        gains = mode_config['pid_gains']

        roll_error = target_attitude['roll'] - current_attitude['roll']
        pitch_error = target_attitude['pitch'] - current_attitude['pitch']
        yaw_error = -current_attitude['yaw_rate']

        # PID simplificado para teste
        roll_output = gains['roll_kp'] * roll_error
        pitch_output = gains['pitch_kp'] * pitch_error
        yaw_output = gains['yaw_kp'] * yaw_error

        limits = mode_config['max_deflection']
        return {
            'roll': max(-limits['roll'], min(limits['roll'], roll_output)),
            'pitch': max(-limits['pitch'], min(limits['pitch'], pitch_output)),
            'yaw': max(-limits['yaw'], min(limits['yaw'], yaw_output))
        }

    def calculate_servo_commands(self, sensor_data):
        """Calcula comandos para servos"""
        config = AIRCRAFT_CONFIGS[self.aircraft_type]
        mode_config = config['flight_modes'][self.flight_mode]

        current_attitude = {
            'roll': sensor_data['roll'],
            'pitch': sensor_data['pitch'],
            'yaw_rate': sensor_data['yaw_rate']
        }

        target_attitude = mode_config['target_attitude']
        pid_outputs = self.calculate_pid_control(current_attitude, target_attitude, mode_config)

        commands = {}

        if self.aircraft_type == 'NAVE_MAE':
            commands['aileron_left'] = 90 - pid_outputs['roll']
            commands['aileron_right'] = 90 + pid_outputs['roll']
            commands['elevator_left'] = 90 - pid_outputs['pitch']
            commands['elevator_right'] = 90 - pid_outputs['pitch']
            commands['rudder'] = 90 + pid_outputs['yaw']

            flaps_deflection = mode_config.get('flaps_position', 0)
            commands['flaps_left'] = 90 + flaps_deflection
            commands['flaps_right'] = 90 + flaps_deflection

            airbrake_deflection = mode_config.get('airbrake_position', 0)
            commands['airbrake'] = 90 + airbrake_deflection

        else:  # PLANADOR
            flaps_base = mode_config.get('flaps_position', 0)
            commands['flaps_left'] = 90 + flaps_base - pid_outputs['roll']
            commands['flaps_right'] = 90 + flaps_base + pid_outputs['roll']
            commands['elevator'] = 90 - pid_outputs['pitch']
            commands['rudder'] = 90 + pid_outputs['yaw']

        # Limitar comandos
        for servo in commands:
            commands[servo] = max(30, min(150, commands[servo]))

        return commands

    def handle_controls(self):
        """Controle simplificado"""
        current_time = time.ticks_ms()
        
        if time.ticks_diff(current_time, self.last_button_time) < 300:
            return
        
        # CHAVE: Selecionar aeronave
        try:
            switch_reading = not self.switch.value()
            if switch_reading != self.switch_last:
                self.switch_last = switch_reading
                new_type = 'NAVE_MAE' if switch_reading else 'PLANADOR'
                
                if new_type != self.aircraft_type:
                    self.aircraft_type = new_type
                    self.update_aircraft_type()
                    self.reset_pid_state()
                    self.last_button_time = current_time
        except:
            pass
        
        # BOTÃO MODO: Ativar sistema OU trocar modos
        try:
            mode_reading = not self.mode_button.value()
            if mode_reading and not self.mode_button_last:
                if not self.system_active:
                    self.activate_system()
                else:
                    self.switch_flight_mode()
                self.last_button_time = current_time
            self.mode_button_last = mode_reading
        except:
            pass
        
        # BOTÃO POWER: Desligar sistema
        if hasattr(self, 'power_button') and self.power_button:
            try:
                power_reading = not self.power_button.value()
                if power_reading and not self.power_button_last:
                    if self.system_active:
                        self.deactivate_system()
                    self.last_button_time = current_time
                self.power_button_last = power_reading
            except:
                pass

    def activate_system(self):
        self.system_active = True
        config = AIRCRAFT_CONFIGS[self.aircraft_type]
        mode_info = config['flight_modes'][self.flight_mode]

        print(f"\n🚀 ESTABILIZAÇÃO ATIVADA!")
        print(f"   Aeronave: {config['name']}")
        print(f"   Modo: {mode_info['name']}")

        self.reset_pid_state()
        if hasattr(self, 'led_system_active'):
            self.led_system_active.value(1)

    def deactivate_system(self):
        self.system_active = False
        print(f"\n🛑 ESTABILIZAÇÃO DESATIVADA!")
        self.set_all_servos_neutral()
        self.reset_pid_state()

        if hasattr(self, 'led_alert'):
            for _ in range(3):
                self.led_alert.value(1)
                time.sleep_ms(150)
                self.led_alert.value(0)
                time.sleep_ms(150)

    def switch_flight_mode(self):
        config = AIRCRAFT_CONFIGS[self.aircraft_type]
        modes = list(config['flight_modes'].keys())

        current_index = modes.index(self.flight_mode)
        next_index = (current_index + 1) % len(modes)
        self.flight_mode = modes[next_index]

        mode_info = config['flight_modes'][self.flight_mode]
        print(f"\n🔄 MODO: {mode_info['name']}")

        self.reset_pid_state()

    def reset_pid_state(self):
        for axis in self.pid_state:
            self.pid_state[axis]['error_prev'] = 0
            self.pid_state[axis]['integral'] = 0

    def update_aircraft_type(self):
        config = AIRCRAFT_CONFIGS[self.aircraft_type]
        first_mode = list(config['flight_modes'].keys())[0]
        self.flight_mode = first_mode

        print(f"\n🔄 AERONAVE: {config['name']}")
        self.set_all_servos_neutral()

    def set_all_servos_neutral(self):
        if hasattr(self, 'servos'):
            neutral_duty = self.angle_to_duty(90)
            for servo in self.servos.values():
                servo.duty(neutral_duty)

    def apply_servo_commands(self, commands):
        """Aplica comandos aos servos"""
        config = AIRCRAFT_CONFIGS[self.aircraft_type]
        active_servos = config['active_servos']

        for servo_name, angle in commands.items():
            if servo_name in active_servos and servo_name in self.servos:
                duty = self.angle_to_duty(angle)
                self.servos[servo_name].duty(duty)

        self.flight_data['commands'] = {k: v for k, v in commands.items() if k in active_servos}

    def update_led_display(self):
        """Sistema de LEDs"""
        if not hasattr(self, 'led_system_active'):
            return
            
        current_time = time.ticks_ms()

        # LED Sistema Ativo (Verde)
        self.led_system_active.value(1 if self.system_active else 0)

        # LED Tipo de Aeronave (Azul)
        if self.aircraft_type == 'NAVE_MAE':
            self.led_aircraft_type.value(1)
        else:
            self.led_aircraft_type.value((current_time // 1000) % 2)

        # LED Modo (Amarelo)
        if self.system_active:
            config = AIRCRAFT_CONFIGS[self.aircraft_type]
            modes = list(config['flight_modes'].keys())
            current_mode_index = modes.index(self.flight_mode) + 1

            cycle_time = (current_time // 400) % (current_mode_index + 2)
            if cycle_time < current_mode_index:
                self.led_mode.value(1)
            else:
                self.led_mode.value(0)
        else:
            self.led_mode.value(0)

    def log_telemetry(self, sensor_data):
        """Log de telemetria"""
        current_time = time.ticks_ms()

        if time.ticks_diff(current_time, self.last_telemetry_time) >= 3000:
            self.last_telemetry_time = current_time

            config = AIRCRAFT_CONFIGS[self.aircraft_type]
            mode_config = config['flight_modes'][self.flight_mode]

            print(f"\n📊 {config['name']} - {mode_config['name']}")
            print(f"   Atitude: R={sensor_data['roll']:.1f}° P={sensor_data['pitch']:.1f}° Y={sensor_data['yaw_rate']:.1f}°/s")
            
            sensor_text = "MPU6050" if sensor_data['sensors_valid'] else "SIMULAÇÃO"
            status_text = "ATIVO" if self.system_active else "INATIVO"
            print(f"   Status: {status_text} | Sensor: {sensor_text}")

    def main_loop(self):
        """Loop principal"""
        self.loop_count += 1

        try:
            sensor_data = self.read_sensors()
            self.flight_data.update(sensor_data)

            self.handle_controls()

            if self.system_active:
                commands = self.calculate_servo_commands(sensor_data)
                self.apply_servo_commands(commands)
                
                if self.loop_count % 30 == 0:
                    self.log_telemetry(sensor_data)
            else:
                self.set_all_servos_neutral()

            self.update_led_display()

        except Exception as e:
            print(f"❌ Erro no loop: {e}")
            self.set_all_servos_neutral()

    def run(self):
        """Execução principal"""
        print("\n" + "=" * 90)
        print("🎯 SISTEMA PRONTO PARA OPERAÇÃO")
        print("=" * 90)
        print()
        print("🎛️ CONTROLES:")
        print("   • CHAVE (GPIO 35): PLANADOR ↔ NAVE MÃE")
        print("   • BOTÃO AZUL (GPIO 34): ATIVAR OU trocar modos")
        if hasattr(self, 'power_button') and self.power_button:
            print("   • BOTÃO VERMELHO (GPIO 18): DESATIVAR")
        print()
        print("🚦 LEDS:")
        print("   🟢 Verde: Sistema ativo")
        print("   🔵 Azul: Tipo de aeronave")
        print("   🟡 Amarelo: Modo de voo")
        print("   🟠 Laranja: Alertas")
        print()
        print("📡 Mova o MPU6050 para testar estabilização!")
        print("=" * 90)

        try:
            while True:
                self.main_loop()
                time.sleep_ms(100)

        except KeyboardInterrupt:
            print("\n🛬 Desligando sistema...")
        finally:
            self.cleanup()

    def cleanup(self):
        """Limpeza do sistema"""
        print("\n🔧 Desligamento seguro...")
        self.set_all_servos_neutral()
        
        if hasattr(self, 'led_system_active'):
            for led in [self.led_system_active, self.led_aircraft_type, self.led_mode, self.led_alert]:
                led.value(0)

        if hasattr(self, 'servos'):
            for servo in self.servos.values():
                try:
                    servo.deinit()
                except:
                    pass

        print("✅ Sistema finalizado com segurança")

def main():
    """Função principal"""
    print("🧪 INICIANDO SISTEMA COM TESTES COMPLETOS...")
    print("Este sistema executará testes iniciais de todos os componentes")

    try:
        sistema = SistemaComTestes()
        sistema.run()
    except Exception as e:
        print(f"❌ Erro crítico: {e}")
        import sys
        sys.print_exception(e)

if __name__ == "__main__":
    main()