class FlightPhysics:
 """Fundamental aerodynamics calculations"""

 AIR_DENSITY = 1.225 # kg/m³ at sea level
 GRAVITY = 9.81 # m/s²

 @staticmethod
 def thrust_equation(prop_diameter_m, rpm, air_density=1.225, Cp=0.05):
 """
 Thrust = Cp * rho * n^2 * D^4

 Cp: Power coefficient (depends on propeller design)
 rho: Air density
 n: RPM converted to rev/s
 D: Propeller diameter in meters
 """
 n = rpm / 60 # Convert RPM to rev/s
 thrust = Cp * air_density * (n ** 2) * (prop_diameter_m ** 4)
 return thrust # in Newtons

 @staticmethod
 def weight(mass_kg):
 """Weight force"""
 return mass_kg * FlightPhysics.GRAVITY

 @staticmethod
 def drag(velocity, drag_coefficient, frontal_area, air_density=1.225):
 """Drag force"""
 return 0.5 * air_density * (velocity ** 2) * drag_coefficient * frontal_area

 @staticmethod
 def hover_thrust_required(mass_kg):
 """Thrust needed to hover"""
 return mass_kg * FlightPhysics.GRAVITY

 @staticmethod
 def thrust_to_weight_ratio(total_thrust_n, weight_n):
 """TWR — greater than 1 means the drone can climb"""
 return total_thrust_n / weight_n

# Calculate thrust for a typical 5-inch prop
prop_diameter = 0.127 # 5 inches = 0.127 meters
rpm = 20000
thrust_n = FlightPhysics.thrust_equation(prop_diameter, rpm)
thrust_grams = thrust_n / FlightPhysics.GRAVITY * 1000

print(f"Thrust at {rpm} RPM: {thrust_n:.2f} N ({thrust_grams:.0f}g)")

# Weight of a typical racing quad
mass = 0.5 # 500g
weight_n = FlightPhysics.weight(mass)
print(f"Weight: {weight_n:.2f} N ({mass*1000:.0f}g)")

# Thrust-to-weight ratio
twr = FlightPhysics.thrust_to_weight_ratio(thrust_n * 4, weight_n)
print(f"TWR: {twr:.1f}:1")

class PIDController:
 """Proportional-Integral-Derivative Controller"""

 def __init__(self, kp, ki, kd, output_limits=(-1000, 1000)):
 self.kp = kp
 self.ki = ki
 self.kd = kd
 self.output_limits = output_limits
 self.integral = 0
 self.prev_error = 0

 def compute(self, setpoint, measurement, dt):
 """Calculate PID output"""
 error = setpoint - measurement

 # Proportional — reacts to current error
 p_term = self.kp * error

 # Integral — corrects accumulated past error
 self.integral += error * dt
 i_term = self.ki * self.integral

 # Derivative — dampens based on rate of change
 derivative = (error - self.prev_error) / dt if dt > 0 else 0
 d_term = self.kd * derivative

 self.prev_error = error

 # Combine and clamp
 output = p_term + i_term + d_term
 return max(self.output_limits[0], min(self.output_limits[1], output))


# Example: Altitude hold
pid_alt = PIDController(kp=2.0, ki=0.5, kd=0.3)
target_alt = 10.0 # meters
current_alt = 0.0
dt = 0.01 # 100Hz loop

print("Altitude Hold Simulation:")
for step in range(50):
 throttle_cmd = pid_alt.compute(target_alt, current_alt, dt)

 # Simple physics: throttle command affects altitude
 acceleration = (throttle_cmd / 1000) * 9.81 - 9.81
 current_alt += 0.5 * acceleration * dt ** 2

 if step % 10 == 0:
 print(f" t={step*dt:.1f}s | Alt={current_alt:.2f}m | "
 f"Error={target_alt-current_alt:.2f}m | Throttle={throttle_cmd:.0f}")

def tune_pid_manual():
 """Manual PID tuning procedure"""
 print("=== PID Tuning Guide ===\n")

 steps = [
 ("1. Start with Kp only", "Set Ki=0, Kd=0, increase Kp until drone oscillates"),
 ("2. Reduce Kp", "Back off Kp by 50% to remove oscillation"),
 ("3. Add Kd", "Increase Kd to dampen remaining oscillations"),
 ("4. Add Ki", "Small Ki to eliminate steady-state error (hover drift)"),
 ("5. Fine-tune", "Adjust all three gains for responsive but stable flight"),
 ]

 for step, instruction in steps:
 print(f" {step}")
 print(f" → {instruction}\n")

 print("Rule of thumb for racing quads:")
 print(" Roll/Pitch: Kp=4.0, Ki=0.04, Kd=15")
 print(" Yaw: Kp=4.0, Ki=0.0, Kd=0")
 print(" Altitude: Kp=1.5, Ki=0.5, Kd=0.3")

tune_pid_manual()

class DroneSimulator2D:
 def __init__(self):
 self.x = 0.0 # horizontal position (m)
 self.y = 5.0 # altitude (m)
 self.vx = 0.0 # horizontal velocity
 self.vy = 0.0 # vertical velocity
 self.mass = 1.0 # kg
 self.gravity = 9.81
 self.max_thrust = 20.0 # Newtons
 self.dt = 0.01

 def simulate_step(self, throttle, pitch_angle):
 """
 throttle: 0-1 (fraction of max thrust)
 pitch_angle: radians (positive = forward tilt)
 """
 # Thrust vector
 thrust = throttle * self.max_thrust
 thrust_x = thrust * math.sin(pitch_angle)
 thrust_y = thrust * math.cos(pitch_angle)

 # Forces
 ax = thrust_x / self.mass
 ay = (thrust_y / self.mass) - self.gravity

 # Update state (Euler integration)
 self.vx += ax * self.dt
 self.vy += ay * self.dt
 self.x += self.vx * self.dt
 self.y += self.vy * self.dt

 # Ground collision
 if self.y < 0:
 self.y = 0
 self.vy = 0

 return {"x": self.x, "y": self.y, "vx": self.vx, "vy": self.vy}

 def hover_test(self, duration=2.0):
 """Test hovering stability"""
 print("=== Hover Test ===")
 steps = int(duration / self.dt)
 for i in range(0, steps, 100):
 t = i * self.dt
 # Altitude hold PID
 error = 5.0 - self.y
 throttle = 0.5 + error * 0.1
 throttle = max(0, min(1, throttle))

 state = self.simulate_step(throttle, 0)
 if i % 200 == 0:
 print(f" t={t:.1f}s | Alt={state['y']:.2f}m | Vel={state['vy']:.2f}m/s")

 def forward_flight(self):
 """Test forward flight with pitch"""
 print("\n=== Forward Flight Test ===")
 self.__init__()
 for i in range(500):
 t = i * self.dt
 # Climb then move forward
 if self.y < 5.0:
 throttle, pitch = 0.7, 0.0
 else:
 throttle, pitch = 0.6, 0.2 # tilt forward 20%

 state = self.simulate_step(throttle, pitch)
 if i % 100 == 0:
 print(f" t={t:.1f}s | Pos=({state['x']:.1f}, {state['y']:.1f})m | "
 f"Vel=({state['vx']:.1f}, {state['vy']:.1f})m/s")

sim = DroneSimulator2D()
sim.hover_test()
sim.forward_flight()