clc 
clear all
close all
%% Choose the trajectory to interpolate (choose anyone)
aligned_traj = 0;
tilted_traj = 1;
pegboard_traj = 0;
%% Load the raw trajectory for interpolation
if aligned_traj
    traj_data = readtable('Trajectory_aligned_no_diagonals_2L_Sep.csv');
end
if tilted_traj
    traj_data = readtable('Trajectory_tilted_no_diagonals_2L_SepAzra.csv');
end
if pegboard_traj
    traj_data = readtable('Trajectory_raw_pegboard_new.csv');
end
%%
pos = table2array(traj_data);
pos = pos(1:20000,:);
figure; plot3(pos(:,1),pos(:,2),pos(:,3))
%%
pos = interparc_3d_U(100000,pos(:,1),pos(:,2),pos(:,3),'spline');  %trajectory
%%
figure; plot3(pos(:,1),pos(:,2),pos(:,3))
%%
if aligned_traj
    writematrix(pos,'Trajectory_interpolated_aligned_lattice_no_diag.csv')
end
if tilted_traj
    writematrix(pos,'Trajectory_interpolated_tilted_lattice_no_diagAzra.csv')
    % To observe the trajectory when lattice is tilted
    x_ang = 45;
    y_ang = 55;
    rot_x = [[1 0 0];
        [0 cos(x_ang*pi/180) -sin(x_ang*pi/180)];
        [0 sin(deg2rad(x_ang)) cos(deg2rad(x_ang))]];
    rot_y = [[cos(y_ang*pi/180) 0 sin(y_ang*pi/180)];
        [0 1  0];
        [-sin(deg2rad(y_ang)) 0 cos(deg2rad(y_ang))]];
    rotm = rot_x * rot_y;
    pos = rotm*pos';
    pos = pos';
end
if pegboard_traj
    writematrix(pos,'Trajectory_interpolated_pegboard_new.csv')
end
%%
figure; plot3(pos(:,1),pos(:,2),pos(:,3))