clear all;
cq=[106.45,29.57,0]; % 重慶
ych=[111.3,30.7,0]; % 宜昌
wh=[114.32,30.52,0]; % 武漢
jj=[115.97,29.72,0]; % 九江
shh=[121.4,31.2,0]; % 上海
%選取重慶、宜昌、武漢、九江、上海五個城市為站點；
%設輪船出站從零開始加速，速度達到20m/s，開始勻速運動；
%快要到達下一站點時，開始減速，設減速與加速時間相同；
%假設加速時間為10分鐘；
%按段劃分，重慶到宜昌0，宜昌到武漢1，武漢到九江2，九江到上海3；
%每一段又分為加速0，勻速1，減速2；
%下劃線表示某一點的數值，非下劃線則表示這一段的所有采樣點的矩陣
%例如T0_0,T0_1,T0_2分別表示重慶到宜昌這一段上加速、勻速、減速的起始時間點
%例如lat11,lat12,lat13分別表示宜昌到武漢的加速、勻速、減速段上的所有的采樣點的緯度矩陣
%==============================START=======================================
%-------------------------------重慶到宜昌----------------------------------
[Xu_cq,Yu_cq,Zu_cq]=user_ecef(cq(1),cq(2),cq(3));
[Xu_ych,Yu_ych,Zu_ych]=user_ecef(ych(1),ych(2),ych(3));
Discq_ych=sqrt((Xu_cq-Xu_ych)^2+(Yu_cq-Yu_ych)^2+(Zu_cq-Zu_ych)^2);
[Xug_ych,Yug_ych,Zug_ych]=ecef_g(cq(1),cq(2),Xu_ych,Yu_ych,Zu_ych,...
                                    Xu_cq,Yu_cq,Zu_cq);
theta0 = atan(Yug_ych/Xug_ych);

%-------加速
T0_0=0;
dt=10;
[long01,lat01,v01]=trace_acc(cq(1),cq(2),theta0,T0_0);
%-------勻速
T0_1=dt*length(long01);
V0_1=v01(length(long01));
long0_1=long01(length(long01));
lat0_1=lat01(length(lat01));
[long02,lat02]=trace_even(long0_1,lat0_1,theta0,V0_1,T0_1,...
                          Xu_cq,Yu_cq,Zu_cq,Discq_ych);
%-------減速進站
long0_2=long02(length(long02));
lat0_2=lat02(length(lat02));
T0_2=dt*(length(long02)+length(long01));
[long03,lat03]=trace_dece(long0_2,lat0_2,theta0,T0_2);
%-----------------------------宜昌到武漢------------------------------------
[Xu_wh,Yu_wh,Zu_wh]=user_ecef(wh(1),wh(2),wh(3));
Disych_wh=sqrt((Xu_ych-Xu_wh)^2+(Yu_ych-Yu_wh)^2+(Zu_ych-Zu_wh)^2);
[Xug_wh,Yug_wh,Zug_wh]=ecef_g(ych(1),ych(2),Xu_wh,Yu_wh,Zu_wh,...
                                 Xu_ych,Yu_ych,Zu_ych);
theta1=atan(Yug_wh/Xug_wh);
%------ 加速
T1_0=0;%起始時間可以任意設置，這里我們假設為0，對結果不會造成影響
[long11,lat11,v11]=trace_acc(ych(1),ych(2),theta1,T1_0);
%------勻速
T1_1=dt*length(long11);
V1_1=v11(length(long11));
long1_1=long11(length(long11));
lat1_1=lat11(length(lat11));
[long12,lat12]=trace_even(long1_1,lat1_1,theta1,V1_1,T1_1,...
                          Xu_ych,Yu_ych,Zu_ych,Disych_wh);
%------減速進站
long1_2=long12(length(long12));
lat1_2=lat12(length(lat12));
T1_2=dt*(length(long11)+length(long12));
[long13,lat13]=trace_dece(long1_2,lat1_2,theta1,T1_2);
%----------------------------武漢到九江------------------------------------
[Xu_jj,Yu_jj,Zu_jj]=user_ecef(jj(1),jj(2),jj(3));
Diswh_jj=sqrt((Xu_wh-Xu_jj)^2+(Yu_wh-Yu_jj)^2+(Zu_wh-Zu_jj)^2);
[Xug_jj,Yug_jj,Zug_jj]=ecef_g(jj(1),jj(2),Xu_jj,Yu_jj,Zu_jj,...
                                  Xu_wh,Yu_wh,Zu_wh);
theta2= atan(Yug_jj/Xug_jj);
%-------加速
T2_0=0;
[long21,lat21,v21]=trace_acc(wh(1),wh(2),theta2,T2_0);
%-------勻速
T2_1=dt*length(long21);
V2_1=v21(length(long21));
long2_1=long21(length(long21));
lat2_1=lat21(length(lat21));
[long22,lat22]=trace_even(long2_1,lat2_1,theta2,V2_1,T2_1,...
                          Xu_wh,Yu_wh,Zu_wh,Diswh_jj);
%--------減速進站
long2_2=long22(length(long22));
lat2_2=lat22(length(lat22));
T2_2=dt*(length(long21)+length(long22));
[long23,lat23]=trace_dece(long2_2,lat2_2,theta2,T2_2);
%---------------------------九江到上海-------------------------------------
[Xu_shh,Yu_shh,Zu_shh]=user_ecef(shh(1),shh(2),shh(3));
Disjj_shh=sqrt((Xu_jj-Xu_shh)^2+(Yu_jj-Yu_shh)^2+(Zu_jj-Zu_shh)^2);
[Xug_shh,Yug_shh,Zug_shh]=ecef_g(shh(1),shh(2),Xu_shh,Yu_shh,Zu_shh,...
                                  Xu_jj,Yu_jj,Zu_jj);
theta3=atan(Yug_shh/Xug_shh);
%-------加速
T3_0=0;
[long31,lat31,v31]=trace_acc(jj(1),jj(2),theta3,T3_0);
%-------勻速
T3_1=dt*length(long31);
V3_1=v31(length(long31));
long3_1=long31(length(long31));
lat3_1=lat31(length(lat31));
[long32,lat32]=trace_even(long3_1,lat3_1,theta3,V3_1,T3_1,...
                          Xu_jj,Yu_jj,Zu_jj,Disjj_shh);
%-------減速進站
long3_2=long32(length(long32));
lat3_2=lat32(length(lat32));
T3_2=dt*(length(long31)+length(long32));
[long33,lat33]=trace_dece(long3_2,lat3_2,theta3,T3_2);
%=====根據程序計算出宜昌、武漢、九江、上海的經緯度（也可以計算途中任意采樣點的經緯度）=======
waterlong = [long01,];
waterlat = [];
ychjs=[long03(length(long03)),lat03(length(lat03))]
whjs=[long13(length(long13)),lat13(length(lat13))]
jjjs=[long23(length(long23)),lat23(length(lat23))]
shhjs=[long33(length(long33)),lat33(length(lat33))]