function [est, err] = fitphase(hdgphase,TITLE,time,order)
%
%  Finds the coefficients of the following equation:
%       
%         Phase = a0 + a1*cos(Heading) + a2*sin(Heading)
%
%                 where Heading = ship's heading
%                       Phase   = calibration angle
%
%  INPUT:
%     hdgphase = array, [Heading  Phase];
%     TITLE    = optional, title for the plot.
%     time     = optional, vector time;
%     order    = order of time term to fit up to.
%
% OUTPUT:
%     est      = estimates, [a0 a1 a2].
%     err      = (fitted phase) - Phase.
%
% ---------------------------------------------------------------------------
%             Willa Zhu,   Aug. 6, 1991
%----------------------------------------------------------------------------

if(nargin == 1) TITLE = []; time = []; end
if(nargin == 2)
   if(isstr(TITLE)) time=[];
   else TITLE = []; end
end
if(nargin == 3) 
   if(isstr(TITLE) == 0) temp = time; time=TITLE; TITLE=temp; end
end

hdg=hdgphase(:,1);
phase=hdgphase(:,2);

H = [ones(size(phase)) cos(hdg*pi/180) sin(hdg*pi/180)];
if(length(time) ~= 0)
  dmean_t = time-mean(time);
  H = [H dmean_t];
  if order > 1
    H = [H dmean_t.^2];
  end
  if order > 2
    H = [H dmean_t.^3];
  end
  if order > 3
    H = [H dmean_t.^4];
  end
end
est = H \ phase;
err = H*est - phase;
if(length(time) ~= 0) 
  phase = phase - est(4)*dmean_t; 
  if(order > 1)
    phase = phase - est(5)*dmean_t.^2;
  end
  if order > 2
    phase = phase - est(6)*dmean_t.^3;
  end
  if order > 3
    phase = phase - est(6)*dmean_t.^4;
  end
end

dummy = [0:2:360]';
H_dummy = [ones(size(dummy)) cos(dummy*pi/180) sin(dummy*pi/180)];
fit_dummy = H_dummy*est(1:3);

title1 = ['Phase = ' num2str(est(1)) ' + ' num2str(est(2)) '*cos(Heading) + ' num2str(est(3)) '*sin(Heading)'];
title2 = [];
if(length(time) ~= 0)
  for (i = 1 : order)
    title2 = [title2 ' + ' num2str(est(i+3)) '*(t-' num2str(mean(time)) ')^' int2str(i)];
  end
end

ymin = min([min(fit_dummy) min(phase)])-0.5;
ymax = max([max(fit_dummy) max(phase)])+1.0;
TITLE=[TITLE ', std(err)=' num2str(std(err))];
plot(hdg,phase,'*',dummy,fit_dummy)
axis([0 360 ymin ymax]); 
xlabel('Ship''s Heading')
ylabel('Phase')
title(TITLE)
text(180,ymax,title1, 'horizontalalignment', 'center', 'verticalalignment', 'top');
if ~isempty(title2)
  text(180,ymax-.08*(ymax-ymin),title2, 'horizontalalignment', 'center', 'verticalalignment', 'top');
end
grid
axis;

