%% 用来快速生成星下点轨迹进行验证（并不符合真实情况）
% by qCwCp
close all;  clear;  clc;

global precision
precision = 0.0002;  % 卫星位置精度

%% 绘制卫星轨道
figure
set(gcf, 'position', [200,50,800,580]);
hold on
grid on
title("卫星绕行地球过程",'FontSize',12,'FontWeight','bold');

% 初始化方式一：已知卫星近地点与远地点高度 
% hStltMin = 417;      % 卫星近地点高度（km）
% hStltMax = 423;      % 卫星远地点高度（km）
% a = (hStltMin+hStltMax)/2 / 6371 + 1
% e = 1 - (hStltMin/6371 + 1)/a 
% e = 0.0004112;
% i = 51.6432/180*pi;
% Omega = 69.9148/180*pi;
% omega = 134.2458/180*pi;

% 初始化方式二：已知半长轴和偏心率 
a = 3; % 以地球半径为单位
e = 0.6;
i = pi/6;
Omega = 0;
omega = pi/4;
% vStltMax = 7.7; % km/s
[xt, yt, zt] = drawOrbit(a, e, i, Omega, omega);

len = length(xt);

rho = zeros(1,len);
for i = 1:len
    rho(i) = norm([xt(i),yt(i),zt(i)]);
end

%% 绘制卫星及星下点
idx = 1;
hSatellite = scatter3(xt(idx),yt(idx),zt(idx),'r','fill');  % 卫星

hSubastral = scatter3(xt(1)/rho(1),yt(1)/rho(1),zt(1)/rho(1),4,'fill');   % 星下点

%% 绘制地球
gridNum = 24;   % longitude: 360/24=15 degree/grid    latitude: 180/24=7.5 degree/grid
[x, y, z] = sphere(gridNum);
earth = imread('landOcean.jpg');

% % 低配
% load topo;
% hEarth = surface(x,y,z,'FaceColor','texturemap','CData',topo,'EdgeAlpha',0.3);
% colormap(topomap1);
% % correction
% M = makehgtform('axisrotate',[0,0,-1],pi);
% m = M(1:3,1:3);
% for i = 1:gridNum+1
%     for j = 1:gridNum+1
%         tmp = [x(i,j),y(i,j),z(i,j)]*m;
%         x(i,j) = tmp(1);
%         y(i,j) = tmp(2);
%         z(i,j) = tmp(3);
%     end
% end
% % refresh
% set(hEarth,'XData',x,'YData',y,'ZData',z);

% 高配
clouds = imread('cloudCombined.jpg');

hEarth = surface(x ,y, z);
hEarth.FaceColor = 'texturemap';        % set color to texture mapping
hEarth.EdgeColor = 'none';              % remove surface edge color
% hEarth.EdgeAlpha = 0.3;
hEarth.CData = earth;                % set color data 

hCloud = surface(x*1.01,y*1.01,z*1.01); 
hCloud.FaceColor = 'texturemap';        % set color to texture mapping
hCloud.EdgeColor = 'none';              % remove surface edge color
hCloud.CData = clouds;                  % set color data 
 
hCloud.FaceAlpha = 'texturemap';        % set transparency to texture mapping
hCloud.AlphaData = max(clouds,[],3);    % set transparency data 

view([80 10])                           % specify viewpoint 
axis equal   

%% 开始运动
step = 1000;
dThetaEarth = 2*pi/step;  % 地球转速（每帧）
% dThetaEarth = 0;
% kv = (vStltMax/(hStltMin+6371))/(2*pi/24/3600)
kv = (rho(round(len/2))/rho(1))^2;
dThetaStltMax =  kv * dThetaEarth;  % 卫星近地点转速（每帧）
C = dThetaStltMax / precision * rho(1)^2;   % dIdx * r^2 = C
numOfFrame = 1000;   % 仿真总帧数

az = zeros(1,numOfFrame+1); % 经度
el = zeros(1,numOfFrame+1); % 纬度
[az(1),el(1)] = cart2sph(xt(1)/rho(1),yt(1)/rho(1),zt(1)/rho(1));

for t = 1:numOfFrame
    % 变换矩阵
    M = makehgtform('axisrotate',[0,0,-1],dThetaEarth);
    m = M(1:3,1:3);

    for i = 1:gridNum+1
        for j = 1:gridNum+1
            tmp = [x(i,j),y(i,j),z(i,j)]*m;
            x(i,j) = tmp(1);
            y(i,j) = tmp(2);
            z(i,j) = tmp(3);
        end
    end
    % refresh
%     set(hEarth,'XData',x,'YData',y,'ZData',z);
    set(hEarth,'XData',x,'YData',y,'ZData',flip(z));
    set(hCloud,'XData',x,'YData',y,'ZData',flip(z));

    dIdx = round(C/rho(idx)^2);
    idx = idx+dIdx;
    if idx > len
        idx = idx-len;
    end
    % refresh
    set(hSatellite,'XData',xt(idx),'YData',yt(idx),'ZData',zt(idx));
    set(hSubastral,'XData',xt(idx)/rho(idx),'YData',yt(idx)/rho(idx),'ZData',zt(idx)/rho(idx));
    
    [az(t+1),el(t+1)] = cart2sph(xt(idx)/rho(idx),yt(idx)/rho(idx),zt(idx)/rho(idx));
    az(t+1) = az(t+1) - t*dThetaEarth;
    while az(t+1)<-pi
        az(t+1) = az(t+1)+2*pi;
    end
    while az(t+1)>pi
        az(t+1) = az(t+1)-2*pi;
    end
    if abs(az(t+1)-az(t))>1 || abs(el(t+1)-el(t))>1
        el(t+1) = NaN;
    end
    
%     pause(0.01);
    drawnow limitrate   % 跳帧刷新（若想更快可以注释掉）
end

%% 绘制星下点轨迹
% 原始绘图代码
% figure('position', get(0,'ScreenSize'));
% hold on
% title("星下点轨迹");
% image([-180,180], [90,-90], earth, 'AlphaData', 0.8);
% % plot(az*180/pi, el*180/pi, 'r', 'LineWidth', 1);
% quiver(az*180/pi, el*180/pi, [0,diff(az*180/pi)], [0,diff(el*180/pi)], 'r', 'LineWidth', 1);
% axis tight
% set(gca,'XTick',-180:30:180,'YTick',-90:30:90);
% grid on
% ax = gca;
% ax.GridAlpha = 1;

% 改进后
figure('position', get(0,'ScreenSize'));
hold on
title("星下点轨迹",'FontSize',12,'FontWeight','bold');
image([-180,180], [90,-90], earth, 'AlphaData', 0.8);
plot(az*180/pi, el*180/pi, 'r', 'LineWidth', 1);

% 下面这段代码是为了绘制出比较好看的箭头
slen = 0;   % 弧长累计
dslen = 30; % 两箭头之间所夹弧长
for t = 1:numOfFrame
    if ~isnan(el(t+1)) && ~isnan(el(t))
        u = diff(az([t,t+1])*180/pi);
        v = diff(el([t,t+1])*180/pi);
        l = hypot(u,v);                 % 两点间距近似为两点间的弧长
        slen = slen+l;
        if slen > dslen % 绘制箭头
            % 坐标矫正，使箭头的头部位于目标点
            px = az(t+1)*180/pi - u/l/4;
            py = el(t+1)*180/pi - v/l/4;

            if abs(px) < 180 && abs(py) < 90
                quiver(px, py, u/l/4, v/l/4, 'r', 'LineWidth',3, 'MaxHeadSize',100);
            end

            slen = 0;   % 累计弧长清零
        end
    end
end

axis tight
set(gca,'XTick',-180:30:180,'YTick',-90:30:90);
grid on
ax = gca;
ax.GridAlpha = 1;