%% Zpracování sil působících na padák
clear all;clc;close all;
%% Načtení souřadnic působišť
load sily.mat

%% Vytvoříme z nich vektory
u = smer-pusobiste;

% komory na padáku
chamber = 1:54;

% alokace pameti
u_n = zeros(size(u));

% projdeme všechny komory a spravíme vektory sil (délky)
for i = 1:length(chamber)
    F = utlum(chamber(i)); % velikost sily
    p = sqrt(F^2/ ((u(i,1)^2 + u(i,2)^2) + u(i,3)^2));
    u_n(i,:) = p .* u(i,:);
end;
% celková z složka - pro normování
z_celk = sum(u_n(:,3));

% gravitační síla
Fg = 80*9.80556;

%% první bod - 100% brzd - rychlost 23km/h
vd = 23/3.6; % m/s
% odhad:
vo = 2;
% dopočty
gama = atan(vo/vd); % rad
v = vd/cos(gama);

% vysledne L a D
vL = Fg / (cos(gama) + sin(gama)^2/cos(gama));
vD = vL*sin(gama)/cos(gama);

% alokace paměti
Dc = zeros(length(chamber),3);
Lc = zeros(length(chamber),3);
F = 0;
% rozpočteme mezi všechny komory
for i = 1:length(chamber)
    
    % natočíme sílu podle gama
    [D L] = perpRot(u(i,:),-gama,utlum(i),utlum(i));
    
    % uložíme pro další zpracování
    Dc(i,:) = D;
    Lc(i,:) = L;
    
    % reverse test
    cl_red = 0.4080;
    cd_red = 0.0968;
    [D L] = perpRot(u(i,:), -gama, utlum(i)*cd_red*v^2, utlum(i)*cl_red*v^2);
    F = F + D+L;
    
%     plot3([pusobiste(i,1) pusobiste(i,1)+u_n(i,1)],...
%           [pusobiste(i,2) pusobiste(i,2)+u_n(i,2)],...
%           [pusobiste(i,3) pusobiste(i,3)+u_n(i,3)],...
%           [pusobiste(i,1) pusobiste(i,1)+D(1)],...
%           [pusobiste(i,2) pusobiste(i,2)+D(2)],...
%           [pusobiste(i,3) pusobiste(i,3)+D(3)],...
%           [pusobiste(i,1) pusobiste(i,1)+L(1)],...
%           [pusobiste(i,2) pusobiste(i,2)+L(2)],...
%           [pusobiste(i,3) pusobiste(i,3)+L(3)]);
%     grid;
%     hold on;


end;
% grid;
% axis equal;

% zjistíme aerodynamické síly normované na jednu komoru bez útlumu
Ds = sum(Dc);
Ls = sum(Lc);

% aerodynamické síly redukované na jednotkovou komoru
Dred = vD / sqrt(sum(Ds.^2));
Lred = vL / sqrt(sum(Ls.^2));

% spočteme koeficienty
cl_1 = Lred/v^2
cd_1 = Dred/v^2

% přičtem gravitaci
F = F + [0 0 -Fg];
% výsledky testu
F


%% druhý bod - 0% brzd i speedu - 37,5km/h, opadání 1,2m/s
vo = 1.1; % m/s
v = 37.5/3.6; % m/s
% dopočty
gama = asin(vo/v);
vd = v*cos(gama);

% vysledne L a D
vL = Fg / (cos(gama) + sin(gama)^2/cos(gama));
vD = vL*sin(gama)/cos(gama);

% alokace paměti
Dc = zeros(length(chamber),3);
Lc = zeros(length(chamber),3);
F = 0;
% rozpočteme mezi všechny komory
for i = 1:length(chamber)
    
    % natočíme sílu podle gama
    [D L] = perpRot(u(i,:),-gama,utlum(i),utlum(i));
    
    % uložíme pro další zpracování
    Dc(i,:) = D;
    Lc(i,:) = L;
    
    % reverse test
    cl_red = 0.1740;
    cd_red = 0.0141;
    [D L] = perpRot(u(i,:), -gama, utlum(i)*cd_red*v^2, utlum(i)*cl_red*v^2);
    F = F + D+L;
    
%     plot3([pusobiste(i,1) pusobiste(i,1)+u_n(i,1)],...
%           [pusobiste(i,2) pusobiste(i,2)+u_n(i,2)],...
%           [pusobiste(i,3) pusobiste(i,3)+u_n(i,3)],...
%           [pusobiste(i,1) pusobiste(i,1)+D(1)],...
%           [pusobiste(i,2) pusobiste(i,2)+D(2)],...
%           [pusobiste(i,3) pusobiste(i,3)+D(3)],...
%           [pusobiste(i,1) pusobiste(i,1)+L(1)],...
%           [pusobiste(i,2) pusobiste(i,2)+L(2)],...
%           [pusobiste(i,3) pusobiste(i,3)+L(3)]);
%     grid;
%     hold on;


end;
% grid;
% axis equal;

% zjistíme aerodynamické síly normované na jednu komoru bez útlumu
Ds = sum(Dc);
Ls = sum(Lc);

% aerodynamické síly redukované na jednotkovou komoru
Dred = vD / sqrt(sum(Ds.^2));
Lred = vL / sqrt(sum(Ls.^2));

% spočteme koeficienty
cl_2 = Lred/v^2
cd_2 = Dred/v^2

% přičtem gravitaci
F = F + [0 0 -Fg];
% výsledky testu
F

%% třetí bod - plný speed - 51km/h
v = 51/3.6; % m/s
% odhady
vo = 2.5;
% dopočty
gama = deg2rad(30);%asin(vo/v);
vd = v*cos(gama);

% vysledne L a D
vL = Fg / (cos(gama) + sin(gama)^2/cos(gama));
vD = vL*sin(gama)/cos(gama);

% alokace paměti
Dc = zeros(length(chamber),3);
Lc = zeros(length(chamber),3);
F = 0;
% rozpočteme mezi všechny komory
for i = 1:length(chamber)
    
    % natočíme sílu podle gama
    [D L] = perpRot(u(i,:),-gama,utlum(i),utlum(i));
    
    % uložíme pro další zpracování
    Dc(i,:) = D;
    Lc(i,:) = L;
    
    % reverse test
    cl_red = 0.0933;
    cd_red = 0.0128;
    [D L] = perpRot(u(i,:), -gama, utlum(i)*cd_red*v^2, utlum(i)*cl_red*v^2);
    F = F + D+L;
    
    plot3([pusobiste(i,1) pusobiste(i,1)+u_n(i,1)],...
          [pusobiste(i,2) pusobiste(i,2)+u_n(i,2)],...
          [pusobiste(i,3) pusobiste(i,3)+u_n(i,3)],...
          [pusobiste(i,1) pusobiste(i,1)+D(1)/10],...
          [pusobiste(i,2) pusobiste(i,2)+D(2)/10],...
          [pusobiste(i,3) pusobiste(i,3)+D(3)/10],...
          [pusobiste(i,1) pusobiste(i,1)+L(1)/20],...
          [pusobiste(i,2) pusobiste(i,2)+L(2)/20],...
          [pusobiste(i,3) pusobiste(i,3)+L(3)/20]);
    grid;
    hold on;


end;
grid;
axis equal;

% zjistíme aerodynamické síly normované na jednu komoru bez útlumu
Ds = sum(Dc);
Ls = sum(Lc);

% aerodynamické síly redukované na jednotkovou komoru
Dred = vD / sqrt(sum(Ds.^2));
Lred = vL / sqrt(sum(Ls.^2));

% spočteme koeficienty
cl_2 = Lred/v^2
cd_2 = Dred/v^2

% přičtem gravitaci
F = F + [0 0 -Fg];
% výsledky testu
F
 
%% Vytvoříme data pro cftool
alpha = [0 40 80];
CLa = [0.0933 0.1740 0.4080];
CDa = [0.0128 0.0141 0.0968];
