clear all;
close all;
clc;

global Q f s p H1 H2 H3 h1 h2 h3 Ma
Q=37325;
f=0.35;
s=0.75;
p=1.8;
R=20;
dvesigma=40;
c=7;
rk=310;
beta=75*pi/180;

%geometricke limity
alfa_min=(p/(2*R));
alfa_max=(p/(2*R))+(dvesigma/(1000*p));
x_min=alfa_min*R;
x_max=alfa_max*R;

m1=6713;    %[kg] hmotnost clanku A1 
m2=5000;    %[kg] hmotnost podvozku
m3=3364;    %[kg] pul hmotnosti neseneho clanku B1
S1=23;      %[m^2] bocni plocha clanku A1
S2=12.4;    %[m^2] polovina bocni plochy clanku B1
tlak=500;   %[N/m^2] bocni tlak vzduchu

pruchod=0;
for a=0.5:0.1:3.0; %[m/s^2] nevyrovnane pricne zrychleni
pruchod=pruchod+1;

v_ms=(a*R)^0.5; %rychlost [m/s]
v_kmh=v_ms*3.6;  %[km/h]
rychlost(pruchod)=v_kmh;

%vnejsi sily
H1=m1*a+S1*tlak;
h1=-0.377;
H2=m2*a;
h2=0.9;
H3=m3*a+S2*tlak;
h3=3.253;
Ma=26140;

%reseni obecne polohy
options=optimset('Display','off','TolX',1e-10); 
solution=fsolve(@heumann1,[Q,p],options);

P=solution(1);
iks=solution(2);

if solution(2)>x_max
    %disp('Vzpøièená poloha');
    poloha_aktual=1;
    P1=2*Q*f*((((x_max^2+s^2)^0.5)/p)+((((p-x_max)^2+s^2)^0.5)/p)+((x_max*(p-x_max))/(p*((x_max^2+s^2)^0.5)))-(((p-x_max)^2)/(p*(((p-x_max)^2+s^2)^0.5))))-...
        H1*(1+h1/p-2*x_max/p)-H2*(1+h2/p-2*x_max/p)-H3*(1+h3/p-2*x_max/p)+Ma/p;
    P2=P1-2*Q*f*((x_max/(x_max^2+s^2)^0.5)-((p-x_max)/(((p-x_max)^2+s^2)^0.5)))-H1-H2-H3;
    Y1=P1*(1-f*((tan(alfa_max)*cot(beta))/(sqrt(((cos(beta)*c/rk)^2)+(tan(alfa_max))^2))));
    Y2=P2*(1-f*((tan(alfa_max)*cot(beta))/(sqrt(((cos(beta)*c/rk)^2)+(tan(alfa_max))^2))));
    Y_ku_Q=(tan(beta)*(sqrt(((cos(beta)*c/rk)^2)+(tan(alfa_max))^2))-f*tan(alfa_max))/((sqrt(((cos(beta)*c/rk)^2)+(tan(alfa_max))^2))+f*tan(alfa_max)*tan(beta));
    k_BPV_1=Y_ku_Q/(Y1/Q);
    k_BPV_2=Y_ku_Q/(Y2/Q);
elseif solution(2)<x_min
    %disp('Tìtivová poloha');
    poloha_aktual=2;
    P1=2*Q*f*((1+(2*s/p)^2)^0.5)+H1*(h1/p)+H2*(h2/p)+H3*(h3/p)+Ma/p;
    P2=-2*Q*f*((1+(2*s/p)^2)^0.5)+H1*(1-h1/p)+H2*(1-h2/p)+H3*(1-h3/p)-Ma/p;
    Y1=P1*(1-f*((tan(alfa_min)*cot(beta))/(sqrt(((cos(beta)*c/rk)^2)+(tan(alfa_min))^2))));
    Y2=P2*(1-f*((tan(alfa_min)*cot(beta))/(sqrt(((cos(beta)*c/rk)^2)+(tan(alfa_min))^2))));
    Y_ku_Q=(tan(beta)*(sqrt(((cos(beta)*c/rk)^2)+(tan(alfa_min))^2))-f*tan(alfa_min))/((sqrt(((cos(beta)*c/rk)^2)+(tan(alfa_min))^2))+f*tan(alfa_min)*tan(beta));
    k_BPV_1=Y_ku_Q/(Y1/Q);
    k_BPV_2=Y_ku_Q/(Y2/Q);
else 
    %disp('Obecná poloha');
    poloha_aktual=3;
    P1=solution(1);
    P2=0;
    iks=solution(2);
    Y1=P1*(1-f*((tan(iks/R)*cot(beta))/(sqrt(((cos(beta)*c/rk)^2)+(tan(iks/R))^2))));
    Y_ku_Q=(tan(beta)*(sqrt(((cos(beta)*c/rk)^2)+(tan(iks/R))^2))-f*tan(iks/R))/((sqrt(((cos(beta)*c/rk)^2)+(tan(iks/R))^2))+f*tan(iks/R)*tan(beta));
    k_BPV_1=Y_ku_Q/(Y1/Q);
end 
Y1_ku_Q(pruchod)=Y1/Q;
Y2_ku_Q(pruchod)=Y2/Q;
Y_ku_Q_krit(pruchod)=Y_ku_Q;
sila1(pruchod)=P1;
sila2(pruchod)=P2;
poloha(pruchod)=poloha_aktual;
bezpecnost(pruchod)=k_BPV_1;
zrychleni(pruchod)=a;
end
figure(1)
%subplot(1,2,1);
plot(rychlost,bezpecnost,rychlost,Y_ku_Q_krit,rychlost,Y1_ku_Q,'LineWidth',3)
grid on
title('Prujezd obloukem R=20m')
xlabel('rychlost [km/h]')
legend('k_B_P_V','Y/Q_k_r_i_t','Y_1/Q')
figure(2)
%subplot(1,2,2);
%plotyy([rychlost,rychlost],[sila1,sila2],rychlost,zrychleni);
plot(rychlost,sila1,rychlost,sila2,rychlost,zrychleni*10000,rychlost,poloha*10000,'LineWidth',3);
%plot(rychlost,sila1,'LineWidth',3);
grid on
title('Prujezd obloukem R=20m')
xlabel('rychlost [km/h]')
ylabel('øídící síla [kN]')
legend('P_1 [kN]','P_2 [kN]')