% n0=	[-1944902.1250000000 4077521.2500000000 4487326.5000000000 ];
% n1=	[-1944873.0000000000 4077511.0000000000 4487348.5000000000 ];
% n2=	[-1944922.0000000000 4077512.0000000000 4487326.5000000000 ];
% n3=	[-1944931.3750000000 4077531.5000000000 4487305.0000000000 ];
% n4=	[-1944882.3750000000 4077530.7500000000 4487326.5000000000 ];
% 
% n01 = n1 - n0, n02 = n2 - n0, n03 = n3 - n0, n04 = n4 - n0;
% 
% VN=(cross(n01, n04) + cross(n04, n03) + cross(n03, n02) + cross(n02, n01)) * 0.25;
% VNorm=VN./norm(VN);
clear,clc,close all


% 打开二进制文件
fileID = fopen('E:\\LAMPCAE_SCANE\\outTestEcho\\GF3_Simulation.gpspos.data', 'rb');  % 假设二进制文件名为 'data.bin'

% 定义每个数据字段的数据类型
% 假设每个数据是双精度浮动数（8字节）
dataType = 'double';

% 读取数据：根据你的数据格式，数据长度为 16 * 8 字节（每个元素8字节）
% 例如：每16个数据项为一组（对应prf_id）
numFields = 19;  % 每组数据的字段数
PRFnumRecords = 15382;  % 假设有100个记录，你可以根据实际情况修改

% 读取所有记录的数据
rawData = fread(fileID, numFields * PRFnumRecords, dataType);

% 关闭文件
fclose(fileID);

% 解析数据为结构体数组
data = struct();
AntDirecX=zeros(PRFnumRecords,1);
AntDirecY=zeros(PRFnumRecords,1);
AntDirecZ=zeros(PRFnumRecords,1);
Px=zeros(PRFnumRecords,1);
Py=zeros(PRFnumRecords,1);
Pz=zeros(PRFnumRecords,1);

Vx=zeros(PRFnumRecords,1);
Vy=zeros(PRFnumRecords,1);
Vz=zeros(PRFnumRecords,1);

% 假设你的数据在 rawData 中按顺序排列
% 例如，解析 prf_id = 0, 1, 2...的相关数据
for prf_id = 0:PRFnumRecords-1
    idx = prf_id * numFields + 1;
    data(prf_id+1).prf_time = rawData(idx);          % prf_time + imageStarttime
    Px(prf_id+1) = rawData(idx+1);              % sateOirbtNode.Px
    Py(prf_id+1) = rawData(idx+2);              % sateOirbtNode.Py
    Pz(prf_id+1) = rawData(idx+3);              % sateOirbtNode.Pz
    Vx(prf_id+1) = rawData(idx+4);              % sateOirbtNode.Vx
    Vy(prf_id+1) = rawData(idx+5);              % sateOirbtNode.Vy
    Vz(prf_id+1) = rawData(idx+6);              % sateOirbtNode.Vz
    AntDirecX(prf_id+1) = rawData(idx+7);       % sateOirbtNode.AntDirecX
    AntDirecY(prf_id+1) = rawData(idx+8);       % sateOirbtNode.AntDirecY
    AntDirecZ(prf_id+1) = rawData(idx+9);       % sateOirbtNode.AntDirecZ
    data(prf_id+1).AVx = rawData(idx+10);            % sateOirbtNode.AVx
    data(prf_id+1).AVy = rawData(idx+11);            % sateOirbtNode.AVy
    data(prf_id+1).AVz = rawData(idx+12);            % sateOirbtNode.AVz
    data(prf_id+1).lon = rawData(idx+13);            % outP.lon
    data(prf_id+1).lat = rawData(idx+14);            % outP.lat
    data(prf_id+1).ati = rawData(idx+15);            % outP.ati
end

% 查看解析后的数据
disp(data);



fileID = fopen('D:\\Programme\\vs2022\\RasterMergeTest\\TestData\\outData\\tmp\\ImageX.dat', 'rb');  % 假设二进制文件名为 'data.bin'
dataType = 'float';
numFields = 5400;  % 每组数据的字段数
numRecords = 5400;  % 假设有100个记录，你可以根据实际情况修改
demX = fread(fileID, numFields * numRecords, dataType);
demX=reshape(demX,[numRecords,numRecords]);
fclose(fileID);


fileID = fopen('D:\\Programme\\vs2022\\RasterMergeTest\\TestData\\outData\\tmp\\ImageY.dat', 'rb');  % 假设二进制文件名为 'data.bin'
dataType = 'float';
numFields = 5400;  % 每组数据的字段数
numRecords = 5400;  % 假设有100个记录，你可以根据实际情况修改
demY = fread(fileID, numFields * numRecords, dataType);
demY=reshape(demY,[numRecords,numRecords]);
fclose(fileID);

fileID = fopen('D:\\Programme\\vs2022\\RasterMergeTest\\TestData\\outData\\tmp\\ImageZ.dat', 'rb');  % 假设二进制文件名为 'data.bin'
dataType = 'float';
numFields = 5400;  % 每组数据的字段数
numRecords = 5400;  % 假设有100个记录，你可以根据实际情况修改
demZ = fread(fileID, numFields * numRecords, dataType);
demZ=reshape(demZ,[numRecords,numRecords]);
fclose(fileID);


%% 绘制成像平面
Rn=884053;
Rf=942069;


data_ant_norm=(AntDirecX.^2+AntDirecY.^2+AntDirecZ.^2).^0.5;

AntDirecX=AntDirecX./data_ant_norm;
AntDirecY=AntDirecY./data_ant_norm;
AntDirecZ=AntDirecZ./data_ant_norm;

Rlist=linspace(Rn,Rf,300);
ImageX=zeros(PRFnumRecords,300);
ImageY=zeros(PRFnumRecords,300);
ImageZ=zeros(PRFnumRecords,300);

for i=1:PRFnumRecords
ImageX(i,:)=Px(i,:) + Rlist.*AntDirecX(i,:);
ImageY(i,:)=Py(i,:) + Rlist.*AntDirecY(i,:);
ImageZ(i,:)=Pz(i,:) + Rlist.*AntDirecZ(i,:);
end
B=[AntDirecX(1,1),AntDirecY(1,1),AntDirecZ(1,1)];
A=[Px(1,1)-Px(2,1),Py(1,1)-Py(2,1),Pz(1,1)-Pz(2,1)] ;
V=[Vx(1,1),Vy(1,1),Vz(1,1)];
% 计算点积
dotProduct = dot(A,B);

% 计算模长
normA = norm(A);
normB = norm(B);

% 计算夹角的余弦值
cosTheta = acos(dotProduct / (normA * normB));
cosTheta*180/pi

tp=[-2023401.250000,4104205.500000,4428297.000000];
TP=Px.*0;
for i=1:PRFnumRecords
TP(i,:)=sqrt((Px(i,:) -tp(1)).^2+(Py(i,:) -tp(2)).^2+(Pz(i,:) -tp(3)).^2);

end

figure,
plot(TP )



PR=(Px.^2+Py.^2+Pz.^2).^0.5;



ImageR=(ImageX.^2+ImageY.^2+ImageZ.^2).^0.5;


figure,
plot(ImageR(:,1)),hold on
plot(ImageR(:,end)),hold on
plot(PR),hold on
legend(["Rn","Rf","Rs"])

a=[Px-mean(demX,"all"),Py-mean(demY,"all"),Pz-mean(demZ,"all")];
a=(a(:,1).^2+a(:,2).^2+a(:,3).^2).^0.5;



figure,
surfl(ImageX,ImageY,ImageZ),shading flat,hold on
scatter3(mean(demX,"all"),mean(demY,"all"),mean(demZ,"all"),'red','filled'),shading flat,hold on
plot3(Px,Py,Pz,'red')
xlabel("X")
ylabel("Y")
zlabel("Z")
view([-90.6000010944003 90]);


dl=sqrt((Px(1)-Px(end)).^2+(Py(1)-Py(end)).^2+(Pz(1)-Pz(end)).^2);

R=7;
ImageR=((Px(10000,1)-R-Px).^2+(Py(10000,1)-R-Py).^2+(Pz(10000,1)-R-Pz).^2).^0.5;
figure,
plot(ImageR)


P=zeros(100*100,3);
pp=1;
for i=1:100
    for j=1:100
        [latitude,longitude,height]=XYZ2LLA(ImageX(i,j),ImageY(i,j),ImageZ(i,j));
        P(pp,:)=[latitude,longitude,height];
        pp=pp+1;
    end
end





%  %% 计算
% 
% 
% Tx=-2001212.875000; 
% Ty=4082207.000000;  
% Tz=4471714.500000;
% R=((demX-Px(1)).^2+(demY-Py(1)).^2+(demZ-Pz(1)).^2).^0.5;
% figure,
% plot(R)
% 
% imgX=ImageX(3000,150);
% imgY=ImageY(3000,150);
% imgZ=ImageZ(3000,150);
% R1=  sqrt((ImageX(1,1)-Px(:,1)).^2+(ImageY(1,1)-Py(:,1)).^2+(ImageZ(1,1)-Pz(:,1)).^2);
% figure,
% plot(R1)
% 
% 
% 
% Timgx=abs(ImageX-Tx);
% Timgy=abs(ImageY-Ty);
% Timgz=abs(ImageZ-Tz);
% 
% [latitude,longitude,height]=XYZ2LLA(ImageX(3000,150),ImageY(3000,150),ImageZ(3000,150))
% 
% 
%