% Calculates feedback gains for cybership ii using a variety of methods
% and models.
%
% author: Thomas DeRensis

addpath('digcontr')
addpath('ele503')
load('sroots.mat');

ul = 0;
%ul = mean(U);
vl = 0;
rl = 0;
%rl = mean(r);
[A, B, C, D, E] = ...
    chooseModel(BoatID.CybershipII, ModelID.LinearizedDPThrusters, ul, vl, rl);

T = 0.1;                % Sampling interval (s)
sampleTime = T;
Ts = 5.0;                 % Time to settle (s)
nStates = length(A);    % num plant states
mStates = 3;            % num measured states

fprintf('Plant has %d states and %d measured states.\n', nStates, mStates);
fprintf('Using %.2f sec sampling interval and %.1f sec settling time.\n', T, Ts);

% Compute zero order hold equivalent model (similar to c2d)
[phi, gamma] = zohe(A, B, T);

fprintf('Checking Controllability... ')
Wc = ctrb(phi, gamma);
if (rank(Wc) == length(phi))
    fprintf('The system is controllable\n');
else
    fprintf('ERROR: Not controllable!\n');
end

fprintf('Checking Observability... ')
Wo = obsv(phi, C);
if (rank(Wo) == length(phi))
    fprintf('The system is observable\n');
else
    fprintf('ERROR: Not observable!\n');
end

[spoles, plantMultiplicity] = ...
    choosePlantPoles(A, Ts, PoleMethod.AddedDamping);

[phia, gammaa, spoles_ad, addMultiplicity] = ...
    chooseAdditionalDynamicsPoles(false, true, Ts, T);
    
spoles = [spoles_ad spoles]; % Combine desired closed-loop poles
%spoles = [ms1/Ts ms2/Ts ms4/Ts]%ms3/Ts];
zpoles = exp(T*spoles);
multiplicity = [addMultiplicity plantMultiplicity];

if T == 0
    [L1, L2, delta] = ...
        choosePlantGains(A, B, C, phia, gammaa, spoles, T, ...
            GainMethod.RFBG2, multiplicity);
else
    [L1, L2, delta] = ...
        choosePlantGains(phi, gamma, C, phia, gammaa, zpoles, T, ...
            GainMethod.Place, multiplicity);
end

%% Observer
%%%%%%%%%%%
useObserver = true;
K = 0;
if useObserver
    observerSpeed = 4.0;
    Tso = Ts/observerSpeed;
    fullOrder = true;
    opoles = chooseObserverPoles(A, B, C, Tso, ...
        PoleMethod.PlantZeros, fullOrder, mStates);

    if fullOrder
        zopoles = exp(T*opoles);
        if T == 0
            K = obg_ts(A, B, C, phia, gammaa, L1, L2, opoles, T);
            %K = place(A', C', opoles)';
            delta = robustTSOB(A, B, C, phia, gammaa, L1, L2, K, T);
        else
            %K = obg_ts(phi, gamma, C, phia, gammaa, L1, L2, zopoles, T);
            K = place(phi', C', zopoles)';
            delta = robustTSOB(phi, gamma, C, phia, gammaa, L1, L2, K, T);
        end
    else
        C1 = eye(mStates);
        Cm = [C1 zeros(mStates,n-mStates)];
        zopoles = exp(T*opoles);
        [F,G,H,K,P]=roo(phi,gamma,Cm,zopoles);
        L11 = L1(:,1:size(Cm));
        L12 = L1(:,size(Cm)+1:end);
        delta = robustTS_RDOB(phi, gamma, Cm, C, phia, gammaa, F, G, H, L11, L12, L2, K, T);    
    end
    fprintf('Observer robustness: %f\n', delta);
end

if useObserver
    if T == 0
        sys = designTSOB(A, B, C, phia, gammaa, L1, L2, K, T);
    else
        sys = designTSOB(phi, gamma, C, phia, gammaa, L1, L2, K, T);
    end
else
    if T == 0
        sys = designTS(A, B, C, phia, gammaa, L1, L2, T);
    else
        sys = designTS(phi, gamma, C, phia, gammaa, L1, L2, T);
    end
end
S = stepinfo(sys, 'SettlingTimeThreshold', 0.01);
Ts0 = S.SettlingTime;
fprintf('Final settling time: %f\n', Ts0);

if T ~= 0
    [A_f, B_f, C_f, D_f] = ...
        createInverseFeedthrough(phi, gamma, C, phia, gammaa, ...
            L1, L2, K, T, useObserver);
end

fprintf('Done designing control system.\n');

genVesselTrack
ic