clear;clc
flag=-1; %% flag=1/-1 forward/backward tracing

FILENAME='/hao/aim2/jingliu/tiegcm_trunk/tiegcm_hao/tiegcm_AEH.nc';
lon= ncread(FILENAME,'lon');
lat=ncread(FILENAME,'lat');
lev=ncread(FILENAME,'ilev');
ut=ncread(FILENAME,'ut');
ZG=double(ncread(FILENAME,'ZG')./10^5);  %% km
UN=double(ncread(FILENAME,'UN')./100);   %% NEUTRAL ZONAL WIND (+EAST)
VN=double(ncread(FILENAME,'VN')./100);   %% northward meridional wind cm/s
WN1=double(ncread(FILENAME,'WN')./100);  %% vertical wind cm/s
O1=double(ncread(FILENAME,'O1'));
O2=double(ncread(FILENAME,'O2'));
TN=double(ncread(FILENAME,'TN'));

timestep=600; %second

%ADJUST VERTICAL WINDS to W/H, where H is the scale height
AMBAR=1./((O2./32)+(O1./16)+(1-O2-O1)./28);
SHGT=8314.*TN./(AMBAR.*8.7);
SHGT(SHGT>10^5)=nan;
WN=WN1./SHGT;
diffUT=diff(ut);

% figure;
% subplot(211); plot(squeeze(SHGT(1,1,:,3)),squeeze(ZG(1,1,:,3)));
% xlabel('Scale height (m)')
% subplot(212); plot(squeeze(WN(1,1,:,3)),squeeze(ZG(1,1,:,3)));
% xlabel('Scaled Wn (s-1)')

[longrid,latgrid]=meshgrid(lon,lat);
%% Initial Position
lat0=50; lon0=0; lev0=-2; ut0=ut(end); lt0=ut0+lon0/15;
%lat0=65; lon0=120; lev0=-2; ut0=ut(end); lt0=ut0+lon0/15;
%lat0=65; lon0=-110; lev0=-2; ut0=ut(end); lt0=ut0+lon0/15;
RE=6378; % km

[rowi0,coli0] = near2(longrid,latgrid,lon0,lat0);
difflev=abs(lev-lev0);
hpos0=find(difflev==min(difflev));
X1=abs(lon(coli0(1))-lon0);
Y1=abs(lat(rowi0(1))-lat0);

%FIND NEAREST NEIGHBOUR POINTS using the initial conditions for lat lon
IPK1 = floor((lon0+180)./2.5) + 1;
IPK1 =mod(IPK1+144,144);
if (IPK1 == 0); IPK1=IPK1 + 144;end
IPKHOLD = IPK1;
IPK2 = mod(IPK1+1,144);
if (IPK2 == 0); IPK2 = IPK2 + 144;end

% IPK3 and IPK4 are used to make transpolar calculations easy
IPK3 = IPK1; IPK4 = IPK2;

JPK1 = floor((lat0+88.75)/2.5);
JPK2 = JPK1+1;
KPK1 = floor((lev0+7)*4);
KPK2 = KPK1 + 1;

S1 = sqrt((2.5-X1)^2 + (2.5-Y1)^2  + 0.0000001);
S2 = sqrt((2.5-X1)^2 + Y1^2  + 0.000001);
S3 = sqrt(X1^2 + (2.5-Y1)^2 +0.000001);
S4 = sqrt(X1^2 + Y1^2+0.000001);
S5 = sqrt((2.5-X1)^2 + (2.5-Y1)^2 +0.000001);
S6 = sqrt((2.5-X1)^2 + Y1^2 + 0.000001);
S7 = sqrt(X1^2 + (2.5-Y1)^2 + 0.000001);
S8 = sqrt(X1^2 + Y1^2 + 0.000001);
AAA = 1.0/((1./S1) + (1./S2) + (1./S3) + (1./S4));

Z1 = abs(lev0-lev(hpos0(1))+1);
W1 = AAA/S1 *Z1; W2 = AAA/S2 *Z1;
W3 = AAA/S3 *Z1; W4 = AAA/S4 *Z1;
W5 = AAA/S5 *(1-Z1);
W6 = AAA/S6 *(1-Z1);
W7 = AAA/S7 *(1-Z1);
W8 = AAA/S8 *(1-Z1);

UPKT = (W1*UN(IPK1,JPK1,KPK1,end)+W2*UN(IPK3,JPK2,KPK1,end) +...
    W3*UN(IPK2,JPK1,KPK1,end)+W4*UN(IPK4,JPK2,KPK1,end) +...
    W5*UN(IPK1,JPK1,KPK2,end)+W6*UN(IPK3,JPK2,KPK2,end) +...
    W7*UN(IPK2,JPK1,KPK2,end)+W8*UN(IPK4,JPK2,KPK2,end));

VPKT = (W1*VN(IPK1,JPK1,KPK1,end)+W2*VN(IPK3,JPK2,KPK1,end) +...
    W3*VN(IPK2,JPK1,KPK1,end)+W4*VN(IPK4,JPK2,KPK1,end) +...
    W5*VN(IPK1,JPK1,KPK2,end)+W6*VN(IPK3,JPK2,KPK2,end) +...
    W7*VN(IPK2,JPK1,KPK2,end)+W8*VN(IPK4,JPK2,KPK2,end));

WPKT = (W1*WN(IPK1,JPK1,KPK1,end)+W2*WN(IPK3,JPK2,KPK1,end)+...
    W3*WN(IPK2,JPK1,KPK1,end)+W4*WN(IPK4,JPK2,KPK1,end)+...
    W5*WN(IPK1,JPK1,KPK2,end)+W6*WN(IPK3,JPK2,KPK2,end)+...
    W7*WN(IPK2,JPK1,KPK2,end)+W8*WN(IPK4,JPK2,KPK2,end));

WPKT1 = (W1*WN1(IPK1,JPK1,KPK1,end)+W2*WN1(IPK3,JPK2,KPK1,end)+...
    W3*WN1(IPK2,JPK1,KPK1,end)+W4*WN1(IPK4,JPK2,KPK1,end)+...
    W5*WN1(IPK1,JPK1,KPK2,end)+W6*WN1(IPK3,JPK2,KPK2,end)+...
    W7*WN1(IPK2,JPK1,KPK2,end)+W8*WN1(IPK4,JPK2,KPK2,end));

ZGKT = (W1*ZG(IPK1,JPK1,KPK1,end)+W2*ZG(IPK3,JPK2,KPK1,end)+...
    W3*ZG(IPK2,JPK1,KPK1,end)+W4*ZG(IPK4,JPK2,KPK1,end)+...
    W5*ZG(IPK1,JPK1,KPK2,end)+W6*ZG(IPK3,JPK2,KPK2,end)+...
    W7*ZG(IPK2,JPK1,KPK2,end)+W8*ZG(IPK4,JPK2,KPK2,end));

dx=UPKT*timestep*flag;
dy=VPKT*timestep*flag;
dz=WPKT*timestep*flag;
dz1=WPKT1*timestep*flag;

phi=lon0/180*pi;ele=pi/2-lat0/180*pi;
[X,Y,Z] = sph2cart(phi,ele,RE+ZGKT);
Xnew=X+dx./10^3;
Ynew=Y+dy./10^3;
Znew=Z+WPKT1*timestep*flag./10^3;

[phinew,elenew,Rnew] = cart2sph(Xnew,Ynew,Znew);
lonnew=phinew*180/pi;
latnew=(pi/2-elenew)*180/pi;
Prelevnew=lev0+dz;
Znew=ZGKT+WPKT1*timestep*flag./10^3;

if (latnew > 90);
    lonnew=180+lonnew;
    latnew=180-latnew;
end
if (latnew < -90)
    lonnew=180+lonnew;
    latnew=-180-latnew;
end
if(lonnew < -180);lonnew =lonnew +360; end
if(lonnew > 180); lonnew =lonnew -360; end

%% Backward Tracing
lat_track=nan(size(ut));
lon_track=lat_track;
H_track=lat_track;
Pressure_track=lat_track;
LT_track=lat_track;
lat_track(end)=latnew; lon_track(end)=lonnew;
H_track(end)=Znew;     Pressure_track(end)=Prelevnew;
LT_track(end)=lt0;

for it=length(ut)-1:-1:1
    %FIND NEAREST NEIGHBOUR POINTS using the initial conditions for lat lon
    IPK1 = floor((lonnew+180)./2.5) + 1;
    IPK1 =mod(IPK1+144,144);
    if (IPK1 == 0); IPK1=IPK1 + 144;end
    IPK2 = mod(IPK1+1,144);
    if (IPK2 == 0); IPK2 = IPK2 + 144;end
    IPKHOLD = IPK1;
    JPKHOLD = JPK1;
    
%     for JJJ=1:6
%         for KKK=1:4
%             for III=1:6
%                 I=IPKHOLD + III - 3;
%                 if(I > 144);I=I-144;end
%                 if(I < 1); I=I+144;end
%                 J=JPKHOLD + JJJ - 3;
%                 AJPT = 1;
%                 if (J > 144)
%                     I = MOD(IPK1+73,144) + 3 - III;
%                 end
%                 if (J < 1)
%                     I = MOD(IPK1+73,144) + 3 - III;
%                 end
%                 if (I > 144);I=I-144;end
%                 if (I < 1);I=I+144;end
%                 if (J > 72);J=145-J;end
%                 if (J < 1);J=1-J;end
%                 K=KPK1+KKK-2;
%                 %
%                 %                 AMBAR=1./((O2(I,J,K,it)/32)+(O1(I,J,K,it)/16)+((1-O2(I,J,K,it)-O1(I,J,K,it))/28));
%                 %                 SHGT=8314.*TN(I,J,K,it)/(AMBAR*8.7);
%                 %                 WN(I,J,K,it)=W(I,J,K,it)/SHGT;
%             end
%         end
%     end
    
    % IPK3 and IPK4 are used to make transpolar calculations easy
    IPK3 = IPK1;
    IPK4 = IPK2;
    
    JPK1 = floor((latnew+88.75)/2.5); JPK2 = JPK1 + 1;
    KPK1 = floor((Prelevnew+7)*4); 
    if KPK1<1; KPK1=KPK1+1; end
    KPK2 = KPK1 + 1;
    
    S1 = sqrt((2.5-X1)^2 + (2.5-Y1)^2  + 0.0000001);
    S2 = sqrt((2.5-X1)^2 + Y1^2  + 0.000001);
    S3 = sqrt(X1^2 + (2.5-Y1)^2 +0.000001);
    S4 = sqrt(X1^2 + Y1^2+0.000001);
    S5 = sqrt((2.5-X1)^2 + (2.5-Y1)^2 +0.000001);
    S6 = sqrt((2.5-X1)^2 + Y1^2 + 0.000001);
    S7 = sqrt(X1^2 + (2.5-Y1)^2 + 0.000001);
    S8 = sqrt(X1^2 + Y1^2 + 0.000001);
    AAA = 1.0/((1./S1) + (1./S2) + (1./S3) + (1./S4));
    
    Z1 = abs(KPK1-(Prelevnew+7)*4+1);
    W1 = AAA/S1 *Z1; W2 = AAA/S2 *Z1;
    W3 = AAA/S3 *Z1; W4 = AAA/S4 *Z1;
    W5 = AAA/S5 *(1-Z1);
    W6 = AAA/S6 *(1-Z1);
    W7 = AAA/S7 *(1-Z1);
    W8 = AAA/S8 *(1-Z1);
    
    UPKT = (W1*UN(IPK1,JPK1,KPK1,it)+W2*UN(IPK3,JPK2,KPK1,it) +...
        W3*UN(IPK2,JPK1,KPK1,it)+W4*UN(IPK4,JPK2,KPK1,it) +...
        W5*UN(IPK1,JPK1,KPK2,it)+W6*UN(IPK3,JPK2,KPK2,it) +...
        W7*UN(IPK2,JPK1,KPK2,it)+W8*UN(IPK4,JPK2,KPK2,it));
    
    VPKT = (W1*VN(IPK1,JPK1,KPK1,it)+W2*VN(IPK3,JPK2,KPK1,it) +...
        W3*VN(IPK2,JPK1,KPK1,it)+W4*VN(IPK4,JPK2,KPK1,it) +...
        W5*VN(IPK1,JPK1,KPK2,it)+W6*VN(IPK3,JPK2,KPK2,it) +...
        W7*VN(IPK2,JPK1,KPK2,it)+W8*VN(IPK4,JPK2,KPK2,it));
    
    WPKT = (W1*WN(IPK1,JPK1,KPK1,it)+W2*WN(IPK3,JPK2,KPK1,it)+...
        W3*WN(IPK2,JPK1,KPK1,it)+W4*WN(IPK4,JPK2,KPK1,it)+...
        W5*WN(IPK1,JPK1,KPK2,it)+W6*WN(IPK3,JPK2,KPK2,it)+...
        W7*WN(IPK2,JPK1,KPK2,it)+W8*WN(IPK4,JPK2,KPK2,it));    %% scaled W
    
    WPKT1 = (W1*WN1(IPK1,JPK1,KPK1,it)+W2*WN1(IPK3,JPK2,KPK1,it)+...
        W3*WN1(IPK2,JPK1,KPK1,it)+W4*WN1(IPK4,JPK2,KPK1,it)+...
        W5*WN1(IPK1,JPK1,KPK2,it)+W6*WN1(IPK3,JPK2,KPK2,it)+...
        W7*WN1(IPK2,JPK1,KPK2,it)+W8*WN1(IPK4,JPK2,KPK2,it));  %% real W
    
    ZGKT = (W1*ZG(IPK1,JPK1,KPK1,it)+W2*ZG(IPK3,JPK2,KPK1,end)+...
        W3*ZG(IPK2,JPK1,KPK1,it)+W4*ZG(IPK4,JPK2,KPK1,it)+...
        W5*ZG(IPK1,JPK1,KPK2,it)+W6*ZG(IPK3,JPK2,KPK2,it)+...
        W7*ZG(IPK2,JPK1,KPK2,it)+W8*ZG(IPK4,JPK2,KPK2,it));
    
    dx=UPKT*timestep*flag./10^3;  %% zonal direction
    dy=VPKT*timestep*flag./10^3;  %% meridonal direction
    dz=WPKT*timestep*flag;
    dz1=WPKT1*timestep*flag./10^3;
    
    dlon=radtodeg(dx/(RE+ZGKT)/cos(latnew*pi/180));
    dlat=radtodeg(dy/(RE+ZGKT));
    
    lonnew=lonnew+dlon;
    latnew=latnew+dlat;
    Prelevnew=Prelevnew+dz;
    Znew=Znew+dz1;
    
%     phi=lonnew/180*pi;ele=pi/2-latnew/180*pi;
%     [X,Y,Z] = sph2cart(phi,ele,RE+ZGKT);
%     Xnew=X+dx; Ynew=Y+dy;
%     Znew=Znew+WPKT1*timestep*flag;
%     
%     [phinew,elenew,Rnew] = cart2sph(Xnew,Ynew,Znew);
%     lonnew=phinew*180/pi;
%     latnew=(pi/2-elenew)*180/pi;
%     Prelevnew=Prelevnew+dz;
    
%     ALNGX=lonnew; 
%     ALATGX=latnew;
%     AHEM=latnew/abs(latnew);
%     for iloop=1:2
%         % SOLVE SPHERICAL TRIANGLE TO OBTAIN COORDS OF NEW TRAJ POINT
%         VMAG = sqrt(UPKT^2 + VPKT^2);
%         
%         % ANGULAR DISTANCE TRAVELLED:
%         BETA = VMAG*timestep/RE;
%         ALMONE = degtorad(-(ALNGX-lonnew));
%         UCLEN = UPKT*cos(ALMONE)-AHEM*VPKT*sin(ALMONE);
%         VCLEN = AHEM*UPKT*sin(ALMONE) + VPKT*cos(ALMONE);
%         
%         %If backward trajectories are being calculated, wind velocities affect the parcel in the opposite sense
%         ALPHA = atan(UCLEN/VCLEN);
%         THNEW = asin(sin(degtorad(ALATGX))*cos(BETA)+cos(degtorad(ALATGX))*sin(BETA)*cos(ALPHA));
%         XXX = sin(BETA) * sin(ALPHA);
%         YYY = cos(degtorad(ALATGX))*cos(BETA)-(sin(degtorad(ALATGX))*sin(BETA)*cos(ALPHA));
%         GLNEW = ALNGX + degtorad(atan(XXX/YYY));
%         ALATGX = radtodeg(THNEW);
%         ALNGX = GLNEW;
%         Prelevnew=Prelevnew+timestep/2.0*WPKT*flag;
%         
%         if (ALATGX > 90);
%             ALNGX=180+ALNGX;
%             ALATGX=180-ALATGX;
%         end
%         if (ALATGX < -90)
%             ALNGX=180+ALNGX;
%             ALATGX=-180-ALATGX;
%         end
%         
%         if(ALNGX < -180);ALNGX =ALNGX+360; end
%         if(ALNGX > 180); ALNGX =ALNGX-360; end
%         
%     end
%     lonnew=radtodeg(ALNGX); 
%     latnew=radtodeg(ALATGX);
    
    if abs(Prelevnew)>8; Prelevnew=nan;end
    
        if (latnew > 90);
            lonnew=180+lonnew;
            latnew=180-latnew;
        end
        if (latnew < -90)
            lonnew=180+lonnew;
            latnew=-180-latnew;
        end
        if(lonnew < -180);lonnew =lonnew +360; end
        if(lonnew > 180); lonnew =lonnew -360; end
    
    lat_track(it)=latnew;
    lon_track(it)=lonnew;
    H_track(it)=Znew;
    Pressure_track(it)=Prelevnew;
    lt=ut(it)+lonnew./15;
    lt=mod(lt,24); 
    if lt<0; lt=lt+24; end
    LT_track(it)=lt;
%     
     theta=(LT_track(it)*15-90)*pi/180;
     radio=abs(90-lat_track(it));
     
     x=radio.*cos(theta);
     y=radio.*sin(theta);
     
     subplot(211)
     plot(x,y,'^') ; hold on
     text(x,y,num2str(ut(it))); hold on
     
     if it==length(ut)-1
         for latt=40:10:80
             theta_c=-pi:pi/100:pi;
             rr_c=(90-latt).*ones(size(theta_c));
             H1=polar(theta_c,rr_c,'k'); hold on; box off
         end
     end
     
     subplot(212)
     plot(ut(it),H_track(it),'^'); hold on

%     plot(LT_track(it),lat_track(it),'.'); hold on;
%     text(LT_track(it),lat_track(it)-1,num2str(ut(it))); hold on
%     set(gca,'xlim',[0 24],'ylim' ,[50 90])
end

% figure;
% theta=(LT_track-12)./12.*pi;
% radio=(90-lat_track).*cos(theta);
% polar(theta,radio,'k.'); hold on

% x=radio.*cos(theta);
% y=radio.*sin(theta);
% 
% subplot(211)
% plot(x,y,'.'); hold on;
% plotion; view(-90,90)
% 
% subplot(212)
% plot(lon_track,lat_track,'.')
