root/DPWP/trunk/DPWP/nortek/nortek_radialtouvw.m

function [ID,SM,EP]=nortek_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.5;

%whats the magnetic variation
magvar=-10;

%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,4096,1);
dmrange=range-meanrange;
std_range=std(dmrange);

%need to decrease the number of range samples

dmrange=decimate(dmrange,2);


%set up sysinfo file
heading=sysinfo(6,:);
pitch=sysinfo(7,:);
roll=sysinfo(8,:);
binheight=sysinfo(11,:);

%Find the std deviation of orbitals 
orbit=orbit/1000;
std_orbit=std(orbit);
orbitnew=orbit;

%interpolate to take out any NaNs
for i=1:3
    ibad=find(abs(orbit(:,i)) > 5*std_orbit(:,i));
    orbit(ibad,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));
    orbitnew(ibad,i)=intValues;   
end

% Do the same for the range data
rangenew=dmrange;

ibad=find(abs(dmrange(:)) > 5*std_range(:));
dmrange(ibad)=NaN;
time=[1:1:length(dmrange(:))];
NaNs=isnan(dmrange(:));
igood=find(NaNs == 0);
ibad=find(NaNs == 1);

intValues= interp1(time(igood),dmrange(igood),time(ibad));
rangenew(ibad)=intValues;

dmrange=rangenew;
    
%change heading, pitch, roll into degrees
     
heading = heading/10;
pitch=pitch/10;
roll=roll/10;

%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,3);
height=binheight;
pos=height*tan(25*pi/180);

xyzpos(:,1)=[0,pos,height];
xyzpos(:,2)=[pos*cos(30*pi/180),-pos*.5,height];
xyzpos(:,3)=[-pos*cos(30*pi/180),-pos*.5,height];
beam4xpos=mean(press)*tan(roll*(pi/180));
beam4ypos=mean(press)*tan(-pitch*(pi/180));

 
% set up the new 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;

%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(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;
new_xyzpos(:,4)=[beam4xpos,beam4ypos,avgdepth];%this is for the vertical beam...the z position will always =the avg depth

xyzpositions=new_xyzpos;




%compute the x,y positions for each beam for each bin and the surface

%sin45=0.7071 
xypositions=ones(1,4);

    
%beam 1
bearing=(heading+magvar)*(pi/180);
distfromz=binheight*tan((25-pitch)*(pi/180));
xpos=sin(bearing)*distfromz;
ypos=cos(bearing)*distfromz;

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

%beam 2
bearing=bearing+2/3*pi;
distfromz=binheight*tan(25*(pi/180));
xpos=sin(bearing)*distfromz;
ypos=cos(bearing)*distfromz;
beam2xpos=xpos+binheight*tan(roll*(pi/180));
beam2ypos=ypos+binheight*tan(-pitch*(pi/180));

%beam 3
bearing=bearing+2/3*pi;
distfromz=binheight*tan(25*(pi/180));
xpos=sin(bearing)*distfromz;
ypos=cos(bearing)*distfromz;
beam3xpos=xpos+binheight*tan(-roll*(pi/180));
beam3ypos=ypos+binheight*tan(-pitch*(pi/180));



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=[-180:2:180];
SM.xaxisdir= 90;
%the estimation parameter
EP.method= 'IMLM';
EP.iter=3;

if data_type == 1
    %For radial velocities and the range beam
    % the datatypes
    ID.datatypes={'elev' 'radial' 'radial' 'radial'};
    % the layout
    ID.layout = [xyzpositions(1,4) xyzpositions(1,1) xyzpositions(1,2) xyzpositions(1,3);xyzpositions(2,4) xyzpositions(2,1) xyzpositions(2,2) xyzpositions(2,3); 
    xyzpositions(3,4) xyzpositions(3,1) xyzpositions(3,2) xyzpositions(3,3)];
    % the data
    ID.data = horzcat(dmrange, orbitnew(:,1), orbitnew(:,2), orbitnew(:,3));

end