Tmp = ((abs(Satellite_Data_XA_YA_ZA(1, :)) < Const_Earth_Re) &...
    (abs(Satellite_Data_XA_YA_ZA(2, :)) < Const_Earth_Re) &...
    (abs(Satellite_Data_XA_YA_ZA(3, :)) < Const_Earth_Re));
% Tmp1 = (((Satellite_Data_XA_YA_ZA(1, :)-Satellite_Start_XA_Tmp).*Satellite_Data_XA_YA_ZA(1, :) <= 0) &...
%     ((Satellite_Data_XA_YA_ZA(2, :)-Satellite_Start_YA_Tmp).*Satellite_Data_XA_YA_ZA(2, :)<= 0) &...
%     ((Satellite_Data_XA_YA_ZA(3, :)-Satellite_Start_ZA_Tmp).*Satellite_Data_XA_YA_ZA(3, :)<= 0));
% 位于衛星和地球球心中間
Tmp1 = ((Satellite_Data_XA_YA_ZA(1, :)-Satellite_Start_XA_Tmp).*Satellite_Data_XA_YA_ZA(1, :) <= 0);
Tmp = Tmp & Tmp1;

% 獲取對應的有效XA，YA和ZA數據
Satellite_Data_XA_YA_ZA_InEarthRange = Satellite_Data_XA_YA_ZA(:, find(Tmp ~=0));
% 獲得對應的地球Z數據
Earth_Z_Positive_Tmp = sqrt(Const_Earth_Re.^2 - Satellite_Data_XA_YA_ZA_InEarthRange(1, :).^2 - ...
    Satellite_Data_XA_YA_ZA_InEarthRange(2, :).^2);
Earth_Z_Negative_Tmp = -Earth_Z_Positive_Tmp;
% 考察地球Z為正時，搜集其坐標數據
Tmp = abs(Earth_Z_Positive_Tmp - Satellite_Data_XA_YA_ZA_InEarthRange(3,:));
Positive_Date_XA_YA_ZA_Get = Satellite_Data_XA_YA_ZA_InEarthRange(:, find(Tmp <= Earth_Satellite_Intersect_Simulate_Eps));
% Positive_Date_XA_YA_ZA_Get(3, :) = Earth_Z_Positive_Tmp(find(Tmp <= Earth_Satellite_Intersect_Simulate_Eps));
% 考察地球Z為負時，搜集其坐標數據
Tmp = abs(Earth_Z_Negative_Tmp - Satellite_Data_XA_YA_ZA_InEarthRange(3,:));
Negative_Date_XA_YA_ZA_Get = Satellite_Data_XA_YA_ZA_InEarthRange(:, find(Tmp <= Earth_Satellite_Intersect_Simulate_Eps));
% Negative_Date_XA_YA_ZA_Get(3, :) = Earth_Z_Negative_Tmp(find(Tmp <= Earth_Satellite_Intersect_Simulate_Eps));
clear Satellite_Data_XA_YA_ZA_InEarthRange Earth_Z_Positive_Tmp Earth_Z_Negative_Tmp;
% 在原先三維球和圓錐圖上添加相交的曲線，和原先的地球和圓錐打印在一起
h_Positive=plot3(Positive_Date_XA_YA_ZA_Get(1, :), Positive_Date_XA_YA_ZA_Get(2, :), ...
    Positive_Date_XA_YA_ZA_Get(3, :),'.');
set(h_Positive,'markersize',6),hold on;
h_Negative=plot3(Negative_Date_XA_YA_ZA_Get(1, :), Negative_Date_XA_YA_ZA_Get(2, :), ...
    Negative_Date_XA_YA_ZA_Get(3, :),'.');
set(h_Negative,'markersize',6),hold on;

% 單獨打印地球和衛星波束的焦點
figure;
Axis_Range = [min(min(XD_Satellite_Index)) max(max(XD_Satellite_Index)) min(min(YD_Satellite_Index)) ...
    max(max(YD_Satellite_Index))];
h_Positive=plot3(Positive_Date_XA_YA_ZA_Get(1, :), Positive_Date_XA_YA_ZA_Get(2, :), ...
    Positive_Date_XA_YA_ZA_Get(3, :),'.');
set(h_Positive,'markersize',6),hold on;
grid on;
axis(Axis_Range);

h_Negative=plot3(Negative_Date_XA_YA_ZA_Get(1, :), Negative_Date_XA_YA_ZA_Get(2, :), ...
    Negative_Date_XA_YA_ZA_Get(3, :),'.');
set(h_Negative,'markersize',6),hold on;
axis(Axis_Range);
grid on;
title('將相交點單獨打印');

clear XD_Satellite_Index YD_Satellite_Index ZD_Satellite_Positive;


%% +++++++++++++++++++++++++++++++++++++
% 得到圓錐和地球相交的經度和緯度范圍，利用公式推導比較精確的坐標
% 分別兩個處理步驟：
% (1) 利用仿真得到初始的經緯度范圍以及各個經緯度對應的解初始值。
% (2) 利用解線性方程得到精確的解。

% 第一步：根據仿真結果去搜索相交的經度和緯度的初始值，需要人工對仿真獲得交點的質量進行評估。
% 根據效果調整Earth_Satellite_Intersect_Simulate_Eps的大小，直到滿意為止
% 所有相交的交點信息，用XA，YA和ZA表示，XA：第一行，YA：第二行，ZA：第三行
All_Intersect_Data_XA_YA_ZA = [Positive_Date_XA_YA_ZA_Get Negative_Date_XA_YA_ZA_Get];
% 所有相交的交點信息，相對經度
All_Intersect_Data_Phi_Radian = atan2(All_Intersect_Data_XA_YA_ZA(2,:), All_Intersect_Data_XA_YA_ZA(1, :));
% 所有相交的交點信息，相對緯度
All_Intersect_Data_Theta_Radian = atan2(All_Intersect_Data_XA_YA_ZA(3,:), ...
    All_Intersect_Data_XA_YA_ZA(1,:) ./ cos(All_Intersect_Data_Phi_Radian));
All_Intersect_Data_Phi_Radian_Mod_2pi = mod(All_Intersect_Data_Phi_Radian, 2*pi);
All_Intersect_Data_Theta_Radian_Mod_2pi = mod(All_Intersect_Data_Theta_Radian, 2*pi);
All_Intersect_Data_Theta_Radian_Mean = (max(All_Intersect_Data_Theta_Radian) + min(All_Intersect_Data_Theta_Radian)) ...
    / 2;
All_Intersect_Data_Theta_Radian_Mod_2pi_Mean = (max(All_Intersect_Data_Theta_Radian_Mod_2pi) + min(All_Intersect_Data_Theta_Radian_Mod_2pi))...
    / 2;
% 下面四個變量是為了顯示方便，對數據處理沒有實質的影響
All_Intersect_Data_Phi_Degree = All_Intersect_Data_Phi_Radian * 180 / pi;
All_Intersect_Data_Theta_Degree = All_Intersect_Data_Theta_Radian * 180 / pi;
All_Intersect_Data_Phi_Degree_Mod_2pi = All_Intersect_Data_Phi_Radian_Mod_2pi * 180 / pi;
All_Intersect_Data_Theta_Degree_Mod_2pi = All_Intersect_Data_Theta_Radian_Mod_2pi * 180 / pi;
All_Intersect_Data_Theta_Degree_Mean = All_Intersect_Data_Theta_Radian_Mean * 180 / pi;
All_Intersect_Data_Theta_Degree_Mod_2pi_Mean = All_Intersect_Data_Theta_Radian_Mod_2pi_Mean * 180 / pi;

figure;
subplot(2,2,1);
plot(All_Intersect_Data_Phi_Degree, All_Intersect_Data_Theta_Degree, '.');
hold on;
plot(All_Intersect_Data_Phi_Degree, ones(1, length(All_Intersect_Data_Phi_Degree))*...
    All_Intersect_Data_Theta_Degree_Mean);
xlabel('Phi(度)');
ylabel('Theta(度)');
grid on;
title('1');
subplot(2,2,2);
plot(All_Intersect_Data_Phi_Degree_Mod_2pi, All_Intersect_Data_Theta_Degree_Mod_2pi, '.');
hold on;
plot(All_Intersect_Data_Phi_Degree_Mod_2pi, ones(1, length(All_Intersect_Data_Phi_Degree))* ...
    All_Intersect_Data_Theta_Degree_Mod_2pi_Mean);
xlabel('Phi(mod 2pi)(度)');
ylabel('Theta(mod 2pi)(度)');
grid on;
title('2');
subplot(2,2,3);
plot(All_Intersect_Data_Phi_Degree, All_Intersect_Data_Theta_Degree_Mod_2pi, '.');
hold on;
plot(All_Intersect_Data_Phi_Degree, ones(1, length(All_Intersect_Data_Phi_Degree))* ...
    All_Intersect_Data_Theta_Degree_Mod_2pi_Mean);
xlabel('Phi(度)');
ylabel('Theta(mod 2pi)(度)');
grid on;
title('3');
subplot(2,2,4);
plot(All_Intersect_Data_Phi_Degree_Mod_2pi, All_Intersect_Data_Theta_Degree, '.');
hold on;
plot(All_Intersect_Data_Phi_Degree_Mod_2pi, ones(1, length(All_Intersect_Data_Phi_Degree_Mod_2pi))* ...
    All_Intersect_Data_Theta_Degree_Mean);
xlabel('Phi(mod 2pi)(度)');
ylabel('Theta(度)');
grid on;
title('4');

% 提示選擇模式
str = {'1';'2';'3';'4'};
[s,v] = listdlg('PromptString','請輸入選擇以便繼續仿真:',...
    'SelectionMode','single',...
    'ListSize', [220 110],...
    'ListString',str);
if(isempty(s))
    fprintf('沒有選擇，退出仿真\n');
    return;
end;

% 選擇合適的數據進行處理
if(s == 1)
    All_Intersect_Data_Phi_Radian = All_Intersect_Data_Phi_Radian;
    All_Intersect_Data_Theta_Radian = All_Intersect_Data_Theta_Radian;
elseif(s == 2)
    All_Intersect_Data_Phi_Radian = All_Intersect_Data_Phi_Radian_Mod_2pi;
    All_Intersect_Data_Theta_Radian = All_Intersect_Data_Theta_Radian_Mod_2pi;
elseif(s == 3)
    All_Intersect_Data_Phi_Radian = All_Intersect_Data_Phi_Radian;
    All_Intersect_Data_Theta_Radian = All_Intersect_Data_Theta_Radian_Mod_2pi;
elseif(s == 4)
    All_Intersect_Data_Phi_Radian = All_Intersect_Data_Phi_Radian_Mod_2pi;
    All_Intersect_Data_Theta_Radian = All_Intersect_Data_Theta_Radian;
end;
clear All_Intersect_Data_Phi_Radian_Mod_2pi All_Intersect_Data_Theta_Radian_Mod_2pi ;

% 進行重新排序，按照從大到小進行排序
[All_Intersect_Data_Phi_Radian, Index_Tmp, n1] = unique(All_Intersect_Data_Phi_Radian, 'first');
All_Intersect_Data_Theta_Radian = All_Intersect_Data_Theta_Radian(Index_Tmp);

% 選擇區分門限
All_Intersect_Data_Theta_Radian_Threshold = (max(All_Intersect_Data_Theta_Radian) + min(All_Intersect_Data_Theta_Radian)) / 2;
% 大于等于平均值的上半部分
Upside_Intersect_Data_Theta_Radian = All_Intersect_Data_Theta_Radian(...
    find(All_Intersect_Data_Theta_Radian>=All_Intersect_Data_Theta_Radian_Threshold));
Upside_Intersect_Data_Phi_Radian = All_Intersect_Data_Phi_Radian(...
    find(All_Intersect_Data_Theta_Radian>=All_Intersect_Data_Theta_Radian_Threshold));

% 小于平均值的下半部分
Downside_Intersect_Data_Theta_Radian = All_Intersect_Data_Theta_Radian(...
    find(All_Intersect_Data_Theta_Radian<All_Intersect_Data_Theta_Radian_Threshold));
Downside_Intersect_Data_Phi_Radian = All_Intersect_Data_Phi_Radian(...
    find(All_Intersect_Data_Theta_Radian<All_Intersect_Data_Theta_Radian_Threshold));

figure;
plot(Upside_Intersect_Data_Phi_Radian, Upside_Intersect_Data_Theta_Radian, 'r');
hold on;
plot(Downside_Intersect_Data_Phi_Radian, Downside_Intersect_Data_Theta_Radian, 'b');
grid on;
title('上半部分和下半部分(1)');


% 通過內插得到均勻采樣數據
Max_Intersect_Data_Phi_Radian = min(max(Upside_Intersect_Data_Phi_Radian), ...
    max(Downside_Intersect_Data_Phi_Radian));
Min_Intersect_Data_Phi_Radian = max(min(Upside_Intersect_Data_Phi_Radian), ...
    min(Downside_Intersect_Data_Phi_Radian));
Tmp = (Max_Intersect_Data_Phi_Radian - Min_Intersect_Data_Phi_Radian) / 100;
All_Intersect_Data_Phi_Radian = (Min_Intersect_Data_Phi_Radian : Tmp : ...
    Max_Intersect_Data_Phi_Radian);
% 經過內插得到初始經度和緯度值，用于后續的精確求解
Upside_Intersect_Data_Theta_Radian = interp1(Upside_Intersect_Data_Phi_Radian, ...
    Upside_Intersect_Data_Theta_Radian, All_Intersect_Data_Phi_Radian);
Downside_Intersect_Data_Theta_Radian = interp1(Downside_Intersect_Data_Phi_Radian, ...
    Downside_Intersect_Data_Theta_Radian, All_Intersect_Data_Phi_Radian);
figure;
plot(All_Intersect_Data_Phi_Radian, Upside_Intersect_Data_Theta_Radian, 'r');
hold on;
plot(All_Intersect_Data_Phi_Radian, Downside_Intersect_Data_Theta_Radian, 'b');
hold on;
grid on;
xlabel('phi');
ylabel('theta');
title('經過內插得到的初始經度和緯度值');

% 第二步：利用方程獲得精確的交點處的經度和緯度
% 根據上半部分的進行搜集
% 設置一些全局變量，便于非線性方程求解
global Const_Earth_Re_Tmp Phi_Radian_Tmp FA2D_Factor_Matrix_Tmp FA2D_Adder_Matrix_Tmp ...
    Antenna_Delta_Phi_w_Radian_Tmp Antenna_Delta_Theta_w_Radian_Tmp;
Const_Earth_Re_Tmp = Const_Earth_Re;
FA2D_Factor_Matrix_Tmp = FA2D_Factor_Matrix;
FA2D_Adder_Matrix_Tmp = FA2D_Adder_Matrix;
Antenna_Delta_Phi_w_Radian_Tmp = Antenna_Delta_Phi_w_Radian;
Antenna_Delta_Theta_w_Radian_Tmp = Antenna_Delta_Theta_w_Radian;
global  Antenna_Psi1_Radian_Tmp ...
    Satellite_Theta_i_Radian_Tmp  Satellite_r_Tmp Satellite_Alpha_Radian_Tmp Antenna_Psi2_Radian_Tmp;
Antenna_Psi1_Radian_Tmp = Antenna_Psi1_Radian;
Satellite_Theta_i_Radian_Tmp = Satellite_Theta_i_Radian;
Satellite_r_Tmp = Satellite_r;
Satellite_Alpha_Radian_Tmp = Satellite_Alpha_Radian;
Antenna_Psi2_Radian_Tmp = Antenna_Psi2_Radian;

% 通過求解得到的Theta結果
Upside_Theta_Search_Result_Set1 = zeros(1, length(All_Intersect_Data_Phi_Radian));
Upside_Theta_Search_Result_Set2 = zeros(1, length(All_Intersect_Data_Phi_Radian));
% 對求解結果逆向驗證的結果
Upside_Calculate_Result_Set1 = zeros(1, length(All_Intersect_Data_Phi_Radian));
Upside_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，這個是精確值，是一個全局變量，傳遞給CalThetaFun1函數
    Phi_Radian_Tmp = All_Intersect_Data_Phi_Radian(w1);
    % 賦初值給Theta
    Theta_Radian_Tmp = Upside_Intersect_Data_Theta_Radian(w1);
    [Theta_Radian_Tmp,Fval,exitflag] = fsolve(@CalThetaFun1, Theta_Radian_Tmp, FSolve_Options);
    Upside_Theta_Search_Result_Set1(1, w1) = Theta_Radian_Tmp;
    % 對求解結果進行驗證
    Tmp = CalThetaFun1(Theta_Radian_Tmp);
    Upside_Calculate_Result_Set1(1, w1) = Tmp;

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

% 通過求解得到的Theta結果
Downside_Theta_Search_Result_Set1 = zeros(1, length(All_Intersect_Data_Phi_Radian));
Downside_Theta_Search_Result_Set2 = zeros(1, length(All_Intersect_Data_Phi_Radian));
% 對求解結果逆向驗證的結果