%
% Chapter 7, Example 7.4, pages 344-347
% Optimal Reference Shaping for Dynamical Systems
% Theory and Applications
% -Tarunraj Singh
%
function example_74
%
uopt = minimax_infty(5.866);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
function [u] = minimax_infty(Tf);
% 
% Program to Solve for the rest-to-rest Control Profile for a
% 2-Mass-system, using Linear Programming
% Minimax Control using a L infinity norm of the Residual Energy
%

Xf = [1;1;0;0];			    % Final States
X0 = [0;0;0;0];				% Initial 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 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);
    [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 = [1+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 = -1*ones(size(f));
	vub =  1*ones(size(f));
	u0 = 0.5*ones(size(f));

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

    % Generate the Sensitivity Curve
    Xf = [1;1;0;0];			    % Final States
    X0 = [0;0;0;0];				% Initial 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 = [1+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) u(1) 0],[0 u(1) u(1)],'r--');
xlabel('Displacement (root{K}x)');
ylabel('Velocity (root(M)\dot{x}))')
keyboard
    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');
        pause(0.1);
    end;
