%% This script creates a CATIA Law file with the current points.

% Here you can specify the points to be reached:
% points = [time,      X, Y, Z, OMEGA, CHI, PHI; ...]
points   = [   2, 188e-3,     0,     0, 0,  0, 0;...
               4, 188e-3, 10e-3,     0, 0,  0, 0;...
               6, 188e-3, 10e-3, 10e-3, 0,  0, 0;...
               8, 188e-3,-10e-3, 10e-3, 0,  0, 0;...
              10, 188e-3,-10e-3,-10e-3, 0,  0, 0;...
              12, 188e-3, 10e-3,     0, 10e-3,  0, 0;...
              14, 188e-3,     0,     0, 0, 20, 0;...
              15, 188e-3,     0,     0, 0, 40, 0;...
              16, 188e-3,     0,     0, 0, 50, 0;...
              17, 188e-3,     0,     0, 0, 60, 0;...
              18, 188e-3,     0,     0, 0, 70, 0;...
              19, 188e-3,     0,     0, 0, 80, 0;...
              10, 188e-3,     0,     0, 0, 90, 0];
    
number_of_points = size(points,1)

%% Write CATIA Law file
fid = fopen('smargon_laws.txt','wt');
fprintf(fid, '//  Law for Befehl.1 bis Befehl.6\n');
fprintf(fid, '---------------------------------\n');
fprintf(fid, '*COLUMNS = *TIME, Befehl.1, Befehl.2, Befehl.3, Befehl.4, Befehl.5, Befehl.6\n');
fprintf(fid, '*INTERPOLATION = polyline,spline,polyline,polyline,polyline,polyline\n');
fprintf(fid, '*UNIT = Deg,m,m,m,m,Deg\n');
fprintf(fid, '*YSCALE = 1,1,1,1,1,1\n');
for i = 1:number_of_points
    t=points(i,1);
    [b1,b2,b3,b4,b5,b6] = calcIK_CATIA(points(i,2),points(i,3),points(i,4),points(i,5),points(i,6),points(i,7));
    fprintf(fid, '%d\t%f\t%f\t%f\t%f\t%f\t%f\n', t,b1,b2,b3,b4,b5,b6);
end;

fclose(fid);