%Script for optimizing drilling process with 2 inputs
%Needs external functions drilling.m, drillsolve.m, objfun.m
%msv.m and exactlocal.m.
%DEH - 2010

clear all
close all
clc

global db Wdbmax tt0 dstring Wdbt R0 K kc D bhp qmud dp Aa g rho rhos

%Parameters:
db = 0.254; %Bit diameter, m
Wdbmax = 178.583; %Maximum WOB per m diameter, tons/m
tt0 = 10; %Trip time, hours
dstring = 0.1; %Diameter of drill string, m
D = 3000; %Depth
Aa = (db^2-dstring^2)*pi/4; %Cross-sectional area of annulus, m2
g = 9.81; %Gravity
R0 = 5; %Formation drillability, m/hr 
tempR0 = R0;
K = 15*R0; %Formation abrasiveness constant, hours 
kc = 5e5/R0; %Specific cutting force, N/m2-ton
Wdbt = 63/R0; %Threshold WOB per m diameter, tons/m
bhp = 470; %BHP, bar
bhpsave = bhp;
qmud = 50; %Mud flow rate, l/s
rhos = 2700; %Formation density, kg/m3
rho = 1400; %Mud density, kg/m3
dp = 0.005; %Diameter of drilled particle, m

%Optimization:
u0 = [30;    %WOB, tons
    100];    %RPM
lb = [0; 0];
ub = [Wdbmax*db; 300];
options = optimset('Display','iter','TolFun',1e-12,'MaxFunEvals',300);
[u,fval,exitflag,output,lambda,grad,hessian] = fmincon(@objfun, u0,[],...
    [],[],[],lb,ub,[],options);
Juu = hessian;

%Nominal optimum u and y:
y0 = drilling(u);

%Gains:
G = zeros(length(y0),length(u));
deltau = 0.01*u;
for i = 1:length(u)
    udev = u+deltau.*[zeros(i-1,1); ones(1,1); zeros(length(u)-i,1)];
    G(:,i) = (drilling(udev) - y0)./(deltau(i));
end
[ny,nu] = size(G);

%Implementation errors:
eimp = [0.1; 3; 30; 0.5; 3; 10];

%Disturbances:
d = [R0;rhos;bhp];
nd = length(d);
deltayopt = zeros(length(y0),nd);
Gd = zeros(length(y0),nd);
F = zeros(length(y0),nd);
deltad = 0.01*d;
for i = 1:nd
    ddev = d+deltad.*[zeros(i-1,1);ones(1,1);zeros(nd-i,1)];
    R0 = ddev(1);
    K = 15*R0;
    kc = 5e5/R0;
    Wdbt = 63/R0;
    rhos = ddev(2);
    bhp = ddev(3);
    Gd(:,i) = (drilling(u)-y0)/deltad(i);
    
    %Re-optimization
    options = optimset('TolFun',1e-12,'MaxFunEvals',300);
    [uopt,fvalopt,exitflag,output] = fmincon(@objfun, u0,[],[],[],[],...
        lb,ub,[],options);
    deltayopt(:,i) = drilling(uopt)-y0;
    F(:,i) = deltayopt(:,i)/deltad(i);
end
bhp = bhpsave;

stepsize = 0.1;
lossplot = zeros(1+ny+nchoosek(ny,nu)+nchoosek(ny,nu+1)+...
    nchoosek(ny,nu+2),1/stepsize+1);

for a = -1:stepsize:1
    c = (1+stepsize+a)/stepsize;
    ddash = a*ones(nd);
    ndash = a*ones(ny);
    Wd = diag([-2.5; 500; 10]).*ddash;
    Wn = diag(eimp).*ndash;
    Ftilde = [F*Wd Wn];

    %Exact Local Method:
    tempG = G;
    tempGd = Gd;
    tempFtilde = Ftilde;
    H1 = zeros(size(lossplot,1),ny);
    H2 = zeros(size(lossplot,1),ny);
    lossvector = zeros(size(lossplot,1),2);
    [H maxloss] = exactlocal(Ftilde,Juu,G,Gd);
    H1(1,:) = H(1,:);
    H2(1,:) = H(2,:);
    lossvector(1,:) = maxloss; 

    for i=1:ny
        G(i,:) = 0;
        Gd(i,:) = 0;
        Ftilde(i,:) = 0;
        [H maxloss] = exactlocal(Ftilde,Juu,G,Gd);
        H1(i+1,:) = H(1,:);
        H2(i+1,:) = H(2,:);
        lossvector(i+1,:) = maxloss;
        G = tempG;
        Gd = tempGd;
        Ftilde = tempFtilde;
    end
    n = ny+2;
    for j = nchoosek(1:ny,nu)'
        G(j,:) = 0;
        Gd(j,:) = 0;
        Ftilde(j,:) = 0;
        [H maxloss] = exactlocal(Ftilde,Juu,G,Gd);
        H1(n,:) = H(1,:);
        H2(n,:) = H(2,:);
        lossvector(n,:) = maxloss;
        n = n+1;
        G = tempG;
        Gd = tempGd;
        Ftilde = tempFtilde;
    end
    m = n;
    for j = nchoosek(1:ny,nu+1)'
        G(j,:) = 0;
        Gd(j,:) = 0;
        Ftilde(j,:) = 0;
        [H maxloss] = exactlocal(Ftilde,Juu,G,Gd);
        H1(m,:) = H(1,:);
        H2(m,:) = H(2,:);
        lossvector(m,:) = maxloss;
        m = m+1;
        G = tempG;
        Gd = tempGd;
        Ftilde = tempFtilde;
    end
    o = m;
    for j = nchoosek(1:ny,nu+2)'
        G(j,:) = 0;
        Gd(j,:) = 0;
        Ftilde(j,:) = 0;
        [H maxloss] = exactlocal(Ftilde,Juu,G,Gd);
        H1(o,:) = H(1,:);
        H2(o,:) = H(2,:);
        lossvector(o,:) = maxloss;
        o = o+1;
        G = tempG;
        Gd = tempGd;
        Ftilde = tempFtilde;
    end

    res = [H1 H2 lossvector];

    [C5 I5] = min(res(2:7,13));
    I5 = I5+1;
    [C4 I4] = min(res(8:22,13));
    I4 = I4+7;
    [C3 I3] = min(res(23:42,13));
    I3 = I3+22;
    [C2 I2] = min(res(43:57,13));
    I2 = I2+42;

    lossplot(:,round(c)) = res(:,13);
end

%Minimum Singular Value method:
eopt = max(abs(F*Wd),[],2);
span = eopt+eimp;
S1 = diag(1./span);
SG = S1*G;
msvrank = msv(SG,Juu); 

%Nullspace method:
nullspaceF = [F(3,1);F(5:6,1)];
Hnullspace = null(nullspaceF')'; 

lossplot2 = lossplot(:,11:21);

Hmatrix = zeros(2*size(H1,1),ny);
for i = 1:size(H1,1)
    Hmatrix(2*i-1,:) = H1(i,:);
    Hmatrix(2*i,:) = H2(i,:);
end

%Results of the exact local method
disp(res)

%Plots
x = -1:stepsize:1;
x2 = 0:stepsize:1;

set(0,'defaultaxesfontsize',14);
set(0,'defaulttextfontsize',14);	
set(0,'DefaultLineLineWidth',1.5);
set(0,'DefaultFigureColor','none');
legFontSize = 14;
scrsz = get(0,'ScreenSize');

figure('Position',[1 scrsz(4)/2 scrsz(3)/2 scrsz(4)/2])
plot(2:6,[lossvector(I2,1),lossvector(I3,1),lossvector(I4,1),...
    lossvector(I5,1),lossvector(1,1)],'k')
ylabel('Loss = J - Jopt, [hours]')
xlabel('No. of measurements')
set(gcf, 'PaperPositionMode', 'auto')  
print -djpeg lossmeas

figure('Position',[1 scrsz(4)/2 scrsz(3)/2 scrsz(4)/2])
plot(x2,lossplot2(1,:),'k-',x2,lossplot2(I5,:),'k:+',...
    x2,lossplot2(I4,:),'k:o',x2,lossplot2(I3,:),'k--d',...
    x2,lossplot2(I2,:),'k--',x2,lossplot2(43,:),'k-.')
h(1) = legend('R, T, P , x$_c$, W, N', 'R, T, P, W, N', 'R, T, W, N',...
    'R, T, N', 'P, W', 'W, N (Const. Inputs)','Location','NorthWest');
ylabel('Loss = J - Jopt, [hours]')
xlabel('Magnitude of |d| / |n|')
set(h,'Interpreter','latex','FontSize',legFontSize)
set(gcf, 'PaperPositionMode', 'auto')  
print -djpeg lossbest

figure('Position',[1 scrsz(4)/2 scrsz(3)/2 scrsz(4)/2])
plot(x2,lossplot2(45,:),'k:*',x2,lossplot2(46,:),'k-.',...
    x2,lossplot2(51,:),'k--',x2,lossplot2(54,:),'k:d',...
    x2,lossplot2(57,:),'k-')
axis([0 1 0 300])
h(1) = legend('x$_c$, W', 'P, N', 'T, x$_c$', 'R, W', 'R, T',...
    'Location','NorthWest');
ylabel('Loss = J - Jopt, [hours]')
xlabel('Magnitude of |d| / |n|')
set(h,'Interpreter','latex','FontSize',legFontSize)
set(gcf, 'PaperPositionMode', 'auto')   
print -djpeg lossbad