Downside_Calculate_Result_Set1 = zeros(1, length(All_Intersect_Data_Phi_Radian));
Downside_Calculate_Result_Set2 = zeros(1, length(All_Intersect_Data_Phi_Radian));
% FSolve的選項
FSolve_Options = optimset('Display','off');  % Turn off Display
for w1 = 1 : 1 : length(All_Intersect_Data_Phi_Radian)
    % 按照第一種方式求解
    % 賦初值給Phi，這個是精確值
    Phi_Radian_Tmp = All_Intersect_Data_Phi_Radian(w1);
    % 賦初值給Theta
    Theta_Radian_Tmp = Downside_Intersect_Data_Theta_Radian(w1);
    [Theta_Radian_Tmp,Fval,exitflag] = fsolve(@CalThetaFun1, Theta_Radian_Tmp, FSolve_Options);
    Downside_Theta_Search_Result_Set1(1, w1) = Theta_Radian_Tmp;
    % 對求解結果進行驗證
    Tmp = CalThetaFun1(Theta_Radian_Tmp);
    Downside_Calculate_Result_Set1(1, w1) = Tmp;

    % 按照第二種方式求解
    % 賦初值給Phi，這個是精確值
    Phi_Radian_Tmp = All_Intersect_Data_Phi_Radian(w1);
    % 賦初值給Theta
    Theta_Radian_Tmp = Downside_Intersect_Data_Theta_Radian(w1);
    [Theta_Radian_Tmp,Fval,exitflag] = fsolve(@CalThetaFun2, Theta_Radian_Tmp, FSolve_Options);
    Downside_Theta_Search_Result_Set2(1, w1) = Theta_Radian_Tmp;
    % 對求解結果進行驗證
    Tmp = CalThetaFun2(Theta_Radian_Tmp);
    Downside_Calculate_Result_Set2(1, w1) = Tmp;
end;
figure;
subplot(2,2,1);
plot(All_Intersect_Data_Phi_Radian, Downside_Calculate_Result_Set1);
title('逆向驗證結果(1)');
subplot(2,2,2);
plot(All_Intersect_Data_Phi_Radian, Downside_Theta_Search_Result_Set1);
xlabel('Phi');
ylabel('Theta');
title('對應求解得到的Theta(1)');
subplot(2,2,3);
plot(All_Intersect_Data_Phi_Radian, Downside_Calculate_Result_Set2);
title('逆向驗證結果(2)');
subplot(2,2,4);
plot(All_Intersect_Data_Phi_Radian, Downside_Theta_Search_Result_Set2);
xlabel('Phi()');
ylabel('Theta()');

% 選擇第二種方式的精確求解結果
Accurate_Intersect_Data_Phi_Radian = All_Intersect_Data_Phi_Radian;
% 獲得的角度值
Accurate_Intersect_Data_Upside_Theta_Radian = Upside_Theta_Search_Result_Set2;
Accurate_Intersect_Data_Downside_Theta_Radian = Downside_Theta_Search_Result_Set2;
% 逆向驗證的計算結果
Upside_Calculate_Result_Set = Upside_Calculate_Result_Set2;
Downside_Calculate_Result_Set = Downside_Calculate_Result_Set2;


% 根據逆向驗證的結果判斷是否需要對求解結果進行前后擴展，使得相交區域完整
% 對前部和后部進行足夠的擴展
Extend_Width_Tmp = 20;
Accurate_Intersect_Data_Phi_Radian_Tmp = zeros(1, Extend_Width_Tmp*2+...
    length(Accurate_Intersect_Data_Phi_Radian));
Accurate_Intersect_Data_Upside_Theta_Radian_Tmp = zeros(size(Accurate_Intersect_Data_Phi_Radian_Tmp));
Accurate_Intersect_Data_Downside_Theta_Radian_Tmp = zeros(size(Accurate_Intersect_Data_Phi_Radian_Tmp));
Upside_Calculate_Result_Set_Tmp = zeros(size(Accurate_Intersect_Data_Phi_Radian_Tmp));
Downside_Calculate_Result_Set_Tmp = zeros(size(Accurate_Intersect_Data_Phi_Radian_Tmp));

Accurate_Intersect_Data_Phi_Radian_Tmp(Extend_Width_Tmp+1 : end-Extend_Width_Tmp) = ...
    Accurate_Intersect_Data_Phi_Radian(1:end); 
Accurate_Intersect_Data_Upside_Theta_Radian_Tmp(Extend_Width_Tmp+1 : end-Extend_Width_Tmp) = ...
    Accurate_Intersect_Data_Upside_Theta_Radian(1:end);  
Accurate_Intersect_Data_Downside_Theta_Radian_Tmp(Extend_Width_Tmp+1 : end-Extend_Width_Tmp) = ...
    Accurate_Intersect_Data_Downside_Theta_Radian(1:end);  
Upside_Calculate_Result_Set_Tmp(Extend_Width_Tmp+1 : end-Extend_Width_Tmp) = ...
    Upside_Calculate_Result_Set(1:end);  
Downside_Calculate_Result_Set_Tmp(Extend_Width_Tmp+1 : end-Extend_Width_Tmp) = ...
    Downside_Calculate_Result_Set(1:end);  

% 對Phi坐標范圍進行前后的擴展
Phi_Radian_Step_Tmp = Accurate_Intersect_Data_Phi_Radian(2) - ...
    Accurate_Intersect_Data_Phi_Radian(1);
Tmp_Front = Accurate_Intersect_Data_Phi_Radian(1) - (1 : 1 : Extend_Width_Tmp)*...
    Phi_Radian_Step_Tmp;
Tmp_Rear = Accurate_Intersect_Data_Phi_Radian(end) + (1 : 1 : Extend_Width_Tmp)*...
    Phi_Radian_Step_Tmp;
Accurate_Intersect_Data_Phi_Radian_Tmp(1 : Extend_Width_Tmp) = Tmp_Front;
Accurate_Intersect_Data_Phi_Radian_Tmp(end-Extend_Width_Tmp+1 : end) = Tmp_Rear;
% 對Theta坐標的前部進行擴展
for w1 =  Extend_Width_Tmp : -1 : 1
    % 賦初值給Phi，這個是精確值
    Phi_Radian_Tmp = Accurate_Intersect_Data_Phi_Radian_Tmp(w1);
    % 對上部進行擴展
    % 賦初值給Theta
    Theta_Radian_Tmp = Accurate_Intersect_Data_Upside_Theta_Radian_Tmp(w1+1);
    [Theta_Radian_Tmp,Fval,exitflag] = fsolve(@CalThetaFun2, Theta_Radian_Tmp, FSolve_Options);
    Accurate_Intersect_Data_Upside_Theta_Radian_Tmp(1, w1) = Theta_Radian_Tmp;
    % 對求解結果進行驗證
    Tmp = CalThetaFun2(Theta_Radian_Tmp);
    Upside_Calculate_Result_Set_Tmp(1, w1) = Tmp;

    % 對下部進行擴展
    % 賦初值給Theta
    Theta_Radian_Tmp = Accurate_Intersect_Data_Downside_Theta_Radian_Tmp(w1+1);
    [Theta_Radian_Tmp,Fval,exitflag] = fsolve(@CalThetaFun2, Theta_Radian_Tmp, FSolve_Options);
    Accurate_Intersect_Data_Downside_Theta_Radian_Tmp(1, w1) = Theta_Radian_Tmp;
    % 對求解結果進行驗證
    Tmp = CalThetaFun2(Theta_Radian_Tmp);
    Downside_Calculate_Result_Set_Tmp(1, w1) = Tmp;
end;

% 對Theta坐標的后部進行擴展
for w1 =  (length(Accurate_Intersect_Data_Phi_Radian_Tmp)-Extend_Width_Tmp+1) : ...
        -1 : length(Accurate_Intersect_Data_Phi_Radian_Tmp)
    % 賦初值給Phi，這個是精確值
    Phi_Radian_Tmp = Accurate_Intersect_Data_Phi_Radian_Tmp(w1);
    % 對上部進行擴展
    % 賦初值給Theta
    Theta_Radian_Tmp = Accurate_Intersect_Data_Upside_Theta_Radian_Tmp(w1-1);
    [Theta_Radian_Tmp,Fval,exitflag] = fsolve(@CalThetaFun2, Theta_Radian_Tmp, FSolve_Options);
    Accurate_Intersect_Data_Upside_Theta_Radian_Tmp(1, w1) = Theta_Radian_Tmp;
    % 對求解結果進行驗證
    Tmp = CalThetaFun2(Theta_Radian_Tmp);
    Upside_Calculate_Result_Set_Tmp(1, w1) = Tmp;

    % 對下部進行擴展
    % 賦初值給Theta
    Theta_Radian_Tmp = Accurate_Intersect_Data_Downside_Theta_Radian_Tmp(w1-1);
    [Theta_Radian_Tmp,Fval,exitflag] = fsolve(@CalThetaFun2, Theta_Radian_Tmp, FSolve_Options);
    Accurate_Intersect_Data_Downside_Theta_Radian_Tmp(1, w1) = Theta_Radian_Tmp;
    % 對求解結果進行驗證
    Tmp = CalThetaFun2(Theta_Radian_Tmp);
    Downside_Calculate_Result_Set_Tmp(1, w1) = Tmp;
end;

Tmp = Accurate_Intersect_Data_Upside_Theta_Radian_Tmp - Accurate_Intersect_Data_Downside_Theta_Radian_Tmp;
[Index_Tmp] = find(Tmp > 0.001);
Index_Tmp = [Index_Tmp(1)-1 Index_Tmp Index_Tmp(end)+1];
% Index_Tmp = (1 : 1 : length(Accurate_Intersect_Data_Upside_Theta_Radian_Tmp));

Accurate_Intersect_Data_Phi_Radian = Accurate_Intersect_Data_Phi_Radian_Tmp(1, Index_Tmp);
Accurate_Intersect_Data_Upside_Theta_Radian = Accurate_Intersect_Data_Upside_Theta_Radian_Tmp(1, Index_Tmp);
Accurate_Intersect_Data_Downside_Theta_Radian = Accurate_Intersect_Data_Downside_Theta_Radian_Tmp(1, Index_Tmp);
Upside_Calculate_Result_Set = Upside_Calculate_Result_Set_Tmp(1, Index_Tmp);
Downside_Calculate_Result_Set = Downside_Calculate_Result_Set_Tmp(1, Index_Tmp);
Accurate_Intersect_Data_Upside_Theta_Radian(1) = ...
    Accurate_Intersect_Data_Downside_Theta_Radian(1);
Accurate_Intersect_Data_Upside_Theta_Radian(end) = ...
    Accurate_Intersect_Data_Downside_Theta_Radian(end);

clear Upside_Theta_Search_Result_Set1 Downside_Theta_Search_Result_Set1 ...
    Upside_Calculate_Result_Set1 Downside_Calculate_Result_Set1;
clear Upside_Theta_Search_Result_Set2 Downside_Theta_Search_Result_Set2 ...
    Upside_Calculate_Result_Set2 Downside_Calculate_Result_Set2;
clear Accurate_Intersect_Data_Phi_Radian_Tmp Accurate_Intersect_Data_Upside_Theta_Radian_Tmp ...
    Accurate_Intersect_Data_Downside_Theta_Radian_Tmp Upside_Calculate_Result_Set_Tmp ...
    Downside_Calculate_Result_Set_Tmp;
    

% 處理便于顯示，沒有數據處理意義上的作用
Accurate_Intersect_Data_Phi_Degree = Accurate_Intersect_Data_Phi_Radian * 180 / pi;
Accurate_Intersect_Data_Upside_Theta_Degree = Accurate_Intersect_Data_Upside_Theta_Radian * 180 / pi;
Accurate_Intersect_Data_Downside_Theta_Degree = Accurate_Intersect_Data_Downside_Theta_Radian * 180 / pi;

figure;
subplot(2,1,1);
plot(Accurate_Intersect_Data_Phi_Degree, Accurate_Intersect_Data_Upside_Theta_Degree, 'r');
hold on;
plot(Accurate_Intersect_Data_Phi_Degree, Accurate_Intersect_Data_Downside_Theta_Degree, 'b');
grid on;
xlabel('Phi');
ylabel('Theta');
title('獲得的精確經緯度');
% max(Accurate_Intersect_Data_Phi_Degree) - min(Accurate_Intersect_Data_Phi_Degree)
% max(Accurate_Intersect_Data_Upside_Theta_Degree) - min(Accurate_Intersect_Data_Downside_Theta_Degree)
clear Accurate_Intersect_Data_Phi_Degree Accurate_Intersect_Data_Phi_Degree ...
    Accurate_Intersect_Data_Upside_Theta_Degree Accurate_Intersect_Data_Downside_Theta_Degree;

subplot(2,1,2);
plot(Accurate_Intersect_Data_Phi_Radian, Upside_Calculate_Result_Set, 'r');
hold on;
plot(Accurate_Intersect_Data_Phi_Radian, Downside_Calculate_Result_Set, 'b');
grid on;
title('逆向檢驗誤差');
clear Upside_Calculate_Result_Set Downside_Calculate_Result_Set;

% 根據得到的經度和緯度打印圖形
X_Tmp = Const_Earth_Re * cos(Accurate_Intersect_Data_Upside_Theta_Radian) .*...
    cos(Accurate_Intersect_Data_Phi_Radian);
Y_Tmp = Const_Earth_Re * cos(Accurate_Intersect_Data_Upside_Theta_Radian) .*...
    sin(Accurate_Intersect_Data_Phi_Radian);
Z_Tmp = Const_Earth_Re * sin(Accurate_Intersect_Data_Upside_Theta_Radian);
Axis_Range = [-Const_Earth_Re Const_Earth_Re -Const_Earth_Re Const_Earth_Re ...
    -Const_Earth_Re Const_Earth_Re];
figure;
plot3(X_Tmp, Y_Tmp, Z_Tmp);
axis(Axis_Range);
title('將所得到的經度緯度用圖形表示出來');
hold on;
X_Tmp = Const_Earth_Re * cos(Accurate_Intersect_Data_Downside_Theta_Radian) .*...
    cos(Accurate_Intersect_Data_Phi_Radian);
Y_Tmp = Const_Earth_Re * cos(Accurate_Intersect_Data_Downside_Theta_Radian) .*...
    sin(Accurate_Intersect_Data_Phi_Radian);
Z_Tmp = Const_Earth_Re * sin(Accurate_Intersect_Data_Downside_Theta_Radian);
Axis_Range = [-Const_Earth_Re Const_Earth_Re -Const_Earth_Re Const_Earth_Re ...
    -Const_Earth_Re Const_Earth_Re];
plot3(X_Tmp, Y_Tmp, Z_Tmp);
axis(Axis_Range);
grid on;
clear X_Tmp Y_Tmp Z_Tmp;

%% +++++++++++++++++++++++++++++++++++++
% 根據求取的經度和緯度方位，獲得經度和緯度的分割間距
Num_Tmp = 100;
% 距離對Theta的求導
Delta_Rst_Theta_Result = zeros(length(Accurate_Intersect_Data_Phi_Radian), Num_Tmp);
% 距離對Phi的求導
Delta_Rst_Phi_Result = zeros(length(Accurate_Intersect_Data_Phi_Radian), Num_Tmp);
% 速度對Theta的求導
Delta_Vst_Theta_Result = zeros(length(Accurate_Intersect_Data_Phi_Radian), Num_Tmp);
% 速度對Phi的求導
Delta_Vst_Phi_Result = zeros(length(Accurate_Intersect_Data_Phi_Radian), Num_Tmp);
% 相對徑向距離
Rst_Result = zeros(length(Accurate_Intersect_Data_Phi_Radian), Num_Tmp);
% 相對徑向速度
Vst_Result = zeros(length(Accurate_Intersect_Data_Phi_Radian), Num_Tmp);
for w1 = 1 : 1 : length(Accurate_Intersect_Data_Phi_Radian)
    Target_Phi = Accurate_Intersect_Data_Phi_Radian(w1);
    for w2 = 1 : 1 : Num_Tmp
        Target_Theta = Accurate_Intersect_Data_Downside_Theta_Radian(w1) + ...
            ((Accurate_Intersect_Data_Upside_Theta_Radian(w1) - ...
            Accurate_Intersect_Data_Downside_Theta_Radian(w1)) / Num_Tmp) * (w2-1);

        [Delta_Rst_Theta, Delta_Rst_Phi, Delta_Vst_Theta, Delta_Vst_Phi, Rst, Vst] = ...
            Calculate_R_V_DeltaR_DeltaV(Satellite_r, Const_Earth_Re, Satellite_Theta_s_Radian, ...
            Satellite_Phi_s_Radian, Satellite_Theta_i_Radian, Earth_We, Satellite_Wp, ...
            Target_Theta, Target_Phi);

        % 保存相關計算結果
        Delta_Rst_Theta_Result(w1, w2) = Delta_Rst_Theta;
        Delta_Rst_Phi_Result(w1, w2) = Delta_Rst_Phi;
        Delta_Vst_Theta_Result(w1, w2) = Delta_Vst_Theta;
        Delta_Vst_Phi_Result(w1, w2) = Delta_Vst_Phi;
        Rst_Result(w1, w2) = Rst;
        Vst_Result(w1, w2) = Vst;
    end;
end;

% 將相對速度變換導數轉換為相對多普勒頻移
Delta_Fd_Theta_Result = -2*Delta_Vst_Theta_Result / Const_Wave_Length;
Delta_Fd_Phi_Result = -2*Delta_Vst_Phi_Result / Const_Wave_Length;
clear Delta_Vst_Theta_Result Delta_Vst_Theta_Result;

figure;
subplot(3,2,1);
mesh(Rst_Result);
xlabel('Phi');
ylabel('Theta');
zlabel('Rst');
subplot(3,2,4);
mesh(Vst_Result);
xlabel('Phi');
ylabel('Theta');
zlabel('Vst');

subplot(3,2,2);
mesh(Delta_Rst_Theta_Result);
xlabel('Phi');
ylabel('Theta');
zlabel('Delt_R/Delt_Theta');
subplot(3,2,5);