function spacecraft_attitude_control_HW2_2025()
    clc; clear; close all;
    format long;
    
    %% Initialize Parameters
    global params;
    params = initialize_parameters();
    
    %% Request 1: Disturbance Torques and Uncontrolled Motion
    fprintf('=== Request 1: Disturbance Torques ===\n');
    simulate_uncontrolled_motion();
    
    %% Request 2: Control System Design (now with PID)
    fprintf('\n=== Request 2: Control System Design ===\n');
    design_control_system();
    
    %% Request 3: Saturation Analysis with Thruster Failure
    fprintf('\n=== Request 3: Saturation Analysis ===\n');
    [t_sat, ww_sat] = analyze_saturation();
    
    %% Request 4: Desaturation Maneuver
    if ~isinf(t_sat)
        fprintf('\n=== Request 4: Desaturation Maneuver ===\n');
        perform_desaturation(t_sat, ww_sat);
    else
        fprintf('\nNo wheel saturation detected - skipping desaturation maneuver\n');
    end
    
    %% Save Results
    save_results();
end

%% Parameter Initialization
function p = initialize_parameters()
    p = struct();
    
    % Constants
    p.mu_moon = 4903e9;                  % Moon's GM [m³/s²]
    p.R_moon = 1737e3;                   % Moon radius [m]
    p.altitude = 8000e3;                 % Orbital altitude [m]
    p.a = p.R_moon + p.altitude;         % Semimajor axis [m]
    p.n = sqrt(p.mu_moon/p.a^3);         % Mean motion [rad/s]
    p.arcsec = pi/648000;                % 1 arcsec in radians
    
    % Spacecraft properties
    p.Ix = 21000; p.Iy = 20000; p.Iz = 22500; % [kg·m²]
    p.Iw = 0.11;                              % Wheel inertia [kg·m²]
    
    % Reaction wheel
    p.ww0 = 271*(2*pi/60);                   % Initial speed [rad/s]
    p.ww_max = 2000*(2*pi/60);               % Max speed [rad/s]
    p.Tw_max = 0.236;                        % Max torque [Nm]
    
    % Disturbance parameters
    p.A = 34;                                % Solar array area [m²]
    p.Cs = 0.8;                              % Reflectivity coefficient
    p.cp_offset = 0.6;                       % Center-of-pressure offset [m]
    p.Phi = 1371;                            % Solar flux [W/m²]
    p.c = 299792458;                         % Speed of light [m/s]
    p.thruster_failure_torque = 0.008;       % Additional disturbance [Nm]
    
    % Thrusters
    p.Ft = 3;                                % Thruster force [N]
    p.b = 1.25;                              % Moment arm [m]
    
    % Initial conditions
    p.theta0 = deg2rad(-29.2537);            % Initial pitch [rad]
    p.dtheta0 = deg2rad(-0.0039);            % Initial pitch rate [rad/s]
    p.X0 = [p.theta0; p.dtheta0; p.ww0; 0];  % State vector [θ, θ̇, ω_w, ∫θ]
    
    % Control parameters
    p.thruster_failure = false;
    p.desaturation_mode = false;
    p.control_active = false;
    
    % Electrical parameters
    p.Km = 0.118;                            % Motor constant [Nm/A]
    p.Kw = 0.003;                            % Back-EMF constant [V/rpm]
    p.R = 1.5;                               % Resistance [Ohm]
    p.Pmax = 18;                             % Max power [W]
    
    % ODE options
    p.options = odeset('RelTol', 1e-8, 'AbsTol', 1e-8);
end

%% Request 1: Uncontrolled Motion
function simulate_uncontrolled_motion()
    global params;
    
    % Ensure control is disabled for uncontrolled simulation
    params.control_active = false;
    
    % Simulate uncontrolled dynamics for 1 hour
    [time, X] = ode113(@spacecraft_dynamics, [0 3600], params.X0, params.options);
    
    % Calculate disturbance torques
    T_grav = zeros(size(time));
    T_srp = zeros(size(time));
    for i = 1:length(time)
        theta = X(i,1);
        % Gravity gradient torque (φ=0 in this case)
        T_grav(i) = (3/2)*(params.mu_moon/params.a^3)*(params.Iz-params.Ix)*sin(2*theta);
        % Solar radiation pressure torque (Sun along X-axis)
        F_srp = (params.Phi/params.c)*params.A*(1+params.Cs);
        T_srp(i) = F_srp * params.cp_offset;
    end
    
    % Plot disturbance torques
    figure('Name', 'Disturbance Torques', 'Position', [100 100 900 700]);
    
    subplot(2,1,1);
    plot(time, T_grav*1e6, 'b', 'LineWidth', 2);
    title('Gravity Gradient Torque', 'FontSize', 14);
    grid on; box on;
    xlabel('Time [s]', 'FontSize', 12);
    ylabel('Torque [µNm]', 'FontSize', 12);
    set(gca, 'FontSize', 11);
    
    subplot(2,1,2);
    plot(time, T_srp*1e6, 'r', 'LineWidth', 2);
    title('Solar Radiation Pressure Torque', 'FontSize', 14);
    grid on; box on;
    xlabel('Time [s]', 'FontSize', 12);
    ylabel('Torque [µNm]', 'FontSize', 12);
    set(gca, 'FontSize', 11);
    
    % Plot uncontrolled attitude
    figure('Name', 'Uncontrolled Attitude', 'Position', [200 200 900 500]);
    plot(time, rad2deg(X(:,1)), 'LineWidth', 2, 'Color', [0 0.5 0]);
    title('Uncontrolled Pitch Angle', 'FontSize', 14);
    grid on; box on;
    xlabel('Time [s]', 'FontSize', 12);
    ylabel('Pitch Angle [deg]', 'FontSize', 12);
    set(gca, 'FontSize', 11);
end

%% Request 2: Control System Design (PID implementation)
function design_control_system()
    global params;
    
    % Tune PID controller
    wn = 0.05;       % Natural frequency [rad/s]
    zeta = 1.0;      % Critical damping
    params.kp = params.Iy * wn^2;
    params.kd = 2 * zeta * sqrt(params.Iy * params.kp);
    params.ki = 0.1;  % Small integral term to eliminate steady-state error
    
    % Enable control for simulation
    params.control_active = true;
    params.thruster_failure = false;
    params.desaturation_mode = false;
    
    % Simulate controlled system for initial acquisition (5 minutes)
    [time, X] = ode113(@spacecraft_dynamics, [0 300], params.X0, params.options);
    
    % Calculate control performance metrics
    theta = X(:,1);
    err = -theta;
    ww = X(:,3);
    
    % Calculate electrical parameters
    Tc = params.kp*err + params.kd*(-X(:,2)) + params.ki*X(:,4); % PID control torque
    iw = Tc/params.Km;                        % Current
    Vm = params.Kw*(ww*60/(2*pi)) + params.R*iw; % Voltage
    Pw = Vm.*iw;                              % Power
    
    final_error = abs(err(end));
    max_ww = max(abs(ww));
    max_err = max(abs(err));
    max_power = max(abs(Pw));
    
    fprintf('PID Controller Gains:\n');
    fprintf('kp = %.4f Nm/rad\n', params.kp);
    fprintf('ki = %.4f Nm/(rad·s)\n', params.ki);
    fprintf('kd = %.4f Nms/rad\n', params.kd);
    
    fprintf('\nControl Performance:\n');
    fprintf('Max pointing error: %.2f arcsec\n', max_err/params.arcsec);
    fprintf('Final pointing error: %.2f arcsec (requirement: <20 arcsec)\n',...
            final_error/params.arcsec);
    fprintf('Max wheel speed: %.1f rpm (limit: %.1f rpm)\n',...
            max_ww*60/(2*pi), params.ww_max*60/(2*pi));
    fprintf('Max power consumption: %.2f W (limit: %.1f W)\n\n',...
            max_power, params.Pmax);
    
    % Plot control performance
    figure('Name', 'Control Performance', 'Position', [100 100 1000 900]);
    
    subplot(3,1,1);
    plot(time, err/params.arcsec, 'LineWidth', 2, 'Color', [0.2 0.4 0.8]);
    yline(20, 'r--', '20 arcsec limit', 'LineWidth', 1.5);
    yline(-20, 'r--', 'LineWidth', 1.5);
    title('Pointing Error During Acquisition', 'FontSize', 14);
    grid on; box on;
    xlabel('Time [s]', 'FontSize', 12);
    ylabel('Error [arcsec]', 'FontSize', 12);
    legend('Pointing Error', 'Limits', 'Location', 'best');
    set(gca, 'FontSize', 11);
    
    subplot(3,1,2);
    plot(time, ww*60/(2*pi), 'LineWidth', 2, 'Color', [0.8 0.2 0.2]);
    yline(params.ww_max*60/(2*pi), 'r--', 'Max Speed', 'LineWidth', 1.5);
    yline(-params.ww_max*60/(2*pi), 'r--', 'LineWidth', 1.5);
    title('Reaction Wheel Speed', 'FontSize', 14);
    grid on; box on;
    xlabel('Time [s]', 'FontSize', 12);
    ylabel('Speed [rpm]', 'FontSize', 12);
    set(gca, 'FontSize', 11);
    
    subplot(3,1,3);
    plot(time, Pw, 'LineWidth', 2, 'Color', [0.5 0 0.5]);
    yline(params.Pmax, 'r--', 'Max Power', 'LineWidth', 1.5);
    title('Wheel Power Consumption', 'FontSize', 14);
    grid on; box on;
    xlabel('Time [s]', 'FontSize', 12);
    ylabel('Power [W]', 'FontSize', 12);
    set(gca, 'FontSize', 11);
end

%% Request 3: Saturation Analysis
function [t_sat, ww_sat] = analyze_saturation()
    global params;
    
    % Simulate nominal operation for 1 hour
    params.control_active = true;
    params.thruster_failure = false;
    [time1, X1] = ode113(@spacecraft_dynamics, [0 3600], params.X0, params.options);
    
    % Simulate with thruster failure for extended period
    params.thruster_failure = true;
    [time2, X2] = ode113(@spacecraft_dynamics, [3600 18000], X1(end,:), params.options);
    
    % Combine results
    time = [time1; time2];
    X = [X1; X2];
    ww = X(:,3);
    
    % Find saturation point (after failure)
    post_failure = time > 3600;
    sat_idx = find(abs(ww(post_failure)) >= 0.95*params.ww_max, 1);
    
    if isempty(sat_idx)
        t_sat = Inf;
        ww_sat = 0;
        fprintf('Wheel does not saturate within 5 hours after failure\n');
    else
        t_sat = time(sat_idx + sum(~post_failure)) - 3600;
        ww_sat = ww(sat_idx + sum(~post_failure));
        fprintf('Wheel saturates at t = %.1f s after failure (%.1f%% of max speed)\n',...
                t_sat, 100*abs(ww_sat)/params.ww_max);
    end
    
    % Plot wheel speed history
    figure('Name', 'Wheel Saturation Analysis', 'Position', [200 200 1000 600]);
    plot(time-3600, ww*60/(2*pi), 'LineWidth', 2, 'Color', [0.1 0.6 0.3]);
    xline(0, 'k--', 'Thruster Failure', 'LineWidth', 1.5);
    yline(params.ww_max*60/(2*pi), 'r--', 'Max Speed', 'LineWidth', 1.5);
    yline(-params.ww_max*60/(2*pi), 'r--', 'LineWidth', 1.5);
    title('Wheel Speed History After Thruster Failure', 'FontSize', 14);
    grid on; box on;
    xlabel('Time After Failure [s]', 'FontSize', 12);
    ylabel('Wheel Speed [rpm]', 'FontSize', 12);
    legend('Wheel Speed', 'Failure Time', 'Speed Limits', 'Location', 'best');
    set(gca, 'FontSize', 11);
end

%% Request 4: Desaturation Maneuver
function perform_desaturation(t_sat, ww_sat)
    global params;
    
    % Find initial state at saturation
    params.control_active = true;
    params.thruster_failure = true;
    params.desaturation_mode = false;
    [time1, X1] = ode113(@spacecraft_dynamics, [0 3600+t_sat], params.X0, params.options);
    X0 = X1(end,:)';
    
    % Perform desaturation
    params.desaturation_mode = true;
    desat_duration = 60; % Initial guess [s]
    [time2, X2] = ode113(@spacecraft_dynamics, [0 desat_duration], X0, params.options);
    
    % Calculate electrical parameters during desaturation
    Tc = zeros(size(time2));
    for i = 1:length(time2)
        ww = X2(i,3);
        if ww > 0.8*params.ww_max
            Tc(i) = -params.Tw_max;
            Tt = -2 * params.Ft * params.b; % Use both thrusters
        elseif ww < -0.8*params.ww_max
            Tc(i) = params.Tw_max;
            Tt = 2 * params.Ft * params.b; % Use both thrusters
        else
            Tc(i) = 0;
            Tt = 0;
        end
    end
    iw = Tc/params.Km;
    Vm = params.Kw*(X2(:,3)*60/(2*pi)) + params.R*iw;
    Pw = Vm.*iw;
    
    % Combine results
    time = [time1; time1(end)+time2];
    X = [X1; X2];
    
    % Analyze results
    theta = X2(:,1);
    ww = X2(:,3);
    recovery_idx = find(abs(ww) <= 0.1*params.ww_max, 1);
    max_pointing_err = max(abs(rad2deg(theta)));
    
    if isempty(recovery_idx)
        fprintf('Desaturation failed to return wheel to safe range\n');
    else
        fprintf('Desaturation successful in %.1f seconds\n', time2(recovery_idx));
    end
    fprintf('Max pointing error: %.3f deg (requirement: <1 deg)\n', max_pointing_err);
    fprintf('Max power during desaturation: %.2f W (limit: %.1f W)\n', max(abs(Pw)), params.Pmax);
    
    % Plot desaturation performance
    figure('Name', 'Desaturation Performance', 'Position', [100 100 1200 900]);
    
    subplot(3,1,1);
    plot(time-3600-t_sat, rad2deg(X(:,1)), 'LineWidth', 2, 'Color', [0.7 0.3 0.1]);
    yline(1, 'r--', '1° Error Limit', 'LineWidth', 1.5);
    yline(-1, 'r--', 'LineWidth', 1.5);
    title('Pointing Error During Desaturation', 'FontSize', 14);
    grid on; box on;
    xlabel('Time After Saturation [s]', 'FontSize', 12);
    ylabel('Error [deg]', 'FontSize', 12);
    legend('Pointing Error', 'Error Limits', 'Location', 'best');
    set(gca, 'FontSize', 11);
    
    subplot(3,1,2);
    plot(time-3600-t_sat, X(:,3)*60/(2*pi), 'LineWidth', 2, 'Color', [0.1 0.5 0.7]);
    yline([params.ww_max, -params.ww_max]*60/(2*pi), 'r--', 'Speed Limits', 'LineWidth', 1.5);
    yline([0.1*params.ww_max, -0.1*params.ww_max]*60/(2*pi), 'g--', 'Safe Range', 'LineWidth', 1.5);
    title('Wheel Speed During Desaturation', 'FontSize', 14);
    grid on; box on;
    xlabel('Time After Saturation [s]', 'FontSize', 12);
    ylabel('Speed [rpm]', 'FontSize', 12);
    set(gca, 'FontSize', 11);
    
    subplot(3,1,3);
    plot(time2, Pw, 'LineWidth', 2, 'Color', [0.6 0.2 0.6]);
    yline(params.Pmax, 'r--', 'Max Power', 'LineWidth', 1.5);
    title('Power Consumption During Desaturation', 'FontSize', 14);
    grid on; box on;
    xlabel('Time [s]', 'FontSize', 12);
    ylabel('Power [W]', 'FontSize', 12);
    set(gca, 'FontSize', 11);
end

%% Core Dynamics Function (updated for PID)
function dX = spacecraft_dynamics(t, X)
    global params;
    theta = X(1); dtheta = X(2); ww = X(3); err_int = X(4);
    
    % Calculate disturbance torques
    T_grav = (3/2)*(params.mu_moon/params.a^3)*(params.Iz-params.Ix)*sin(2*theta);
    F_srp = (params.Phi/params.c)*params.A*(1+params.Cs);
    T_srp = F_srp * params.cp_offset;
    T_dist = T_grav + T_srp;
    
    % Additional disturbance if thruster failed
    if params.thruster_failure && t > 3600
        T_dist = T_dist + params.thruster_failure_torque;
    end
    
    % Control logic
    if params.control_active
        if params.desaturation_mode
            % Desaturation control logic
            if ww > 0.8*params.ww_max
                Tc = -params.Tw_max;
                Tt = -2 * params.Ft * params.b; % Use both thrusters
            elseif ww < -0.8*params.ww_max
                Tc = params.Tw_max;
                Tt = 2 * params.Ft * params.b; % Use both thrusters
            else
                Tc = 0;
                Tt = 0;
            end
        else
            % PID control with anti-windup
            err = -theta;
            derr = -dtheta;
            Tc = params.kp*err + params.kd*derr + params.ki*err_int;
            
            % Anti-windup protection
            if abs(Tc) > params.Tw_max
                Tc = sign(Tc) * params.Tw_max;
            end
            Tt = 0;
        end
    else
        % No control
        Tc = 0;
        Tt = 0;
    end
    
    % State derivatives
    dX = zeros(4,1);
    dX(1) = dtheta;
    dX(2) = (T_dist + Tc + Tt) / params.Iy;
    dX(3) = -Tc / params.Iw;
    dX(4) = -theta; % Integral of error (for PID control)
end

%% Utility Function
function save_results()
    if ~exist('Plots', 'dir')
        mkdir('Plots');
    end
    
    figs = findobj('Type', 'figure');
    for i = 1:length(figs)
        fig = figs(i);
        name = get(fig, 'Name');
        if isempty(name)
            name = sprintf('Figure_%d', fig.Number);
        end
        saveas(fig, fullfile('Plots', [strrep(name, ' ', '_') '.png']));
        saveas(fig, fullfile('Plots', [strrep(name, ' ', '_') '.fig']));
    end
end