clear;
clc;
close all;

format long g;

% Лабораторная работа: Анализ модели движения ЛА
% Вариант 1

% рысканье a_i
a1 = 0.635;
a2 = 5.470;
a3 = 2.720;
a4 = 0.269;
a5 = 3.260;
a6 = 0.709;
a7 = 0.043;

% крен b_i
b1 = 3.100;
b2 = 20.200;
b3 = 17.600;
b4 = 0.072;
b5 = -0.518;
b6 = 0.057;
b7 = 0.065;

c1 = 0.560;
c2 = 1.635;
c3 = 1.468;
c4 = 0.765;
c5 = 0.160;
c6 = 0.989;
c7 = 0.171;
c8 = -0.082;

e1 = 0.025;
e2 = 0.246;
e3 = -0.379;

fprintf('================ ПРОДОЛЬНОЕ ДВИЖЕНИЕ ================\n\n');

%% Полное характеристическое уравнение продольного движения

A1 = c1 + c4 + c5 + e1;
A2 = c1*c4 + c1*e1 + c2 + c4*e1 + c5*e1 + c8*e2;
A3 = c1*c4*e1 + c1*c8*e2 + c2*e1 + c5*c7*e2 - c7*e3 + c8*e3;
A4 = c7 * (c2*e2 - c4*e3);

char_long_full = [1 A1 A2 A3 A4];
roots_long_full = roots(char_long_full);

fprintf('Полное характеристическое уравнение продольного движения:\n');
fprintf('s^4 + %.6f s^3 + %.6f s^2 + %.6f s + %.6f = 0\n\n', ...
    A1, A2, A3, A4);

fprintf('Корни полного продольного движения:\n');
disp(roots_long_full);

if all(real(roots_long_full) < 0)
    fprintf('Вывод: полное продольное движение устойчиво.\n\n');
else
    fprintf('Вывод: полное продольное движение неустойчиво.\n\n');
end

%% Короткопериодическое движение

kp_A1 = c1 + c4 + c5;
kp_A2 = c1*c4 + c2;

char_kpd = [1 kp_A1 kp_A2];
roots_kpd = roots(char_kpd);

fprintf('Характеристическое уравнение КПД:\n');
fprintf('s^2 + %.6f s + %.6f = 0\n\n', kp_A1, kp_A2);

fprintf('Корни КПД:\n');
disp(roots_kpd);

if all(real(roots_kpd) < 0)
    fprintf('Вывод: короткопериодическое движение устойчиво.\n\n');
else
    fprintf('Вывод: короткопериодическое движение неустойчиво.\n\n');
end

%% Длиннопериодическое движение

d1 = e1 - c7*e3/c2 + c8*e3/c2;
d2 = c7 * (c2*e2 - c4*e3) / c2;

char_dpd = [1 d1 d2];
roots_dpd = roots(char_dpd);

fprintf('Характеристическое уравнение ДПД:\n');
fprintf('s^2 + %.6f s + %.6f = 0\n\n', d1, d2);

fprintf('Корни ДПД:\n');
disp(roots_dpd);

if all(real(roots_dpd) < 0)
    fprintf('Вывод: длиннопериодическое движение устойчиво.\n\n');
else
    fprintf('Вывод: длиннопериодическое движение неустойчиво.\n\n');
end

%% Передаточная функция W_wz_deltaB

s = tf('s');

W_wz_deltaB = -c3 * (s + c4) / (s^2 + (c1 + c4 + c5)*s + (c1*c4 + c2));

fprintf('Передаточная функция W_wz_deltaB(s):\n');
W_wz_deltaB

figure;
step(W_wz_deltaB);
grid on;
title('Переходный процесс W_{\omega_z / \delta_B}(s)');
xlabel('t, c');
ylabel('\omega_z');

fprintf('\n================ БОКОВОЕ ДВИЖЕНИЕ ================\n\n');

%% Полное характеристическое уравнение бокового движения

B1 = a1 + a4 + b1;
B2 = a1*a4 + a2 + a1*b1 + b1*a4 + b2*b7 - a6*b6;
B3 = b1*a2 + b1*a1*a4 + b2*b4 + b2*(a1*b7 - b6) ...
     - a6*(a4*b6 + a2*b7);
B4 = b4 * (a1*b2 - a2*a6);

char_lat_full = [1 B1 B2 B3 B4];
roots_lat_full = roots(char_lat_full);

fprintf('Полное характеристическое уравнение бокового движения:\n');
fprintf('s^4 + %.6f s^3 + %.6f s^2 + %.6f s + %.6f = 0\n\n', ...
    B1, B2, B3, B4);

fprintf('Корни полного бокового движения:\n');
disp(roots_lat_full);

if all(real(roots_lat_full) < 0)
    fprintf('Вывод: полное боковое движение устойчиво.\n\n');
else
    fprintf('Вывод: полное боковое движение неустойчиво.\n\n');
end

%% Проверка возможности разделения бокового движения

separation_value = b1 * (a2 + a1*a4) / ...
    (b1*a1*a4 + b1*a2 + b2*(a1*b7 - b6) ...
    - a6*(a4*b6 + a2*b7) - b2*b4);

fprintf('Критерий разделения бокового движения:\n');
fprintf('K = %.6f\n', separation_value);

if separation_value >= 0.9
    fprintf('Вывод: боковое движение можно разделить на креновое и рысканье-скольжение.\n\n');
else
    fprintf('Вывод: разделение бокового движения недостаточно обосновано.\n\n');
end

%% Движение рысканья-скольжения

f1 = a1 + a4;
f2 = a2 + a1*a4;

char_yaw = [1 f1 f2];
roots_yaw = roots(char_yaw);

fprintf('Характеристическое уравнение движения рысканья-скольжения:\n');
fprintf('s^2 + %.6f s + %.6f = 0\n\n', f1, f2);

fprintf('Корни движения рысканья-скольжения:\n');
disp(roots_yaw);

if all(real(roots_yaw) < 0)
    fprintf('Вывод: движение рысканья-скольжения устойчиво.\n\n');
else
    fprintf('Вывод: движение рысканья-скольжения неустойчиво.\n\n');
end

%% Быстрое креновое движение

root_roll = -b1;

fprintf('Корень быстрого кренового движения:\n');
fprintf('s = %.6f\n', root_roll);

if root_roll < 0
    fprintf('Вывод: быстрое креновое движение устойчиво.\n\n');
else
    fprintf('Вывод: быстрое креновое движение неустойчиво.\n\n');
end

%% Передаточная функция W_wx_deltaE

W_wx_deltaE = -b3 / (s + b1);

fprintf('Передаточная функция W_wx_deltaE(s):\n');
W_wx_deltaE

figure;
step(W_wx_deltaE);
grid on;
title('Переходный процесс W_{\omega_x / \delta_\epsilon}(s)');
xlabel('t, c');
ylabel('\omega_x');

%% Передаточная функция W_wy_deltaH

W_wy_deltaH = (-a3*s - (a3*a4 - a2*a7)) / ...
    (s^2 + (a1 + a4)*s + (a2 + a1*a4));

fprintf('Передаточная функция W_wy_deltaH(s):\n');
W_wy_deltaH

figure;
step(W_wy_deltaH);
grid on;
title('Переходный процесс W_{\omega_y / \delta_H}(s)');
xlabel('t, c');
ylabel('\omega_y');

if ~exist('figures', 'dir')
    mkdir('figures');
end

figure(1);
saveas(gcf, fullfile('figures', '01_W_wz_deltaB.png'));

figure(2);
saveas(gcf, fullfile('figures', '02_W_wx_deltaE.png'));

figure(3);
saveas(gcf, fullfile('figures', '03_W_wy_deltaH.png'));

fprintf('Графики сохранены в папку figures.\n');