clear all;
bj=[116.46,39.92,0];
zhzh=[113.7,34.5,0];
wch=[114.32,30.35,0];
chsh=[112.92,28.33,0];
gzh=[113.25,23.13,0];
%選取北京、鄭州、武昌、長沙、廣州五個城市為站點
%設火車出站從零開始加速，速度達到180km/h即50m/s時，開始勻速運動
%假設加速時間為10分鐘
v=50;%m/s
Re=6378137;%米
f=1/298.257;%扁平率
trans=2*pi/360;
%------計算北京到鄭州的距離---------
[Xu_bj,Yu_bj,Zu_bj]=user_ecef(bj(1),bj(2),bj(3));
[Xu_zhzh,Yu_zhzh,Zu_zhzh]=user_ecef(zhzh(1),zhzh(2),zhzh(3));
Disbj_zhzh=sqrt((Xu_bj-Xu_zhzh)^2+(Yu_bj-Yu_zhzh)^2+(Zu_bj-Zu_zhzh)^2);
[Xug_zhzh,Yug_zhzh,Zug_zhzh]=ecef_g(bj(1),bj(2),Xu_zhzh,Yu_zhzh,Zu_zhzh,Xu_bj,Yu_bj,Zu_bj);
%計算在北京的用戶水平坐標系下的鄭州的位置
theta=pi+atan(Yug_zhzh/Xug_zhzh);
% vn=v*sin(theta);
% ve=v*cos(theta);%將速度分解為東向和北向
v0=0;
t0=0;
dt=10;%s
Tacc=600;%s
a=v/Tacc;
while v0<v
    long0=bj(1);
    lat0=bj(2); 
    Rm=Re*(1+2*f-3*f*sin(trans*lat0)^2);
    Rn=Re/((1-f)^2*sin(trans*lat0)^2+cos(trans*lat0)^2);   
    deltalong=(v0*10+100/24)*cos(theta)/(Rm*cos(trans*lat0)*trans);%十秒間隔內經度的增加量
    deltalat=(v0*10+100/24)*sin(theta)/(Rn*trans);%10秒間隔內緯度的增加量
    long0=long0+deltalong;
    lat0=lat0+deltalat;
    t0=t0+dt; 
    v0=v0+a*dt;%
end
   long1=long0;
   lat1=lat0;
    %[Xujs_zhzh,Yujs_zhzh,Zujs_zhzh]=user_ecef(long0,lat0,0);
 % 北京到鄭州勻速
 while v0==v0;%m/s
     vn=v0*sin(theta);
     ve=v0*cos(theta);
     Rm=Re*(1+2*f-3*f*sin(trans*lat1)^2);
     Rn=Re/((1-f)^2*sin(trans*lat1)^2+cos(trans*lat1)^2);   
     deltalong=ve*10/(Rm*cos(trans*lat1)*trans);
     deltalat=vn*10/(Rn*trans);
     long1=long1+deltalong;
     lat1=lat1+deltalat;
     [Xu,Yu,Zu]=user_ecef(long1,lat1,0);
     Dis=sqrt((Xu_bj-Xu)^2+(Yu_bj-Yu)^2+(Zu_bj-Zu)^2);
     if Dis<=Disbj_zhzh-0.5*a*Tacc^2
          t0=t0+dt;
          v0=v;
     else
         t2=t0;
         break;
     end     
   end
long2=long1;
lat2=lat1;
while v0>0%減速進鄭州站
    Rm=Re*(1+2*f-3*f*sin(trans*lat2)^2);
    Rn=Re/((1-f)^2*sin(trans*lat2)^2+cos(trans*lat2)^2);   
    deltalong=(v*dt-a*dt*(t0-t2)-100/24)*cos(theta)/(Rm*cos(trans*lat2)*trans);
    deltalat=(v*dt-a*dt*(t0-t2)-100/24)*sin(theta)/(Rn*trans);
    long2=long2+deltalong;
    lat2=lat2+deltalat;
    t0=t0+dt;
    v0=v-a(t0-t2);
%     [Xu,Yu,Zu]=user_ecef(long2,lat2,0);
%     Dis=sqrt((Xu_bj-Xu)^2+(Yu_bj-Yu)^2+(Zu_bj-Zu)^2);
end