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
%NOTE: also hardwired into radial.m
adcpheight=0.4;

%whats the magnetic variation
magvar=-10; 

%set up sysinfo file
samplesInBurst=sysinfo(3,:);
binsOut=sysinfo(8,:);
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;

% calculate the total number of orbital bins output (usually 20)
orbOut=binsOut*4;

%interpolate to take out any NaNs and QC for bad data in orbital data
std_orbit=ones(1,orbOut);
for i=1:orbOut
    
    %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:orbOut
    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:orbOut
    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;


%reshape the orbital matrix to 3 dimensions

radial=reshape(orbitnew,samplesInBurst,4,binsOut);
   
%  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,binsOut);
v=reshape(v,samplesInBurst,binsOut);
w=reshape(w,samplesInBurst,binsOut);
error_vel=reshape(error_vel,samplesInBurst,binsOut);

[m,n] = size(u);
uno = ones(m,binsOut);
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=avgdepth; 
%the spectral matrix structure
SM.freqs=[0.01:0.01:0.4];
SM.dirs=[0:2:360]; %note that SM.dirs is hardwired into specmultiplot (line 123)
SM.xaxisdir= 90;
%the estimation parameter
EP.method= 'IMLM';
EP.iter=50;
EP.nfft=256;


if data_type == 1
    %For uvw and pressure for the highest three bins (3,4,5)
    
    % 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 bin3height bin3height bin3height bin4height bin4height bin4height bin5height bin5height bin5height];
    % the data
    ID.data = horzcat(press, u_all(:,3), v_all(:,3), w_all(:,3), u_all(:,4), v_all(:,4), w_all(:,4), u_all(:,5), v_all(:,5), w_all(:,5));
    
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 using highest 3 bins depending on the number of bins
    % recorded
    
    %highest bin
    bin3=5;
    %second highest
    bin2=4;
    %third highest
    bin1=3;
    
    ID.layout = [xyzpositions(1,1,bin3) xyzpositions(1,2,bin3) xyzpositions(1,3,bin3) xyzpositions(1,4,bin3) xyzpositions(1,1,bin2) xyzpositions(1,2,bin2) xyzpositions(1,3,bin2) xyzpositions(1,4,bin2) xyzpositions(1,1,bin1) xyzpositions(1,2,bin1) xyzpositions(1,3,bin1) xyzpositions(1,4,bin1);
    xyzpositions(2,1,bin3) xyzpositions(2,2,bin3) xyzpositions(2,3,bin3) xyzpositions(2,4,bin3) xyzpositions(2,1,bin2) xyzpositions(2,2,bin2) xyzpositions(2,3,bin2) xyzpositions(2,4,bin2) xyzpositions(2,1,bin1) xyzpositions(2,2,bin1) xyzpositions(2,3,bin1) xyzpositions(2,4,bin1); 
    xyzpositions(3,1,bin3) xyzpositions(3,2,bin3) xyzpositions(3,3,bin3) xyzpositions(3,4,bin3) xyzpositions(3,1,bin2) xyzpositions(3,2,bin2) xyzpositions(3,3,bin2) xyzpositions(3,4,bin2) xyzpositions(3,1,bin1) xyzpositions(3,2,bin1) xyzpositions(3,3,bin1) xyzpositions(3,4,bin1)];
    % the data
    %set up values based on what bins are available
    orb=orbOut;
    ID.data = horzcat(orbitnew(:,orb-3),orbitnew(:,orb-2),orbitnew(:,orb-1),orbitnew(:,orb),orbitnew(:,orb-7),orbitnew(:,orb-6),orbitnew(:,orb-5),orbitnew(:,orb-4),orbitnew(:,orb-11),orbitnew(:,orb-10),orbitnew(:,orb-9),orbitnew(:,orb-8));

    
end