clc;
clear;
%% definice kruhovky
D=0.1; % [m]
v=15; % [m/min]
v=v/60; % [m/min]
R=D/2;
 
dt=0.001; % [s]
dx=v*dt;
dfi=dx/R;
 
fi=((-1*pi):dfi:(3*pi))';
 
x=R*cos(fi);
y=R*sin(fi);


%% nacteni systemu 
load('Vanad_PolReg');

sys=sys({'X_nastroj';'Y_nastroj'},:);

switch 2
    case 1 % polohova regulace
%% simulace
t=(0:(length(x)-1))'*dt;
u=zeros(length(t),4);
u(:,1)=x-x(1);
u(:,2)=y-y(1);
u(:,3)=([0;diff(u(:,1))]+[diff(u(:,1));0])/(t(2)-t(1))/2; % vX
u(:,4)=([0;diff(u(:,2))]+[diff(u(:,2));0])/(t(2)-t(1))/2; % vY

simul=lsim(sys,u,t);

x_nas=(simul(:,1)+x(1));
y_nas=(simul(:,2)+y(1));


% x_nas=(simul((length(x_nab)+1):end,1)+x(1));
% y_nas=(simul((length(x_nab)+1):end,2)+y(1));

log_ind=(fi<0)|(fi>(2*pi));
log_ind=~log_ind;
ind=1:length(fi);
ind=ind(log_ind);
 x_nas=(simul((ind(1)-1):(ind(end)+1),1)+x(1));
 y_nas=(simul((ind(1)-1):(ind(end)+1),2)+y(1));
%% zobrazeni kruhovitosti
r_nas=sqrt(x_nas.^2+y_nas.^2);
dr_nas=r_nas-R;
% unwrap
fi_nas=atan2(y_nas,x_nas);
dfi_nas=diff(fi_nas);
for i=1:length(dfi_nas)
    if abs(dfi_nas(i))>6;
        dfi_nas(i)=dfi_nas(i)-(sign(dfi_nas(i))*2*pi);
    end;
end;
fi_nas(:)=cumsum([fi_nas(1);dfi_nas]);

kruhovitost(fi_nas,dr_nas*1e6,'mkm',[num2str(R*1e3) 'mm'])

    case 1 % polohova regulace 
        t=(0:0.002:3)';
        v=0.25;
    rampa=t*v-1*v;
    rampa(t<=1)=0;
    rampa(t>=2)=rampa(t==2);
    y=lsim(sys('DeltaX1','X_zad'),rampa,t);
    y1=lsim(sys('DeltaX2','X_zad'),rampa,t);
    y2=lsim(sys('DeltaY','Y_zad'),rampa,t);
end