%% Determinne direction of maximum sway force
%%%%%%%% Step through the timestep (TM;column) and determine correction
%%%%%%%% rotation on z axis (theta) for each TM maximizing tailbeat signal

%%% Load data from Jorgensen2015_Ai2_find_up.m
%%% variables x2a, y2a, z2a, depth

%%% initiate working vars
xr = zeros(size(x2a));
yr = zeros(size(x2a));

        for idx = 1:size(x2a,2)

                y = y2a(:,idx);
                x = x2a(:,idx);
                d = depth(:,idx);
                
                %%STEP 2 in MS - get rotation angle theta in degrees to maximize Y, then rotate all
                theta(idx) = fft_rot(x,y);
                
                z=zeros(size(x)); %initiate working variable
                
                        for k = 1:length(x) % transform all x,y pairs - faster using non-matrix?
                         [xr(k,idx) yr(k,idx) z(k,idx)] = rotz3(x(k),y(k),z(k),theta(idx));
                        end

                    % STEP 3 in MS - as the shark moves up x should become more negative - test crosscorr        
                    [cc lg] = crosscorr(xr(:,idx),diff(smooth(smooth(smooth(d,50),50),50)));

                    % STEP 3 in MS - if the croscorr is positive, flip 180 degrees
                    if mean(cc)>=0; xr(:,idx)=-xr(:,idx); yr(:,idx)=-yr(:,idx); end 


        end

% %% Save final transformed data (AFTER USER UNCOMMENTS THIS SECTION)
% %%%% with single vectirs foreach variable
% %%%% USER TEST RESULTING DATA BEFORE SAVING AND PREVENT OVERWRITING
% %%%% PREVIOUSLY SAVED RESULTS BY SAVING AS UNIQUELY NAMED FILE
% x3a=xr(:);
% y3a=yr(:);
% z3a=z2a(:);
% depth = depth(:);
% 
% Save Transformed_xyzd x3a y3a z3a depth