%% 地球静止轨道卫星(FY-2D)
close all;  clear;  clc;

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

writer = VideoWriter('demo2.mp4','MPEG-4');
writer.FrameRate = 60;
open(writer);

%% 绘制卫星轨道
figure('position', get(0,'ScreenSize'));
tiledlayout(6,3,'TileSpacing','none','Padding','compact');
ax1 = nexttile([5,1]);
hold on
grid on
title("卫星绕行地球过程",'FontSize',12,'FontWeight','bold');
hStltMin = 36326;      % km
hStltMax = 36431;      % km
a = (hStltMin+hStltMax)/2 / 6371 + 1
e = 0.0012334;
orbit_i = 7.7194/180*pi;
Omega = 51.2812/180*pi;
omega = 2.6141/180*pi;
kv = 0.98195310; % 近圆轨道
theta0 = -(34.5824/180*pi+pi);
[xt, yt, zt] = drawOrbit(a, e, orbit_i, Omega, omega);

len = length(xt);
idx = 1;

hAn1 = annotation('textbox',[.04, .18, 1, .04],'String',['a=',num2str(a,'%.2f'),'（半长轴）'],'LineStyle','none');
hAn2 = annotation('textbox',[.04, .14, 1, .04],'String',['e=',num2str(e,'%.2f'),'（偏心率）'],'LineStyle','none');
hAn3 = annotation('textbox',[.15, .18, 1, .04],'String',['i=',num2str(orbit_i/pi*180,'%.2f'),'\circ（轨道倾角）'],'LineStyle','none');
hAn4 = annotation('textbox',[.15, .14, 1, .04],'String',['\Omega=',num2str(Omega/pi*180,'%.2f'),'\circ（升交点赤经）'],'LineStyle','none');
hAn5 = annotation('textbox',[.28, .18, 1, .04],'String',['\omega=',num2str(omega/pi*180,'%.2f'),'\circ（近地点张角）'],'LineStyle','none');
hAn6 = annotation('textbox',[.28, .14, 1, .04],'String',['f=',num2str(idx/len*360,'%.2f'),'\circ（真近点角）'],'LineStyle','none');

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

%% 绘制卫星及星下点
hSatellite = scatter3(xt(idx),yt(idx),zt(idx),10,'r','fill');    % 卫星
hSubastral = scatter3(xt(1)/rho(1),yt(1)/rho(1),zt(1)/rho(1),4,'fill');   % 星下点
hL3 = plot3([0,xt(idx)],[0,yt(idx)],[0,zt(idx)],'y','LineWidth',0.01);

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

% 高配
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([-150 10])                           % specify viewpoint 
axis equal   

%% 开始运动
nEarth = 1;   % 仿真时间内地球所转圈数
step = 1000;
dThetaEarth = 2*pi/step;  % 地球转速（每帧）
% dThetaEarth = 0;
dThetaStltMax =  kv * dThetaEarth;          % 卫星近地点转速（每帧）
C = dThetaStltMax / precision * rho(1)^2;   % dIdx * r^2 == C
numOfFrame = round(step*nEarth);            % 仿真总帧数

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));
az(1) = az(1) - theta0;
while az(1)<-pi
    az(1) = az(1)+2*pi;
end
while az(1)>pi
    az(1) = az(1)-2*pi;
end

ax2 = nexttile([5,2]);
hold on
title("星下点轨迹",'FontSize',12,'FontWeight','bold');
image([-180,180], [90,-90], earth, 'AlphaData', 0.8);
hL4 = plot(az(1)*180/pi, el(1)*180/pi, 'r', 'LineWidth', 1);
scatter(az(1)*180/pi, el(1)*180/pi, 6,  'r', 'filled');
axis tight
axis equal
set(ax2,'XTick',-180:30:180,'YTick',-90:30:90);
grid on
ax2.GridAlpha = 1;
slen = 0;
dslen = 30;

M = makehgtform('axisrotate',[0,0,-1],theta0);
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',flip(z));
set(hCloud,'XData',x,'YData',y,'ZData',flip(z));
    
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',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));
    set(hL3,'XData',[0,xt(idx)],'YData',[0,yt(idx)],'ZData',[0,zt(idx)]);
    set(hL4,'XData',az(1:t)*180/pi,'YData',el(1:t)*180/pi);
    set(hAn6,'String',['f=',num2str(idx/len*360,'%.2f'),'\circ（真近点角）']);
    
    [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 - theta0;
    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
    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 - 6*u/l;
            py = el(t+1)*180/pi - 6*v/l;
            if abs(px) < 180 && abs(py) < 90
                quiver(px, py, 6*u/l, 6*v/l, 'r', 'LineWidth',1.2, 'MaxHeadSize',100);
            end
            slen = 0;
        end
    end

%     pause(0.01);
    % drawnow
    writeVideo(writer,getframe(gcf));
end

close(writer);