%%%JORGENSEN et 2015 ADDITIONAL FILES 2 - Salvador Jorgensen (first coded January 2011)
%%%DETERMINE DIRECTION OF PASSIVE ACELERATION FORCE OF EARTH'S GRAVITY
%%%ASSUMING A KNOWN AVERAGE ANIMAL OREINTATION
%%%After determining the time window from Jorgensen2015_AI1_time_window.m
%%%take this value defined in number of time steps (sampling intervals)
%%%and transform the coordinate system for all data in each bin to to
%%%allign Z axis wth gravitational pull - i.e. 'find up'
%%%
%%%This routine calls coordinate transformation functions roty3.m rotx3.m
%%%and rotz3.m also included as additional files in Jorgensen 2015


%% USER DEFINE DATA FILE and load data
 xyzd = load('USER ENTER DATA FILE NAME AND PATHWAY') 
%%% data should be an array with three columns corresponding with X,Y,Z,
%%% and depth
%%% accceleration traces respectively

%%% USER ENTER TIME STEP
tm = 1800; %time step is given in number of sampling intervals



%% Determine get mean X, Y and Z value for each time step

xtm = (reshape(xyzd(:,1),tm,length(xyz)/tm)); % reshape data array for efficiency
xm = mean(xtm); % take mean of each time window

ytm = (reshape(xyzd(:,2),tm,length(xyz)/tm)); % reshape data array for efficiency
ym = mean(ytm); % take mean of each time window

ztm = (reshape(xyzd(:,3),tm,length(xyz)/tm)); % reshape data array for efficiency
zm = mean(ztm); % take mean of each time window

depth = (reshape(xyzd(:,4),tm,length(xyz)/tm));



%% Loop through and solve transformation angles for each window  
%%% by setting mean X and Y of each window to zero

    %%%initiate working variables
    zs = zeros(size(xm));
    a=zs;a1=zs;x1=zs;x2=zs;y1=zs;y2=zs;z1=zs;z2=zs;
    %%%rotate axes to find 
        for d = 1: length(xm)
            %%% FIRST ROTATION make vector
            a(d) = rad2deg(atan2(zm(d),xm(d)))+90;
            [x1(d) y1(d) z1(d) ] = roty3(xm(d),ym(d),zm(d), a(d));

            %%% SECOND Rotation
            a1(d) = rad2deg(atan2(z1(d),y1(d)))+90;
            [x2(d) y2(d) z2(d) ] = rotx3(x1(d),y1(d),z1(d), a1(d));
        end

%%%% Diagnostic plot show successive transformation angles and provides
%%%% info on rate of tag position shifing
subplot(2,1,1);plot(1:length(a), [a' a1'], 'o')
subplot(2,1,2); plot(1:length(x2), [x2' y2' z2'])


%% Apply transformation

    %%initiate working variables
    zs = zeros(size(ztm));
    x1=zs;x2a=zs;y1=zs;y2a=zs;z1=zs;z2a=zs;
    ds = size(ztm,2);
    %%%loop through windows and successively apply transformations
        for d = 1:ds;
            for e = 1:size(ztm,1);
                %%% FIRST rotation make vector
                [x1(e,d) y1(e,d) z1(e,d)] = roty3(xtm(e,d),ytm(e,d),ztm(e,d), a(d));
                %%% SECOND rotation
                [x2a(e,d) y2a(e,d) z2a(e,d)] = rotx3(x1(e,d),y1(e,d),z1(e,d), a1(d));
            end
        end



%%%USER save variables x2a, y2a, z2a, depth which are the new X,Y,Z corrected for first step
%%% and corresponding depth data. These ewill be entered in next routine: Jorgensen2014_AI4_ for final
%%% transformation