%======================================================================
%                    F I X C O M P A S S . M 
%                    doc: Fri Dec 30 14:59:27 2011
%                    dlm: Sun Mar  6 12:15:39 2016
%                    (c) 2011 A.M. Thurnherr
%                    uE-Info: 42 0 NIL 0 0 72 0 2 4 NIL ofnI
%======================================================================

function [d,p]=fixcompass(d,p)
% 
% fix compass issues
%
% p.fix_compass=1  apply offset only
% p.fix_compass=2  fix up looking compass to down looking compass + offset
% p.fix_compass=3  fix down looking compass to up looking compass + offset
%
% p.hdg_offset will apply a fixed rotation to the compass. In most cases you
%        will want to set the compass that you trust to 0 and rotate the other one
%        by the difference in direction between the two beam 3 headings as mounted 
%        on the frame
%        : [downoffset  upoffset]
%
% once you done this rotup2down gets disabled since I assume you know what the offset was
%
% M. Visbeck LDEO 2004
%

% Changes by ant:
%	Dec 30, 2011: - BUG: p.fix_compass = 2 and = 3 did not work; bug report and fix 
%						 provided by Roman Tarakanov <rtarakanov@gmail.com> today
%						 bug fix verified with PALE data (old code produces garbage results)
%	Mar  6, 2016: - BUG: p.fix_compass = 1 did not work for single-head profiles

disp('FIXCOMPASS adjust compass')

% use tilt sensors to figure out allingment between instruments
if p.fix_compass > 1
	diary off
	  hoff2=fminsearch('checktilt',0,[],[d.rol(2,:);d.pit(2,:);d.rol(1,:);d.pit(1,:)]);
	diary on
    disp([' mean instrument alignment based on tilt is ',num2str(hoff2),' deg'])
end

[lb,lt]=size(d.ru);
if p.fix_compass==2
 p=setdefv(p,'hdg_offset',[0 hoff2]);
elseif p.fix_compass==3
 p=setdefv(p,'hdg_offset',[-hoff2 0]);
else
 p=setdefv(p,'hdg_offset',[0 0]);
end
p=setdefv(p,'hdg_offset_used',[0 0]);

rot=p.hdg_offset;

rotv(1,:)=ones(1,lt)*rot(1);
disp([' offset down looking compass by : ',num2str(rot(1))]) 
p.hdg_offset_used(1)=p.hdg_offset_used(1)+rot(1);

if p.fix_compass==3
  disp(' use up looking compass on down looking ADCP')
  rotv(1,:)=d.hdg(2,:)-d.hdg(1,:)+rot(1);
  p.rotup2down=0;
end

if length(d.izu)>0
  rotv(2,:)=ones(1,lt)*rot(2);
  disp([' offset up looking compass by : ',num2str(rot(2))]) 
  p.hdg_offset_used(2)=p.hdg_offset_used(2)+rot(2);
  if p.fix_compass==2
   disp(' use down looking compass on up looking ADCP')
   rotv(2,:)=d.hdg(1,:)-d.hdg(2,:)+rot(2);
   p.rotup2down=0;
  end
end

rotm=meshgrid(rotv(1,:),d.izd);
[d.ru(d.izd,:),d.rv(d.izd,:)]=uvrot(d.ru(d.izd,:),d.rv(d.izd,:),rotm);

if length(rotv(1,:))==size(d.bvel,1)
 [d.bvel(:,1),d.bvel(:,2)]=uvrot(d.bvel(:,1),d.bvel(:,2),rotv(1,:)');
else
 [d.bvel(1,:),d.bvel(2,:)]=uvrot(d.bvel(1,:),d.bvel(2,:),rotv(1,:));
end

if length(d.izu)>0
 rotm=meshgrid(rotv(2,:),d.izu);
 [d.ru(d.izu,:),d.rv(d.izu,:)]=uvrot(d.ru(d.izu,:),d.rv(d.izu,:),rotm);
end