pkg load geometry;


%trayectoria analitica
%A, b -- sistema, x0 -- condicion inicial, u -- control, t -- tiempo (puede ser un vector)
%t(1) -- tiempo del condicion inicial: x(t(1))=x0
function retval=analytic_trajectory(A,b,x0,u,t) %in the v-basis
  lambda=A(1,1); mu=A(2,1);
%  M=[exp(lambda*t(1))*sin(mu*t(1)) exp(lambda*t(1))*cos(mu*t(1));
%  exp(lambda*t(1))*(-cos(mu*t(1))) exp(lambda*t(1))*sin(mu*t(1))];
%  
%  part_m=[-lambda -mu; mu -lambda]/(lambda^2+mu^2);
%  c=M\(x0-part_m*b*u);
%  retval=[c(1)*exp(lambda*t).*sin(mu*t)+c(2)*exp(lambda*t).*cos(mu*t); c(1)*exp(lambda*t).*(-cos(mu*t))+c(2)*exp(lambda*t).*sin(mu*t)]+part_m*b*u;
  M=[exp(lambda*t(1))*cos(mu*t(1)) exp(lambda*t(1))*sin(mu*t(1));
  exp(lambda*t(1))*(sin(mu*t(1))) -exp(lambda*t(1))*cos(mu*t(1))];
  
  part_m=[-lambda -mu; mu -lambda]/(lambda^2+mu^2);
  c=M\(x0-part_m*b*u);
  retval=[c(1)*exp(lambda*t).*cos(mu*t)+c(2)*exp(lambda*t).*sin(mu*t); c(1)*exp(lambda*t).*(sin(mu*t))-c(2)*exp(lambda*t).*cos(mu*t)]+part_m*b*u;
end

function traj=analytic_trajectory_in_original_basis(u_ref, gamma_v, x_1, control);
  param.polygon_faces=100;
  param.trajectory_steps=250;
  
  %gamma_v=i_ref/u_ref;
  A=[-gamma_v 1; -1 0]; b=[0 1]'; 


  %x_0=[0.8 1.4214]'
  %x_1=[1 gamma]'*0.8;


  %ew=(-gamma_v(i)+sqrt(gamma_v(i)^2-4))/2;

  %lambda=real(ew); mu=imag(ew);
  %x_1=[1 gamma_v]'*u_ref;
  
  [v l]=eig(A);
  lambda=real(l)(1); mu=imag(l)(1);
  v(:,1)=real(v(:,1));
  v(:,2)=imag(v(:,2));
  
  A_n=inv(v)*A*v;
  x_1_n=inv(v)*x_1;
  b_n=inv(v)*b;
  
  time_l = linspace(0, pi/mu, 100);
  traj=analytic_trajectory(A_n, b_n, x_1_n, control, -time_l);
  traj=v*traj;
end


function surface=get_switch_surface(A, b, control, x1, time_l, param)
  
  %t -- tiempo inverso, demora la media de vuelta de la adjoint
  t=linspace(0,-time_l,param.polygon_faces);
  t
  %calculamos trayectoria del inicio con control u=0
  %empezando del punto de establecimiento deseado.
  %en la direccion inversa durante media vuelta
  %eso tambien nos da la primera rama de la superfice switchea
  for i=1:length(control)
    surface{i}=analytic_trajectory(A, b, x1, control{i}, t);
  end
end

function retval=plot_some(gamma_v, u_ref, style="-.k");
  hold on;

  %%datos iniciales para la aproximacion
  %para mejorar la aproximacion no es suficiente
  %aumentar la cantidad de los superficies del polygono y disminuir el radio
  %de la bola final
  %Tambien es necesario aumentar la densidad de los pasos trajectory_steps
  param.polygon_faces=100;
  param.trajectory_steps=250;
  param.r_ball=0.01;
  
  retval={};
  for i=1:length(gamma_v)
    A=[-gamma_v(i) 1; -1 0]; b=[0 1]'; 


    %x_0=[0.8 1.4214]'
    %x_1=[1 gamma]'*0.8;


    %ew=(-gamma_v(i)+sqrt(gamma_v(i)^2-4))/2;

    %lambda=real(ew); mu=imag(ew); 
    x_1=[1 gamma_v(i)]'*u_ref;
    
    [v l]=eig(A);
    lambda=real(l)(1); mu=imag(l)(1);
    v(:,1)=real(v(:,1));
    v(:,2)=imag(v(:,2));
    A_n=inv(v)*A*v
    x_1_n=inv(v)*x_1;
    b_n=inv(v)*b;
    
    surf=get_switch_surface(A_n, b_n, {0 1}, x_1_n, pi/mu, param);
    trajcont=analytic_trajectory(A_n, b_n, surf{2}(:,end), 0, -linspace(0, pi/mu, param.polygon_faces));
    trajcont=v*trajcont;
    
    for j=1:length(surf)
      surf{j}=v*surf{j};
      h(j)=plot(surf{j}(1,:), surf{j}(2,:), style, 'LineWidth', 1);
      
    end
    set(h(1), "color", "blue");
    set(h(2), "color", "red");
    plot(trajcont(1, :), trajcont(2, :), style);
    retval{i}=trajcont;
    %legend({"$S_0$", "$S_1$"});
  end
  
  ylim([-0.3 2]);
  xlim([0 1.4]);
  box on;
end



function [polygon, upper_curve_of_poly]=gen_polygon_for_filling(points, u_ref, gamma);
  polygon=[u_ref(1) gamma(1)*u_ref(1); u_ref(2) gamma(1)*u_ref(2)];
  [point1 ind1] = intersectLinePolygon([u_ref(2) 0 0 2], points);
  [point2 ind2] = intersectLinePolygon([0 0 1 gamma(2)], points);
  upper_curve_of_poly=vertcat(point1(2,:), points(ind1(2)+1:ind2(2),:), point2(2,:));
  polygon=vertcat(polygon, upper_curve_of_poly, [u_ref(1) gamma(2)*u_ref(1)]);3
end


function polygon=gen_polygon_for_filling2(points, u_ref, gamma);
%  polyline1 = [u_ref(1)*[1 gamma(1)]; u_ref(1)*[1 gamma(2)]; u_ref(2)*[1 gamma(2)];
%                u_ref(2)*[1 gamma(1)]; u_ref(1)*[1 gamma(1)]];
%  polygon=intersectPolylines(points, polyline1);
   [point1 ind1] = intersectLinePolygon([0 0 1 gamma(1)], points);
   [point2 ind2] = intersectLinePolygon([0 0 1 gamma(2)], points);
   upper_curve_of_poly=vertcat(point1(3,:), points(ind1(3)+1:ind2(2),:), point2(2,:));
   polygon=vertcat(u_ref(1)*[1 gamma(1)], upper_curve_of_poly, u_ref(1)*[1 gamma(2)]);
   
%   l = floor(length(points)/4)
%   clippoly=[0 0; points(l:3*l,:)]
%   origpoly=[0 0; u_ref(2) u_ref(2)*gamma(2); u_ref(2) u_ref(2)*gamma(1)]
%  polygon = sutherlandHodgman(origpoly, clippoly)
end

u_ref=[0.1 0.8];
u_op=0.7
#plot_some([0.08 0.4 0.6 1 1.5 1.7 1.95], u_ref);
#plot_some([0.08 0.4 0.6 1 1.5 1.7 1.95], 0.5);

%plot_some([0.04 0.6 1.7 1.9], u_ref);

##########

gamma=[0.1 0.4];
gamma_op=0.15

close all;
hold on;
grid on;
#generate upper polygon
#########
pbaspect("auto");

xlabel("$x_1$");
ylabel("$x_2$");

surf=plot_some(gamma, u_ref(2), "-k");

gammalines={[0 0; xlim()(2) xlim()(2)*gamma(1)], [0 0; xlim()(2) xlim()(2)*gamma(2)]};

#plot([u_ref(2) u_ref(2)], [0 ylim()(2)], "-.b");
plot([0 xlim()(2)], [0 xlim()(2)*gamma(1)], "--k");

plot([0 xlim()(2)], [0 xlim()(2)*gamma(2)], "--k");


point1=intersectPolylines(gammalines{2}, surf{1}');
#drawArrow([0 0 point1], 0.1, 0.05);
#annotation("arrow", [0.0785784; 0.819716], [0.188118; 0.371473]);

a=[0 0];
b=point1;

annotations{1}.a=a;
annotations{1}.b=b;

function listener(annotations)
  for i=1:length(annotations)
    h1=annotations{i}.h;
    a=get_normalized_coords(annotations{i}.a);
    b=get_normalized_coords(annotations{i}.b);
    set(h1, "position", [a' b'-a']);
  end
end

annotations{1}.h=annotation("arrow", [a(1) b(1)], [a(2) b(2)], "headlength", 5, "headwidth", 2.5, "linestyle", "--");
text(point1(1)-0.1, point1(2)+0.05, "$r_1$");

b=surf{2}(:,1)';

annotations{2}.a=a;
annotations{2}.b=b;
annotations{2}.h=annotation("arrow", [a(1) b(1)], [a(2) b(2)], "headlength", 5, "headwidth", 2.5, "linestyle", "--");

addlistener (gca, "position", @(h) listener(annotations));


text(surf{2}(1,1)-0.1, surf{2}(2,1)+0.05, "$r_2$");
%traj = [flip(traj2,2)(:,1:end-1) traj1];
#text(traj(:,end)(1), traj(:, end)(2), "$P$");

%poly_for_fill_bottom = gen_polygon_for_filling2(traj', u_ref, gamma);
%h=fillPolygon(vertcat(poly_for_fill, reversePolygon(poly_for_fill_bottom)));
%set(h, "facealpha", 0.25, "facecolor", "blue");
%text(0.6, 0.8, "$\\mathcal B$");

#build the new surfaces and B'
%u1=0.8*u_op;
%surf=plot_some([gamma_op], u1, "-k");
%[poly_for_fill upper_curve]=gen_polygon_for_filling(surf', u_ref, gamma);
%traj1=analytic_trajectory_in_original_basis(u1, gamma_op, u1*[1 gamma_op]', 0);
%traj2=analytic_trajectory_in_original_basis(u1, gamma_op, u1*[1 gamma_op]', 1);

%traj = [flip(traj2,2)(:,1:end-1) traj1];
%poly_for_fill_bottom = gen_polygon_for_filling2(traj', u_ref, gamma);
%poly_difference=vertcat(poly_for_fill, reversePolygon(poly_for_fill_bottom));
%h=fillPolygon(poly_difference);
%set(h, "facealpha", 0.25, "facecolor", "green");


#plot([u_ref(2) u_ref(2)], [ylim()(1) ylim()(2)], "k", "linewidth", 0.2);
#text(u_ref(2), ylim()(1)-0.2*yticks()(2), "$u_M$"); xlabel("$U$"); ylabel("$I$");

#plot([u_ref(1) u_ref(1)], [ylim()(1) ylim()(2)], "k", "linewidth", 0.2);
#text(u_ref(1), ylim()(1)-0.2*yticks()(2), "$u_m$");

#u1=u_op*0.8;
#surf=plot_some([gamma_op], u1, "-k");
#plot(u1, u1*gamma_op, ".k", "markersize", 10);
#text(u1-0.1, u1*gamma_op+0.1, "$x'$");

aspect_c=daspect()(1)/daspect()(2);

textxpos=(1-u_ref(2))/2+u_ref(2);
text(0.35, -0.08, "$\\gamma_1$", "rotation", atan(gamma(1)*aspect_c)*180/pi);

textxpos=(1-u_ref(2))/2+u_ref(2);
text(0.3, 0.24,"$\\gamma_2$", "rotation", atan(gamma(2)*aspect_c)*180/pi);

#plot(u_op, u_op*gamma_op, ".k", "markersize", 10);

text(u_ref(2)-0.08, u_ref(2)*gamma(1)+0.05, "$x^1$");
plot(u_ref(2), u_ref(2)*gamma(1), ".k", "markersize", 10);

text(u_ref(2)+0.02, u_ref(2)*gamma(2)+0.1, "$x^2$");
plot(u_ref(2), u_ref(2)*gamma(2), ".k", "markersize", 10);


set(gcf, "papersize", [3.5 3.5], "paperposition", [0 0 3.5 3.5]);
print("figure3.tex", "-dpdflatexstandalone");
#print("figure3.tex", "-dtikz");