%**************************************************************************
%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)
%   x_p                 %First derivative dx/dz
%   x_pp                %Second derivative d(dx/dz)/dz (m^-1)
%   y                   %Trajectory y position (m)
%   y_p                 %First derivative dy/dz
%   y_pp                %Second derivative d(dy/dz)/dz (m^-1)
%   x_filt              %Filtered trajectory x position (m)
%   x_p_filt            %Filtered first derivative dx/dz
%   x_pp_filt           %Filtered second derivative d(dx/dz)/dz (m^-1)
%   y_filt              %Filtered trajectory y position (m)
%   y_p_filt            %Filtered first derivative dy/dz
%   y_pp_filt           %Filtered second derivative d(dy/dz)/dz (m^-1)
%   x_slope_cor         %X trajectory minus linear fit
%   x_p_slope_cor       %X trajectory slope minus linear fit slope
%   y_slope_cor         %Y trajectory minus linear fit
%   y_p_slope_cor       %Y trajectory slope minus linear fit slope
%
%Zachary Wolf
%12/23/05

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

%Extract required information
gamma = param.gamma;
z = field.z;
bx = field.bx;
int_bx_dz = field.int_bx_dz;
int_int_bx_dz_dz = field.int_int_bx_dz_dz;
by = field.by;
int_by_dz = field.int_by_dz;
int_int_by_dz_dz = field.int_int_by_dz_dz;
bx_filt = field.bx_filt;
int_bx_dz_filt = field.int_bx_dz_filt;
int_int_bx_dz_dz_filt = field.int_int_bx_dz_dz_filt;
by_filt = field.by_filt;
int_by_dz_filt = field.int_by_dz_filt;
int_int_by_dz_dz_filt = field.int_int_by_dz_dz_filt;

%Calculate trajectories
[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);

%Calculate filtered trajectories
[x_filt, x_p_filt, x_pp_filt] = traj_calc_x_traj(gamma, by_filt, int_by_dz_filt, int_int_by_dz_dz_filt);
[y_filt, y_p_filt, y_pp_filt] = traj_calc_y_traj(gamma, bx_filt, int_bx_dz_filt, int_int_bx_dz_dz_filt);

%Save the trajectories
traj.z = z;
traj.x = x;
traj.x_p = x_p;
traj.x_pp = x_pp;
traj.y = y;
traj.y_p = y_p;
traj.y_pp = y_pp;
traj.x_filt = x_filt;
traj.x_p_filt = x_p_filt;
traj.x_pp_filt = x_pp_filt;
traj.y_filt = y_filt;
traj.y_p_filt = y_p_filt;
traj.y_pp_filt = y_pp_filt;

%Plot the trajectories plus derivatives
%traj_plot_traj_deriv(plt_file, param, 'X', z, x, x_filt, x_p, x_p_filt, x_pp, x_pp_filt);
%traj_plot_traj_deriv(plt_file, param, 'Y', z, y, y_filt, y_p, y_p_filt, y_pp, y_pp_filt);

%Plot the trajectories alone
traj_plot_traj(plt_file, param, 'X', z, x, x_filt);
traj_plot_traj(plt_file, param, 'Y', z, y, y_filt);

%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));

%Slope corrected trajectories -----

%Fit the filtered trajectory in 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_fit_traj(z_start, z_end, z, x_filt);
[y_slope, y_intercept, y_resid_filt] = traj_fit_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_slope_cor_traj(plt_file, param, 'X', z, x_slope_cor);
traj_plot_slope_cor_traj(plt_file, param, 'Y', 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.x_slope_cor = x_slope_cor;
traj.x_p_slope_cor = x_p_slope_cor;
traj.y_slope_cor = y_slope_cor;
traj.y_p_slope_cor = y_p_slope_cor;

%Done
