clc
close all
clear all

im=imread('05_g.JPG');
GB=(im(:,:,2)+im(:,:,3))/2;
imshow(GB)
hold on

r0=4;                        

%% posun po obraze
[vely, velx] = size(GB);
[x_pos,y_pos] = ginput(1);

krok = 10;
konst = 50;    % ke zvětšení
souradnice_dle_imshow = -1;

if x_pos < velx/2
    oko = -1; % leve oko
else
    oko = 1;
end

%% horní

ffi0_1=[0:krok:110];
for j=1:length(ffi0_1)
    n(j)=-0.0012*j^4+0.0212*j^3-0.0054*j^2+0.8134*j+13.189;
    rr1=4:n(j);
    for i=1:length(rr1)
        bb(j)=exp(-1.9+3.9*tanh(-(ffi0_1(j)-121)/14));
        cc(j)=1.9+1.4*tanh((ffi0_1(j)-121)/14);
        fi(i,j)=ffi0_1(j)+bb(j).*(rr1(i)-r0).^cc(j);
        x1{j}(i)=oko*konst*rr1(i).*cos(fi(i,j)*pi/180);
        y1{j}(i)=konst*rr1(i).*sin(fi(i,j)*pi/180);
    end
end
clear n; clear j; clear i; clear bb; clear cc

ffi0_2=[120:krok:180];

for j=1:length(ffi0_2)
        n(j)=-0.2222*j^3+2.5476*j^2-11.016*j+38.714;
        rr2=4:n(j);

    for i=1:length(rr2)
        bb(j)=exp(-1.9+3.9*tanh(-(ffi0_2(j)-121)/14));
        cc(j)=1.9+1.4*tanh((ffi0_2(j)-121)/14);
        fi2(i,j)=ffi0_2(j)+bb(j).*(rr2(i)-r0).^cc(j);
        x2{j}(i)=oko*konst*rr2(i).*cos(fi2(i,j)*pi/180);
        y2{j}(i)=konst*rr2(i).*sin(fi2(i,j)*pi/180);
    end
end
clear n; clear j; clear i; clear bb; clear cc


ffi0_3=[-170:10:-100];
for j=1:length(ffi0_3)
        n(j)=0.0884*j^3-1.0087*j^2+5.3791*j+2.5714;
        rr3=4:n(j);    
    for i=1:length(rr3)
        bb(j)=-exp(0.7+0.5*tanh(-(-ffi0_3(j)-90)/25));
        cc(j)=1.0+0.5*tanh((-ffi0_3(j)-90)/25);
        fi3(i,j)=ffi0_3(j)+bb(j).*(rr3(i)-r0).^cc(j);
        x3{j}(i)=oko*konst*rr3(i).*cos(fi3(i,j)*pi/180);
        y3{j}(i)=konst*rr3(i).*sin(fi3(i,j)*pi/180);
    end
end
clear n; clear j; clear i; clear bb; clear cc

ffi0_4=[-90:10:0];
for j=1:length(ffi0_4)
        n(j)=-0.0361*j^3+0.9219*j^2-7.8904*j+36.733;
        rr4=4:n(j); 
    for i=1:length(rr4)
        bb(j)=-exp(0.7+0.5*tanh(-(-ffi0_4(j)-90)/25));
        cc(j)=1.0+0.5*tanh((-ffi0_4(j)-90)/25);
        fi4(i,j)=ffi0_4(j)+bb(j).*(rr4(i)-r0).^cc(j);
        x4{j}(i)=oko*konst*rr4(i).*cos(fi4(i,j)*pi/180);
        y4{j}(i)=konst*rr4(i).*sin(fi4(i,j)*pi/180);
    end
end
clear n; clear j; clear i; clear bb; clear cc
 %% modifikování souřadnic a vykreslování
 
for i=1:length(ffi0_1)
      plot(x1{i}+x_pos,y1{i}*souradnice_dle_imshow+y_pos)
end

for i=1:length(ffi0_2)
      plot(x2{i}+x_pos,y2{i}*souradnice_dle_imshow+y_pos)
end

for i=1:length(ffi0_3)
      plot(x3{i}+x_pos,y3{i}*souradnice_dle_imshow+y_pos)
end
% % 
for i=1:length(ffi0_4)
      plot(x4{i}+x_pos,y4{i}*souradnice_dle_imshow+y_pos)
end

%% zanesení informace o pozici

% p1 = [y_pos,0];
% p2 = [y_pos,velx];
% plot([p1(2),p2(2)],[p1(1),p2(1)],'Color','g','LineWidth',0.5)
% 
% p1 = [0,x_pos];
% p2 = [vely,x_pos];
% plot([p1(2),p2(2)],[p1(1),p2(1)],'Color','g','LineWidth',0.5)

[xb,yb] = ginput(1);

xn = (xb-x_pos)*oko;
yn = (yb-y_pos)*souradnice_dle_imshow;

plot(xb,yb,'og')


% % úhly podle soustavy souřadnic
if x_pos < velx/2 % levé oko	
    if xb>x_pos && yb<y_pos
        phi = atan(yn/xn)*180/pi*oko;                % 1. kvadrant
    elseif xb<x_pos && yb<y_pos
         phi = 180+atan(yn/xn)*180/pi*oko;           % 2. kvadrant
    elseif xb<x_pos && yb>y_pos
         phi = 180+atan(yn/xn)*180/pi*oko;
    elseif xb>x_pos && yb>y_pos
         phi = 270+90+atan(yn/xn)*180/pi*oko;
    end
else % pravé oko
    if xb>x_pos && yb<y_pos
        phi = atan(yn/xn)*180/pi;                % 1. kvadrant
    elseif xb<x_pos && yb<y_pos
         phi = 180+atan(yn/xn)*180/pi;           % 2. kvadrant
    elseif xb<x_pos && yb>y_pos
         phi = 180+atan(yn/xn)*180/pi;
    elseif xb>x_pos && yb>y_pos
         phi = 270+90+atan(yn/xn)*180/pi;
    end
end

if phi>180
        uhel = phi-180;
    else
        uhel=phi;
end

poc=['Směrovost je ', num2str(uhel), ' stupňů'];
warndlg(poc)

