root/DPWavesProc/trunk/DPWavesProc/adcp_matlab/radialtouvw.m

function [ID,SM,EP]=radialtouvw(pressure,range,orbit,sysinfo,data_type)


%This code takes the loaded orbit, pressure and system info data from an
%ADCP waves output and computes the uvw velocities in earth coordinates.
%It also interpolates to remove bad data points and prepares the data
%structure required for running the DIWASP program.  The output will be in
%the structures ID, SM and EP.

%data_type is:
%1 to use uvw and pressure to generate the data structures,
%2 to use ranges
%3 to use radial velocity data



%make sure to load: pressure=load('pressure data'), 
%orbit=load('orbital data'),sysinfo=load('sysinfo data') and range

%what is the transducer face height off the bottom?
adcpheight=0.4;

%whats the magnetic variation
magvar=-10;

%set up sysinfo file
samplesInBurst=sysinfo(3,:);
bin1height=sysinfo(9,:);
bin2height=sysinfo(10,:);
bin3height=sysinfo(11,:);
bin4height=sysinfo(12,:);
bin5height=sysinfo(13,:);
heading=sysinfo(18,:);
pitch=sysinfo(19,:);
roll=sysinfo(20,:);

%set up pressure
press=pressure/1000;

%find the average depth
avgdepth=mean(press)+adcpheight;

%set up range
range=range/1000;
meanrange=mean(range);
meanrange=repmat(meanrange,samplesInBurst,1);
dmrange=range-meanrange;
std_range=std(dmrange);

% Take out any bad data points in orbital data 
ibad=find(orbit < -32000);
orbit(ibad) = NaN;
orbit=orbit/1000;
orbitnew=orbit;
%interpolate to take out any NaNs and QC for bad data in orbital data
std_orbit=ones(1,20);
for i=1:20
    
    %first take out any points outside of 4 std deviations
    std_orbit(i)=nanstd(orbit(:,i));
    ibad_std=find(abs(orbit(:,i)) > 4*std_orbit(i));
    orbit(ibad_std,i)=NaN;
end
%find the avg std deviation for each group 4 beams
for i=4:4:20
    avgstd_orbit(i-3:i)=mean(std_orbit(i-3:i));
end

%now remove the points outside 4avg std dev and interp
for i=1:20
    ibad_std=find(abs(orbit(:,i)) > 4*avgstd_orbit(i));
    orbit(ibad_std,i)=NaN;
    
    time=[1:1:length(orbit(:,i))];
    NaNs=isnan(orbit(:,i));
    igood=find(NaNs == 0);
    ibad=find(NaNs == 1);

    intValues= interp1(time(igood),orbit(igood,i),time(ibad),'linear','extrap');
    orbitnew(ibad,i)=intValues;   
end

% Do similar QC for the range data
rangenew=dmrange;
for i=1:4
    ibad=find(abs(dmrange(:,i)) > 4*std_range(:,i));
    dmrange(ibad,i)=NaN;
    time=[1:1:length(range(:,i))];
    NaNs=isnan(dmrange(:,i));
    igood=find(NaNs == 0);
    ibad=find(NaNs == 1);

    intValues= interp1(time(igood),dmrange(igood,i),time(ibad),'linear','extrap');
    rangenew(ibad,i)=intValues;
end

dmrange=rangenew;
    
%change heading, pitch, roll into degrees
     
heading = heading/100;
pitch=pitch/100;
roll=roll/100;
pitch_out=pitch;
roll_out=roll;

%When converting to earth coordinates, the directions will be wrong unless we
%correct for bottom/up facing ADCPs, simply by rotating the roll by 180   
roll=roll+180;

%Set up geometry for transformation from beam to instrument
C1 = 1;
A1 = 1/(2*sin(20*pi/180));
B1 = 1/(4*cos(20*pi/180));
D1 = A1/sqrt(2);

% coordinate transformation matrix
CH = cos((heading+magvar)*pi/180);
SH = sin((heading+magvar)*pi/180);
CP = cos((pitch)*pi/180);
SP = sin((pitch)*pi/180);
CR = cos((roll)*pi/180);
SR = sin((roll)*pi/180);

%  let the matrix elements be ( a b c; d e f; g h j);
a = CH.*CR + SH.*SP.*SR;  b = SH.*CP; c = CH.*SR - SH.*SP.*CR;
d = -SH.*CR + CH.*SP.*SR; e = CH.*CP; f = -SH.*SR - CH.*SP.*CR;
g = -CP.*SR;              h = SP;     j = CP.*CR;
uno=1;

%reshape the orbital matrix to 3 dimensions

radial=reshape(orbitnew,samplesInBurst,4,5);
   
%  Compute uvw velocities and change to earth coordinates
  
u = C1*A1*(radial(:,1,:)-radial(:,2,:));
v = C1*A1*(radial(:,4,:)-radial(:,3,:));
w = B1*(radial(:,1,:)+radial(:,2,:)+radial(:,3,:)+radial(:,4,:));
error_vel = D1*(radial(:,1,:)+radial(:,2,:)-radial(:,3,:)-radial(:,4,:));
   
u=reshape(u,samplesInBurst,5);
v=reshape(v,samplesInBurst,5);
w=reshape(w,samplesInBurst,5);
error_vel=reshape(error_vel,samplesInBurst,5);

[m,n] = size(u);
uno = ones(m,5);
unew = u.*(uno*a) + v.*(uno*b) + w.*(uno*c);
vnew = u.*(uno*d) + v.*(uno*e) + w.*(uno*f);
wnew = u.*(uno*g) + v.*(uno*h) + w.*(uno*j);

u_all = unew;v_all=vnew;w_all=wnew;
error_vel_all = error_vel;

%compute the original x,y,z positions for each beam for each bin to be accurate we need to take out the adcpheight

xyzpos=ones(3,4,5);
heights=[bin1height bin2height bin3height bin4height bin5height avgdepth]-adcpheight;
pos=heights*tan(20*pi/180);
for i=1:5
    xyzpos(:,1,i)=[pos(i),0,heights(i)];
    xyzpos(:,2,i)=[-pos(i),0,heights(i)];
    xyzpos(:,3,i)=[0,pos(i),heights(i)];
    xyzpos(:,4,i)=[0,-pos(i),heights(i)];
end

% set up the new coordinate transformation matrix
CH = cos((heading+magvar)*pi/180);
SH = sin((heading+magvar)*pi/180);
CP = cos((pitch_out)*pi/180);
SP = sin((pitch_out)*pi/180);
CR = cos((-roll_out)*pi/180);
SR = sin((-roll_out)*pi/180);

%  let the matrix elements be ( a b c; d e f; g h j), a slightly different
%  matrix from before;
a = CH.*CR - SH.*SP.*SR;  b = SH.*CP; c = -CH.*SR - SH.*SP.*CR;
d = -SH.*CR - CH.*SP.*SR; e = CH.*CP; f = SH.*SR - CH.*SP.*CR;
g = CP.*SR;              h = SP;     j = CP.*CR;

%transform the original x,y,z positions to the new positions accounting for
%heading, pitch and roll... we also add adcpheight back in
new_xyzpos=ones(3,4,5);
new_xyzpos(1,:,:)=xyzpos(1,:,:)*a+xyzpos(2,:,:)*b+xyzpos(3,:,:)*c;
new_xyzpos(2,:,:)=xyzpos(1,:,:)*d+xyzpos(2,:,:)*e+xyzpos(3,:,:)*f;
new_xyzpos(3,:,:)=xyzpos(1,:,:)*g+xyzpos(2,:,:)*h+xyzpos(3,:,:)*j+adcpheight;

xyzpositions=new_xyzpos;


%now we need to figure out the xyz positions at the surface for the range
%sin45=0.7071

binheight=heights(:,6);
    
%beam 3
bearing=(heading+magvar)*(pi/180);
distfromz=binheight*tan((20-pitch_out)*(pi/180));
xpos=sin(bearing)*distfromz;
ypos=cos(bearing)*distfromz;

distroll=binheight*tan(roll_out*(pi/180));
beam3xpos=xpos+0.7071*distroll;
beam3ypos=ypos+0.7071*distroll;
    
%beam 4
bearing=bearing+pi;
distfromz=binheight*tan((20+pitch_out)*(pi/180));
xpos=sin(bearing)*distfromz;
ypos=cos(bearing)*distfromz;

distroll=binheight*tan(roll_out*(pi/180));
beam4xpos=xpos+0.7071*distroll;
beam4ypos=ypos+0.7071*distroll;

%beam 1
bearing=bearing-pi/2;
distfromz=binheight*tan((20+roll_out)*(pi/180));
xpos=sin(bearing)*distfromz;
ypos=cos(bearing)*distfromz;

distroll=binheight*tan(pitch_out*(pi/180));
beam1xpos=xpos+0.7071*distroll;
beam1ypos=ypos+0.7071*distroll;

%beam2
bearing=bearing+pi;
distfromz=binheight*tan((20-roll_out)*(pi/180));
xpos=sin(bearing)*distfromz;
ypos=cos(bearing)*distfromz;

distroll=binheight*tan(pitch_out*(pi/180));
beam2xpos=xpos+0.7071*distroll;
beam2ypos=ypos+0.7071*distroll;

xypositions(:,:)=[beam1xpos beam2xpos beam3xpos beam4xpos; beam1ypos beam2ypos beam3ypos beam4ypos];
    
%Put into structures for DIWASP


%sampling frequency
ID.fs=2;
%depth
ID.depth=(adcpheight+mean(press));
%the spectral matrix structure
SM.freqs=[0.01:0.01:0.4];
SM.dirs=[0:2:360];
SM.xaxisdir= 90;
%the estimation parameter
EP.method= 'EMEP';
EP.iter=100;


if data_type == 1
    %For uvw and pressure for bins 2,3,4
    % the datatypes
    ID.datatypes={'pres' 'velx' 'vely' 'velz' 'velx' 'vely' 'velz' 'velx' 'vely' 'velz'};
    % the layout
    ID.layout = [ 0 0 0 0 0 0 0 0 0 0;0 0 0 0 0 0 0 0 0 0; 
    adcpheight bin2height bin2height bin2height bin3height bin3height bin3height bin4height bin4height bin4height];
    % the data
    ID.data = horzcat(press, u_all(:,2), v_all(:,2), w_all(:,2), u_all(:,3), v_all(:,3), w_all(:,3), u_all(:,4), v_all(:,4), w_all(:,4));
    
elseif data_type == 2
    %For the ranges of each beam
    % the datatypes
    ID.datatypes={'elev' 'elev' 'elev' 'elev'};
    % the layout
    ID.layout = [xypositions(1,1) xypositions(1,2) xypositions(1,3) xypositions(1,4);
    xypositions(2,1) xypositions(2,2) xypositions(2,3) xypositions(2,4); 
    avgdepth avgdepth avgdepth avgdepth];
    % the data
    ID.data = horzcat(dmrange(:,1), dmrange(:,2), dmrange(:,3), dmrange(:,4));

elseif data_type == 3
    % For the radial velocities bins 2,3,4
    % the datatypes
    ID.datatypes={'radial' 'radial' 'radial' 'radial' 'radial' 'radial' 'radial' 'radial' 'radial' 'radial' 'radial' 'radial'};
    % the layout
    ID.layout = [xyzpositions(1,1,2) xyzpositions(1,2,2) xyzpositions(1,3,2) xyzpositions(1,4,2) xyzpositions(1,1,3) xyzpositions(1,2,3) xyzpositions(1,3,3) xyzpositions(1,4,3) xyzpositions(1,1,4) xyzpositions(1,2,4) xyzpositions(1,3,4) xyzpositions(1,4,4);
    xyzpositions(2,1,2) xyzpositions(2,2,2) xyzpositions(2,3,2) xyzpositions(2,4,2) xyzpositions(2,1,3) xyzpositions(2,2,3) xyzpositions(2,3,3) xyzpositions(2,4,3) xyzpositions(2,1,4) xyzpositions(2,2,4) xyzpositions(2,3,4) xyzpositions(2,4,4); 
    xyzpositions(3,1,2) xyzpositions(3,2,2) xyzpositions(3,3,2) xyzpositions(3,4,2) xyzpositions(3,1,3) xyzpositions(3,2,3) xyzpositions(3,3,3) xyzpositions(3,4,3) xyzpositions(3,1,4) xyzpositions(3,2,4) xyzpositions(3,3,4) xyzpositions(3,4,4)];
    % the data
    ID.data = horzcat(orbitnew(:,5),orbitnew(:,6),orbitnew(:,7),orbitnew(:,8),orbitnew(:,9),orbitnew(:,10),orbitnew(:,11),orbitnew(:,12),orbitnew(:,13),orbitnew(:,14),orbitnew(:,15),orbitnew(:,16));

    % the layout using bins 3,4,5
    %ID.layout = [xyzpositions(1,1,5) xyzpositions(1,2,5) xyzpositions(1,3,5) xyzpositions(1,4,5) xyzpositions(1,1,4) xyzpositions(1,2,4) xyzpositions(1,3,4) xyzpositions(1,4,4) xyzpositions(1,1,3) xyzpositions(1,2,3) xyzpositions(1,3,3) xyzpositions(1,4,3);
   % xyzpositions(2,1,5) xyzpositions(2,2,5) xyzpositions(2,3,5) xyzpositions(2,4,5) xyzpositions(2,1,4) xyzpositions(2,2,4) xyzpositions(2,3,4) xyzpositions(2,4,4) xyzpositions(2,1,3) xyzpositions(2,2,3) xyzpositions(2,3,3) xyzpositions(2,4,3); 
   % xyzpositions(3,1,5) xyzpositions(3,2,5) xyzpositions(3,3,5) xyzpositions(3,4,5) xyzpositions(3,1,4) xyzpositions(3,2,4) xyzpositions(3,3,4) xyzpositions(3,4,4) xyzpositions(3,1,3) xyzpositions(3,2,3) xyzpositions(3,3,3) xyzpositions(3,4,3)];
    % the data
   % ID.data = horzcat(orbitnew(:,17),orbitnew(:,18),orbitnew(:,19),orbitnew(:,20),orbitnew(:,13),orbitnew(:,14),orbitnew(:,15),orbitnew(:,16),orbitnew(:,9),orbitnew(:,10),orbitnew(:,11),orbitnew(:,12));

end