%state x=[beta phi rollrate yalrate]
A_long = [-0.1315,0.14858,0.32434,-0.93964;
    -0,-0,1,0.33976;
    -10.614,0,-1.1793,1.0023;
    0.099655,0,-0.0018174,-0.25855];
%control input u=[aileron rudder]
B_long = [0.00012049,0.00032897;
    0,0;
    -0.1031578,0.020987;
    -0.002133,-0.010715];
C_long = eye(4);                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     
D_long = zeros(5,2);
Ts_sys = 0.01;
Ts_controller = 0.1;
Np = 10;
sys = ss(A_long, B_long, C_long, 0);
sys = c2d(sys,Ts_sys);      
mpcobj = mpc(sys, Ts_controller, Np);
mpcobj.PredictionHorizon = 15;
mpcobj.ControlHorizon = 15;
%set control input constraints
mpcobj.ManipulatedVariables(1).Min = -90;
mpcobj.ManipulatedVariables(1).Max = 90;
mpcobj.ManipulatedVariables(2).Min = -90;
mpcobj.ManipulatedVariables(2).Max = 90;
% Set state (output variable) weight Q
mpcobj.Weights.OutputVariables = [1 100 10 120];

% Set the control input weight R 
%mpcobj.Weights.ManipulatedVariables = [0.07 0.02];
Tfinal = 350;  % Simulation time 
r = [0 10 5 5];   % Setpoint for the outputs
sim(mpcobj, Tfinal, r);
[y,t,u,xp,xc,SimOptions] = sim(mpcobj, Tfinal, r);
rollrate = y(:,3);
yalrate = y(:,4);
% Open the MPC Designer app for interactive tuning
%mpcDesigner(mpcobj);
% Or use review command for a quick look at the controller performance
%review(mpcobj);


% Initial position and heading
x = 0;
y = 0;
altitude = 750;
heading = 0; % Assuming initial heading is 0 degrees

% Assuming we have vectors 'rollrate' and 'yalrate' from your simulation output
% And assuming constant forward speed (this is a simplification)
speed = 250; % Example constant speed in m/s
dt = 0.1; % Time step of the simulation in seconds

% Initialize position vectors
x_pos = zeros(length(rollrate), 1);
y_pos = zeros(length(rollrate), 1);
altitude_pos = zeros(length(rollrate), 1);

for i = 1:length(rollrate)
    % Update heading and altitude based on rollrate and yalrate
    % This is a simplification and not physically accurate
    heading = heading + yalrate(i) * dt;
    %altitude = altitude + yalrate(i) * dt;

    % Calculate the change in position
    dx = speed * cos(heading) * dt;
    dy = speed * sin(heading) * dt;

    % Update position
    x = x + dx;
    y = y + dy;

    % Store position
    x_pos(i) = x;
    y_pos(i) = y;
    altitude_pos(i) = altitude;
end

% Plot the 3D flight path
figure;
plot3(x_pos, y_pos, altitude_pos);
grid on;
xlabel('X [m]');
ylabel('Y [m]');
zlabel('Altitude [m]');
title('3D Flight Path Approximation');