% process WG RDI data from beam coords to earth coords using IMU

clear
load('data\STOKES_PLD1_NOVATEL_21_ALL.mat')

%%%%%%%%%%% rename needed variables from structure and trim 
aa = 150001;
bb = 11978000;

v_time = PLD1_NOVATEL_21.time(aa:bb);
p = PLD1_NOVATEL_21.novatel_pitch(aa:bb);
r = PLD1_NOVATEL_21.novatel_roll(aa:bb);
h = PLD1_NOVATEL_21.novatel_azimuth(aa:bb);
e = PLD1_NOVATEL_21.novatel_velocity_east(aa:bb);
n = PLD1_NOVATEL_21.novatel_velocity_north(aa:bb);
d = -PLD1_NOVATEL_21.novatel_velocity_up(aa:bb); 
lat = PLD1_NOVATEL_21.novatel_latitude(aa:bb);
lon = PLD1_NOVATEL_21.novatel_longitude(aa:bb);
alt = PLD1_NOVATEL_21.novatel_height(aa:bb);

%%%%%%%%%%%% find and interpolate over gaps and glitches in vectornav data
I=find(lat<=32.97 | lon>= -117.29);

lat(I) = NaN; lon(I) = NaN;

dt = 1/20; % sampling interval, seconds

vtime = v_time(1):dt/86400:v_time(end);

p = interp1q(v_time(~isnan(p))',p(~isnan(p))',vtime')'/180*pi;
r = interp1q(v_time(~isnan(r))',r(~isnan(r))',vtime')'/180*pi;
h = interp1q(v_time(~isnan(h))',h(~isnan(h))',vtime')'/180*pi;
e = interp1q(v_time(~isnan(e))',e(~isnan(e))',vtime')';
n = interp1q(v_time(~isnan(n))',n(~isnan(n))',vtime')';
d = interp1q(v_time(~isnan(d))',d(~isnan(d))',vtime')';
lat = interp1q(v_time(~isnan(lat))',lat(~isnan(lat))',vtime')';
lon = interp1q(v_time(~isnan(lon))',lon(~isnan(lon))',vtime')';
alt = interp1q(v_time(~isnan(alt))',alt(~isnan(alt))',vtime')';

pdot = gradient(p,dt);
rdot = gradient(r,dt);
hdot = gradient(unwrap(h),dt);

hdot(abs(hdot)>.6) = NaN;
hdot = interp1q(vtime(~isnan(hdot))',hdot(~isnan(hdot))',vtime')';
%%
load('data\STOKES_PLD2_ADCP_ALL.mat')

%%%%%%%%%%% generate cr6 time for each ping
% rename needed variables from structure and trim
A = 8225;
B = 599000;

hour = PLD2_ADCP.cr6_time_4(A:B);
minute = PLD2_ADCP.cr6_time_5(A:B);
second = PLD2_ADCP.cr6_second_decimal(A:B);
year = 2021;
month = 3;
I = find(diff(hour)==-23);
I = [0 I length(hour)];
day = hour;
for i=1:length(I)-1
    day( I(i)+1:I(i+1) )=3+i;
end
time = datenum(year,month,day,hour,minute,second);
%time_RDI = PLD2_ADCP.time(A:B);

%%%%%%%%%%%% build arrays from vectors in structure
% E=[]; N=[]; Z=[];           % real-time velocities in earth coords
V1=[]; V2=[]; V3=[]; V4=[];   % velocities in beam coords
% E1=[]; E2=[]; E3=[]; E4=[]; % echo intensities
C1=[]; C2=[]; C3=[]; C4=[];   % correlations

N = 21; %there are 50 bins, but water depth is always less than bin 21
for i=1:N 
%     eval( ['E = [E; PLD2_ADCP.TCE_' num2str(i) '];' ] )
%     eval( ['N = [N; PLD2_ADCP.TCN_' num2str(i) '];' ] )
%     eval( ['Z = [Z; PLD2_ADCP.TCZ_' num2str(i) '];' ] )
    for j=1:4
        eval( ['V' num2str(j) '= [V' num2str(j) '; PLD2_ADCP.Velocity' num2str(j) '_' num2str(i) '(A:B)];' ] )
        %eval( ['E' num2str(j) '= [E' num2str(j) '; PLD2_ADCP.EchoIntense' num2str(j) '_' num2str(i) '(A:B)];' ] )
        eval( ['C' num2str(j) '= [C' num2str(j) '; PLD2_ADCP.Correlation' num2str(j) '_' num2str(i) '(A:B)];' ] )
    end
end

C = C1+C2+C3+C4;  % aggregate correlation

%%%%%%%%%%%% remove duplicate records
T1234 = [time' V1' V2' V3' V4' C'];
T1234 = unique(T1234,'rows');
time = T1234(:,1)';
V1 = T1234(:,2:N+1)';
V2 = T1234(:,N+2:2*N+1)';
V3 = T1234(:,2*N+2:3*N+1)';
V4 = T1234(:,3*N+2:4*N+1)';
C = T1234(:,4*N+2:5*N+1)';
N1 = length(time);

%% %%%%%%%%%% mask bad data
V1(C<475) = NaN; % mask velocity estimates with low correlation
V2(C<475) = NaN;
V3(C<475) = NaN;
V4(C<475) = NaN;

%%%%%%%%%%%%%% pick out IMU indicies corresponding to each ping
% CR6 recorded ping time seems to run behind the VN300 by ~.35 sec
III = round(interp1q(vtime',(1:length(vtime))',time')) - 7;

p = p(III);
r = r(III);
h = h(III);
e = e(III);
n = n(III);
d = d(III);
lat = lat(III);
lon = lon(III);
pdot = pdot(III);
rdot = rdot(III);
hdot = hdot(III);

%%%%%%%%%%%% build rotation matrices
% heading rotation matrix
Rh(1,1,:) = cos(h);   Rh(1,2,:) = sin(h);  Rh(1,3,:) = 0;
Rh(2,1,:) = -sin(h);  Rh(2,2,:) = cos(h);  Rh(2,3,:) = 0;
Rh(3,1,:) = 0;        Rh(3,2,:) = 0;       Rh(3,3,:) = 1;

% pitch rotation matrix
Rp(1,1,:) = cos(p);   Rp(1,2,:) = 0;       Rp(1,3,:) = -sin(p);
Rp(2,1,:) = 0;        Rp(2,2,:) = 1;       Rp(2,3,:) = 0;
Rp(3,1,:) = sin(p);   Rp(3,2,:) = 0;       Rp(3,3,:) = cos(p);

% roll rotation matrix
Rr = Rp;  % to set size

Rr(1,1,:) = 1;        Rr(1,2,:) = 0;       Rr(1,3,:) = 0;
Rr(2,1,:) = 0;        Rr(2,2,:) = cos(r);  Rr(2,3,:) = -sin(r);
Rr(3,1,:) = 0;        Rr(3,2,:) = sin(r);  Rr(3,3,:) = cos(r);

%%%%%%%%%%%% bin depth mapping
% beam order clockwise looking at face: 3,1,4,2. "lats" and "lons" contain the
% angular positions of the 4 beams, with the nose of the WG defined as (0,0).
lats = -70*ones(1,4);     
lons = [225 45 315 135];  % beam 3 points port and forward.
lons = 360 - lons;    % switch from heading (cw) to longitude (ccw) convention

% beam unit vectors in rectilinear instrument coords
F = cosd(lats).*cosd(lons);  % forward
P = cosd(lats).*sind(lons);  % port
U = sind(lats);              % up

FPU = [F; P; U];
% transform beam position vectors from "instrument" to "ship" coords
FPU = pagemtimes(Rr,FPU);
FPU = pagemtimes(Rp,FPU);

bin_ratio = squeeze(FPU(3,:,:))/sind(lats(1)); % fractional depth change

cellsize = 2;
blanking = 2;
ranges(1,1,:) = cellsize*(1:N)' - cellsize/2 + blanking;

% compute indices of bins with actual depths closest to the nominal depths
bins = round((ranges.*bin_ratio - blanking + cellsize/2)/cellsize);
% dims: 1 beam, 2 ping, 3 bin
bins = permute(bins,[3 2 1]); % now dims: 1 bin, 2 ping, 3 beam

bins(bins<1) = 1; % there are very few of these
bins(bins>N) = N; % lots of these, but they only affect the bottom bin

% replace velocities from tilted beams with ones from nominal depths
V1 = V1(squeeze(bins(:,:,1)) + (0:N1-1)*N);
V2 = V2(squeeze(bins(:,:,2)) + (0:N1-1)*N);
V3 = V3(squeeze(bins(:,:,3)) + (0:N1-1)*N);
V4 = V4(squeeze(bins(:,:,4)) + (0:N1-1)*N);

I = [3 4 7 8]';  % mask velocity estimates with sub contamination
V1(I,:) = NaN;
V4(I,:) = NaN;

%%%%%%%%%%%% transform from beam to instrument coords
alpha = 20;              % beam angle
a = 1/(2*sind(alpha));
b = 1/(4*cosd(alpha));
D = a/sqrt(2);

X = a*(V1-V2);           % relative current in direction beam 1 --> beam 2
Y = a*(V4-V3);           % relative current in direction beam 4 --> beam 3
Z = b*(V1+V2+V3+V4);     % relative current toward adcp face

%%%%%%%%%%%% rotate into WG coords
RDI_angle = 135;       
CR = cosd(RDI_angle);
SR = sind(RDI_angle);

vel1 = .001*(CR*X + SR*Y); % stbd --> port current
vel2 = .001*(CR*Y - SR*X); % fwd --> aft current
vel3 = .001*Z;             % keel --> mast current

er = .001*D*(V1+V2-V3-V4); % error velocity

%% %%%%%%%%% transfrom from WG to Earth coords using IMU data
FPU = [-1.392 0 -.15]'; % RDI position in m: Forward Port Up

% transform adcp position vector from WG to Earth coords
FPU = pagemtimes(Rr,FPU);
FPU = pagemtimes(Rp,FPU);
FPU = pagemtimes(Rh,FPU);

% WG angular velocity vector in Earth coordinates
w_NWU(1,:) = rdot.*cos(h).*cos(p) - pdot.*sin(h);
w_NWU(2,:) = -rdot.*sin(h).*cos(p) - pdot.*cos(h);
w_NWU(3,:) = rdot.*sin(p) - hdot;
w_NWU = reshape(w_NWU,3,1,N1);

% velocity vector, in Earth coords, of adcp due to WG angular velocity
mast_NWU = cross(w_NWU,FPU);

u = zeros(N,N1); v = u; w = u;

for i=1:N  % for each depth bin
    UVW = [-vel2(i,:); vel1(i,:); vel3(i,:)];
    UVW = reshape(UVW,3,1,N1);
    
    % transfrom current vector from WG coords to Earth coords
    UVW = pagemtimes(Rr,UVW);
    UVW = pagemtimes(Rp,UVW);
    UVW = pagemtimes(Rh,UVW);
    
    % Current velocity components are the sum of relative current, translational
    % motion of IMU, and rotational motion of sensor relative to IMU
    V = ( squeeze(UVW(1,:,:)) + n' + squeeze(mast_NWU(1,:,:)))';
    U = (-squeeze(UVW(2,:,:)) + e' - squeeze(mast_NWU(2,:,:)))';
    W = ( squeeze(UVW(3,:,:)) - d' + squeeze(mast_NWU(3,:,:)))';
    
    % fill arrays with transformed currents
    u(i,:) = U; v(i,:) = V; w(i,:) = W;
end

h = mod(h*180/pi,360);
I = find(abs(u)>1 | abs(v)>1 | abs(w)>1);
u(I) = NaN; v(I) = NaN; w(I) = NaN;
save('data\Stokes_proc.mat', 'u', 'v', 'w', 'time', 'lat', 'lon', 'h')