%% Simulation Tool for Underwater Laser Triangulation
%% 2022, Hannes Sardemann, IPF, TU Dresden

clc
clearvars
close all

%% Input Parameter:
% Refraction Indices:
nAir = 1.000292; % Air
nGlass = 1.45;   % Glass
nWater = 1.3330;  % Water

% Interface air-glass:
thicknessGlass = 4; % (mm)
zGlass = 0;  % heightof Interface Air-Glass (mm)

% Interface glass - water:
zWater = zGlass-thicknessGlass; % height of interface glass-water (mm)

% Ground:
zGround = -100; % height of planar ground surface (mm)

% Position Laser:
xLaser = 70;
yLaser = 0;
zLaser = 20;
positionLaser = [xLaser;yLaser;zLaser];

% Orientation Laser:
omegaL = 0 * pi/180;
phiL = 45 * pi/180;
kappaL = 0 * pi/180;

% Laser opening angle:
fovL = 90;   % deg

% Position Camera:
X0 = [-70;0;20];  % (mm)

% Orientation Camera:
omegaC = 0 * pi/180;    
phiC = 135 * pi/180;   
kappaC = 90 * pi/180; 
RCamera =  eul2rotm([omegaC,phiC,kappaC],'XYZ');
MCamera = [[RCamera,X0];[0,0,0,1]];

% Interior Orientation Camera:
c = 35; % focal length
xmm = 36; % sensor size x in mm
ymm = 24; % sesnor size y in mm
xpixels = 360;  % sensor size x pixels
ypixels = 240;  % sensor size y pixels
sensor = [xmm,ymm,xpixels,ypixels];
IOR = [c;zeros(13,1)];

%% RayTracing Laser
[IntersectionsLaserGlass,vectorsLaserGlass,...
    IntersectionsLaserWater,vectorsLaserWater,IntersectionsLaserGround]  =...
    raytracingLaser(positionLaser,omegaL,phiL,kappaL,fovL,1000,...
    zGlass,zWater,zGround,nAir,nGlass,nWater);

% plot laser:
figure
ax1 = gca;
hold(ax1,'on')
grid(ax1,'on')
axis(ax1,'equal');
xlabel(ax1,'x (mm)')
ylabel(ax1,'y (mm)')
zlabel(ax1,'z (mm)')
rotate3d(ax1,'on')
plotLaserPolygon(ax1,positionLaser,IntersectionsLaserGlass,IntersectionsLaserWater,IntersectionsLaserGround,'g')
plotLaserdiode(ax1,omegaL,phiL,kappaL,positionLaser)

% Plot Ground Points:
plot3(ax1,IntersectionsLaserGround(1,:),IntersectionsLaserGround(2,:),IntersectionsLaserGround(3,:),'r','LineWidth',3)


%% Raytracing Camera:
[measurementImage,xyzImageWorld,laserpixel,...
    borderPixelX,borderPixelY,imageBorderWorld,IntersectionsImageborderGlass,...
    IntersectionsImageborderWater,vectorsImageborderWater,IntersectionsImageborderGround]  = ...
    raytracingCamera(X0,omegaC,phiC,kappaC,c,xmm,ymm,xpixels,ypixels,...
    zGlass,zWater,zGround,nAir,nGlass,nWater,IntersectionsLaserGround);

% Plot interfaces:
xminPlanes = round(min([imageBorderWorld(1,:),xLaser,X0(1),IntersectionsImageborderGround(1,:),IntersectionsLaserGround(1,:)]));
yminPlanes = round(min([imageBorderWorld(2,:),xLaser,X0(2),IntersectionsImageborderGround(2,:),IntersectionsLaserGround(2,:)]));
xmaxPlanes = round(max([imageBorderWorld(1,:),xLaser,X0(1),IntersectionsImageborderGround(1,:),IntersectionsLaserGround(1,:)]));
ymaxPlanes = round(max([imageBorderWorld(2,:),xLaser,X0(2),IntersectionsImageborderGround(2,:),IntersectionsLaserGround(2,:)]));
interfaceX = [xminPlanes,xmaxPlanes,xmaxPlanes,xminPlanes,xminPlanes];
interfaceY = [ymaxPlanes,ymaxPlanes,yminPlanes,yminPlanes,ymaxPlanes];
interfaceZ1 = [zGlass,zGlass,zGlass,zGlass,zGlass];
interdaceZ2 = [zWater,zWater,zWater,zWater,zWater];

s1 = surf(ax1,[interfaceX;interfaceX],[interfaceY;interfaceY],[interfaceZ1;interdaceZ2],'EdgeColor','none','FaceColor','b');
p1 = fill3(ax1,interfaceX', interfaceY', interfaceZ1','b','EdgeColor','none');
p2 = fill3(ax1,interfaceX', interfaceY', interdaceZ2','b','EdgeColor','none');
alpha(s1,0.5);
alpha(p1,0.5);
alpha(p2,0.5);

plotPlane(ax1,[0;0;zGround],[0;0;1],xminPlanes,yminPlanes,xmaxPlanes,ymaxPlanes)

% Plot camera field of view:
plotCameraRays(ax1,X0,imageBorderWorld,...
    IntersectionsImageborderGlass,IntersectionsImageborderWater,IntersectionsImageborderGround)

% plot measurement image in 3D model:
plotMeasurmentImage3D(ax1,xmm,ymm,xpixels,ypixels,c,MCamera,imageBorderWorld,measurementImage)

% plot measurement image (2D)
figure
ax3 = gca;
imshow(measurementImage)
hold(ax3,'on');
title(ax3,'Measurement Image')

%% Image Measurement (manually, can be replaced by LSM)
% [xpix,ypix] = ginput(1);
xpix = 102;
ypix = 189;
plot(ax3,xpix,ypix,'rx','MarkerSize',20,'LineWidth',3)

[imagePointWorld,intersectionImageGlass,vectorImageGlass,...
    intersectionImageWater,vectorImageWater,...
    intersectionLaserGlass,intersectionLaserWater,vectorLaserWater,...
    intersectionPoint3D,distance] = ...
    Imagepoint_Laser_intersection_iterative(xpix,ypix,c,sensor,MCamera,...
    nAir,nGlass,nWater,zGlass,zWater,...
    [xLaser;yLaser;zLaser],omegaL,phiL,kappaL,fovL,IOR);


% Plot image ray:
lengthRays = (zGlass-zGround)*1.2;
plot3(ax1,[imagePointWorld(1),intersectionImageGlass(1),intersectionImageWater(1)],...
    [imagePointWorld(2),intersectionImageGlass(2),intersectionImageWater(2)],...
    [imagePointWorld(3),intersectionImageGlass(3),intersectionImageWater(3)],'r','LineWidth',1)
quiver3(ax1,intersectionImageWater(1),intersectionImageWater(2),intersectionImageWater(3),...
    lengthRays*vectorImageWater(1),lengthRays*vectorImageWater(2),lengthRays*vectorImageWater(3),...
    'AutoScale','off','Color','r','LineWidth',1)
plot3(ax1,intersectionPoint3D(1),intersectionPoint3D(2),intersectionPoint3D(3),'rx','MarkerSize',5,'LineWidth',1)

% Plot sub-beam:
plot3(ax1,[xLaser,intersectionLaserGlass(1),intersectionLaserWater(1)],...
    [yLaser,intersectionLaserGlass(2),intersectionLaserWater(2)],...
    [zLaser,intersectionLaserGlass(3),intersectionLaserWater(3)],'r','LineWidth',1)
quiver3(ax1,intersectionLaserWater(1),intersectionLaserWater(2),intersectionLaserWater(3),...
    lengthRays*vectorLaserWater(1),lengthRays*vectorLaserWater(2),lengthRays*vectorLaserWater(3),...
    'AutoScale','off','Color','r','LineWidth',1)


