%% Clear Workspace and Close figures
clear; close all; clc;

%% Intialize Laplace variable
s = zpk('s');

%% Add the getAsynchronousError to path
addpath('./src/');

% Parameters

mg = 3000; % Mass of granite [kg]
ms = 50;   % Mass of Spindle [kg]

kg = 1e8; % Stiffness of granite [N/m]
ks = 5e7; % Stiffness of spindle [N/m]

% Compute Mass and Stiffness Matrices

Mm = diag([ms, mg]);
Km = diag([ks, ks+kg]) - diag(ks, -1) - diag(ks, 1);

% Compute resonance frequencies

A = [zeros(size(Mm)) eye(size(Mm)) ; -Mm\Km zeros(size(Mm))];
eigA = imag(eigs(A))/2/pi;
eigA = eigA(eigA>0);
eigA = eigA(1:2);

% From model_damping compute the Damping Matrix

modal_damping = 1e-5;

ab = [0.5*eigA(1) 0.5/eigA(1) ; 0.5*eigA(2) 0.5/eigA(2)]\[modal_damping ; modal_damping];

Cm = ab(1)*Mm +ab(2)*Km;

% Define inputs, outputs and state names

StateName = {...
    'xs', ... % Displacement of Spindle [m]
    'xg', ... % Displacement of Granite [m]
    'vs', ... % Velocity of Spindle [m]
    'vg', ... % Velocity of Granite [m]
            };
StateUnit = {'m', 'm', 'm/s', 'm/s'};

InputName = {...
    'f' ... % Spindle Force [N]
            };
InputUnit = {'N'};

OutputName = {...
    'd' ... % Displacement [m]
             };
OutputUnit = {'m'};

% Define A, B and C matrices

% A Matrix
A = [zeros(size(Mm)) eye(size(Mm)) ; ...
     -Mm\Km          -Mm\Cm];

% B Matrix
B_low = zeros(length(StateName), length(InputName));
B_low(strcmp(StateName,'vs'), strcmp(InputName,'f')) =  1;
B_low(strcmp(StateName,'vg'), strcmp(InputName,'f')) = -1;
B = blkdiag(zeros(length(StateName)/2), pinv(Mm))*B_low;

% C Matrix
C = zeros(length(OutputName), length(StateName));
C(strcmp(OutputName,'d'), strcmp(StateName,'xs')) =  1;
C(strcmp(OutputName,'d'), strcmp(StateName,'xg')) = -1;

% D Matrix
D = zeros(length(OutputName), length(InputName));

% Generate the State Space Model

sys = ss(A, B, C, D);
sys.StateName = StateName;
sys.StateUnit = StateUnit;

sys.InputName = InputName;
sys.InputUnit = InputUnit;

sys.OutputName = OutputName;
sys.OutputUnit = OutputUnit;

% Bode Plot
% The transfer function from a disturbance force $f$ to the measured displacement $d$ is shown figure [[fig:spindle_f_to_d]].


freqs = logspace(-1, 3, 1000);

figure;
plot(freqs, abs(squeeze(freqresp(sys('d', 'f'), freqs, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');

% Save the model

save('./mat/spindle_model.mat', 'sys');