%**************************************************************************
%phase_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_p                 %First derivative dx/dz
%   y_p                 %First derivative dy/dz
%   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_cor         %X trajectory minus linear fit
%   y_slope_cor         %Y trajectory minus linear fit
%   x_p_slope_cor       %X trajectory slope minus linear fit slope
%   y_p_slope_cor       %Y trajectory slope minus linear fit slope
%   x_by                %X trajectory, uncoupled, calculated using only By (m)
%   x_p_by              %X trajectory slope, uncoupled, calculated using only By (m)
%   x_pp_by             %X trajectory slope of slope, uncoupled, calculated using only By (m)
%   y_bx                %Y trajectory, uncoupled, calculated using only Bx (m)
%   y_p_bx              %Y trajectory slope, uncoupled, calculated using only Bx (m)
%   y_pp_bx             %Y trajectory slope of slope, uncoupled, calculated using only Bx (m)
%
%Zachary Wolf
%5/20/11

function [traj] = phase_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);

%Plot the trajectories
traj_plot_x_vs_z(plt_file, z, x);
traj_plot_y_vs_z(plt_file, z, 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 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] = traj_linfit_traj(z_start, z_end, z, x);
[y_slope, y_intercept, y_resid] = traj_linfit_traj(z_start, z_end, z, y);

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

%Uncoupled trajectories -----
[x_by, x_p_by, x_pp_by] = traj_calc_x_traj(gamma, by, int_by_dz, int_int_by_dz_dz);
[y_bx, y_p_bx, y_pp_bx] = traj_calc_y_traj(gamma, bx, int_bx_dz, int_int_bx_dz_dz);

%Save the results
traj.z = z;
traj.x = x;
traj.y = y;
traj.x_p = x_p;
traj.y_p = y_p;
traj.vx = vx;
traj.vy = vy;
traj.vz = vz;
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;
traj.x_by = x_by;
traj.x_p_by = x_p_by;
traj.x_pp_by = x_pp_by;
traj.y_bx = y_bx;
traj.y_p_bx = y_p_bx;
traj.y_pp_bx = y_pp_bx;

%Done
