clear all
close all
warning off all

% name of file
% fname = 'Marathon_AM1_10cols.xlsx';
% fname = 'Marathon_AM5_10cols.xlsx';
% fname = 'Marathon_AM7_10cols.xlsx';
fname = 'Marathon_AM9_10cols.xlsx';

% Change: Tolerances increased to 0.6 to find datum
% Change: Tolerances increased to 0.6 to find datum


if strcmp(fname,'Marathon_AM1_10cols.xlsx')==1
    sname = 'AM1'; % sheet name
    dat = 10; % number of data sets
    g = 10;
    xus = 2;
    yds = 2; 
    col = [35561 36386 35848 36253 35876 35684 35831 35533 35736 35516]; % end of columns
    tol = [0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5];%tolerance
elseif strcmp(fname,'Marathon_AM5_10cols.xlsx')==1
    sname = 'AM5'; % sheet name
    dat = 9; % number of data sets
    g = 10;
    xus = 2;
    yds = 2; 
    col = [35696 36314 35938 35692 35947 36132 35837 35901 35819]; % end of columns
    tol = [0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5];%tolerance
elseif strcmp(fname,'Marathon_AM7_10cols.xlsx')==1
    sname = 'AM7'; % sheet name
    dat = 10; % number of data sets
    g = 10;
    xus = 2;
    yds = 2; 
    col = [36491 35970 36217 35339 36100 35286 35421 35880 35535 35079]; % end of columns
    tol = [0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5];%tolerance
elseif strcmp(fname,'Marathon_AM9_10cols.xlsx')==1
    sname = 'AM9'; % sheet name
    dat = 10; % number of data sets
    g = 10;
    xus = 2;
    yds = 2; 
    col = [35646 35538 35957 35675 36020 36082 35490 35703 35303 35670]; % end of columns
    tol = [0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5];%tolerance
end

% for all data sets
figure
hold on
grid on
xlabel('mm')
ylabel('mm')
p = 10;
dsr = 100;
R = cell(1,dat);
S = cell(1,dat);
P = zeros(dat,2);
K = zeros(1,dat);
c = [114 107 107 107 107 109 109 109 109 109]; % red black black black black blue cyan green magenta yellow blue.....(again!)

for j = 1:dat
    
    % read data
    if j < 8
        c1 = char(65+4*(j-1));
        c2 = char(66+4*(j-1));
    else
        c1 = [char(65),char(67+4*(j-8))];
        c2 = [char(65),char(68+4*(j-8))];
    end
    rname = [c1,'3:',c2,num2str(col(j))];
    r = xlsread(fname,sname,rname);
    R{j} = [flipud(r(:,1)) flipud(r(:,2))]; % coordinates
    
    % down sample
    n = floor((col(j)-2)/dsr);
    for i = 1:n
        S{j}(i,:) = R{j}(i*dsr,:);
    end
    
    % find datum
    i = 0;
    while 1 % iterator to go through all values starting from rhs
        k = p+i;
        up = S{j}(k+1:k+p-1,2); % p-1 values up of index
        dn = S{j}(k-p+1:k-1,2); % p-1 values down of index
        dup = up-S{j}(k,2);
        ddn = S{j}(k,2)-dn;
        if mean(dup)<tol(j)*mean(ddn) % all values up of index are less than those down of index
            K(j) = k;
            P(j,:) = S{j}(K(j),:); % extract datum
            break
        else
            i = i+1; % add one to index
        end
    end
    
    % plot raw data
    plot(R{j}(:,1),R{j}(:,2)/1e3,[char(c(j)),'-'])
    plot(P(j,1),P(j,2)/1e3,'o')
    
end
daspect([1 1 1])
axis([0 10 -6 4])

% for all sets but the first
R_ = cell(1,dat);
S_ = cell(1,dat);
dR = zeros(dat,2);
figure
hold on
grid on
xlabel('mm')
ylabel('mm')
R_{1} = R{1};
S_{1} = S{1};
plot(R_{1}(:,1),R_{1}(:,2)/1e3,[char(c(1)),'-'])
plot(P(1,1),P(1,2)/1e3,'o')
for j = 2:dat
    dR(j,:) = P(j,:)-P(1,:); % datum displacement
    R_{j}(:,1) = R{j}(:,1)-dR(j,1); % matched coordinates to first set
    R_{j}(:,2) = R{j}(:,2)-dR(j,2);
    S_{j}(:,1) = S{j}(:,1)-dR(j,1); % matched coordinates to first set down sampled
    S_{j}(:,2) = S{j}(:,2)-dR(j,2);
    plot(R_{j}(:,1),R_{j}(:,2)/1e3,[char(c(j)),'-'])
end
daspect([1 1 1])
axis([0 10 -6 4])

% match gradients (down sampled)
g_ = g*ones(1,dat); % to this number of points after the datum

T = cell(1,dat);
Q = zeros(dat,2);
gr = zeros(1,dat);
for j = 1:dat
    T{j}(:,1) = S_{j}(:,1)-P(1,1);
    T{j}(:,2) = S_{j}(:,2)-P(1,2);
    Q(j,:) = T{j}(K(j)-g_(j),:);
    gr(j) = Q(j,2)/Q(j,1);
end

th = zeros(1,dat);
Q_ = zeros(dat,2);
M = cell(1,dat);
T_ = cell(1,dat);
T_{1} = T{1};
Q_(1,:) = Q(1,:);
figure
hold on
grid on
xlabel('mm')
ylabel('mm')
plot(T_{1}(:,1),T_{1}(:,2)/1e3,[char(c(1)),'-'])
plot(0,0,[char(c(1)),'o'])
plot(Q_(1,1),Q_(1,2)/1e3,[char(c(1)),'o'])
for j = 2:dat
    s = 1*(gr(j)<gr(1))-1*(gr(j)>gr(1));
    th(j) = s*acos(dot(Q(1,:),Q(j,:))/(norm(Q(1,:))*norm(Q(j,:))));
    M{j} = [cos(th(j)),-sin(th(j));sin(th(j)),cos(th(j))];
    for i = 1:size(T{j},1)
        T_{j}(i,:) = (M{j}*T{j}(i,:)')';
    end
    Q_(j,:) = (M{j}*Q(j,:)')';
    plot(T_{j}(:,1),T_{j}(:,2)/1e3,[char(c(j)),'-'])
    plot(Q_(j,1),Q_(j,2)/1e3,[char(c(j)),'o'])
end
daspect([1 1 1])
axis([-8 2 -4 4])

figure
hold on
grid on
xlabel('mm')
ylabel('mm')

for j = 1:dat
    Q_(j,2) = Q_(j,2)/1e3;
    T_{j}(:,2) = T_{j}(:,2)/1e3;
end

th_ = -atan(Q_(1,2)/Q_(1,1));
M_ = [cos(th_),-sin(th_);sin(th_),cos(th_)];
W_ = cell(1,dat);
X_ = zeros(dat,2);
plot(0,0,[char(c(1)),'o'])
for j = 1:dat
    for i = 1:size(T_{j},1)
        W_{j}(i,:) = (M_*T_{j}(i,:)')';
    end
    X_(j,:) = (M_*Q_(j,:)')';
    plot(W_{j}(:,1),W_{j}(:,2),[char(c(j)),'-'])
    plot(X_(j,1),X_(j,2),[char(c(j)),'o'])
end
daspect([1 1 1])

% new

for j = 1:dat
    W_{j} = movmean(W_{j},5);
end

f = find(W_{1}(:,1)<=-xus&W_{1}(:,2)>=-yds);
W10{1} = W_{1}(f,:);

% figure
% plot(W10{1}(:,1),W10{1}(:,2),'-k')

nw = length(f);
th0 = -pi/2;
Rm = [cos(th0),-sin(th0);sin(th0),cos(th0)];

for i = 1:nw-1
   
    T1(i,:) = W10{1}(i+1,:)-W10{1}(i,:);
    N1(i,:) = T1(i,:)*Rm;
    N1(i,:) = N1(i,:)./norm(N1(i,:));
    W1{1}(i,1) = (W10{1}(i+1,1)+W10{1}(i,1))/2;
    W1{1}(i,2) = (W10{1}(i+1,2)+W10{1}(i,2))/2;
    
end

N = zeros(nw-1,2);
W0 = zeros(nw-1,2);
sfc = 0.2; % scale factor for normal line projection 
nn = 5000; % number of points on projection 
V = cell(1,nw-1);

figure
hold on
grid on

for i = 1:nw-1
    W0(i,:) = W1{1}(i,:);
    pu = W0(i,:)+sfc*N1(i,:);
    pd = W0(i,:)-sfc*N1(i,:);
    V{i}(:,1) = linspace(pu(1),pd(1),nn);
    V{i}(:,2) = linspace(pu(2),pd(2),nn);
    plot(V{i}(:,1),V{i}(:,2))
end
plot(W0(:,1),W0(:,2),[char(c(1)),'-'])
daspect([1 1 1])

figure
hold on
grid on
xlabel('mm')
ylabel('mm')
plot(W_{1}(:,1),W_{1}(:,2),[char(c(1)),'-'])
S = (W0(:,1).^2+W0(:,2).^2).^0.5;

for j = 2:dat
    
    f = find(W_{j}(:,1)<=-xus&W_{j}(:,2)>=-yds);
    W1{j} = W_{j}(f,:);
     
    p1 = zeros(1,nw-1);
    for i = 1:nw-1
        yt = interp1(W1{j}(:,1),W1{j}(:,2),V{i}(:,1),'linear','extrap');
        [dyt,p1(i)] = min(abs(yt-V{i}(:,2)));
        D{j}(i) = ((W0(i,1)-V{i}(p1(i),1))^2+(W0(i,2)-V{i}(p1(i),2))^2)^0.5;
    end
    
    [Dm(j),p2(j)] = max(D{j});
    W2{j} = V{p2(j)}(p1(p2(j)),:);
    W0a = W0(p2(j),:);
    Dm1(j) = (W2{j}(1)<W0a(1))*Dm(j)-(W2{j}(1)>W0a(1))*Dm(j);
    
    A{j} = cumtrapz(S,D{j});
    
    plot(W_{j}(:,1),W_{j}(:,2),[char(c(j)),'-'])
    plot([W0a(1,1),W2{j}(1,1)],[W0a(1,2),W2{j}(1,2)],[char(c(j)),'o-'])
end
daspect([1 1 1])

figure
hold on
grid on
for j = 2:dat
    plot(S,D{j},[char(c(j)),'-'])
    plot(S(p2(j)),Dm(j),[char(c(j)),'o'])
end

figure
hold on
grid on
for j = 2:dat
    plot(S,A{j},[char(c(j)),'-'])
end

% end new

figure
qqq = 5;
plot(W_{qqq}(:,1),W_{qqq}(:,2),'b-')
xlabel('mm')
ylabel('mm')
grid on
daspect([1 1 1])