%%
clear; close all; clc;

%% 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;

%%
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'};

%% Generate the State Space Model
% 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));

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

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

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

%%
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]');

exportFig('spindle_f_to_d', 'normal-normal');

%% Save the model
save('./mat/spindle_model.mat', 'sys');