% EDDYFIT  Merge the functionality of map_argos_blair and fl_pos_fit_blair
%  Construct a continuous record of eddy center position and rotational
%  velocity (as well as drifter radius) from each 100m-drogued drifter
%  trajectory.
%
%

drogues = {'52484' '52486' '52485'};
%drogues = {'52486' '52484' '52485'};
filenames = {'l_argos_pos.txt' 'drft_shp.txt' 'drift_52485.txt'};
%filenames = {'drft_shp.txt' 'l_argos_pos.txt' 'drift_52485.txt'};

do_plot = 0;
do_pause = 0;
do_save = 1;
do_disp = 0;
save_smoothed = 0;

timewin = 7; % number of days in window
timestep = 0.5; % number of days to slide window

for ii = 1:length(drogues);
%for ii = 3;
  drogue2use = drogues{ii}

  clear dtnum Lonmean Latmean
  
%%%%%%%%%%%%%% LOAD THE 52484 POSITION DATA %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% load the ARGOS beacon 52484 position data for the drouge deployed at the end of Tracer 1
%disp(['starting to textread ' filenames{ii} ]);
[sn, val, val1, val2, s1, val3, s2, s3, lat, lon, n, val4] = ...
    textread(filenames{ii},'%d %d %d %d %c %d %s %s %f %f %f %d');
%disp('done with textread')

if strcmp(drogue2use,'52486'),
  lon = lon(23:end);
  lat = lat(23:end);
  s1 = s1(23:end);
  s2 = s2(23:end);
  s3 = s3(23:end);
end

%%%%%%%%%%%%%% RESAMPLE THE TIME SERIES TO REMOVE BAD/DUPLICATE POINTS %%%%%%%%%%%%%%%%%%%%%%
% convert longitude to degrees west
ln=lon-360;
% eliminate some bad data points from the time series
lx=find(ln< -67);
% resample longitude and latitude to account for removal of bad data points
Lonlx = ln(lx);
Latlx = lat(lx);

% Cull the date and time from the positions file
for mm = 1:length(s1)
    dtnum(mm) = datenum([s2{mm} ' ' s3{mm}],'yyyy-mm-dd HH:MM:SS');
end
% remove the date/time points that correspond to -67deg longitude cut off
dtnum = dtnum(lx);

% remove duplicate ARGOS fixes from the time series and use the index ind1 from this
% to resample the longitude and latitude time series
[unique_dtnum, ind1, ind2]  = unique(dtnum);
unique_Lonlx = Lonlx(ind1);
unique_Latlx = Latlx(ind1);


%%%%%%%%%%%%%% FILTER THE TIME SERIES TO REMOVE HIGH FREQUENCY NOISE %%%%%%%%%%%%%%%%%%%%%%
% filter the longitude and latitude time series to remove some of the high frequency noise
% run a 5-point median filter for the data
Lon = medfilt1(unique_Lonlx,5);
Lat = medfilt1(unique_Latlx,5);
% 3-point boxcar filter for data
for kk = 2:length(unique_Lonlx)-1
    Lonmean(kk-1) = mean(unique_Lonlx(kk-1:kk+1));
    Latmean(kk-1) = mean(unique_Latlx(kk-1:kk+1));
end

%%%%%%%%%%%%%% CONVERT DATE TO YEAR DAY FOR 2005 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
frstday=datenum(2005,1,1,0,0,0);
netda=unique_dtnum-frstday;
netda=netda+1;
mld = unique_dtnum;


if save_smoothed
%%%%%%%%%%%%% OUTPUT FILTERED TIME SERIES TO FILE %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%fid = fopen('Drogue_52484_smoothed.txt','w');
oname = ['Drogue_' drogue2use '_smoothed.txt'];
fid = fopen(oname,'w');
for pp = 1:length(Lat)
    fprintf(fid,'%f %f %f \n',netda(pp),Lon(pp),Lat(pp));
end
fclose(fid);
end

% Now drifter data are in Lat, Lon, mld

remd = timestep - rem((max(mld)-min(mld)-timewin),timestep);
mld_mid = [min(mld)+timewin/2-remd/2:timestep:max(mld)-timewin/2+remd/2];

[lon_fit,lat_fit,omega_fit,rad_fit,theta_fit,u_fit,v_fit] = deal(mld_mid*NaN);

% Loop over windows in timeseries
for jj = 1:length(mld_mid),
  

  % Compile initial guesses
  iwin = find(mld>=mld_mid(jj)-timewin/2 & mld<=mld_mid(jj)+timewin/2);
  twin = mld(iwin)';
  lonwin = Lon(iwin);
  latwin = Lat(iwin);

  lonref = mean(lonwin);
  latref = mean(latwin);
  
phi = latref*pi/180;
xwin = 111000/cos(phi)*(lonwin - lonref); % convert from degrees to distance in m
ywin = 111000*(latwin-latref);
px = polyfit(twin-mld_mid(jj),xwin,1);
u0 = px(1)/86400; x00 = px(2);
xres = xwin - (x00 + u0*(twin-mld_mid(jj))*86400);
py = polyfit(twin-mld_mid(jj),ywin,1);
v0 = py(1)/86400; y00 = py(2);
yres = ywin - (y00 + v0*(twin-mld_mid(jj))*86400);
%r = sqrt(2)*min(std(xwin),std(ywin));
r0 = sqrt(2)*mean([std(xres) std(yres)]);

omega0 = -1.1e-5;
xr0 = interp1(twin,xres,mld_mid(jj));
yr0 = interp1(twin,yres,mld_mid(jj));
th0 = atan2(yr0,xr0);

if do_disp
  %th0
  %u0
  %v0
  %datestr(mld_mid(jj))
  %y00
  %x00
end

% create a vector to pass to the fitting routine
Z0=[x00 y00 r0 th0 omega0 u0 v0];

% set options for the fminsearch function and call this function
options = optimset('MaxFunEvals',1e20,'MaxIter',1e20);
%[X,fval,exitflag,output]=fminsearch(@my_fun_blair,Z0,options,drogue2use);
%disp('starting fminsearch...');
[X,fval,exitflag,output]=fminsearch('fit_sqdist',Z0,options,twin-mld_mid(jj),xwin,ywin);
%disp('done with fminsearch');

% Display results of the fit on the command line
xout = X(1);
yout = X(2);
rad_fit(jj) = X(3);
theta_fit(jj) = X(4);
omega_fit(jj) = X(5);
u_fit(jj) = X(6);
v_fit(jj) = X(7);

lon_fit(jj) = lonref + xout/111000*cos(phi);
lat_fit(jj) = yout/111000 + latref;


if do_plot

  phi = latref*pi/180; % rad
%x=111000/cos(phi)*(lon - lonref); % m
%y=111000*(lat-latref); % m
% make a single value
z=xwin+i*ywin;  

% reference time
t0 = mld_mid(jj);
% convert to seconds
tsec_ref = (twin-t0)*3600*24;

clf
zfun0 = ((x00+i*y00)+ r0 * exp(i*(th0+omega0*tsec_ref))... 
    + (u0+i*v0)*tsec_ref ); 
zfun = ((xout+i*yout) + ...
    rad_fit(jj) * exp(i*(theta_fit(jj)+omega_fit(jj)*tsec_ref)) + ... 
    (u_fit(jj)+i*v_fit(jj))*tsec_ref ); 
plot([xwin real(zfun)]',[ywin imag(zfun)]','y.-'); hold on;
plot(real(zfun),imag(zfun),'.-',xwin,ywin,'.-',real(zfun0),imag(zfun0));
grid on

plot(x00 + u0*(twin-mld_mid(jj))*86400,y00 + v0*(twin-mld_mid(jj))*86400,'k.');
plot(x00,y00,'rp',[x00 x00+xr0],[y00 y00+yr0],'r--');

set(gca,'dataaspectratio',[1 cos(latref*pi/180) 1]);
drawnow
end % if do_plot

if do_disp
%nlon = (xout+uout*(now-t0-datenum(2005,1,1,0,0,0))*86400)/111000*cos(phi) + lonref;
%nlat = (yout+vout*(now-t0-datenum(2005,1,1,0,0,0))*86400)/111000 + latref;
disp(['Drogue ' drogue2use]);
disp(['Window: ' datestr(twin(1)) ' to ' datestr(twin(end)) ]);
disp(['Eddy center: ' num2str(lat_fit(jj)) ' N, ' num2str(-lon_fit(jj)) ' W']);
disp(['Rotation period: ' num2str(2*pi/(-omega_fit(jj)*86400)) ' days']);
disp(['Eddy speed: U=' num2str(u_fit(jj),2) ' m/s, V=' num2str(v_fit(jj),2) ' m/s']);
end

if do_pause
  disp('paused'); pause
end

end

if do_save
% save accumulated eddy timeseries
ofile = ['edfit_' drogue2use '.mat'];
save(ofile,'mld_mid','lon_fit','lat_fit','omega_fit','rad_fit',...
    'theta_fit','u_fit','v_fit','timewin','timestep');
end

end
