%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%---------Stereo-------%%%%%%%%%%%%%%%%%%%
%%%%%%%%Reconstructed 3D wave field elevation%%%%%%%
%%%%%%%%%%%-------SUMOS MISION 2021-------%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%The 3D point cloud is already computed using the WASS pipeline (Bergamasco
%2017)
% This script is written for reprojection
% Camera frame of coordinate ->World frame of coordinate
% and
% Save on NETCDF file the associated 3D field
% (c) g.marechal march 2021


%POUR LA PROCHAINE FOIS, IL FAUT FILTRER TOUTES LES DONNEES DE LA SBG, CF
%LE HS QUI TIRE AUTANT LA GUEULE QUE LES LE NUAGE DE POINTS
clc;clear;close all;clear path

set(0,'defaultAxesFontName', 'Serif')
set(0,'defaultTextFontN', 'Serif')
set(0,'DefaultAxesFontSize',16)
set(0,'DefaultTextFontSize',16)
set(0,'defaultfigurecolor',[1 1 1]*1)
scrsz = get(0,'ScreenSize');
%%
% files where frames are stored
stereo_dir=sprintf('D:/SUMOS_STEREO');
acquisition='TIF_20210304_083550'; %calm sea
%acquisition='TIF_20210219_082101';
%acquisition='TIF_20210223_073054';
%acquisition='TIF_20210222_120137';

listfile=sprintf('%s_filelist.txt',acquisition);
acqui_dir=sprintf('%s/%s',stereo_dir,acquisition);
save_path="C:/Users/stereo/Desktop/test_subplot";

% files where SBG data are stored
indir_SBG='C:\Users\stereo\Desktop\SBG_data\'; %sbg dir
infile_SBG='sbg_20210304_080000.dat'; %edit here with the associated sbg file %calm sea
%infile_SBG='sbg_20210219_080000.dat'; %edit here with the associated sbg file
%infile_SBG='sbg_20210223_070000.dat'; %edit here with the associated sbg file
%infile_SBG='sbg_20210222_120000.dat'; %edit here with the associated sbg file

fonctions_path='C:\Users\stereo\Desktop\MATLAB_SUMOS_Gmare\functions';
addpath(fonctions_path)
%% Specific data for SUMOS mission
% Rotation matrices to switch between the camera coordinate frame and the
% SBG coordinate frame
cx=cosd(58.9);sx=sind(58.9);  %test,58.9
%cx=cosd(90-34.44-2.66);sx=sind(90-34.44-2.66);  %=52.9
cz=cosd(-1.68);sz=sind(-1.68);
% MAGM M_Cam2SBG=[cz sz 0;-sz cz 0;0 0 1]*[ 1 0 0;0 cx +sx;0 -sx cx]*[-1,0,0;0,-1,0;0,0,1];
% MALM
%cx=cosd( 31.4);sx=sind( 31.4);   % Rotation en roulis, Louis Marie
cx=cosd( 31.05);sx=sind(31.05); %31.1
cy=cosd(  0.0);sy=sind(  0.0);   % Rotation en tangage
cz=cosd(-1.68);sz=sind(-1.68);  % Rotation en cap.

Mx=[  1   0  0;  0  cx -sx;  0  sx  cx];
My=[ cy   0 sy;  0   1   0;-sy   0  cy];
Mz=[ cz -sz  0; sz  cz   0;  0   0   1];
M_Cam2SBG=Mz*My*Mx*[-1,0,0;0,0,1;0,1,0];

%Lever arms zero SBG towards zero camera
% In SBG frame
% See in script Dessin Camera 1
LA_Cam1=[796;100;45]*1e-3;

% Baseline between the cameras.
Baseline = 4219e-3;

% Height of cameras above sea level.
Alt_Cameras=12.8; %perhaps modified during the cruise

J=0.000004; %factor for the MAP filtering
%% Load the specific acquisition 
i_plot=0; %1 for yes
min_frame=40; %(39, 40) do work!, 59,60 doesn't work 
max_frame=40;
step_frame=1;

min_offshore_view=-20; % distance from the camera toward offshore reconstructed
max_offshore_view=-100; % distance from the camera toward offshore reconstructed

%perspective : mettre un max flottant, reconstruire le champ de vagues
%jusqu'� que les trous dans le champ soient trop cons�quents. (ex : 50 m pour l'acquisition SUMOS 20210219_082101)

cpt=0;
GPS_ALT=[];
EKE_ACC_ALT=[];
MEAN_PLANE_ALT=[];
HS_ALT_WASS=[];
HS_ENU=[];
    

for i_frame=min_frame:step_frame:max_frame %for loop on all frame pairs
iidx=i_frame;
indir =sprintf('%s/%06d_wd',acqui_dir,iidx); %work on USB key
outdir = indir;
framesdir =  [outdir,'/frames/'];
mkdir( framesdir );



%%% plot raw image
%cd(indir)
img = imread(sprintf('%s/undistorted/00000001.png',indir)); %00000001 -> because cam 1

iptsetpref('ImshowBorder','loose');

imgrgb = repmat(img,[1,1,3]);

%decomment the folowing lines to plot the frame
    %hhfig = figure;
    %imshow(imgrgb);
%%% IMU
% Read IMU data

%IMU = sbg_load_fast(fname_sbg,save_mat,fname_out);


% 
% %S=load(fullfile(indir_SBG,infile_SBG));
% 

%dis_indir_frames=ls(sprintf('%s%06d*',indir_frames,iidx));

if i_frame==min_frame

S = sbg_load_fast(fullfile(indir_SBG,infile_SBG),0);%P.Sutherland function %verif here !!!
% 
S = sbg_imu_time_fields(S); %Peter Sutherland's function : strcuture that contain SBG info.
%there is one SBG file per hour -> record over 2 hours, needs to load 2
%SBG file (ex : 01:00 and 02:00.)

% %%%%%%%%%%
% %extract struct data at specific time
% %%%%%%%%%%
 %method of interpolation: nearest, very bad idea. (4 frames for 1 SBG
 %output)
EBTS=S.EVENT_B.TS_fix;
t  =interp1(S.UTC_TIME.TS_fix,S.UTC_TIME.time,EBTS,'linear');
Lon=interp1(S.EKF_NAV.TS_fix,S.EKF_NAV.LONGITUDE,EBTS,'linear');
Lat=interp1(S.EKF_NAV.TS_fix,S.EKF_NAV.LATITUDE ,EBTS,'linear');
Alt=interp1(S.EKF_NAV.TS_fix,S.EKF_NAV.ALTITUDE ,EBTS,'linear'); % alt from acceleration
Q0 =interp1(S.EKF_QUAT.TS_fix,S.EKF_QUAT.Q0,EBTS,'linear');
Q1 =interp1(S.EKF_QUAT.TS_fix,S.EKF_QUAT.Q1,EBTS,'linear');
Q2 =interp1(S.EKF_QUAT.TS_fix,S.EKF_QUAT.Q2,EBTS,'linear');
Q3 =interp1(S.EKF_QUAT.TS_fix,S.EKF_QUAT.Q3,EBTS,'linear');
Alt_GPS=interp1(S.GPS1_POS.TS_fix,S.GPS1_POS.ALT,EBTS,'linear');

disp('===================================================================')
disp('========== Filtering of the Altitude measured by the GPS ==========')
disp('===================================================================')
%%%%%%%%%%%%%%
%---Altitude filtering
%%%%%%%%%%%%%%

%rajout� un test sur la disponibilit� des variables de la SBG (quaternion,
%lon,lat, altitude, ...) Si data pas dispo. au moins coup� l'acquisition.

%save data in matlab format for the frame
%cd(indir)
%delete('sbg_20210219_080000.mat')
[path,name,ext]=fileparts(infile_SBG);
save(fullfile(indir,strrep(infile_SBG,'dat','mat')),'t','Lon','Lat','Alt','Q0','Q1','Q2','Q3');
S=load(fullfile(indir,sprintf('%s.mat',name)));

disp('===============The DCM====================')
disp('2(q1�+q2�)-1  2(q2q3-q1q4)   2(q1q3+q2q4)')
disp('2(q2q3+q1q4)  2(q1�+q3�)-1   2(q3q4-q1q2)')
disp('2(q2q4+q1q3)  2(q3q4+q1q2)   2(q1�+q4�)-1')
disp('==========================================')
end

%%%%%%%%%%%
%---READ .tif file and find the good frame + timestamp
%%%%%%%%%%%
text_file_read=fileread(sprintf('%s/%s',stereo_dir,listfile));
frame_id=sprintf('%06d',iidx);
startindex=regexp(text_file_read,frame_id);
filename=text_file_read(startindex:startindex+33);
ts=sscanf(filename,'%d_%ld_%d.tif'); %extract the timestamps in the filename
ts=ts(2)/1e7/86400+datenum(1601,1,1);
[~,imin]=min(abs(ts-S.t));



% SBG information
WGS=[S.Lat(imin);S.Lon(imin);S.Alt(imin)];  % altitude from SBG is never used

Q  =[S.Q0(imin);S.Q1(imin);S.Q2(imin);S.Q3(imin)];

% Construct the DCM from the SBG quaternions.

DCM=[2*(Q(1)^2+Q(2)^2)-1    ,2*(Q(2)*Q(3)-Q(1)*Q(4)), 2*(Q(1)*Q(3)+Q(2)*Q(4));...
     2*(Q(2)*Q(3)+Q(1)*Q(4)),2*(Q(1)^2+Q(3)^2)-1    , 2*(Q(3)*Q(4)-Q(1)*Q(2));...
     2*(Q(2)*Q(4)-Q(1)*Q(3)),2*(Q(3)*Q(4)+Q(1)*Q(2)), 2*(Q(1)^2+Q(4)^2)-1   ];

roll =atan2d(DCM(3,2),DCM(3,3));
pitch=asind(DCM(3,1));
yaw  =atan2d(DCM(2,1),DCM(1,1));

DCMyaw=[cosd(yaw) -sind(yaw) 0;sind(yaw) cosd(yaw) 0;0 0 1];


%%% cloud points
cd(indir)
addpath(fonctions_path)
mesh = load_camera_mesh();%The reconstructed point cloud (3xNpoints), which dimension is x,y or z?
Nd=1;

mesh=mesh(1:end,1:Nd:end);
n_pts=size(mesh,2);

%%%
    %apply the transformation btw sbg and camera (rotation + translation)
        %apply the transformation btw sbg and camera (rotation + translation)
% MAGM
plane=importdata('plane.txt');

a=plane(1);
b=plane(2);
c=plane(3);
d=plane(4);
q=(1-c)/(a.^2+b.^2);
R=eye(3);
T=zeros(3,1);
R(1,1)=1-a*a*q;
R(1,2)=-a*b*q;
R(1,3)=-a;
R(2,1)=-a*b*q;
R(2,2)=1-b*b*q;
R(2,3)=-b;
R(3,1)=a;
R(3,2)=b;
R(3,3)=c;

T(1)=0;
T(2)=0;
T(3)=d;

mesh_sea0=R*mesh+repmat(T,1,n_pts);
mesh_sea=Baseline*mesh_sea0;


ind=find(mesh_sea(2,:)>=max_offshore_view & mesh_sea(2,:) <=min_offshore_view); %60m normally (threshold good reconstruction, edit it for other acquisitions)
    
mesh_sea=mesh_sea(:,ind);
% mesh_SBG=M_Cam2SBG*Baseline*mesh+ repmat(LA_Cam1,1,n_pts); %WASS consistency

% LM
% Dans les axes de la SBG.
%mesh_SBG_orig = M_Cam2SBG*Baseline*mesh+LA_Cam1; 
mesh_SBG = M_Cam2SBG*Baseline*mesh+LA_Cam1; 


mesh_SBG=mesh_SBG(:,ind); %I comment here

% Dans les axes g�ographiques.
mesh_NED = DCM * mesh_SBG;
%mesh_NED(3,:)=mesh_NED(3,:)-Alt_Cameras;
% Un truc interm�diaire avec X vers l'arri�re du bateau, Y vers tribord, Z vers le haut
mesh_ATU = [-1 0 0;0 1 0;0 0 -1]*DCMyaw' * mesh_NED;
% Dans des axes g�ographiques Est, North, Up
mesh_ENU = [ 0 1 0;1 0 0; 0 0 -1]*mesh_NED;

H=-d*Baseline; %Alti. based on the mean plane (directly from WASS)



%%%%%%%%%%
%---Compute Hs
%%%%%%%%%%
vec=[1:1:size(mesh_ENU,2)];

mesh0=mesh_sea(3,vec);%WASS
mesh0_ENU=mesh_ENU(3,vec);% Our Algorithm, the sum (+H) is useless in a std calculation
hs_WASS=4*sqrt(var(-mesh0(:)));
hs_ENU=4*sqrt(var(mesh0_ENU(:))); 

HS_ALT_WASS=[HS_ALT_WASS,hs_WASS];
HS_ENU=[HS_ENU,hs_ENU];
%     
figure(8),clf
plot(HS_ALT_WASS)
hold on
plot(HS_ENU)  
xlabel('Frames Id.')
ylabel('Hs[m]')
legend('From WASS','from SBG')


figure
plot(-mesh0)
hold on
plot(mesh0_ENU+13.6)


    %mesh_DCM=DCM*mesh_SBG;
    
    %WGS coordinates, switching to ENU instead of NED
    %mesh_WGS=[mesh(2,:)/1852/60/cosd(WGS(1));mesh(1,:)/1852/60;-mesh(3,:)];
    % Then add the SBG location to the WGS coordinates
    %mesh_WGS(1,:)=mesh_WGS(1,:)+WGS(2); %north -> east
    %mesh_WGS(2,:)=mesh_WGS(2,:)+WGS(1); %east -> north
    %mesh_WGS(3,:)=mesh_WGS(3,:)+Alt_Cameras;
    
%%% PLOT
    %plot cloud point in ENU coordinate
    %cmap=redblue(256);
    if i_plot==1 %plot, yes/no, 1/0
        
    figure(2),clf
    %subplot(1,2,1)
    colormap jet
    %scatter3(mesh_WGS(1,vec),mesh_WGS(2,vec),Alt_Cameras-mesh_WGS(3,vec),1,Alt_Cameras-mesh_WGS(3,vec))%put all in WGS
    %scatter3(mesh_ENU(1,vec),mesh_ENU(2,vec),mesh_ENU(3,vec),1,mesh_ENU(3,vec))
    scatter3(mesh_ENU(1,vec),mesh_ENU(2,vec),mesh_ENU(3,vec)+(S.Alt(imin)-Alt_GPS(imin)+Alt_Cameras),1,mesh_ENU(3,vec)+(S.Alt(imin)-Alt_GPS(imin)+Alt_Cameras))

    title('mesh_ENU with 12.8 + SBG accelerometer','interpreter','none')
    axis equal
    caxis([-3,3])
    colorbar
    
    xlabel('X[m]')
    ylabel('Y[m]')


%     subplot(1,2,2)
%     plot(t,S.Alt)
%     hold on
%     plot(t,ones(1,length(t))*Alt_Cameras,'k')
%     plot(t(imin),S.Alt(imin),'x','Markersize',20,'color','r')
%     title(sprintf('%d',iidx))
%     xlabel('variable time SBG')
%     ylabel('variables Alt SBG')
    %cd('C:\Users\stereo\Desktop\3D_reconstructed_20210219\');
    
    %saveas(gcf,sprintf('%1.f.png',cpt))
    vec=[1:20:size(mesh_sea,2)];
    
    cpt=cpt+1;
    fig_0=figure(4),clf
    colormap jet
    % ajouter un moins sur x.. pas compris pourquoi mais c'est mieux...
    scatter3(-mesh_sea(1,vec),mesh_sea(2,vec),-mesh_sea(3,vec),1,-mesh_sea(3,vec))
    title('mesh sea')

%    title(sprintf('Mean eta = %f m',mean(Alt_Cameras-mesh_DCM(3,vec))))
    axis equal
    caxis([-3,3])
    colorbar
    
    figure(5),clf
    colormap jet
    scatter3(mesh_ENU(1,vec),mesh_ENU(2,vec),mesh_ENU(3,vec)+Alt_Cameras,1,mesh_ENU(3,vec)+Alt_Cameras)
    title('mesh_ENU with Alt = 12.8','interpreter','none')
    axis equal
    caxis([-3,3])
    colorbar 
    
    %mesh_NED(3,:)=mesh_NED(3,:)-Alt_Cameras;

    figure(55),clf
    colormap jet
    scatter3(mesh_ENU(1,vec),mesh_ENU(2,vec),mesh_ENU(3,vec)+H,1,mesh_ENU(3,vec)+H)
    title('mesh_ENU with Altitude from mean plane','interpreter','none')
    axis equal
    caxis([-3,3])
    colorbar 

    %%%%%%%%%%%%%%%
    %----SUBPLOTS
    %%%%%%%%%%%%%%%
    
    fig=figure(100),clf
    subplot(2,2,1)
    colormap jet
    scatter3(mesh_ENU(1,vec),mesh_ENU(2,vec),mesh_ENU(3,vec)+H,1,mesh_ENU(3,vec)+H)
    title('mesh_ENU with Altitude from mean plane','interpreter','none')
    axis equal
    caxis([-3,3])
    colorbar
    
    subplot(2,2,2)
   colormap jet
    %scatter3(mesh_WGS(1,vec),mesh_WGS(2,vec),Alt_Cameras-mesh_WGS(3,vec),1,Alt_Cameras-mesh_WGS(3,vec))%put all in WGS
    %scatter3(mesh_ENU(1,vec),mesh_ENU(2,vec),mesh_ENU(3,vec),1,mesh_ENU(3,vec))
    %scatter3(mesh_ENU(1,vec),mesh_ENU(2,vec),mesh_ENU(3,vec)+(S.Alt(imin)-Alt_GPS(imin)+Alt_Cameras),1,mesh_ENU(3,vec)+(S.Alt(imin)-Alt_GPS(imin)+Alt_Cameras))
    scatter3(mesh_ENU(1,vec),mesh_ENU(2,vec),mesh_ENU(3,vec)+(S.Alt(imin)),1,mesh_ENU(3,vec)+(S.Alt(imin)))
    title('mesh_ENU with standalone SBG Kalman filter','interpreter','none')
    %title('mesh_ENU with 12.8+ SBG accelerometer','interpreter','none')
    axis equal
    caxis([-3,3])
    colorbar
    
    xlabel('X[m]')
    ylabel('Y[m]')

    
    subplot(2,2,[3,4])
    img = imread('undistorted/00000001.png');

    iptsetpref('ImshowBorder','loose');

    imgrgb = repmat(img,[1,1,3]);
    %img = imread(sprintf('%s%s',indir,file_img));

    colormap jet
    imshow(imgrgb);
    set(gcf, 'Position', get(0, 'Screensize'));
    saveas(fig,sprintf("%s/test_%i.png",save_path,cpt))
    end
    disp(H);
    disp(S.Alt(imin));
    disp(Alt_GPS(imin));
    %ALT.
    GPS_ALT=[GPS_ALT,Alt_GPS(imin)];
    EKE_ACC_ALT=[EKE_ACC_ALT,S.Alt(imin)];
    MEAN_PLANE_ALT=[MEAN_PLANE_ALT,H];


  
    %%%%%%%%%%%%
    %----Plot Alt.
    %%%%%%%%%%%%
    if cpt==99
    figure(12),clf
    plot(GPS_ALT)
    hold on
    plot(EKE_ACC_ALT)
    plot(MEAN_PLANE_ALT)

    legend('Alt. GPS','Alt. SBG','Alt. WASS')
    xlabel('frames number')
    ylabel('Altitude [m]')
    end
  
    %%%%%%%%%%%%
    %----Plot hs woth respect to the different method.
    %%%%%%%%%%%%
    
    
    
%     figure(6),clf
%     colormap jet
%     scatter3(mesh_SBG(1,vec),mesh_SBG(2,vec),Alt_Cameras-mesh_SBG(3,vec),1,Alt_Cameras-mesh_SBG(3,vec))
%     title('mesh SBG with Altitude at 12.8m')
% %    title(sprintf('Mean eta = %f m',mean(Alt_Cameras-mesh_DCM(3,vec))))
%     axis equal
%     caxis([-3,3])
%     colorbar 
    
end
figure(8),clf
plot(HS_ALT_WASS)
hold on
plot(HS_ENU)  
legend('Hs mean plane WASS','Hs mean plane SBG')

  %%
  