% Trova le tangenti ai punti dati spostandoli in un riferimento col centro
% della crf al centro degli assi. Quando non c'e' wrapping calcola la
% distanza cmq tra i punti e mette come tangenti i punti iniziali. Infine
% se i punti sono sulla crf non usa ottimizzazione ma identifica tangente
% col punto.
% lato = 1;% 1 dx o sotto----  -1 sx o sopra
% problematico GA perche' sulla crf
function [L,T1_glob,T2_glob,contact] = Wrapping2(centro_glob,R,P1,P2,lato,graphics)

% lavoro sempre con circonferenza al centro degli assi: l'algoritmo č pių
% preciso.
v1 = P1-centro_glob;
v2 = P2-centro_glob;

dist_P = v2-v1;
if cross([1 0 0 ],[v1/norm(v1) 0])>0 == [0 0 1]
%     display('P1 sopra la circonferenza')
else
%     display('P1 sotto la circonferenza')
    lato = lato*-1;
end
if cross([1 0 0 ],[v2/norm(v2) 0])<0 == [0 0 1]
%     display('P2 sotto la circonferenza')
else
%     display('P2 sopra la circonferenza')
end

if cross([1 0 0 ],[v1/norm(v1) 0])>0 == [0 0 1] & cross([1 0 0 ],[v2/norm(v2) 0])>0 == [0 0 1]
%     display('P1 e P2 sopra la circonferenza')
    lato = lato*-1;
end
%direzione: se P1 č sopra P2:
%-----> lato = 1 wrapping  a dx
%-----> lato = -1 wrapping a sx
% se P1 e sotto P2 č il contrario

%stima della distanza tra centro e vettore che unisce i punti (limit
%condition)
h = cross([-lato*v1,0],[dist_P,0]/norm([dist_P,0]));

if h(3)>R
%     display('   '),display('NO WRAPPING!!!'),display('   ')
    L = norm([dist_P,0]);
    contact = 0;
    T1_glob = P1;
    T2_glob = P2;
    if strcmp(graphics,'on') == 1
        %grafica normalizzata
%         plot(v1(1), v1(2),'o'),plot(v2(1), v2(2),'o')
%         plot([v1(1),v2(1)], [v1(2),v2(2)],'g')
        %grafica globale
            t = 0:(2*pi)/300:2*pi;
        CRF = [R*cos(t'), R*sin(t')];
        plot(centro_glob(:,1)+CRF(:,1), centro_glob(:,2)+CRF(:,2),'k'),hold on
        plot([P1(1),P2(1)], [P1(2),P2(2)],'k')
        plot(P1(1), P1(2),'o','MarkerEdgeColor','k','MarkerFaceColor','w');
        plot(P2(1), P2(2),'o','MarkerEdgeColor','k','MarkerFaceColor','w')
        
    end
    return
else
%     display('   '),display('WRAP!!!'),display('   ')
    contact = 1;
end

%% algorithm
% pos punto tangenza + vettore tangente  = pos_punto est
%% tangente nel verso delle theta positive se lato = 1
% l'algoritmo chiude OT1P1 in pratica
dist1 = sqrt(norm(v1)^2-R^2);

fun_min1= @(x) R*[cos(x) sin(x)]+lato*dist1*[-sin(x) cos(x)]-v1;
options1=optimset('TolFun',10e-13,'TolX',10e-11);

x0 = pi/4;
[sol1,fval,exitflag] = fsolve(fun_min1,x0,options1);
% retry with new initial point
if exitflag == -2
    x0 = sol1;
    [sol1,fval,exitflag] = fsolve(fun_min1,x0,options1);
end
% if no solution then it doesn't work
if exitflag == -2
    display('La tangente superiore non converge!!!')
    waitforbuttonpress
end
T1 = [R*cos(sol1), R*sin(sol1)];
if(dist1<10e-6)
    T1 = v1;
%     display('PUNTO SULLA CRF');
%     waitforbuttonpress
end
%% tangente nel verso opposto al precedente
% l'algoritmo chiude OT1P2 in pratica
%tangente nel verso delle theta negative
dist2 = sqrt(norm(v2)^2-R^2);
fun_min2= @(x) R*[cos(x) sin(x)]-lato*dist2*[-sin(x) cos(x)]-v2;
x0 = -pi/4;
options2=optimset('TolFun',10e-11,'TolX',10e-11);
[sol2,fval2,exitflag2] = fsolve(fun_min2,x0,options2);
% retry with new initial point
if exitflag2 == -2
    x0 = sol+20/180*pi;
    [sol2,fval2,exitflag2] = fsolve(fun_min2,x0,options2);
end
% if no solution then it doesn't work
if exitflag2 == -2
    display('La tangente inferiore non converge!!!')
    waitforbuttonpress
end
T2 = [R*cos(sol2), R*sin(sol2)];
if(dist2<10e-6)
    T2 = v2;
%     display('PUNTO SULLA CRF');
%     waitforbuttonpress
end
%%  length mi fido
% arco (archi minori di 180)
L_arch = R*asin(norm(cross([T1/R,0], [T2/R,0])));

if L_arch<0
    display('ARCO DI WRAPPING MINORE DI ZERO: FORSE WRAPPING NON CORRETTO?')
    waitforbuttonpress
end
%dist lineare
Lin_vect1 = (v1-T1); L1 = hypot(Lin_vect1(:,1),Lin_vect1(:,2));
Lin_vect2 = (v2-T2); L2 = hypot(Lin_vect2(:,1),Lin_vect2(:,2));
%controllato GA e' affidabile nel wrapping
% L1
% L2
% L_arch
% waitforbuttonpress

L = L1+L2+L_arch;

%% points in global coordinates
T1_glob = T1+centro_glob;
T2_glob = T2+centro_glob;
% local graphics of the wrapping in normalized coordinates
%% grafica punti base

if strcmp(graphics,'on') == 1
%% grafica normalizzata (no casi di no wrapping)
%     t = 0:(2*pi)/300:2*pi;
%     CRF = [R*cos(t'), R*sin(t')];
%     line(CRF(:,1), CRF(:,2)),hold on
%     % punti esterni
%     plot(v1(1), v1(2),'o'),plot(v2(1), v2(2),'o')
%     axis equal
%     % tangenti
%     plot([v1(1),T1(1)], [v1(2),T1(2)],'r')
%     plot([v2(1),T2(1)], [v2(2),T2(2)])
%% grafica globale (no casi di no wrapping)

    t = 0:(2*pi)/300:2*pi;
    CRF = [R*cos(t'), R*sin(t')];
    plot(centro_glob(:,1)+CRF(:,1), centro_glob(:,2)+CRF(:,2),'k'),hold on
    % punti esterni
    
    
    % tangenti
    plot([P1(1),T1_glob(1)], [P1(2),T1_glob(2)],'k');
    plot([P2(1),T2_glob(1)], [P2(2),T2_glob(2)],'k');
    plot(P1(1), P1(2),'o','MarkerEdgeColor','k','MarkerFaceColor','w');
    plot(P2(1), P2(2),'o','MarkerEdgeColor','k','MarkerFaceColor','w');
    axis equal
end
clc