close all

omegae=7.292115018682096e-05;
re=6378160;

daystart=2; %starting day 1 to 365
daycount=3; %number of days to plot
i0=floor(daystart*2*pi/omegae/150);
i1=i0+floor(daycount*2*pi/omegae/150);
%i0=1; i1=length(t); %plot everything

plt=figure('Position', [100, 100, 400, 300]);
cphi=linspace(0,pi,30);
ctheta=linspace(0,2*pi,40);
[cphi,ctheta]=meshgrid(cphi,ctheta);
c1=0.9*re*sin(cphi).*cos(ctheta);
c2=0.9*re*sin(cphi).*sin(ctheta);
c3=0.9*re*cos(cphi); 
surf(c1, c2, c3, ones(size(c1)))
hold on
plt1=plot3(reshape(x(1,1,i0:i1),[1 i1-i0+1]),reshape(x(1,2,i0:i1),[1 i1-i0+1]),reshape(x(1,3,i0:i1),[1 i1-i0+1]),'b');
for j=2:size(x,1)
plt2=plot3(reshape(x(j,1,i0:i1),[1 i1-i0+1]),reshape(x(j,2,i0:i1),[1 i1-i0+1]),reshape(x(j,3,i0:i1),[1 i1-i0+1]),'r');
end
hold off
xlabel('x [m]')
ylabel('y [m]')
zlabel('z [m]')
legend([plt1 plt2],'chief','deputies')
title('Orbit in ECEF frame')
axis equal

figure('Position', [100, 100, 400, 300])
plot3(reshape(x(1,1,i0:i1),[1 i1-i0+1]),reshape(x(1,2,i0:i1),[1 i1-i0+1]),reshape(x(1,3,i0:i1),[1 i1-i0+1]),'b');
hold on
for j=2:size(x,1)
plot3(reshape(x(j,1,i0:i1),[1 i1-i0+1]),reshape(x(j,2,i0:i1),[1 i1-i0+1]),reshape(x(j,3,i0:i1),[1 i1-i0+1]),'r');
end
hold off
xlabel('x [m]')
ylabel('y [m]')
zlabel('z [m]')
legend('chief','deputies')
title('Orbit deviation in ECEF frame')
axis equal

figure('Position', [100, 100, 400, 300])
plot3(reshape(targH(1,1,i0:i1),[1 i1-i0+1]),reshape(targH(1,2,i0:i1),[1 i1-i0+1]),reshape(targH(1,3,i0:i1),[1 i1-i0+1]),'b')
hold on
for j=1:size(x,1)-1
plot3(reshape(H(j,1,i0:i1),[1 i1-i0+1]),reshape(H(j,2,i0:i1),[1 i1-i0+1]),reshape(H(j,3,i0:i1),[1 i1-i0+1]),'r')
plot3(reshape(H(j,1,1),[1 1]),reshape(H(j,2,1),[1 1]),reshape(H(j,3,1),[1 1]),'ro',reshape(H(j,1,end),[1 1]),reshape(H(j,2,end),[1 1]),reshape(H(j,3,end),[1 1]),'rx')
end
hold off
xlabel('x [m]')
ylabel('y [m]')
zlabel('z [m]')
legend('Reference orbit','Relative orbit','propagation start','propagation end')
title('Relative orbit in Hill frame')
axis equal

j=5;

errHact=H-targH;
figure('Position', [100, 100, 400, 300])
plot(t(i0:i1)./(2*pi/omegae),reshape(errHact(j,1:3,i0:i1),[3,i1-i0+1]));
xlabel('time [sidereal days]')
ylabel('Deviation [m]')
title('Deviation from Reference Orbit')
legend('x','y','z')

out1=reshape(sqrt(errHact(:,1,:).^2+errHact(:,2,:).^2+errHact(:,3,:).^2),[9,length(t)]);
figure('Position', [100, 200, 400, 300])
plot(t(i0:i1)./(2*pi/omegae),out1(:,i0:i1));
xlabel('time [sidereal days]')
ylabel('Deviation [m]')
title('Deviation from Reference Orbit')
mystr=cell(9,1);
for i=1:9
    mystr(i,1)=cellstr(sprintf('Deputy %d',i));
end
legend(mystr)

figure
plot(t(i0:i1)./(2*pi/omegae),reshape(Fa(j+1,1:3,i0:i1),[3,i1-i0+1])*70);
xlabel('time [sidereal days]')
ylabel('Actuation force [N]')
title('Actuation Force in Hill Frame')
legend('x','y','z')

figure('Position', [100, 100, 400, 300])
plot(t(i0:i1)./(2*pi/omegae),reshape(movmean(Fa_req(1,1:3,i0:i1),50,3),[3,i1-i0+1])*70e6);
xlabel('time [sidereal days]')
ylabel('Actuation force [N]')
title('Required Actuation Force in Hill Frame')
legend('x','y','z')

figure('Position', [100, 100, 400, 300])
plot(t(i0:i1)./(2*pi/omegae),(reshape(a_SRP(j+1,1:3,i0:i1),[3,i1-i0+1])-reshape(a_SRP(1,1:3,i0:i1),[3,i1-i0+1]))*70);
xlabel('time [sidereal days]')
ylabel('Force [N]')
title('Differential Solar Radiation Pressure')
legend('x','y','z')

if(1)
figure
plot(t(i0:i1)./(2*pi/omegae),reshape(measH(j,1:3,i0:i1)-H(j,1:3,i0:i1),[3,i1-i0+1]));
xlabel('time [sidereal days]')
ylabel('Deviation [m]')
title('Error in state vector measurement')
legend('x','y','z')

Herr=measH(:,1:3,:)-H(:,1:3,:);
Herr=sqrt(Herr(:,1,:).^2+Herr(:,2,:).^2+Herr(:,3,:).^2);
figure('Position', [100, 200, 400, 300])
plot(t(i0:i1)./(2*pi/omegae),reshape(Herr(:,i0:i1),[9,i1-i0+1]));
xlabel('time [sidereal days]')
ylabel('Error Magnitude [m]')
title('Error in state vector measurement')
mystr=cell(9,1);
for i=1:9
    mystr(i,1)=cellstr(sprintf('Deputy %d',i));
end
legend(mystr)

figure('Position', [100, 100, 400, 300])
plot3(reshape(measH(j,1,i0:i1)-H(j,1,i0:i1),[1,i1-i0+1]),reshape(measH(j,2,i0:i1)-H(j,2,i0:i1),[1,i1-i0+1]),reshape(measH(j,3,i0:i1)-H(j,3,i0:i1),[1,i1-i0+1]));
xlabel('x [m]')
ylabel('y [m]')
zlabel('z [m]')
title('Error in state vector measurement')
axis equal

figure
plot(t(i0:i1)./(2*pi/omegae),reshape(1e6*Tout_d(j,:,i0:i1),[12,i1-i0+1]));
xlabel('time [sidereal days]')
ylabel('Thrust [\mu N]')
title('Thruster Output')
legend('Thruster 1','Thruster 2','Thruster 3','Thruster 4','Thruster 5','Thruster 6','Thruster 7','Thruster 8','Thruster 9','Thruster 10','Thruster 11','Thruster 12')

Taux=reshape(sum(Tout_d(:,:,:),2),[9 length(t)]);
Taux2=sum(Taux*150,2)/70/(t(end)/3600/24)*365.25;
mystr=cell(9,1);
for i=1:9
    mystr(i,1)=cellstr(sprintf('Deputy %d - %3.1f m/s/yr',i,Taux2(i)));
    mystr(i,1)=cellstr(sprintf('Deputy %d',i));
end
figure('Position', [100, 200, 400, 300])
out3=1e6*Taux;
plot(t(i0:i1)./(2*pi/omegae),out3(:,i0:i1))
xlabel('time [siderealdays]')
ylabel('Thrust [\mu N]')
title('Total Thrust')
legend(mystr)
clear Taux

if(0)
    figure
plot(t(i0:i1)./(2*pi/omegae),msato);
xlabel('time [sidereal days]')
ylabel('Mass [kg]')
title('Satellite mass propagation')
legend('Cheif sat','Deputy sat 1','Deputy sat 2','Deputy sat 3','Deputy sat 4','Deputy sat 5','Deputy sat 6','Deputy sat 7','Deputy sat 8','Deputy sat 9')
end
end