%**************************************************************************
%epu_anal_traj
%This function performs an alalysis of the beam trajectory.
%
%Input:
%param, parameter structure
%dat_file, output data file name
%plt_file, output plot file name
%field, field structure
%
%Output:
%traj, structure containing trajectory information
%   z                   %Translated Z, with z=0 at undulator center (m)
%   x                   %Trajectory x position (m)
%   y                   %Trajectory y position (m)
%   x_filt              %Filtered trajectory x position (m)
%   y_filt              %Filtered trajectory y position (m)
%   x_p                 %First derivative dx/dz
%   y_p                 %First derivative dy/dz
%   x_p_filt            %Filtered first derivative dx/dz
%   y_p_filt            %Filtered first derivative dy/dz
%   x_pp                %Second derivative d^2x/dz^2 (1/m)
%   y_pp                %Second derivative d^2y/dz^2 (1/m)
%   vx                  %Velocity in the x direction (m/s)
%   vy                  %Velocity in the y direction (m/s)
%   vz                  %Velocity in the z direction (m/s)
%   x_slope             %X trajectory linear fit slope
%   y_slope             %Y trajectory linear fit slope
%   x_slope_cor         %X trajectory minus linear fit (m)
%   y_slope_cor         %Y trajectory minus linear fit (m)
%   x_p_slope_cor       %X trajectory slope minus linear fit slope
%   y_p_slope_cor       %Y trajectory slope minus linear fit slope
%
%Zachary Wolf
%5/18/10

function [traj] = epu_anal_traj(param, dat_file, plt_file, field)

%Extract required information
gamma = param.gamma;
z = field.z;
bx = field.bx;
by = field.by;
bz = field.bz;
lambda_u = field.lambda_u;
z_peak = field.z_peak;
int_bx_dz = field.int_bx_dz;
int_int_bx_dz_dz = field.int_int_bx_dz_dz;
int_by_dz = field.int_by_dz;
int_int_by_dz_dz = field.int_int_by_dz_dz;

%Calculate the velocities
%Use first order, see LCLS-TN-15-3
[vx, vy, vz] = traj_calc_vel(gamma, int_bx_dz, int_by_dz);

%Calculate the trajectories
%Use first order, see LCLS-TN-15-3
[x, x_p, x_pp] = traj_calc_x_traj(gamma, by, int_by_dz, int_int_by_dz_dz);
[y, y_p, y_pp] = traj_calc_y_traj(gamma, bx, int_bx_dz, int_int_bx_dz_dz);

%Apply a lambda filter to x and y, x_p and y_p
x_filt = filt_apply_lambda_filt(lambda_u, z, x);
y_filt = filt_apply_lambda_filt(lambda_u, z, y);
x_p_filt = filt_apply_lambda_filt(lambda_u, z, x_p);
y_p_filt = filt_apply_lambda_filt(lambda_u, z, y_p);

%Plot the trajectories
%traj_plot_x_vs_z(plt_file, z, x);
%traj_plot_x_vs_z_filt(plt_file, z, x, x_filt);
traj_plot_x_vs_z_filt_pole(plt_file, z, x, x_filt, z_peak);
%traj_plot_y_vs_z(plt_file, z, y);
%traj_plot_y_vs_z_filt(plt_file, z, y, y_filt);
traj_plot_y_vs_z_filt_pole(plt_file, z, y, y_filt, z_peak);
traj_plot_xy_vs_z(plt_file, z, x, y);
traj_plot_y_vs_x(plt_file, x, y);

%Write the trajectory exit information to the data file
traj_dat_traj_exit(dat_file, x(end), x_p(end), y(end), y_p(end));

%Plot the velocities
% traj_plot_vx_vs_z(plt_file, z, vx);
% traj_plot_vy_vs_z(plt_file, z, vy);
% traj_plot_vz_vs_z(plt_file, z, vz);

%Slope corrected trajectories -----

%Fit the filtered trajectory in the center of the undulator
z_start = field.z_peak(1 + param.num_end_poles);
z_end = field.z_peak(end - param.num_end_poles);
[x_slope, x_intercept, x_resid_filt] = traj_linfit_traj(z_start, z_end, z, x_filt);
[y_slope, y_intercept, y_resid_filt] = traj_linfit_traj(z_start, z_end, z, y_filt);

%Find the residuals of the un-filtered trajectory to the linear fit
x_resid = x - (x_slope * z + x_intercept);
y_resid = y - (y_slope * z + y_intercept);

%Make the residuals, the slope corrected trajectory, start at zero
x_slope_cor = x_resid - x_resid(1);
y_slope_cor = y_resid - y_resid(1);

%Plot the slope corrected trajectories
traj_plot_x_vs_z_slope_corr(plt_file, z, x_slope_cor);
traj_plot_y_vs_z_slope_corr(plt_file, z, y_slope_cor);

%Calculate the corrected trajectory slopes
x_p_slope_cor = x_p - x_slope;
y_p_slope_cor = y_p - y_slope;

%Save the results
traj.z = z;
traj.x = x;
traj.y = y;
traj.x_filt = x_filt;
traj.y_filt = y_filt;
traj.x_p = x_p;
traj.y_p = y_p;
traj.x_p_filt = x_p_filt;
traj.y_p_filt = y_p_filt;
traj.x_pp = x_pp;
traj.y_pp = y_pp;
traj.vx = vx;
traj.vy = vy;
traj.vz = vz;
traj.x_slope = x_slope;
traj.y_slope = y_slope;
traj.x_slope_cor = x_slope_cor;
traj.y_slope_cor = y_slope_cor;
traj.x_p_slope_cor = x_p_slope_cor;
traj.y_p_slope_cor = y_p_slope_cor;

%Done
