%
% Chapter 7, Example 7.6, pages ???-???
% Optimal Reference Shaping for Dynamical Systems
% Theory and Applications
% -Tarunraj Singh
%
function example_76
%

% 
% Generate graphs for the L1 problem for the benchmark floating oscillator system
%
Tmima = [0.72559469386507   1.69129146074669   2.95932867687683   4.22476395515656   5.18926511945091   5.90937097399709]; 
Tf = Tmima(end);
u = minimax_L1_2mass(Tf);  % The time 5.90937 corresponds to the Minimax solution to the residual energy
uopt = u(2:end);

%
% Robust solution for the 2-mass-spring-dashpot system 
%  1-2exp(-.7255s)+2exp(-1.6912s)-2exp(-2.9593s)
%   +2exp(-4.2247s)-2exp(-5.1892s)+exp(-5.9093s)
%                                                       
% Generate sensitivity plot for the minimax solution
No_samp = length(uopt);		
No_samp = 3001;
T = Tf/(No_samp-1);             % Sampling Time
t = linspace(0,Tf,No_samp);

uminimax = 1*ones(size(t));
uminimax(find((t>Tmima(1)) & (t < Tmima(2)))) = -1;
uminimax(find((t> Tmima(2)) & (t < Tmima(3)) )) = 1;
uminimax(find(t>Tmima(3) & t < Tmima(4))) = -1;
uminimax(find(t > Tmima(4) & t < Tmima(4))) = 1;
uminimax(find(t > Tmima(5))) = -1;

    Xf = [1;1;0;0];			    % Inital States
    X0 = [0;0;0;0];				% Final States
    kp = 1;
    fopt = 1;

    
indk = 1;
kvec = linspace(0.7,1.3,51);
for k = kvec,
        M = [1 0;0 1];
        K = [k -k;-k k];
        C = 0.0*K;
        D = [1;0];
        usim = uminimax;
    
    Ac = [zeros(size(M)) eye(size(M)); -inv(M)*K -inv(M)*C];
    Bc = [zeros(length(M),1);inv(M)*D];
    Cc = eye(2*size(M));
    Dc = zeros(2*length(M),1);
    sysc = ss(Ac,Bc,Cc,Dc);
    
	sysd = c2d(sysc,T);
    t = linspace(0,Tf,No_samp);
    [yy,tt,xx] = lsim(sysd,usim,[],X0);
    % To match the optmization formulation, the lsim solution should be transitioned one more time step
    [G,H,Cc,Dc] = ssdata(sysd);
    XFINAL = G*xx(end,:)'+H*usim(end);
    XFINAL = xx(end,:)';
    
    K = [kp+k -k;-k k]; % Testing using a positive definite weight for 2 mass-spring system
    if fopt == 1,
        W1 = chol(K)';
        W2 = chol(M)';
    elseif fopt == 2
        W1 = eye(length(M));W2 = W1;
    else
        W1 = sqrtm(K);
        W2 = sqrtm(M);
    end;
    
    Res_StatesD(indk,:) = (W1*(XFINAL(1:length(M))-Xf(1:length(M))))';
    Res_StatesV(indk,:) = (W2*XFINAL(length(M)+1:2*length(M)))';
    Res_eng(indk) = (XFINAL(1:length(M))-Xf(1:length(M)))'*K*(XFINAL(1:length(M))-Xf(1:length(M)))...
                                 + XFINAL(length(M)+1:2*length(M))'*M*XFINAL(length(M)+1:2*length(M));
    indk = indk+1;
end;
figure(3)
hold on
plot(kvec,sqrt(0.5*Res_eng),'co-');

figure(4)
hold on
plot(abs(Res_StatesD),abs(Res_StatesV),'o');

figure(5)
hold on
subplot(3,2,1), plot(abs(Res_StatesD(:,1)),abs(Res_StatesV(:,1)),'o');
text(u(1)/1.2,u(1)/1.2,'x1-xdot1')
hold on
subplot(3,2,2), plot(abs(Res_StatesD(:,2)),abs(Res_StatesV(:,2)),'o');
text(u(1)/1.2,u(1)/1.2,'x2-xdot2')
hold on
subplot(3,2,3), plot(abs(Res_StatesD(:,1)),abs(Res_StatesV(:,2)),'o');
text(u(1)/1.2,u(1)/1.2,'x1-xdot2')
hold on
subplot(3,2,4), plot(abs(Res_StatesD(:,2)),abs(Res_StatesV(:,1)),'o');
text(u(1)/1.2,u(1)/1.2,'x2-xdot1')
hold on
subplot(3,2,5), plot(abs(Res_StatesD(:,1)),abs(Res_StatesD(:,2)),'o');
text(u(1)/1.2,u(1)/1.2,'x1-x2')
hold on
subplot(3,2,6), plot(abs(Res_StatesV(:,1)),abs(Res_StatesV(:,2)),'o');
text(u(1)/1.2,u(1)/1.2,'xdot1-xdot2')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
function [u] = minimax_L1_2mass(Tf);
% 
% Program to Solve for the rest-to-rest Control Profile for a 
% 2-Mass-system, using Linear Programming
% Minimax Control using a L1 norm of the Residual Energy
%
% Robust solution for the 2-mass-spring-dashpot system 
%  1-2exp(-.7255s)+2exp(-1.6912s)-2exp(-2.9593s)
%   +2exp(-4.2247s)-2exp(-5.1892s)+exp(-5.9093s)

    Xf = [1;1;0;0];			    % Inital States
    X0 = [0;0;0;0];				% Final States

No_samp = 1001;				% Sampling Time
T = Tf/(No_samp-1);
exitflag = 1;

kvec = linspace(0.7,1.3,21);
indk = 1;
AINE = [];
BINE = [];
for k = kvec,
        M = [1 0;0 1];
        K = [k -k;-k k];
        C = 0.0*K;
        D = [1;0];

    Ac = [zeros(size(M)) eye(size(M)); -inv(M)*K -inv(M)*C];
    Bc = [zeros(length(M),1);inv(M)*D];
    Cc = eye(2*size(M));
    Dc = zeros(2*length(M),1);
    sysc = ss(Ac,Bc,Cc,Dc);
    omgvals = diag(eye(size(M))); %eig(K,M);% 

	sysd = c2d(sysc,T);
    [G,H,Cc,Dc] = ssdata(sysd);

    f = zeros((1+No_samp),1);			% Cost function coefficient
    f(1,1) = 1;
 
    % Both the Cholesky decomposition and the sqrtm forms results in the same optimal solutions
    kp = 1;
    K = [kp+k -k;-k k]; % Testing using a positive definite weight for 2 mass-spring system
           
    fopt = 1;
    if fopt == 1,
        W1 = chol(K)';
        W2 = chol(M)';
    elseif fopt == 2
        W1 = eye(length(M));W2 = W1;
    else
 
        W1 = sqrtm(K);
        W2 = sqrtm(M);
    end;

	% Equality Constraint calculation
	Aieq = [];
	for ind = 1:(No_samp)
   		Aieq = [G^(ind-1)*H Aieq];
	end;
    FTran = G^No_samp*X0;
    % Constraints for approximating the L1 norm
    LM = length(M);
    for inda = 1:2^(2*length(M))
        Coeffs = nc2dp(inda,2*LM);
        Aieq1 = [-1 Coeffs(1:LM)*W1*Aieq(1:LM,:)+Coeffs(LM+1:2*LM)*W2*Aieq(LM+1:2*LM,:)];
        bieq1 = -(Coeffs(1:LM)*W1*(FTran(1:LM,1)-Xf(1:LM))+Coeffs(LM+1:2*LM)*W2*FTran(LM+1:2*LM,1));
        AINE = [AINE;Aieq1];
        BINE = [BINE;bieq1];
    end; 
end;

Flag = 2;
if Flag == 1
	vlb =  0*ones(size(f));
	vub =  1*ones(size(f));
	u0 = 0.5*ones(size(f));
else
   	vlb = -1*ones(size(f));
	vub =  1*ones(size(f));
	u0 = 0.5*ones(size(f));
end;

	% Minimax Robust Controller
    [nr,nc] = size(AINE);
    Snr = num2str(nr);
    Snc = num2str(nc);
    String1 = strcat('Number of Constraints = ',Snr);
    String2 = strcat('Number of Unknowns = ',Snc);
    disp(String1);
    disp(String2);
	[u,fval,exitflag] = linprog(f,AINE,BINE,[],[],vlb,vub,u0);

    % Generate the Sensitivity Curve
    Xf = [1;1;0;0];			    % Inital States
    X0 = [0;0;0;0];				% Final States

indk = 1;
kvec = linspace(0.7,1.3,51);
for k = kvec,
        M = [1 0;0 1];
        K = [k -k;-k k];
        C = 0.0*K;
        D = [1;0];
        usim = u(2:end);
  
    Ac = [zeros(size(M)) eye(size(M)); -inv(M)*K -inv(M)*C];
    Bc = [zeros(length(M),1);inv(M)*D];
    Cc = eye(2*size(M));
    Dc = zeros(2*length(M),1);
    sysc = ss(Ac,Bc,Cc,Dc);
    
	sysd = c2d(sysc,T);
    t = linspace(0,Tf,No_samp);
    [yy,tt,xx] = lsim(sysd,usim,[],X0);
    % To match the optmization formulation, the lsim solution should be transitioned one more time step
    [G,H,Cc,Dc] = ssdata(sysd);
    XFINAL = G*xx(end,:)'+H*usim(end);

    K = [kp+k -k;-k k]; % Testing using a positive definite weight for 2 mass-spring system
    if fopt == 1,
        W1 = chol(K)';
        W2 = chol(M)';
    elseif fopt == 2
        W1 = eye(length(M));W2 = W1;
    else
        W1 = sqrtm(K);
        W2 = sqrtm(M);
    end;

    
    Res_StatesD(indk,:) = (W1*(XFINAL(1:length(M))-Xf(1:length(M))))';
    Res_StatesV(indk,:) = (W2*XFINAL(length(M)+1:2*length(M)))';
    Res_eng(indk) = (XFINAL(1:length(M))-Xf(1:length(M)))'*K*(XFINAL(1:length(M))-Xf(1:length(M)))...
                                 + XFINAL(length(M)+1:2*length(M))'*M*XFINAL(length(M)+1:2*length(M));
    indk = indk+1;
end;
figure(3)
plot(kvec,sqrt(0.5*Res_eng));
xlabel('Stiffness');
ylabel('Square Root of Residual Energy')

figure(4)
plot(abs(Res_StatesD),abs(Res_StatesV),'*');
hold on
plot([u(1) 0],[0 u(1)],'r--');
xlabel('disprk');
ylabel('velrm');

figure(5)
subplot(3,2,1), plot(abs(Res_StatesD(:,1)),abs(Res_StatesV(:,1)),'*'); 
hold on; plot([u(1) 0],[0 u(1)],'r--');
subplot(3,2,2), plot(abs(Res_StatesD(:,2)),abs(Res_StatesV(:,2)),'*'); 
hold on; plot([u(1) 0],[0 u(1)],'r--');
subplot(3,2,3), plot(abs(Res_StatesD(:,1)),abs(Res_StatesV(:,2)),'*'); 
hold on; plot([u(1) 0],[0 u(1)],'r--');
subplot(3,2,4), plot(abs(Res_StatesD(:,2)),abs(Res_StatesV(:,1)),'*'); 
hold on; plot([u(1) 0],[0 u(1)],'r--');
subplot(3,2,5), plot(abs(Res_StatesD(:,1)),abs(Res_StatesD(:,2)),'*'); 
hold on; plot([u(1) 0],[0 u(1)],'r--');
subplot(3,2,6), plot(abs(Res_StatesV(:,1)),abs(Res_StatesV(:,2)),'*'); 
hold on; plot([u(1) 0],[0 u(1)],'r--');
    if exitflag > 0
        
	    t = linspace(0,Tf,No_samp);
        k = 1;

        usim = u(2:end);
        M = [1 0;0 1];
        K = [k -k;-k k];
        C = 0.0*K;
        D = [1;0];

        Ac = [zeros(size(M)) eye(size(M)); -inv(M)*K -inv(M)*C];
        Bc = [zeros(length(M),1);inv(M)*D];
        Cc = eye(2*size(M));
        Dc = zeros(2*length(M),1);
        sysc = ss(Ac,Bc,Cc,Dc);

	    sysd = c2d(sysc,T);

	    figure(1)
	    [yy,tt,xx] = lsim(sysd,usim,[],X0);
        subplot(211), plot(tt,xx(:,1:length(M))); xlabel('Time'); ylabel('Displacement');
        subplot(212), stairs(t,xx(:,(length(M)+1):2*length(M))); xlabel('Time'); ylabel('Velocity');
	    figure(2)
        stairs(t,usim); xlabel('Time'); ylabel('Control');
        axis([0 t(end)+1 -1.2 1.2])
        pause(0.1);
    end;
    

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Applied linear optimal control: examples and alogrithms By Arthur Earl
%% Bryson page: 295
function dp = nc2dp(nc,N)
%
% Finds dp given a decimal corner number nc.
%
for j=1:N
   if nc-1<2^(N-j),
      dp(j) = -1;
   else
      dp(j) = +1;
      nc=nc-2^(N-j);
   end
end;

