% =========================================================================
% AUTHOR       : Brian Ha  
% MENTOR       : Gene Massion
% DATE CREATED : 2018-07-11
% LAST REVISED : 2018-08-03
% MBARI GROUP  : Chemical Sensors Lab
% PROJECT      : Coastal Profiling Float (CPF) Depth Control
% =========================================================================

% Clean Up
close all
clear variables
clc


% User-Defined Simulation Parameters
% =========================================================================
referenceCommandSize = 1;    % [m]
outputDisturbanceTime = 250; % [sec]
outputDisturbanceSize = 1;   % [m]
tFinal = 500;                % [s]


% Float Parameters
% =========================================================================
floatLength = 1.016;        % [m] This is equal to 40 in.
floatDiameter = 0.2032;     % [m] This is equal to 8 in.
PlatformArea = (pi/4) * floatDiameter^2; % [m^2]. This the frontal area of a cylinder.

PlatformMass = 38;          % [kg]
VirtualMassFactor = 1.25;
m = PlatformMass * VirtualMassFactor;  % [kg]

pumpDisplacement = 1.56e-7; % [m^3/rev] Per the Oildyne datasheet
pumpEff = 0.6;

samplingFrequency = 3;      % [sec]


% Constraints
% =========================================================================
upperMotorSpeedLimit = 50;   % [revs/sec] This is equal to 3000 rpm.
lowerMotorSpeedLimit = -50;  % [revs/sec]


% Assumptions
% =========================================================================
densitySW = 1025;      % [kg/m^3] This is for seawater. Ignores compressibility.
g = 9.807;             % [m/s^2] Gravitational acceleration.
velFreeStream = 0.1;   % [m/s] This is equal to 10 cm/s.
kinematicViscosity = 13.60e-7; % [m^2/s]. Taken from http://web.mit.edu/seawater/2017_MIT_Seawater_Property_Tables_r2a.pdf


% Estimation of drag coefficient range
% =========================================================================
Re = velFreeStream*floatDiameter/kinematicViscosity; % Just checking Reynolds Number...

% Cd min is interpolated below from the values (based on frontal area) given for a
% flat-faced cylinder on Table 7.3 (page 491) of Fluid Mechanics, 7th Ed.
% by Frank M. White. Copyright 2011. Available at
% https://hellcareers.files.wordpress.com/2016/01/fluid-mechanics-seventh-edition-by-frank-m-white.pdf.

CdMin = (0.99 - 0.87)/(8 - 4) * (floatLength/floatDiameter - 4) + 0.87;

% Cd max was assumed from Laughlin Barker's paper (2014 MBARI intern).
CdMax = 1.5;

Cd = CdMin; % CdMax or CdMin


% Plant
% =========================================================================

% Equilibrium Point
x_eq = [0;0;0];
u_eq = 0;

% % Symbolic Linearization about Equilibrium (via Taylor Series Expansion)
% [Aeq, Beq] = symLin(x_eq, u_eq, densitySW, Cd, PlatformArea, g, m, pumpDisplacement, pumpEff);
 C = [1 0 0];
 D = 0;

%P = ss(Aeq, Beq, C, D);


%Plant Linearized at Equilibrium Point (hand calculation, for verification)
A = [0 1 0; 0 -densitySW*Cd*PlatformArea*x_eq(2)/m densitySW*g/m; 0 0 0];
B = [0;0;pumpDisplacement*pumpEff];

P = ss(A,B,C,D);


% Kalman Controllability Test
% =========================================================================
disp('Kalman Controllability Test')
disp(rank(ctrb(P)))


% Kalman Observability Test
% =========================================================================
disp('Kalman Observability Test')
disp(rank(obsv(P)))


% Compensators
% =========================================================================
s = tf('s');

Dual_Lead_1 = 45*(s+0.015)*(s+0.001)/((s+0.1)*(s+0.15));   % Continuous Time Winner
Dual_Lead_2 = 45*(s+0.015)*(s+0.001)/((s+0.17)*(s+0.15));  % Discrete Time Winner
%Dual_Lead_2_plus_Lag = Dual_Lead_2 * (s + 0.0001)/(s + 0.000001); % ~2000 sec settling time
Dual_Lead_2_plus_PI = Dual_Lead_2 * (s + 0.008)/s; % Better for steps > 1 m

C = Dual_Lead_2;  % Dual_Lead_1, Dual_Lead_2, or Dual_Lead_plus_PI


% Important Transfer Functions
% =========================================================================

L = tf(P)*C;            % Open-Loop Transfer Function
S = minreal(1/(1 + L)); % Command and Noise Sensitivity Function
T = minreal(1 - S);     % Plant Output Disturbance Sensitivity Function 
CS = minreal(C*S);      % Control Signal Sensitivity Function
SP = minreal(S*P);      % Plant Input Disturbance Sensivity Function

%
% =========================================================================
% LINEAR TIME-INVARIANT (LTI) Continuous Time Analysis
% =========================================================================
figure(1)

opt = stepDataOptions('StepAmplitude', referenceCommandSize');

subplot(2,2,1)
step(T, opt)
[y, t] = step(T, opt);
hold on
plot([100:5:t(end)], 1.05*referenceCommandSize*ones(1, length([100:5:t(end)])), 'r--')
plot([100:5:t(end)], 0.95*referenceCommandSize*ones(1, length([100:5:t(end)])), 'r--')
grid on
title('Command and Noise Response')
set(findall(gcf, 'type', 'line'), 'linewidth', 2)

subplot(2,2,2)
step(S, opt)
grid on
title('Output Disturbance Rejection')
set(findall(gcf, 'type', 'line'), 'linewidth', 2)


subplot(2,2,3)
step(CS, opt)
[y, t] = step(C*S);
hold on
plot(t, upperMotorSpeedLimit*ones(1, length(t)), 'r--')
plot(t, lowerMotorSpeedLimit*ones(1, length(t)), 'r--')
grid on
title('Control Signal Response')
set(findall(gcf, 'type', 'line'), 'linewidth', 2)


subplot(2,2,4)
step(SP, opt)
grid on
title('Input Disturbance Rejection')
set(findall(gcf, 'type', 'line'), 'linewidth', 2)


figure(2)
options = bodeoptions('cstprefs');
options.FreqUnits = 'Hz';
bode(L, options)
grid on
title('Open Loop TF Bode Plot')
set(findall(gcf, 'type', 'line'), 'linewidth', 2)

allmargin(L)

figure(3)
nyquist(L)

% Draw a black unit circle
circle = -1+ (-s+1)/(s+1);
omegac = logspace(-3,3,400);
[reC,imC] = nyquist(circle,omegac);
reC = squeeze(reC);
imC = squeeze(imC);
hold on
plot(reC,imC,'k-',reC,-imC,'k-')

axis([-3 3 -3 3])
axis('equal')
set(findall(gcf, 'type', 'line'), 'linewidth', 2)
title('Nyquist Plot')


figure(4)
options = bodeoptions('cstprefs');
options.FreqUnits = 'Hz';
options.MagUnits = 'abs';
bode(S, options)
hold on
bode(T, options)
grid on
title('Sensitivity and Comp. Sensitivity Functions')
legend('S', 'T')
set(findall(gcf, 'type', 'line'), 'linewidth', 2)


figure(5)
options = bodeoptions('cstprefs');
options.FreqUnits = 'Hz';
bode(T, options)
grid on
title('Closed Loop TF Bode Plot')
set(findall(gcf, 'type', 'line'), 'linewidth', 2)
%

% =========================================================================
% NON-LINEAR SIMULATION & ANALYSIS
% =========================================================================
%{

% Continuous vs. Discrete Time Comparison
% =========================================================================
C = Dual_Lead_1;
[time, ~, ref, depth, drag, ctrl] = sim('CPF_Model_my_plant');

C_discrete = c2d(Dual_Lead_1, samplingFrequency, 'matched');
[time_d, ~, ref_d, depth_d, drag_d, ctrl_d] = sim('CPF_Model_my_plant_with_discrete_controller');


figure(6)
subplot(3,1,1)
plot(time, depth);
hold on
plot(time_d, depth_d)
plot(time, ref, 'k--');
set(findall(gcf, 'type', 'line'), 'linewidth', 2)
legend('Continuous', 'Discrete')
grid on
ylabel('Depth [m]')
title('Effects of Discretization on Step Response with Dual Lead Controller #1')

subplot(3,1,2)
plot(time, drag);
hold on
plot(time_d, drag_d)
set(findall(gcf, 'type', 'line'), 'linewidth', 2)
legend('Continuous', 'Discrete')
grid on
ylabel('Drag Force [N]')

subplot(3,1,3)
plot(time, ctrl);
hold on
plot(time_d, ctrl_d)
set(findall(gcf, 'type', 'line'), 'linewidth', 2)
legend('Continuous', 'Discrete')
grid on
xlabel('Time [sec]')
ylabel('Motor Speed [revs/sec]')


% Dual Lead 1 vs. Dual Lead 2
% =========================================================================
C_discrete = c2d(Dual_Lead_2, samplingFrequency, 'matched');
[time_d2, ~, ~, depth_d2, drag_d2, ctrl_d2] = sim('CPF_Model_my_plant_with_discrete_controller');


figure(7)
subplot(3,1,1)
plot(time_d, depth_d)
hold on
plot(time_d2, depth_d2)
plot(time, ref, 'k--');
set(findall(gcf, 'type', 'line'), 'linewidth', 2)
legend('Dual Lead 1', 'Dual Lead 2')
grid on
ylabel('Depth [m]')
title('Dual Lead Controller #1 vs. Dual Lead Controller #2')

subplot(3,1,2)
plot(time_d, drag_d)
hold on
plot(time_d2, drag_d2)
set(findall(gcf, 'type', 'line'), 'linewidth', 2)
legend('Dual Lead 1', 'Dual Lead 2')
grid on
ylabel('Drag Force [N]')

subplot(3,1,3)
plot(time_d, ctrl_d)
hold on
plot(time_d2, ctrl_d2)
set(findall(gcf, 'type', 'line'), 'linewidth', 2)
legend('Dual Lead 1', 'Dual Lead 2')
grid on
xlabel('Time [sec]')
ylabel('Motor Speed [revs/sec]')


% Dual Lead 2 vs. Dual Lead 2 + PI for Step Commands > 1 m
% =========================================================================
referenceCommandSize = 5;    % [m]
outputDisturbanceSize = 0;   % [m]
tFinal = 5000;

C_discrete = c2d(Dual_Lead_2, samplingFrequency, 'matched');
[time_d2, ~, ref_d2, depth_d2, drag_d2, ctrl_d2] = sim('CPF_Model_my_plant_with_discrete_controller');

C_discrete = c2d(Dual_Lead_2_plus_PI, samplingFrequency, 'matched');
[time_d_lag, ~, ~, depth_d_lag, drag_d_lag, ctrl_d_lag] = sim('CPF_Model_my_plant_with_discrete_controller');


figure(8)
subplot(3,1,1)
plot(time_d2, depth_d2)
hold on
plot(time_d_lag, depth_d_lag)
plot(time_d2, ref_d2, 'k--');
set(findall(gcf, 'type', 'line'), 'linewidth', 2)
legend('Dual Lead 2', 'Dual Lead 2 + PI')
grid on
ylabel('Depth [m]')
title('Dual Lead + PI Compensation')

subplot(3,1,2)
plot(time_d2, drag_d2)
hold on
plot(time_d_lag, drag_d_lag)
set(findall(gcf, 'type', 'line'), 'linewidth', 2)
legend('Dual Lead 2', 'Dual Lead 2 + PI')
grid on
ylabel('Drag Force [N]')

subplot(3,1,3)
plot(time_d2, ctrl_d2)
hold on
plot(time_d_lag, ctrl_d_lag)
set(findall(gcf, 'type', 'line'), 'linewidth', 2)
legend('Dual Lead 2', 'Dual Lead 2 + PI')
grid on
xlabel('Time [sec]')
ylabel('Motor Speed [revs/sec]')
%}


% Dual Lead 2 vs. Dual Lead 2 with Error Governor for Step Commands > 1 m
% % =========================================================================
% referenceCommandSize = -1;      % [m]
% outputDisturbanceSize = 0;      % [m]
% tFinal = 500;                  % [s]
% errorGovernorUpperLimit = 1.7;  % [m]
% errorGovernorLowerLimit = -1.7; % [m]
% 
% C_discrete = c2d(Dual_Lead_2, samplingFrequency, 'matched');
% [time_d2, ~, ref_d2, depth_d2, drag_d2, ctrl_d2] = sim('CPF_Model_my_plant_with_discrete_controller');
% [time_d_gov, ~, ~, depth_d_gov, drag_d_gov, ctrl_d_gov] = sim('CPF_Model_my_plant_with_discrete_controller_and_error_governor');
% 
% 
% figure(9)
% subplot(3,1,1)
% plot(time_d2, depth_d2)
% hold on
% plot(time_d_gov, depth_d_gov)
% plot(time_d2, ref_d2, 'k--');
% set(findall(gcf, 'type', 'line'), 'linewidth', 2)
% legend('Dual Lead 2', 'Dual Lead 2 + Error Governor', 'location', 'southeast')
% grid on
% ylabel('Depth [m]')
% title('Dual Lead 2 + Error Governor')
% 
% subplot(3,1,2)
% plot(time_d2, drag_d2)
% hold on
% plot(time_d_gov, drag_d_gov)
% set(findall(gcf, 'type', 'line'), 'linewidth', 2)
% legend('Dual Lead 2', 'Dual Lead 2 + Error Governor', 'location', 'southeast')
% grid on
% ylabel('Drag Force [N]')
% 
% subplot(3,1,3)
% plot(time_d2, ctrl_d2)
% hold on
% plot(time_d_gov, ctrl_d_gov)
% set(findall(gcf, 'type', 'line'), 'linewidth', 2)
% legend('Dual Lead 2', 'Dual Lead 2 + Error Governor', 'location', 'southeast')
% grid on
% xlabel('Time [sec]')
% ylabel('Motor Speed [revs/sec]')


%% Functions
function [A, B] = symLin(x_eq, u_eq, densitySW, Cd, area, grav, mass, pumpDisplacement, pumpEff)
% Calculates the Jacobian A and B matrices about x_eq and u_eq

% Part I of II: Symbolic Calculation of General Jacobian

% Symbolic State Vector, x
position = sym('position');
velocity = sym('velocity');
delta_volume = sym('delta_volume');

x = [position; velocity; delta_volume];

% Symbolic Input Vector, u
motor_speed = sym('motor_speed'); % [rev/s]

u = [motor_speed];

% Non-Linear ODEs
x_dot(1,1) = velocity;
x_dot(2,1) = (-0.5*densitySW*area*Cd*velocity^2 + densitySW*grav*delta_volume)/mass;
x_dot(3,1) = motor_speed*pumpDisplacement*pumpEff;

A = jacobian(x_dot, x);
B = jacobian(x_dot, u);

% Part II of II: Calculation of Jacobian at x_eq and u_eq
position = x_eq(1);
velocity = x_eq(2);
delta_volume = x_eq(3);

motor_speed = u_eq;

A = eval(subs(A));
B = eval(subs(B));

end