%
% Chapter 7, Example 7.3, pages 340-344
% Optimal Reference Shaping for Dynamical Systems
% Theory and Applications
% -Tarunraj Singh
%
function example_73
%
% Minimax solution for the single mass-spring-dashpot system 0.3452+0.4730exp(-3.1703s)+0.1818exp(-6.3405s)
%
uopt = minimax1(6.3405);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
function [u] = minimax1(Tf);
% 
% Program to Solve for the rest-to-rest Control Profile for a 
% 1-Mass-system, using Linear Programming
% Minimax Control using a L infinity norm of the Residual Energy
%
% Minimax solution for the single mass-spring-dashpot system 0.3452+0.4730exp(-3.1703s)+0.1818exp(-6.3405s)
%

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

No_samp = 501;				% 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];
         K = [k];
         C = [0.2];
         D = [k];

    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)));

	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
   % K = [2*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;
    Aieq1 = [-1*ones(size(Aieq,1)/2,1) W1*Aieq(1:length(M),:)];
    Aieq2 = -[1*ones(size(Aieq,1)/2,1) W1*Aieq(1:length(M),:)];
    bieq1 = -W1*(FTran(1:length(M),1)-Xf(1:length(M)));    
    bieq2 =  W1*(FTran(1:length(M),1)-Xf(1:length(M)));  
    
    Aieq3 = [-1*ones(size(Aieq,1)/2,1) W2*Aieq(length(M)+1:2*length(M),:)];
    Aieq4 = -[1*ones(size(Aieq,1)/2,1) W2*Aieq(length(M)+1:2*length(M),:)];
    bieq3 = -W2*FTran(length(M)+1:2*length(M),1);    
    bieq4 =  W2*FTran(length(M)+1:2*length(M),1);  
    
    Aieq = [Aieq1
            Aieq2
            Aieq3
            Aieq4];
        
    Bieq = [bieq1
            bieq2
            bieq3
            bieq4];

    AINE = [AINE;Aieq];
    BINE = [BINE;Bieq];
end;


	vlb =  0*ones(size(f));
	vub =  1*ones(size(f));
	u0 = 0.5*ones(size(f));
    
   	% Inequality constraint which requires control input to 
	% progressively increase to the final value
	A = zeros(No_samp-1,No_samp);
	for ind = 1:(No_samp-1)
		A(ind,ind) = 1;
		A(ind,ind+1) = -1;
	end;
	Adelta = [zeros(size(A,1),1) A];
    bdelta = zeros(No_samp-1,1);
    AINE = [AINE;Adelta];
    BINE = [BINE;bdelta];
    

	% Minimax Robust Controller
	[u,fval,exitflag] = linprog(f,AINE,BINE,[],[],vlb,vub,u0);

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

indk = 1;
kvec = linspace(0.7,1.3,51);
for k = kvec,
         M = [1];
         K = [k];
         C = [0.2];
         D = [k];
         usim = u(2:end);

    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;

    
    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);

    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) u(1) 0],[0 u(1) u(1)],'r--');
xlabel('Displacement (\sqrt{K}x)');
ylabel('Velocity (\sqrt{M}\dot{x}))')

    if exitflag > 0
        
	    t = linspace(0,Tf,No_samp);
        k = 1;

         usim = u(2:end);
         M = [1];
         K = [k];
         C = [0.2];
         D = [k];

        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');
        pause(0.1);
    end;
