%% Bottom Normal Simulation (BottomNormalSim.m)
% This simulation is intended to verify math solutions for resolving the bottom
% normal from beam range data collected from a downward looking janus array with
% a known beam angle. An assumption of perfectly narrow beam width is here to
% start. Beams are located at 45 degrees offset quadrants from 0 (vehicle forward)
%% Create Environment

clear; clf;

% In meters
dx = 0.05
x = -5:dx:5;
y = x;

[X,Y,Z] = peaks(x);

%Rescale
x = x*10;
y = y*10;

X = X *10;
Y = Y*10;

%Bias the depth for more accurate z's
Z = Z-100;
%% Create Flight Path

% Fly 10m above highest peak along the central axis
xidx = floor(size(x,2)/2)-10;
dvlX =  x(xidx) * ones(size(x));
dvlY = y;

alt = 3;
dvlZ = alt + Z(:,xidx);

%% Plot Flight Path Over Surface;
subplot (2,2,[1 3]);
surf(X,Y,Z,'EdgeColor','none'); hold on;
plot3(dvlX,dvlY,dvlZ);
xlabel('x'); ylabel('y');
axis equal;

%% Calculate One Observation Along Flight Path

% Establish the scene
subplot(222);
plot(dvlY,dvlZ, dvlY, Z(:,xidx),'LineWidth',2);
grid on;
axis equal;
hold on;
ylabel('Depth (m)');
xlabel('Distance (m)');
title('Normal Detection');
gammamean = NaN*ones(size(dvlY));
gammastd = NaN*ones(size(dvlY));

%determine the normal to this curve just within the plane of flight

Gamma = dx./diff(Z(:,xidx));

%iidx = floor(length(x)/2);
b = 20;
minZ = min(min(Z));
% establish beam vector
beta = 15 * pi/180;

%beam yaws
theta = [45 135 225 315 ] * pi/180;

for iidx = b:length(x)-b
    xi = dvlX(iidx);
    yi = dvlY(iidx);
    zi = dvlZ(iidx);
    
    
    
    if iidx>b
        for n=1:length(hdl)
            set(hdl{n},'Visible','Off');
        end
    end
    
    
    %%
    % Create a search vector that extends from the lowest point in the survey
    % to the current depth of the observation.
    
    Rz =linspace(minZ,zi, length(x)*2);
    
    %%
    % Solve for vectors that represent each beam:
    
    R1x = sin(beta)*cos(theta(1)).*(zi-Rz) + xi;
    R1y = sin(beta)*sin(theta(1)).*(zi-Rz) + yi;
    
    R2x = sin(beta)*cos(theta(2)).*(zi-Rz) + xi;
    R2y = sin(beta)*sin(theta(2)).*(zi-Rz) + yi;
    
    R3x = sin(beta)*cos(theta(3)).*(zi-Rz) + xi;
    R3y = sin(beta)*sin(theta(3)).*(zi-Rz) + yi;
    
    R4x = sin(beta)*cos(theta(4)).*(zi-Rz) + xi;
    R4y = sin(beta)*sin(theta(4)).*(zi-Rz) + yi;
    
    %%
    % Solve for closest point along beam vector
    
    R1 = mindist(R1x,R1y,Rz,X,Y,Z);
    R2 = mindist(R2x,R2y,Rz,X,Y,Z);
    R3 = mindist(R3x,R3y,Rz,X,Y,Z);
    R4 = mindist(R4x,R4y,Rz,X,Y,Z);
    %%
    % Now Create a two point martix and a bottom position, then plot:
    
    R1m = [xi yi zi;
        xi+sin(beta)*cos(theta(1))*R1, ...
        yi+sin(beta)*sin(theta(1))*R1, ...
        zi-cos(beta)*R1];
    P1 = R1m(2,:);
    
    R2m = [xi yi zi;
        xi+sin(beta)*cos(theta(2))*R2, ...
        yi+sin(beta)*sin(theta(2))*R2, ...
        zi-cos(beta)*R2];
    P2 = R2m(2,:);
    
    R3m = [xi yi zi;
        xi+sin(beta)*cos(theta(3))*R3, ...
        yi+sin(beta)*sin(theta(3))*R3, ...
        zi-cos(beta)*R3];
    P3 = R3m(2,:);
    
    R4m = [xi yi zi;
        xi+sin(beta)*cos(theta(4))*R4, ...
        yi+sin(beta)*sin(theta(4))*R4, ...
        zi-cos(beta)*R4];
    P4 = R4m(2,:);
    
    P = [P1 ; P2; P3; P4];
    
    %Determine LSQ Fit credit to Val Schmidt at UNH for this one.
    C = planefit(P(:,1)',P(:,2)',P(:,3)');
    
    %determine the plane for plotting purposes
    xx = linspace(xi-b/4,xi+b/4,10);
    yy = linspace(yi-b/4,yi+b/4,10);
    [XX,YY] = meshgrid(xx,yy);
    ZZ = C(1)*XX + C(2)*YY + C(3);
    
    % plot it
    scale=0.75*alt ;
    subplot (2,2,[1 3]);
    hdl{1} = plot3(xi,yi,zi,'rs');
    hdl{10} = plot3(R1x,R1y,Rz,'b');
    hdl{11} = plot3(R2x,R2y,Rz,'b');
    hdl{12} = plot3(R3x,R3y,Rz,'b');
    hdl{13} = plot3(R4x,R4y,Rz,'b');
    hdl{2} = plot3(R1m(:,1),R1m(:,2),R1m(:,3),'r.-');
    hdl{3} = plot3(R2m(:,1),R2m(:,2),R2m(:,3),'r.-');
    hdl{4} = plot3(R3m(:,1),R3m(:,2),R3m(:,3),'r.-');
    hdl{5} = plot3(R4m(:,1),R4m(:,2),R4m(:,3),'r.-');
    hdl{9} = surf(XX,YY,ZZ);
    ylim([yi-10, yi+10]);
    xlim([xi-20, xi+20]);
    view(57,20);
    subplot(2,2,2);
    hdl{6} = plot(yi,zi,'rs');
    xlim([yi-alt*3.5 yi+alt*3.5])
    ylim([zi-alt*1.5 zi+alt*1.5]);
    hdl{7} = quiver(yi,zi-alt,...
        scale*sin(gammamean(iidx)), ...
        scale*cos(gammamean(iidx)),'r');
    
    hdl{8} = quiver(yi,zi-alt,scale*-C(2),scale*1,'b');
    
    subplot(224)
    
    
    
    drawnow;
    %pause(.05);
    
end



%     %% Determine the Normal from the four ranges
%     %
%
%     N123  = cross(P1-P2,P1-P3);
%     N134  = cross(P1-P3,P1-P4);
%     N124  = cross(P1-P2,P1-P4);
%     N234  = cross(P2-P3,P2-P4);
%     N1324 = cross(P1-P3,P2-P4);
%
%     %normalize
%     N = zeros(5,3);
%
%     N(1,:)  = N123/norm(N123);
%     N(2,:)  = N134/norm(N134);
%     N(3,:)  = N124/norm(N124);
%     N(4,:)  = N234/norm(N234);
%     N(5,:)  = N1324/norm(N1324);
%
%     % calculate the angle vs inertial frame in the pitching plane
%
%     gamma   = atan(N(:,2)./N(:,3));
%     gammastd(iidx)  = std(gamma);
%     gammamean(iidx) = mean(gamma);

