%% 用来生成轨道要素动画
% by qCwCp
close all;  clear;  clc;

writer = VideoWriter('orbit_output.mp4','MPEG-4'); % 父文件夹不能
frameRate = writer.FrameRate;
open(writer);

figure('position', get(0,'ScreenSize'));
camva(4);
camva('manual');
hold on
axis off
annotation('textbox',[0.465, 0.87, 1, 0.1],'String','卫星轨道六要素','FontSize',12,'FontWeight','bold','LineStyle','none');

%% 绘制地球
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, flip(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,flip(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 

axis equal


R = 5;
hQ1 = quiver3(0,0,0, 0,0,R, 'k','AutoScale','off','MaxHeadSize',0.1,'LineWidth',0.5);
quiver3(0,0,0, 0,0,R, 'r','AutoScale','off','MaxHeadSize',0.1,'LineWidth',0.5);
[xs, ys, zs] = sphere(24);
surface(R * xs, R * ys, R * zs, 'FaceColor','y', 'FaceAlpha',0.05, 'LineStyle',':','EdgeColor',[0.9290 0.6940 0.1250],'LineWidth',0.05);

precision = 0.0002;
theta = -pi:precision:pi;
[xt,yt] = pol2cart(theta,R);
zt = zeros(1,length(xt));
plot3([xt xt(1)],[yt yt(1)],[zt zt(1)]);
plot3([0,-R],[0,0],[0,0],'Marker','o','MarkerSize',1.5);
text(-R, 0, 0, ' 春分点');

%% 转换视角
for elView = 0:0.9:90
    view([0,elView])
    
    drawnow
    writeVideo(writer,getframe(gcf));
end

%% 绘制卫星轨道
theta = -pi:precision:pi;
len = length(theta);
a = 1;
e = 0;
i = 0;
Omega = 0;
omega = 0;
rho = a*(1-e^2)./(1-e*cos(theta));
[xt,yt] = pol2cart(theta,rho);
zt = zeros(1,len);
hOrbit = plot3([xt xt(1)],[yt yt(1)],[zt zt(1)]);  % 卫星轨道
hAn1 = annotation('textbox',[.2, .85, 1, .04],'String',['a=',num2str(a,'%.2f'),'（半长轴）'],'LineStyle','none');
hAn2 = annotation('textbox',[.2, .80, 1, .04],'String',['e=',num2str(e,'%.2f'),'（偏心率）'],'LineStyle','none');
hAn3 = annotation('textbox',[.2, .75, 1, .04],'String',['i=',num2str(i/pi*180,'%.2f'),'\circ（轨道倾角）'],'LineStyle','none');
hAn4 = annotation('textbox',[.2, .70, 1, .04],'String',['\Omega=',num2str(Omega/pi*180,'%.2f'),'\circ（升交点赤经）'],'LineStyle','none');
hAn5 = annotation('textbox',[.2, .65, 1, .04],'String',['\omega=',num2str(omega/pi*180,'%.2f'),'\circ（近地点张角）'],'LineStyle','none');
for a = 1:0.02:2.8
    rho = a*(1-e^2)./(1-e*cos(theta));
    [xt,yt] = pol2cart(theta,rho);
    set(hOrbit,'XData',[xt,xt(1)],'YData',[yt,yt(1)],'ZData',[zt,zt(1)]);
    set(hAn1,'String',['a=',num2str(a,'%.2f'),'（半长轴）']);
    
    drawnow
    writeVideo(writer,getframe(gcf));
end

for e = 0:0.005:0.5
    rho = a*(1-e^2)./(1-e*cos(theta));
    [xt,yt] = pol2cart(theta,rho);
    set(hOrbit,'XData',[xt,xt(1)],'YData',[yt,yt(1)],'ZData',[zt,zt(1)]);
    set(hAn2,'String',['e=',num2str(e,'%.2f'),'（偏心率）']);
    
    drawnow
    writeVideo(writer,getframe(gcf));
end

nt0 = [0,0,R];
xt0 = xt;
yt0 = yt;
zt0 = zt;

pause(1);
for i = 1:1*frameRate
    writeVideo(writer,getframe(gcf));
end

%% 转换视角
azView = 0:0.5:160;
elView = interp1([0,160],[90,10],azView);
for i = 1:length(azView)
    view([azView(i),elView(i)])
    
    drawnow
    writeVideo(writer,getframe(gcf));
end

pause(1);
for i = 1:1*frameRate
    writeVideo(writer,getframe(gcf));
end

idxRise = 0;
idxFall = 0;
%% 轨道倾角
for i = linspace(0,pi/6,100)
    M1 = makehgtform('axisrotate',[1,0,0],i);
    m1 = M1(1:3,1:3);
    for el = 1:len
        tmp = [xt0(el),yt0(el),zt0(el)]*m1;
        xt(el) = tmp(1);
        yt(el) = tmp(2);
        if zt(el) == tmp(3)
            if idxRise == 0
                idxRise = el;
                idxFall = idxRise + round(len/2);
                if idxFall > len
                    idxFall = idxFall-len;
                end
                hL1 = plot3(xt([idxRise,idxFall]),yt([idxRise,idxFall]),zt([idxRise,idxFall]),'Marker','o','MarkerSize',1.5);
                hT1 = text(xt(idxRise),yt(idxRise),zt(idxRise),' 升交点');
                hT2 = text(xt(idxFall),yt(idxFall),zt(idxFall),' 降交点');
            end
        else
            zt(el) = tmp(3);
        end
    end
    nt = nt0 * m1;
    set(hOrbit,'XData',[xt,xt(1)],'YData',[yt,yt(1)],'ZData',[zt,zt(1)]);
    set(hQ1,'UData',nt(1),'VData',nt(2),'WData',nt(3));
    set(hAn3,'String',['i=',num2str(i/pi*180,'%.2f'),'\circ（轨道倾角）']);
    
    drawnow
    writeVideo(writer,getframe(gcf));
end

nt1 = nt;
xt1 = xt;
yt1 = yt;
zt1 = zt;

pause(1);
for i = 1:1*frameRate
    writeVideo(writer,getframe(gcf));
end

%% 转换视角
azView = 160:0.5:240;
elView = interp1([160,240],[10,10],azView);
for i = 1:length(azView)
    view([azView(i),elView(i)])
    
    drawnow
    writeVideo(writer,getframe(gcf));
end

pause(1);
for i = 1:1*frameRate
    writeVideo(writer,getframe(gcf));
end

%% 升交点赤经
for Omega = linspace(0,pi/4,100)
    M2 = makehgtform('axisrotate',[0,0,-1],Omega);
    m2 = M2(1:3,1:3);
    for el = 1:len
        tmp = [xt1(el),yt1(el),zt1(el)]*m2;
        xt(el) = tmp(1);
        yt(el) = tmp(2);
        zt(el) = tmp(3);        
    end
    nt = nt1 * m2;
    set(hOrbit,'XData',[xt,xt(1)],'YData',[yt,yt(1)],'ZData',[zt,zt(1)]);
    set(hQ1,'UData',nt(1),'VData',nt(2),'WData',nt(3));
    set(hL1,'XData',xt([idxRise,idxFall]),'YData',yt([idxRise,idxFall]),'ZData',zt([idxRise,idxFall]));
    set(hT1,'Position',[xt(idxRise),yt(idxRise),zt(idxRise)]);
    set(hT2,'Position',[xt(idxFall),yt(idxFall),zt(idxFall)]);
    set(hAn4,'String',['\Omega=',num2str(Omega/pi*180,'%.2f'),'\circ（升交点赤经）']);
    
    drawnow
    writeVideo(writer,getframe(gcf));
end

nt2 = nt;
xt2 = xt;
yt2 = yt;
zt2 = zt;
idxRise2 = idxRise;
idxFall2 = idxFall;

pause(1);
for i = 1:1*frameRate
    writeVideo(writer,getframe(gcf));
end

hL2 = plot3(xt([idxRise,idxFall]),yt([idxRise,idxFall]),zt([idxRise,idxFall]),'Marker','o','MarkerSize',1.5);
hT3 = text(xt(1),yt(1),zt(1),' 近地点');
hT4 = text(xt(1 + round(len/2)),yt(1 + round(len/2)),zt(1 + round(len/2)),' 远地点');
%% 近地点张角
for omega = linspace(0,pi/2,120)
    idxRise = 0;
    minAbsZ = inf;
    M3 = makehgtform('axisrotate',-nt,omega);
    m3 = M3(1:3,1:3);
    for el = 1:len
        tmp = [xt2(el),yt2(el),zt2(el)]*m3;
        xt(el) = tmp(1);
        yt(el) = tmp(2);
        zt(el) = tmp(3);
        if el ~= 1
            if zt(el) > zt(el-1) && abs(zt(el)) < minAbsZ
                minAbsZ = abs(zt(el));
                idxRise = el;
            end
        end
    end
    idxFall = idxRise + round(len/2);
    if idxFall > len
        idxFall = idxFall-len;
    end
    set(hOrbit,'XData',[xt,xt(1)],'YData',[yt,yt(1)],'ZData',[zt,zt(1)]);
    set(hL1,'XData',xt([idxRise2,idxFall2]),'YData',yt([idxRise2,idxFall2]),'ZData',zt([idxRise2,idxFall2]));
    set(hL2,'XData',xt([idxRise,idxFall]),'YData',yt([idxRise,idxFall]),'ZData',zt([idxRise,idxFall]));
    set(hT1,'Position',[xt(idxRise),yt(idxRise),zt(idxRise)]);
    set(hT2,'Position',[xt(idxFall),yt(idxFall),zt(idxFall)]);
    set(hT3,'Position',[xt(1),yt(1),zt(1)]);    
    set(hT4,'Position',[xt(1 + round(len/2)),yt(1 + round(len/2)),zt(1 + round(len/2))]);
    set(hAn5,'String',['\omega=',num2str(omega/pi*180,'%.2f'),'\circ（近地点张角）']);
    
    drawnow
    writeVideo(writer,getframe(gcf));
end

pause(1);
for i = 1:1*frameRate
    writeVideo(writer,getframe(gcf));
end

%% 转换视角
azView = 240:0.8:620;
elView = interp1([240,620],[10,30],azView);
for i = 1:length(azView)
    view([azView(i),elView(i)])
    
    drawnow
    writeVideo(writer,getframe(gcf));
end

pause(1);
for i = 1:1*frameRate
    writeVideo(writer,getframe(gcf));
end

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),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)]);
hAn6 = annotation('textbox',[.2, .60, 1, .04],'String',['f=',num2str(idx/len*360,'%.2f'),'\circ（真近点角）'],'LineStyle','none');

pause(1.5);
for i = 1:1.5*frameRate
    writeVideo(writer,getframe(gcf));
end

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

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',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(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;
    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
        el(t+1) = NaN;
    end

    drawnow
    writeVideo(writer,getframe(gcf));
end

pause(1);
for i = 1:1*frameRate
    writeVideo(writer,getframe(gcf));
end

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 - 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
end

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

for i = 1:3*frameRate
    writeVideo(writer,getframe(gcf));
end

close(writer);
msgbox('Video Writing Completed')