coords=...%T080078-05
    [-96, -20192.0, -615.8;...
    -92, -20524.7, -617.4;...
    -185, -20692.0, 665;...
    -185, -20772.3, -600.5;...
    -185, -20978.0, 215.2;...
    -185, -20032.0, 255.0; ...
    -185, -20031.3, 720.0];
coords2=...  %PRM, SM2, PMMT2, PMMT1, SM1, MC3+MC1/2 (in mm)
    [-96, -20192.0, -615.8;...
    -92, -20524.7, -617.4;...
    -185, -20692.0, 665;...
    -185, -20772.3, -600.5;...
    -185, -20978.0, 215.2;...
    -185, -20031.65, 255+(720-255.0)/2];

for i=2:6
    distancesBW(i-1)=sqrt((coords2(i, 1)-coords2(i-1, 1))^2+(coords2(i, 2)-coords2(i-1, 2))^2+(coords2(i, 3)-coords2(i-1, 3))^2);
end
distances=1e-3*fliplr(distancesBW);  %distance from optics before, starting at midpoint of MC1+MC3

M=cell(7,1); %make a cell array of ABCD matricesfor each element
 n=[1, 2, 4, 6, 7];  %numbers of free space propagations
for i=1:5
    M{n(i)}=[1, distances(i); 0, 1];
end

figure(1)
plot(coords2(:,2), coords2(:,3))
axis equal
ROC=[12.8, -6.8];  %ROC of PMMT1, PMMT2 in meters

m=[3,5];
for i=1:2
    M{m(i)}=[1, 0; -2/ROC(i), 1];  % not taking angles into account
end

ABCD=cell(6,1); %ABCD from MC waist to PRM, SM1 to PRM, 
for i=1:7
    m=[1,0;0,1];
    for f=i:7
        m=M{f}*m;
    end
    ABCD{i}=m;
end


%resulting ABCD is:  1 from MC waist to PRM
                     %2 from SM1 to PRM
                     %3 from PMMT1 to PRM
                      % 5 from PMMT2 to PRM
                      %7 from SM2 to PRM
