clear; close; clc;

% a) trajektoria y=x*sinx;
tPth = linspace(0,1,100)';
pth = [0.9*tPth, 0.5*tPth.*sin(10*tPth)];
initPos = [0,-0.1]; initTh = -pi/2;

v=0.1; a=4; ksi=1/sqrt(2);
dt=0.001; plotStep=100;

k2=a^2; k3=2*ksi*a;
pos=initPos; th=initTh; %initPos to miejsce startu robota
w=0; posSv=[];
xmin=min(pth(:,1)); xmax=max(pth(:,1));
ymin=min(pth(:,2)); ymax=max(pth(:,2));
marg=0.1; xd=xmax-xmin; yd=ymax-ymin; crad=0.1*min([xd,yd]);
plot(pth(:,1),pth(:,2)); hold on;
circ=rectangle('Position',[pos(1)-crad/2,pos(2)-crad/2,crad,crad],...
'Curvature',[1,1],'LineWidth',0.5);
axis([xmin-marg*xd,xmax+marg*xd,ymin-marg*yd,ymax+marg*yd]);
axis equal; grid;
cnt=1;
while(norm(pos-pth(end,:))>0.01)
    posSv=[posSv;pos];
    th=th+w*dt; pos=pos+[cos(th),sin(th)]*v*dt;
    [k,l,thr,crv]=track(pos,pth);
    dth=th-thr; dth=mod(dth+pi,2*pi)-pi;
    u=-k2*v*l*sinc(dth/pi)-k3*v*dth; % u=-k2*v*l-k3*v*dth; (alg. liniowy)
    w=u+v*cos(dth)*crv/(1-l*crv);


cnt=cnt+1;
if cnt>plotStep
    set(circ,'Position',[pos(1)-crad/2,pos(2)-crad/2,crad,crad]);
    plot([pos(1) pth(k,1)],[pos(2) pth(k,2)],'g');
    plot(pth(k,1),pth(k,2),'.b','MarkerSize',10);
    plot(pos(1),pos(2),'.r','MarkerSize',10);
    plot(posSv(:,1),posSv(:,2),'r','LineWidth',1.5);
    posSv=[]; cnt=1; pause(plotStep*dt);
end
end
hold off;
function [k,l,thr,crv]=track(pos,pth)
    [k,l]=dsearchn(pth,pos);
    if k==1, k=2; elseif k==length(pth), k=k-1; end
    a=pth(k-1,:); b=pth(k,:); c=pth(k+1,:);
    u=[b-a,0]; v=[pos-a,0]; w=[c-b,0];
    cr=cross(u,v); lsgn=sign(cr(3)); cr=cross(u,w); csgn=sign(cr(3));
    thr=angle((b-a)*[1;1i]); l=l*lsgn;
    crv=2*sqrt(1-((a-b)*(c-b)'/(norm(a-b)*norm(c-b)))^2)/norm(c-a);
    crv=csgn*real(crv); crvLim=1000; crv=min(max(crv,-crvLim),crvLim);
end