% test bodu a derivaci na SIN
clear all, close all, clc;

%parametry simulace
r = 0.0019; %[mm] polomer kruznice
krok = 0.00002;
A = 0.0011; %[mm]
distance = 0.002;
x_v = 0.0052;  %[mm] 
x_max = 0.0075; %[mm] delka SIN v delkove souradnici
mu = 0.1; %koeficient treni
k = 3300; %tuhost pruziny
rameno_N = 0.001256; %[mm]
y_pred = 0.0004; %[mm]
om = 2*pi/x_v;
h = figure(10);
clf(h);


%% cyklus pres x_dotyku
data=[]; %zaznam vypoctenych dat
for(xd=0:0.00005:x_max); %xd=>x_dotyk
  hold on;
  grid on;
  axis equal;
  xlabel('x [m]');
  ylabel('y [m]');
  title('Dotyk SIN');
  xs=[0:krok:x_max];
  ys=A*sin(om*xs);
  plot(xs, ys, '-k'); %SIN
    
  yd=A*sin(om*xd);
  plot(xd, yd, '.r', 'MarkerSize', 10); %dotyk na sinusovce
  tecna=A*om*cos(om*xd);
  phi=atan(tecna);
  %vektory pro vykresleni
  tt=[cos(phi)*distance
      sin(phi)*distance];
  nn=[-sin(phi)*distance
       cos(phi)*distance];
  %jednotkove vektory
  te=[cos(phi)
      sin(phi)];
  ne=[-sin(phi)
       cos(phi)];

  %vykresleni tecny a normaly
  plot([xd, xd+tt(1)], [yd, yd+tt(2)], '--b'); %tecna k sinusovce
  plot([xd, xd+nn(1)], [yd, yd+nn(2)], '--r'); %normala k sinusovce
  
  %% vykresleni stredu kruznice a kruznice
  sk=ne*r;
  %stred
  skx=xd+sk(1);
  sky=yd+sk(2);
  plot(skx, sky, '.m', 'MarkerSize', 15);
  uhel=[0:0.01:2*pi];
  xk=skx+r*cos(uhel);
  yk=sky+r*sin(uhel);

  %% zaznam dat
  % Vypocet slozky sily V_x (smerem do SIN)
  yp=A + sky;
  Fm=-(k*(yp+y_pred))/cos(phi)*sqrt(1+mu^2)*cos(phi+pi/2+atan(mu));
 
  %Vypoèet momentu
  
  M = Fm*rameno_N*100;
  
  %Pøevod na stupnì
  O = ((2*pi*12.56)/360);
  st = 1/O;
  S = skx*st*1000;
  
  xm=3/2*pi/om; %mezni poloha stredu kruznice
  if((xd<xm) && (skx<xm))
    plot(xk, yk, '-k');
    text(skx, sky, ['phi=',num2str(phi/2/pi*180)]);
    data=[data; [skx*1000, sky*1000, phi, Fm, M, S]];
  end;
  if((xd>xm) && (skx>xm))
    plot(xk, yk, '-k');
    text(skx, sky, ['phi=',num2str(phi/2/pi*180)]);
    data=[data; [skx*1000, sky*1000, phi, Fm, M, S]];
  end;
  if(~((xd<xm) && (skx<xm)) && ~((xd>xm) && (skx>xm)))
    plot(xk, yk, '-r');
  end;

  pause(0.1);
  if(xd<x_max)
      clf;
  end;
end;


%% Vykresleni vysledku
figure(20);
axis equal;
grid on;
hold on;
title('Poloha stredu kruznice pri pohybu po SIN');
xlabel('Poloha stredu kruznice xs [mm]');
xlabel('Poloha stredu kruznice ys [mm]');
plot(data(:,1), data(:,2));
plot(xs*1000, ys*1000, '--k'); %SIN


figure(30);
grid on;
hold on;
title('Prubeh sily V_x');
xlabel('Poloha stredu kruznice xs [mm]');
ylabel('Sila V_x [N]');
plot(data(:, 1), data(:, 4))

figure(40);
grid on;
hold on;
title('Prubeh momentu');
xlabel('Poloha stredu kruznice xs [mm]');
ylabel('Moment [N.cm]');
plot(data(:, 1), data(:, 5));

figure(50);
grid on;
hold on;
title('Prubeh momentu');
xlabel('Poloha stredu kruznice xs [°]');
ylabel('Moment [N.cm]');
plot(data(:, 6), data(:, 5), 'r');
