use kaptan_flight_config::profile::*;
use kaptan_math::Vector3;
use kaptan_navigation::{
    BarometricReference, CalibrationRevision, ContactGeometry, FrameId, NavigationInstallation,
    NavigationSources, RangeInstallation, SensorReferenceId, SourceId,
};

pub const fn candidate() -> CandidateProfile {
    CandidateProfile {
        safety: SafetySettings {
            prearm_dwell_microseconds: 500_000,
        },
        timing: TimingSettings {
            maximum_imu_interval_microseconds: 20_000,
            imu_freshness_microseconds: 20_000,
            local_state_freshness_microseconds: 250_000,
            offboard_timeout_microseconds: 300_000,
        },
        control: ControlSettings {
            position_gain_per_second_squared: 0.8,
            velocity_gain_per_second: 1.2,
            maximum_horizontal_acceleration_mps2: 3.0,
            maximum_upward_acceleration_mps2: 2.0,
            maximum_downward_acceleration_mps2: 2.0,
            attitude_roll_pitch_gain_per_second: 4.0,
            attitude_yaw_gain_per_second: 2.0,
        },
        thrust: ThrustSettings {
            mass_kg: 1.2,
            maximum_collective_thrust_newtons: 18.0,
            maximum_tilt_tangent: 0.700_207_53,
        },
        rate_pid: PidSettings {
            proportional: 0.080,
            integral: 0.006,
            derivative: 0.0,
            output_limit: 0.10,
            integral_limit: 0.025,
            yaw_proportional: 0.990,
            yaw_integral: 0.074,
        },
        actuator_limits: ActuatorLimits {
            minimum: 0.0,
            maximum: 1.0,
            slew_per_second: 8.0,
        },
        flight: FlightSettings {
            command_ttl_us: 250_000,
            fallback_hold_us: 1_000_000,
            output_lifetime_us: 50_000,
            control_period_us: 10_000,
            control_deadline_us: 20_000,
            time_error_bound_us: 2_000,
            gyro_psd: 2.5e-7,
            acceleration_psd: 2.5e-5,
            gyro_bias_psd: 1e-10,
            acceleration_bias_psd: 1e-8,
            barometric_datum_psd: 1e-5,
        },
    }
}

pub const fn installation() -> NavigationInstallation {
    NavigationInstallation {
        sources: NavigationSources {
            imu: SourceId::new(1),
            local_position_velocity: SourceId::new(2),
            magnetic: SourceId::new(3),
            pressure: SourceId::new(4),
            range: SourceId::new(5),
        },
        range: RangeInstallation {
            frame: FrameId::new(3),
            offset_frd_metres: Vector3::new(0.0, 0.0, 0.04),
            direction_frd: Vector3::new(0.0, 0.0, 1.0),
            quantization_error_metres: 0.0005,
        },
        contact: ContactGeometry {
            half_span_x_metres: 0.12,
            half_span_y_metres: 0.12,
            centre_z_metres: 0.065,
            radius_metres: 0.015,
        },
        atmosphere: BarometricReference {
            pressure_pascals: 101_325.0,
            temperature_kelvin: 288.15,
        },
        magnetic_field_ned_microtesla: Vector3::new(20.0, 0.0, 45.0),
        calibration: CalibrationRevision::new(1),
        reference: SensorReferenceId(1),
    }
}

pub const fn capabilities() -> VehicleCapabilities {
    VehicleCapabilities::quad_x(installation())
}