%
% Chapter 7, Example 7.5, pages ???-???
% Optimal Reference Shaping for Dynamical Systems
% Theory and Applications
% -Tarunraj Singh
%
function example_75
%
% Minimax solution for the single mass-spring-dashpot system 0.3452+0.4730exp(-3.1703s)+0.1818exp(-6.3405s)
%
Tf = 6.3405
u = minimax_L1(Tf);  % The time 6.3405 corresponds to the Minimax solution to the residual energy
uopt = u(2:end);

%
% Minimax solution for the single mass-spring-dashpot system 0.3452+0.4730exp(-3.1703s)+0.1818exp(-6.3405s)
%
% Generate sensitivity plot for the minimax solution
uminimax = (0.3452+0.4730)*ones(size(uopt));
No_samp = length(uopt);				
T = Tf/(No_samp-1);             % Sampling Time
t = linspace(0,Tf,No_samp);
uminimax(find(t<3.1703)) = 0.3452;

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

    fopt = 3;
    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,[],[0;0]);
    % To match the optmization formulation, the lsim solution should be transitioned one more time step
    [G,H,Cc,Dc] = ssdata(sysd);
    XFINAL = xx(end,:)';%G*xx(end,:)'+H*usim(end);

    Res_StatesD(indk,:) = (W1*(XFINAL(1:length(M))-1))';
    Res_StatesV(indk,:) = (W2*XFINAL(length(M)+1:2*length(M)))';
    Res_eng(indk) = (XFINAL(1:length(M))-1)'*K*(XFINAL(1:length(M))-1)...
                                 + 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),'ro-');

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
function [u] = minimax_L1(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 Weighted States
%
    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);
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))); %eig(K,M);% 

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

    fopt = 3;
    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; % End of the Uncertain K Loop
  

    f = zeros((1+No_samp),1);			% Cost function coefficient
    f(1,1) = 1;

	vlb =  0*ones(size(f));
	vub =  [100;1*ones(No_samp,1)];
	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
    [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;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);
    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,t,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);

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

    if exitflag > 0
        
	    t = linspace(0,Tf,No_samp);
        k = 1;
         M = [1];
         K = [k];
         C = [0.2];
         D = [k];

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

	    figure(1)
	    [yy,tt,xx] = lsim(sysd,usim,[],X0);
        % Equality Constraint calculation
        [G,H,Cc,Dc] = ssdata(sysd);
	    Aieq = [];
	    for ind = 1:(No_samp)
   		    Aieq = [G^(ind-1)*H Aieq];
	    end;
        
        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;
[u(1,1) fval]

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