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

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

colors = colororder;

addpath('./mat/');

%% Measured height for all the APA at the 8 locations
apa1  = 1e-6*[0, -0.5 , 3.5  , 3.5  , 42   , 45.5, 52.5 , 46];
apa2  = 1e-6*[0, -2.5 , -3   , 0    , -1.5 , 1   , -2   , -4];
apa3  = 1e-6*[0, -1.5 , 15   , 17.5 , 6.5  , 6.5 , 21   , 23];
apa4  = 1e-6*[0, 6.5  , 14.5 , 9    , 16   , 22  , 29.5 , 21];
apa5  = 1e-6*[0, -12.5, 16.5 , 28.5 , -43  , -52 , -22.5, -13.5];
apa6  = 1e-6*[0, -8   , -2   , 5    , -57.5, -62 , -55.5, -52.5];
apa7  = 1e-6*[0, 19.5 , -8   , -29.5, 75   , 97.5, 70   , 48];
apa7b = 1e-6*[0, 9    , -18.5, -30  , 31   , 46.5, 16.5 , 7.5];

apa = {apa1, apa2, apa3, apa4, apa5, apa6, apa7b};

%% X-Y positions of the measurements points
W = 20e-3;   % Width [m]
L = 61e-3;   % Length [m]
d = 1e-3;    % Distance from border [m]
l = 15.5e-3; % [m]

pos = [[-L/2 + d;      W/2 - d],
       [-L/2 + l - d;  W/2 - d],
       [-L/2 + l - d; -W/2 + d],
       [-L/2 + d;     -W/2 + d],
       [L/2 - l + d;   W/2 - d],
       [L/2 - d;       W/2 - d],
       [L/2 - d;      -W/2 + d],
       [L/2 - l + d;  -W/2 + d]];

%% Using fminsearch to find the best fitting plane
apa_d = zeros(1, 7);
for i = 1:7
    fun = @(x)max(abs(([pos; apa{i}]-[0;0;x(1)])'*([x(2:3);1]/norm([x(2:3);1]))));
    x0 = [0;0;0];
    [x, min_d] = fminsearch(fun,x0);
    apa_d(i) = min_d;
end