clc;
clear all;
close all;
format long;
warning on all;
%%
% 獲取參數，參數模式選擇
% 以下說明的參數默認情況為：
% 地球自轉，衛星高度為Re/2，衛星轉動速度和衛星高度匹配，雷達波長為8mm，發射脈寬為1us
% 采樣頻率為1MHz，衛星所在維度為0，經度為180
% 0: 地球轉動，衛星轉動（轉動速度和地球近似同步），沒有距離模糊，多普勒頻率模糊
% 1: 地球轉動，衛星轉動（轉動速度和地球近似同步），有大的距離模糊，多普勒頻率不模糊
% 2: 地球轉動，衛星轉動（轉動速度和地球近似同步），有大的小的距離模糊，多普勒頻率不模糊
% 3: 地球不轉動，衛星轉動（轉動速度很大），有大的小的距離模糊，多普勒頻率不模糊，采用理想模型
Simulate_Mode = 4;


%% +++++++++++++++++++++++++++++++++++++
% 仿真常數區
% 地球平均半徑，單位為m
Const_Earth_Re = 6371.004e3;
% 雷達信號波長，單位為m
Const_Wave_Length = 8e-3;

%% +++++++++++++++++++++++++++++++++++++
% 仿真參數區
% 是否是真實仿真: 1是真實仿真，0表示理想地球模型
IsRealSimulate_Set = [1 1 1 0 1];

% 地球平均角速度，單位為弧度/s
Tmp = 2*pi / (23.9345 * 3600);
Earth_We_Set = [Tmp Tmp Tmp 0 Tmp];

% 雷達參數區
% 雷達信號PRT，單位為s
Radar_PRF_Set= [2/3 2e3 20e3 30e3 2e3];
Radar_PRT_Set = 1./Radar_PRF_Set;
% 雷達發射波束寬度，單位為s
Radar_Pluse_Width_Set = [1e-6 1e-6 1e-6 1e-6 1e-6];
% 采樣頻率，對應于發射雷達脈沖的采樣點數
Radar_Sample_Point_Of_Pluse_Width_Set = [1 1 1 1 1];
% 雷達相干積累脈沖個數
Radar_Coherent_PRT_Num_Set = [64 64 64 128 32];
% 雷達距離分辨率，單位為m
Radar_Range_Resolution_Set = 3e8 * Radar_Pluse_Width_Set / 2;
% 雷達多普勒頻率分辨率，單位為Hz
Radar_Fd_Resolution_Set = 1./(Radar_PRT_Set .* Radar_Coherent_PRT_Num_Set);
% 雷達采樣頻率，單位為Hz
Radar_Fs_Set = 1./Radar_Pluse_Width_Set .* Radar_Sample_Point_Of_Pluse_Width_Set;
% 地面目標后向散射因子，根據地形不同而不同
Factor_0D_Set = [0.01 0.01 0.01 0.01 0.01];

% 仿真時選擇散射單元劃分時的經度和緯度分割因子，如果選擇公式對應間隔，對應因子為1
% 論文中說明選擇的因子是0.5，但由于仿真時間和內存的限制，實際仿真選擇因子都是1
Min_Delta_Theta_Factor_Set = [1 1 1 1/2 1/2];
Min_Delta_Phi_Factor_Set = [1 1 1 1/2 1/2];
% 求取地球和衛星相交曲線時的誤差容限，利用仿真求取交線時
Earth_Satellite_Intersect_Simulate_Eps_Set = [200e3 200e3 200e3 450e3 750e3];
% 衛星高度
Satellite_h_Set = [Const_Earth_Re/2 Const_Earth_Re/2 Const_Earth_Re/2 Const_Earth_Re/2 ...
    500e3] ;
% 衛星到地心的距離
Satellite_r_Set = Satellite_h_Set + Const_Earth_Re;
% 衛星的角速度，單位為弧度/s
% Satellite_Wp_Set = sqrt(3.9863e14/(Satellite_r_Set.^3));
Satellite_Wp_Set = [Earth_We_Set(1)*1.01 Earth_We_Set(2)*1.01 ...
    Earth_We_Set(3)*1.01 Earth_We_Set(1) sqrt(3.9863e14./(Satellite_r_Set(5).^3))];
% 衛星所在緯度
Satellite_Theta_s_Degree_Set = [0 0 0 0 0];
Satellite_Theta_s_Radian_Set = Satellite_Theta_s_Degree_Set * pi /180;
% 衛星所在經度，有效范圍是0～180度，但不要搞成90度
Satellite_Phi_s_Degree_Set = [180 180 180 180 180];
Satellite_Phi_s_Radian_Set = Satellite_Phi_s_Degree_Set * pi /180;
% 衛星在軌道平面內與X軸的夾角
Satellite_Alpha_Radian_Set = acos(cos(Satellite_Theta_s_Radian_Set).* cos(Satellite_Phi_s_Radian_Set))+1e-20;
% 衛星軌道平面與赤道平面的夾角
Satellite_Theta_i_Radian_Set = asin(sin(Satellite_Theta_s_Radian_Set) ./ sin(Satellite_Alpha_Radian_Set));
for w1 = 1 : 1 : length(Satellite_Theta_i_Radian_Set)
    if(abs(imag(Satellite_Theta_i_Radian_Set(w1)) < 0.001))
        Satellite_Theta_i_Radian_Set(w1) = real(Satellite_Theta_i_Radian_Set(w1));
    else
        fprintf('參數出現錯誤，退出程序\n');
        return;
    end;
end;
% 天線在雷達坐標系中的俯仰角
Antenna_Theta_c_Degree_Set = [0 0 0 0 0];
Antenna_Theta_c_Radian_Set = Antenna_Theta_c_Degree_Set * pi / 180;
% 天線在雷達坐標系中的方位角
Antenna_Phi_c_Degree_Set = [0 0 0 0 0];
Antenna_Phi_c_Radian_Set = Antenna_Phi_c_Degree_Set * pi / 180;
% 天線坐標系與雷達坐標系轉換時的中間變量
Antenna_Psi1_Radian_Set = atan(tan(Antenna_Theta_c_Radian_Set) .* sin(Antenna_Phi_c_Radian_Set));
Antenna_Psi2_Radian_Set = asin(sin(Antenna_Theta_c_Radian_Set) .* cos(Antenna_Phi_c_Radian_Set));
% 天線波束俯仰角寬度
Antenna_Delta_Theta_w_Degree_Set = [6 6 6 2 2];
Antenna_Delta_Theta_w_Radian_Set = Antenna_Delta_Theta_w_Degree_Set * pi / 180;
% 天線波束方位角寬度
Antenna_Delta_Phi_w_Degree_Set = [6 6 6 2 2];
Antenna_Delta_Phi_w_Radian_Set = Antenna_Delta_Phi_w_Degree_Set * pi / 180;

%% +++++++++++++++++++++++++++++++++++++
% 是否是真實仿真: 1是真實仿真，0表示理想地球模型
IsRealSimulate = IsRealSimulate_Set(Simulate_Mode + 1);
% 地球自轉角速度
Earth_We = Earth_We_Set(Simulate_Mode + 1);

% 衛星到地心的距離
Satellite_r = Satellite_r_Set(Simulate_Mode + 1);
% 衛星的角速度，單位為弧度/s
Satellite_Wp = Satellite_Wp_Set(Simulate_Mode + 1);
% 衛星軌道平面與赤道平面的夾角
Satellite_Theta_i_Radian = Satellite_Theta_i_Radian_Set(Simulate_Mode + 1);
% 衛星所在緯度，以弧度表示
Satellite_Theta_s_Radian = Satellite_Theta_s_Radian_Set(Simulate_Mode + 1);
% 衛星在軌道平面內與X軸的夾角
Satellite_Alpha_Radian = Satellite_Alpha_Radian_Set(Simulate_Mode + 1);
% 衛星所在經度，以弧度表示
Satellite_Phi_s_Radian = Satellite_Phi_s_Radian_Set(Simulate_Mode + 1);
% 天線坐標系與雷達坐標系轉換時的中間變量
Antenna_Psi1_Radian = Antenna_Psi1_Radian_Set(Simulate_Mode + 1);
Antenna_Psi2_Radian = Antenna_Psi2_Radian_Set(Simulate_Mode + 1);
% 天線波束俯仰角寬度
Antenna_Delta_Theta_w_Radian = Antenna_Delta_Phi_w_Radian_Set(Simulate_Mode + 1);
% 天線波束方位角寬度
Antenna_Delta_Phi_w_Radian = Antenna_Delta_Theta_w_Radian_Set(Simulate_Mode + 1);
% 地面目標后向散射因子，根據地形不同而不同
Factor_0D = Factor_0D_Set(Simulate_Mode + 1);
% 求取地球和衛星相交曲線時的誤差容限，利用仿真求取交線時
Earth_Satellite_Intersect_Simulate_Eps = Earth_Satellite_Intersect_Simulate_Eps_Set(Simulate_Mode + 1);

% 雷達距離分辨率
Radar_Range_Resolution = Radar_Range_Resolution_Set(Simulate_Mode + 1);
% 雷達多普勒頻率分辨率
Radar_Fd_Resolution = Radar_Fd_Resolution_Set(Simulate_Mode + 1);
% 雷達的采樣頻率
Radar_Fs = Radar_Fs_Set(Simulate_Mode + 1);
% 雷達信號PRT，單位為s
Radar_PRT = Radar_PRT_Set(Simulate_Mode + 1);
% 雷達發射波束寬度，單位為s
Radar_Pluse_Width = Radar_Pluse_Width_Set(Simulate_Mode + 1);
% 雷達相干積累次數
Radar_Coherent_PRT_Num = Radar_Coherent_PRT_Num_Set(Simulate_Mode + 1);
% 雷達脈沖采樣點數
Radar_Sample_Point_Of_Pluse_Width = Radar_Sample_Point_Of_Pluse_Width_Set(Simulate_Mode + 1);
% 仿真時選擇的經度和緯度分割因子
Min_Delta_Theta_Factor = Min_Delta_Theta_Factor_Set(Simulate_Mode + 1);
Min_Delta_Phi_Factor = Min_Delta_Phi_Factor_Set(Simulate_Mode + 1);

%% +++++++++++++++++++++++++++++++++++++
% +++++++++++++++++
fprintf('雷達PRF: %5.9f\n', 1/Radar_PRT);
fprintf('距離分辨率: %5.9f\n', Radar_Range_Resolution);
fprintf('多普勒頻率分辨率: %5.9f\n', Radar_Fd_Resolution);
fprintf('最大不模糊距離: %5.9f\n', Radar_PRT * 3e8 / 2);
% 繪制衛星波束和地球的示意圖
% 繪制地球
XA_Earth_Index = (-Const_Earth_Re : 4e4 : Const_Earth_Re);
YA_Earth_Index = (-Const_Earth_Re : 4e4 : Const_Earth_Re);
[XA_Earth_Index, YA_Earth_Index] = meshgrid(XA_Earth_Index, YA_Earth_Index);
% Satellite_Data_XA_YA_ZA = zeros(size(XA_Earth_Index));
% 繪制地球
ZA_Earth_Positive = sqrt(Const_Earth_Re.^2 - XA_Earth_Index.^2 - YA_Earth_Index.^2);
ZA_Earth_Negative = -sqrt(Const_Earth_Re.^2 - XA_Earth_Index.^2 - YA_Earth_Index.^2);
ZA_Earth_Positive_Saved = ZA_Earth_Positive;
ZA_Earth_Negative_Saved = ZA_Earth_Negative;
ZA_Earth_Positive(find(imag(ZA_Earth_Positive) ~=0)) = 0;
ZA_Earth_Negative(find(imag(ZA_Earth_Negative) ~=0)) = 0;
figure;
% colormap(gray) %將當前的顏色設置為灰色
mesh(XA_Earth_Index, YA_Earth_Index, ZA_Earth_Positive);
hold on;
mesh(XA_Earth_Index, YA_Earth_Index, ZA_Earth_Negative, 'FaceColor','red');
hold on;
grid on;
clear XA_Earth_Index YA_Earth_Index ZA_Earth_Positive ZA_Earth_Negative ...
    ZA_Earth_Positive_Saved ZA_Earth_Negative_Saved;

% +++++++++++++++++
% 繪制原始圓錐，這里是利用D坐標倒推A坐標
XD_Satellite_Index = [-Const_Earth_Re/20 : 3e3 : Const_Earth_Re/20];
YD_Satellite_Index = [-Const_Earth_Re/20 : 3e3 : Const_Earth_Re/20];
[XD_Satellite_Index, YD_Satellite_Index] = meshgrid(XD_Satellite_Index, YD_Satellite_Index);
ZD_Satellite_Positive = sqrt(2*(XD_Satellite_Index.^2)/(Antenna_Delta_Phi_w_Radian^2) + ...
    2*(YD_Satellite_Index.^2)/(Antenna_Delta_Theta_w_Radian^2));
ZD_Satellite_Negative = -ZD_Satellite_Positive;
ZD_Satellite_Positive_Saved = ZD_Satellite_Positive;
ZD_Satellite_Negative_Saved  =ZD_Satellite_Negative;
% 打印出原始的圓錐圖形
% colormap(hot);
mesh(XD_Satellite_Index, YD_Satellite_Index, ZD_Satellite_Positive);
hold on;

% +++++++++++++++++
% 將D坐標軸中的衛星坐標轉換到A坐標中
% 轉換矩陣第1行
FA2D_Factor_Matrix11 = -cos(Antenna_Psi2_Radian)*sin(Satellite_Alpha_Radian) + ...
    cos(Satellite_Alpha_Radian)*cos(Antenna_Psi1_Radian)*sin(Antenna_Psi2_Radian);
FA2D_Factor_Matrix12 = cos(Satellite_Alpha_Radian)*cos(Satellite_Theta_i_Radian)*cos(Antenna_Psi2_Radian) + ...
    (cos(Satellite_Theta_i_Radian)*cos(Antenna_Psi1_Radian)*sin(Satellite_Alpha_Radian) - ...
    sin(Satellite_Theta_i_Radian)*sin(Antenna_Psi1_Radian))*sin(Antenna_Psi2_Radian);
FA2D_Factor_Matrix13 = cos(Satellite_Alpha_Radian)*cos(Antenna_Psi2_Radian)*sin(Satellite_Theta_i_Radian) + ...
    (cos(Antenna_Psi1_Radian)*sin(Satellite_Alpha_Radian)*sin(Satellite_Theta_i_Radian) + ...
    cos(Satellite_Theta_i_Radian)*sin(Antenna_Psi1_Radian))*sin(Antenna_Psi2_Radian);

FA2D_Factor_Matrix21 = cos(Satellite_Alpha_Radian)*sin(Antenna_Psi1_Radian);
FA2D_Factor_Matrix22 = cos(Antenna_Psi1_Radian)*sin(Satellite_Theta_i_Radian) + ...
    cos(Satellite_Theta_i_Radian)*sin(Satellite_Alpha_Radian)*sin(Antenna_Psi1_Radian);
FA2D_Factor_Matrix23 = -cos(Satellite_Theta_i_Radian)*cos(Antenna_Psi1_Radian) + ...
    sin(Satellite_Alpha_Radian)*sin(Satellite_Theta_i_Radian)*sin(Antenna_Psi1_Radian);

FA2D_Factor_Matrix31 = -cos(Satellite_Alpha_Radian)*cos(Antenna_Psi1_Radian)...
    *cos(Antenna_Psi2_Radian) - sin(Satellite_Alpha_Radian)*sin(Antenna_Psi2_Radian);
FA2D_Factor_Matrix32 = cos(Antenna_Psi2_Radian)*sin(Satellite_Theta_i_Radian)*sin(Antenna_Psi1_Radian) + ...
    cos(Satellite_Theta_i_Radian)*(-cos(Antenna_Psi1_Radian)*cos(Antenna_Psi2_Radian)*...
    sin(Satellite_Alpha_Radian) + cos(Satellite_Alpha_Radian)*sin(Antenna_Psi2_Radian));
FA2D_Factor_Matrix33 = -cos(Antenna_Psi2_Radian)*(cos(Antenna_Psi1_Radian)*sin(Satellite_Alpha_Radian)*...
    sin(Satellite_Theta_i_Radian) + cos(Satellite_Theta_i_Radian)*sin(Antenna_Psi1_Radian)) + ...
    cos(Satellite_Alpha_Radian)*sin(Satellite_Theta_i_Radian)*sin(Antenna_Psi2_Radian);

% 轉換關系: [XD YD ZD] = FA2D_Factor_Matrix * [XA YA ZA] + FA2D_Adder_Matrix;
FA2D_Factor_Matrix = [FA2D_Factor_Matrix11 FA2D_Factor_Matrix12 FA2D_Factor_Matrix13;
    FA2D_Factor_Matrix21 FA2D_Factor_Matrix22 FA2D_Factor_Matrix23;
    FA2D_Factor_Matrix31 FA2D_Factor_Matrix32 FA2D_Factor_Matrix33];
FA2D_Adder_Matrix = [(-Satellite_r*cos(Antenna_Psi1_Radian)*sin(Antenna_Psi2_Radian)); ...
    (-Satellite_r*sin(Antenna_Psi1_Radian)); ...
    (Satellite_r*cos(Antenna_Psi1_Radian)*cos(Antenna_Psi2_Radian))];

% 轉換關系: [XA YA ZA] = FD2A_Factor_Matrix * [[XD YD ZD] - FA2D_Adder_Matrix]
FD2A_Factor_Matrix = inv(FA2D_Factor_Matrix);
[R_Num, C_Num] = size(XD_Satellite_Index);
Satellite_D_Data_Matrix_Org = zeros(3, R_Num*C_Num);
for w1 = 1 : 1 : R_Num
    Satellite_D_Data_Matrix_Org(1, (w1-1)*C_Num+1 : 1 : w1*C_Num) = XD_Satellite_Index(w1, 1 : end) ...
        - FA2D_Adder_Matrix(1);
    Satellite_D_Data_Matrix_Org(2, (w1-1)*C_Num+1 : 1 : w1*C_Num) = YD_Satellite_Index(w1, 1 : end) ...
        - FA2D_Adder_Matrix(2);
    Satellite_D_Data_Matrix_Org(3, (w1-1)*C_Num+1 : 1 : w1*C_Num) = ZD_Satellite_Positive(w1, 1 : end) ...
        - FA2D_Adder_Matrix(3);
end;
% 把D坐標系中的圓錐轉換到A坐標系中
Satellite_Data_XA_YA_ZA = FD2A_Factor_Matrix * Satellite_D_Data_Matrix_Org;
clear Satellite_D_Data_Matrix_Org;
XD_Satellite_Index = zeros(R_Num, C_Num);
YD_Satellite_Index = zeros(R_Num, C_Num);
ZD_Satellite_Positive = zeros(R_Num, C_Num);
for w1 = 1 : 1 : R_Num
    XD_Satellite_Index(w1, 1 : end) = Satellite_Data_XA_YA_ZA(1, (w1-1)*C_Num+1 : 1 : w1*C_Num);
    YD_Satellite_Index(w1, 1 : end) = Satellite_Data_XA_YA_ZA(2, (w1-1)*C_Num+1 : 1 : w1*C_Num);
    ZD_Satellite_Positive(w1, 1 : end) = Satellite_Data_XA_YA_ZA(3, (w1-1)*C_Num+1 : 1 : w1*C_Num);
end;

% 打印出經過坐標變換的圓錐圖形
mesh(XD_Satellite_Index, YD_Satellite_Index, ZD_Satellite_Positive);
xlabel('x');
ylabel('y');
hold on;
title('地球和圓錐的立體圖形打印');

% +++++++++++++++++
% 繪制衛星波束和地球的交點
% 尋找在圓錐坐標范圍中屬于地球范圍內的坐標
Satellite_Start_XA_Tmp = Satellite_r * cos(Satellite_Theta_s_Radian) * cos(Satellite_Phi_s_Radian);
Satellite_Start_YA_Tmp = Satellite_r * cos(Satellite_Theta_s_Radian) * sin(Satellite_Phi_s_Radian);
Satellite_Start_ZA_Tmp = Satellite_r * sin(Satellite_Theta_s_Radian);
% 這里通過兩次限定，獲取感興趣的坐標軸范圍
% 在地球坐標范圍內