% Matlab Init                                              :noexport:ignore:

%% dcm_kinematics.m
% Computation of the DCM kinematics

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

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

%% Path for functions, data and scripts
addpath('./mat/'); % Path for data

%% Simscape Model - Nano Hexapod
addpath('./STEPS/')

%% Colors for the figures
colors = colororder;

%% Frequency Vector
freqs = logspace(1, 3, 1000);

% Bragg Angle

%% Tested bragg angles
bragg = linspace(5, 80, 1000); % Bragg angle [deg]
d_off = 10.5e-3; % Wanted offset between x-rays [m]

%% Vertical Jack motion as a function of Bragg angle
dz = d_off./(2*cos(bragg*pi/180));

%% Jack motion as a function of Bragg angle
figure;
plot(bragg, 1e3*dz)
xlabel('Bragg angle [deg]'); ylabel('Jack Motion [mm]');



% #+name: fig:jack_motion_bragg_angle
% #+caption: Jack motion as a function of Bragg angle
% #+RESULTS:
% [[file:figs/jack_motion_bragg_angle.png]]


%% Required Jack stroke
ans = 1e3*(dz(end) - dz(1))



% #+name: fig:schematic_sensor_jacobian_inverse_kinematics
% #+caption: Inverse Kinematics - Interferometers
% #+RESULTS:
% [[file:figs/schematic_sensor_jacobian_inverse_kinematics.png]]


% From the Figure [[fig:sensor_111_crystal_points]], the inverse kinematics can be solved as follow (for small motion):
% \begin{equation}
% \bm{J}_{s,111}
% =
% \begin{bmatrix}
% 1 &  0.07 & -0.015 \\
% 1 &  0    &  0.015 \\
% 1 & -0.07 & -0.015
% \end{bmatrix}
% \end{equation}


%% Sensor Jacobian matrix for 111 crystal
J_s_111 = [1,  0.07, -0.015
           1,  0,     0.015
           1, -0.07, -0.015];



% #+name: fig:schematic_sensor_jacobian_inverse_kinematics
% #+caption: Inverse Kinematics - Actuators
% #+RESULTS:
% [[file:figs/schematic_actuator_jacobian_inverse_kinematics.png]]

% Based on the geometry in Figure [[fig:actuator_jacobian_111_points]], we obtain:
% \begin{equation}
% \bm{J}_{a,111}
% =
% \begin{bmatrix}
% 1 &  0.14 & -0.1525 \\
% 1 &  0.14 &  0.0675 \\
% 1 & -0.14 & -0.0425
% \end{bmatrix}
% \end{equation}


%% Actuator Jacobian - 111 crystal
J_a_111 = [1,  0.14, -0.1525
           1,  0.14,  0.0675
           1, -0.14, -0.0425];

save('mat/dcm_kinematics.mat', 'J_a_111', 'J_s_111')