%%El sistema
close all;
graphics_toolkit("qt");

R=20; L=2e-3; C=40e-6; E=40;
A=[-1/(R*C) 1/C; -1/L 0];
%A=[0 1/C; -1/C 0];
b2=[0 E/L]';

%%Los parametros inicial
u_ref=32;
x_1=[u_ref u_ref/R]';
x_0=[0 0]';

%calculamos vectores propios
[v l]=eig(A);
v(:,1)=real(v(:,1));
v(:,2)=imag(v(:,2));

%frecuencia de la rotacion del adjoint
freq_p=imag(l(1,1));
p_sw = abs(pi/freq_p); %period of time, when p switches

%Transformacion del sistema en nueva base
A_n=inv(v)*A*v;
b_n=inv(v)*b2;
x_1_n=inv(v)*x_1;
x_0_n=inv(v)*x_0;


%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;
end

%DEQ
function retval=f(x,u,t,params)
  retval=params.A*x + params.b2*u(t);
end

%x=analytic_trajectory (A_n,l,[0 0]',[1 2.5]',0,0:0.1:1);

%t -- tiempo inverso, demora la media de vuelta de la adjoint
t=linspace(0,-abs(p_sw),1000);



%Encontramos el punto donde las trayectorias se cruzan.
%fminsearch funciona poco mejor que fsolve
%fminsearch( @(t_v) norm((analytic_trajectory(A_n,b_n,x_1_n,0,[0 -abs(t_v(1))])-analytic_trajectory (A_n,b_n,[0; 0],1,[0 abs(t_v(2))]))(:,end)), [0; 0],rr)
 

%Encontramos el punto donde las trayectorias se cruzan.
%Usamos abs(t_v()) para q asegurar la direccion correcta de la busqueda
%En general la manera fea, pero asi funciona :)
t_switch=fsolve( @(t_v) (analytic_trajectory(A_n,b_n,x_1_n,0,[0 -abs(t_v(1))])-
              analytic_trajectory (A_n,b_n,x_0_n,1,[0 abs(t_v(2))]))(:,end),
              [1e-4; 1e-4],
              optimset("TolX",1e-8));

%%Ahora t_switch(2) -- tiempo hasta se ocurre el switcheo u=1->u=0
%t_switch(1) -- tiempo dentro el t_switch(2) y punto deseado.
t_switch=abs(t_switch);

%Probar, si el punto del cruce esta dentro del periodo maximo
if max(t_switch>p_sw)>=1
  disp "Zero outside of region, try one more time with another start point"
end



%trayectoria1=lsode(@(x,t) A_n*x+inv(v)*b2, [0 0]', linspace(0,3.0517e-4,1000));

%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
surface1=analytic_trajectory(A_n, b_n, x_1_n, 0, t);

%tenemos un switch. Eso significa, que el tiempo maximo de switcheo siempre sea pi,
%asi podemos calcular la superficie simplemente girando los primeros dos
%ramas alrededor del centro de la evolucion del estado.
ang=pi;
Mr=[cos(ang) -sin(ang);
    sin(ang) cos(ang)];

%centro de la evolucion del estado para la rama u=1
centre_of_system=-inv(A_n)*b_n;

%Atencion: -p_sw porque el systema evoluciona en el tiempo *atras*!
surface12=(Mr*(surface1-centre_of_system)).*exp(-A_n(1,1)*p_sw)+centre_of_system;

%surface1=horzcat(surface1, analytic_trajectory(A_n, b_n, surface1(:,end), 1, t));
%surface2=lsode(@(x,t) A_n*x+inv(v)*b2, inv(v)*x_1, t);

%calculamos otra rama de la superficie switchea
%tiempo inverso con u=1
surface2=analytic_trajectory(A_n, b_n, x_1_n, 1, t);

%centro de la transformacion del superficie para la rama u=1
%efectivamente es inv(A_n)*[0 0]'
centre_of_system=[0 0]';
surface22=(Mr*(surface2-centre_of_system)).*exp(-A_n(1,1)*p_sw)+centre_of_system;

x=analytic_trajectory(A_n, b_n, x_0_n, 1, linspace(0,t_switch(2),1000));

%pintamos el diagrama fasico en la base canonica.
plot((v*surface1)(1,:),(v*surface1)(2,:),";u=0;",(v*surface2)(1,:),(v*surface2)(2,:),";u=1;",
     (v*surface12)(1,:),(v*surface12)(2,:),"k;superficie;",(v*surface22)(1,:),(v*surface22)(2,:),"k",(v*x)(1,:),(v*x)(2,:),"r;trajectory;");
%plot((surface1)(1,:),(surface1)(2,:),";u=0;",(surface2)(1,:),(surface2)(2,:),";u=1;",(x)(1,:),(x)(2,:));

%transorm
x=v*x;
text_place=x(:,end)
%text(text_place(1),text_place(2),["t= " num2str(t_switch(2))])
text(text_place(1),text_place(2), sprintf("t= %f\nU= %f\nI= %f", t_switch(2),x(1,end), x(2,end)))
xlabel("Voltage"); ylabel("Corriente");
set(gca,'xgrid','on', 'ygrid','on'); 



disp "Switch times:", t_switch(2), t_switch(1)

%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%
%Otro punto... solo para demostrar
x_0=[150 0]'; 
x_0_n=inv(v)*x_0;


%tiempo del switcheo
%Aqui calculamos el tiempo en la manera numerica. Como la distancia minima hasta
%el superficie switcheo
t_switch=fminsearch( @(t) min( norm(surface12-
              analytic_trajectory (A_n,b_n,x_0_n,0,[0 abs(t)])(:,end), "columns")),
              %6.3054e-5,
              0,
              optimset("TolX",1e-15));
x=analytic_trajectory(A_n, b_n, x_0_n, 0, linspace(0,abs(t_switch),1000));


disp "\n\nTrayectoria 2"
disp "controla u=0 mientras tiempo", disp(t_switch)

%switcheamos otra vez exactamente a pasar del tiempo p_sw
x=horzcat(x, analytic_trajectory(A_n, b_n, x(:,end), 1, linspace(0,p_sw,1000)));

disp "\ncontrola u=1 mientras tiempo", disp(p_sw)

%calculamos el tiempo final hasta el establecimiento
t_switch=fminsearch( @(t) min( norm(x(:,end)-
              analytic_trajectory (A_n,b_n,x_1_n,0,[0 -abs(t)])(:,end), "columns")),
              %6.3054e-5,
              1e-5,
              optimset("TolX",1e-15));

x=horzcat(x, analytic_trajectory(A_n, b_n, x(:,end), 0, linspace(0,abs(t_switch),1000)));          
disp "\ncontrola u=0 mientras tiempo", disp(t_switch)

x=v*x;
hold on;
plot(x(1,:),x(2,:),"--r;trajectory 2;");
%hold off;
h = findobj(gca, 'type', 'line');
set(h, 'LineWidth', 2);

print("superficies_switcheos_extendidos.tex", "-S320,200", "-dtikz");

%print("superficies_switcheos_extendidos.tex", "-dtikz");

