function [ ln, lt, z, ts ] = particle_track_ode_grid3d_LonLat(LonD, LatD, Z, U, V, W, tt, ...
                                                tspan, ccD, options, odesolver )
% PARTICLE_TRACK_ODE_GRID  Generates particle tracks from a set of currents
% defined on a rectangular grid using PDE solver
%
% Usage: [x,y,z,ts] = particle_track_ode_grid2d(X,Y,Z,U,V,W,tt,tspan,cc,options,odesolver )
%
% This function is largely inspired by Bruce Lipphardt's trajectories code.
% The principal difference is the method of interpolating from the grid to
% an arbitrary space time point is different (he uses interpn, I use
% interp2 plus linear interpolation by hand between previous and next
% time of current time).  Also, this assumes that 
%
% Inputs
% ------
% X = x-coordinate of rectangular grid
% Y = Y-coordinate of rectangular grid
% Z = Z-coordinate of rectangular grid
%
% U,V,W = currents at locations in X,Y, and Z.  The fourth dimension should
% be time.  Units should be consistent with those in X and tt.
%
% tt = times at which the currents are defined.  Must have length equal
% to the four dimension of U.
%
% tspan = time period over which to generate tracks.  See ode45 for more
% details on possible tspan formats (some of which might not work with
% this function).
%
% cc = a three column matrix of initial positions for the particles.
%
% options = options for the ODE solver.  See odeset, ode45 and the other
% solvers for details on format and creating.  Defaults to no special options.
%
% odesolver = ode solver function to use.  Defaults to ode45.
%
% OUTPUTS:
%
% x,y,z = coordinates of particles.  Each column is a different starting
% position (row of cc) and each row is a time in ts.
%
% ts = times of positions in x and y.  A column vector.
%
% NOTE: This function appears to be hopelessly slow in matlab R13 (6.5)
% unless you play with the default tolerances of ode45.  See ODESET
% function and, in particular, the absolute and relative tolerances for
% more information.  You might also want to play with MaxStep, the
% maximum time step to take.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if ~exist( 'odesolver', 'var' )
  odesolver = 'ode45';
end

if ~exist( 'options', 'var' )
  options = [];
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Bring Lon and Lat to metric units as U,V and W.
m_proj('mercator');
[LonM,LatM]=m_ll2xy(LonD,LatD);
Earth_Radius = 6378*1000; %in m
Lon=LonM.*Earth_Radius;
Lat=LatM.*Earth_Radius;

% Bring particle geograpical location to metric units (dist from origin)
[ccXm,ccYm]=m_ll2xy(ccD(:,1),ccD(:,2));
ccX=ccXm.*Earth_Radius;
ccY=ccYm.*Earth_Radius;

%reconstruct location matrix
cc=[ ccX(:), ccY(:), ccD(:,3) ];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Do tracking.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
ntraj = size(cc,1);

y0 = [ cc(:,1); cc(:,2); cc(:,3) ];

[ts,A] = feval( odesolver, @ptrack_ode_worker_func3d, tspan, y0, options, ...
		Lon, Lat, Z, U, V, W, tt );

[lnM,ltM,z] = deal( A(:,1:ntraj), A(:,ntraj+1:2*ntraj), A(:,2*ntraj+1:end) );

%%%%%% Back to lon , lat
[ln,lt]=m_xy2ll(lnM./Earth_Radius,ltM./Earth_Radius);

%%%%%%----------------------Subfunctions--------------------%%%%%%%
function f = ptrack_ode_worker_func3d( T, y, X, Y, Z, ux, uy, uz, tt )
% This function will be used to actually do tracking.
% Assumes that T is a scalar.

% Break up X, Y and Z coordinates.
ntraj = length(y)/3;
[xx,yy,zz] = deal( y(1:ntraj), y(ntraj+1:2*ntraj), y(2*ntraj+1:end) );

% Find timesteps cooresponding to T
d = tt - T;

% If outside time range, return NaNs
if all(d<0) | all(d>0)
  f = repmat(NaN,size(y));
  return
end

% Otherwise, linearly interpolate.
dlt = max( d( d<0 ) );
dgt = min( d( d>=0 ) );
if dgt == 0
  [ux,uy,uz] = deal( ux( :, :, :, d == 0 ), uy( :, :, :, d == 0 ), ...
      uz( :, :, :, d == 0 ) ); 
else  
  ilt = d == dlt;
  igt = d == dgt;
  dd = dgt - dlt;
  clt = dgt / dd;
  cgt = -dlt / dd;
  ux = clt * ux( :, :, :, ilt ) + cgt * ux( :, :, :, igt );
  uy = clt * uy( :, :, :, ilt ) + cgt * uy( :, :, :, igt );
  uz = clt * uz( :, :, :, ilt ) + cgt * uz( :, :, :, igt );
end

% Then fit to specific points of interest.
nn = ~isnan(xx) & ~isnan(yy) & ~isnan(zz);
[UX,UY,UZ] = deal( repmat( NaN, size(xx) ) );

UX(nn) = interp3( X, Y, Z, ux, xx(nn), yy(nn), zz(nn), 'linear' );
UY(nn) = interp3( X, Y, Z, uy, xx(nn), yy(nn), zz(nn), 'linear' );
UZ(nn) = interp3( X, Y, Z, uz, xx(nn), yy(nn), zz(nn), 'linear' );

% UX(nn) = interp3( X, Y, Z, ux, xx(nn), yy(nn), zz(nn), 'cubic' ); % Out of memory
% UY(nn) = interp3( X, Y, Z, uy, xx(nn), yy(nn), zz(nn), 'cubic' ); % Out of memory
% UZ(nn) = interp3( X, Y, Z, uz, xx(nn), yy(nn), zz(nn), 'cubic' ); % Out of memory

f = [ UX(:); UY(:); UZ(:) ];
