%
% Macro to implement a control allocation algorithm performing
% a mixed optimization of the norm of the error and of the deviation
% from a preferred value.
% (c) Marc Bodson, June 18, 2020.
% The optimization criterion is:
%   J=||cb.u-ad||+wc||u-upref||, subject to umin<=u<=umax.
% The vector norm is the l1 norm, wc is an adjustable
% weighting factor, and upref is the preferred control position.
% Inputs: cb[n,m],ad[n,1],umin[m,1],umax[m,1],upref[m,1]
% Outputs: u[m,1],a[n,1] (a=cb.u)
% The program uses the simplex algorithm with direct initialization.
% For details, see M. Bodson, “Evaluation of Optimization Methods
% for Control Allocation, AIAA Journal of Guidance, Control, and Dynamics,
% vol. 25, no. 4, pp. 703-711, 2002.
%
% Parameters
wc=1e-3;          % weight for the control deviations
epsx=10.*eps;     % small parameters for simplex algorithm
epsxd=10^6.*eps;  % eps is Matlab's floating point accuracy
% Prepare the problem for simplex algorithm
[n,m] = size(cb);
lpos=zeros(m,1);lneg=zeros(m,1);ubase=upref;
npos=0;axp=zeros(n,m);hxp=zeros(m,1);
nneg=0;axn=zeros(n,m);hxn=zeros(m,1);
for i=1:m
    if umax(i)>upref(i)
        npos=npos+1;lpos(npos)=i;
        axp(:,npos)=cb(:,i);hxp(npos)=umax(i)-upref(i);
        if umin(i)>upref(i)
            ubase(i)=umin(i);hxp(npos)=umax(i)-umin(i);
        end
    end
    if umin(i)<upref(i)
        nneg=nneg+1;lneg(nneg)=i;
        axn(:,nneg)=-cb(:,i);hxn(nneg)=-umin(i)+upref(i);
        if umax(i)<upref(i)
            ubase(i)=umax(i);hxn(nneg)=umax(i)-umin(i);
        end
    end
end
npn=npos+nneg;mx=n;nx=npn+n+n;id=[1:npn+n];ib=[npn+n+1:nx];
ax=[axp(:,1:npos) axn(:,1:nneg)];bx=ad-cb*ubase;
for i=1:n;if bx(i)<0;ax(i,:)=-ax(i,:);bx(i)=-bx(i);end;end
ax=[ax -eye(n) eye(n)];cx=[wc*ones(npn,1);ones(n,1);ones(n,1)];
ipn=ones(nx,1);aer=sum(bx);
hx=[hxp(1:npos);hxn(1:nneg);aer*ones(n,1);aer*ones(n,1)];
% Simplex algorithm.
[mx,nx]=size(ax);nb=mx;nd=nx-mx;binv=eye(mx);xb=bx;
done=0;iter=0;
warning off;iterm=nchoosek(nb+nd,nb);warning on;
nbp=nb+1;ica=zeros(nbp,1);iva=zeros(nbp,1);
tma=zeros(nbp,nb);itma=zeros(nbp,1);
while done==0
    dj=cx(id)-ax(:,id)'*(binv'*cx(ib));[djm,js]=min(dj);nva=0;
    if (djm<-epsxd)
        ids=id(js);dbd=binv*ax(:,ids);ddm=hx(ids);nva=1;ica(1)=1;
        for i=1:nb
            if dbd(i)>epsx;dd=xb(i)/dbd(i);dde=dd-ddm;
                if dde<-epsx;nva=1;ica(1)=2;iva(1)=i;ddm=dd;end
                if (abs(dde)<epsx);nva=nva+1;ica(nva)=2;iva(nva)=i;end
            end
            if dbd(i)<-epsx;dd=(xb(i)-hx(ib(i)))/dbd(i);dde=dd-ddm;
                if dde<-epsx;nva=1;ica(1)=3;iva(1)=i;ddm=dd;end
                if (abs(dde)<epsx);nva=nva+1;ica(nva)=3;iva(nva)=i;end
            end
        end
    else done=1;
    end
    if (nva>1)
        is=1;if(ica(1)==1);tma(1,:)=zeros(1,nb);is=2;end
        for i=is:nva;tma(i,:)=binv(iva(i),:)/dbd(iva(i));end
        found=0;itma=[1:nva];
        for j=1:nb
            if (found==0)
                tmam=tma(itma(1),j);nvan=1;
                for i=2:nva % look for a unique minimum in a column
                    tman=tma(itma(i),j);tmae=tman-tmam;
                    if tmae<-epsx;nvan=1;itma(1)=itma(i);tmam=tman;end
                    if (abs(tmae)<epsx);nvan=nvan+1;itma(nvan)=itma(i);end
                end
                nva=nvan;if nva==1;found=1;end
            end
        end
        icas=ica(itma(1));is=iva(itma(1));
    else
        icas=ica(1);is=iva(1);
    end
    if (done==0)
        switch icas
            case 1
                bx=bx-hx(ids)*ax(:,ids);ax(:,ids)=-ax(:,ids);
                ipn(ids)=-ipn(ids);cx(ids)=-cx(ids);
            case 2
                ibs=ib(is);id(js)=ibs;ib(is)=ids;
                bidb=binv*(ax(:,ids)-ax(:,ibs));
                if abs(1+bidb(is))<10*epsx;warning('Poor conditioning');end
                binv=binv-bidb*(binv(is,:)/(1+bidb(is)));
            case 3
                ibs=ib(is);id(js)=ibs;ib(is)=ids;
                bx=bx-hx(ibs)*ax(:,ibs);ax(:,ibs)=-ax(:,ibs);
                ipn(ibs)=-ipn(ibs);cx(ibs)=-cx(ibs);
                bidb=binv*(ax(:,ids)+ax(:,ibs)); % sign of ax(:,ibs) changed
                if abs(1+bidb(is))<10*epsx;warning('Poor conditioning');end
                binv=binv-bidb*(binv(is,:)/(1+bidb(is)));
        end
        xb=binv*bx;jopt1=cx(ib)'*xb;
    else
        done=1;
    end
    iter=iter+1;
    if iter>=iterm
        error('Maximum number of iterations reached');
    end
end
x=zeros(nx,1);
for i=1:nb;ibi=ib(i);
    if ipn(ibi)<0;x(ibi)=hx(ibi)-xb(i);else;x(ibi)=xb(i);end
end
for i=1:nd;idi=id(i);if ipn(idi)<0;x(idi)=hx(idi);end;end
% Obtain control vector and achieved a=CB.u vector
u=ubase;
for i=1:npos;u(lpos(i))=u(lpos(i))+x(i);end
for i=1:nneg;u(lneg(i))=u(lneg(i))-x(i+npos);end
a=cb*u;