%% 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)

clear; figure(1); clf;
%% Key Simulation Parameters

%%
% close the loop on the dvl control
loopclosed = true;

%% 
% add randomized noise to the topography
% roughness applies to a percent of 1m scale.
addnoise = false;
roughness = 0.1; 

%%
% Surface Exageration - scale constant for topography.
exageration = 1;
%%
%flight altitude in meters
alt = 3.5;


%% Create Environment

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

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

Z = exageration.*Z;

if addnoise == true
    Z = randn(size(Z))*roughness+Z;
end

%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 above highest peak along the central axis
xidx = floor(size(x,2)/2)-10;
dvlX =  x(xidx) * ones(size(x));
dvlY = y;

mbswath = 2*alt*tand(70);
mbatrack = 2*alt*tand(.5);
dvlZ = alt + Z(:,xidx);

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


%% Calculate One Observation Along Flight Path

% Establish the scene

subplot(333);
plot(dvlY,dvlZ, dvlY, Z(:,xidx),'LineWidth',2);
grid on;
axis equal;
hold on;
ylabel('Depth (m)');
xlabel('Distance (m)');
title('Normal Detection');

%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

% beam width
beta = 30 * pi/180;

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

%calculate pitch, or theta
theta = (pi/2 - beta/2)*ones(size(psi));

Bd = 0 * pi/180;

iidx = b:length(x)-b;
F(length(iidx)) = struct('cdata', [], 'colormap',[]);

subplot(339)
axis off;
text(0,.5, {'Articulating Frame Simulation ' ,date, ...
    ['Altitude: ' num2str(alt)], ...
    ['Noise: ' num2str(addnoise) ' Roughness: ' num2str(roughness)],...
    'Loop Closed on raw offset of normal to dvl'},...
    'Fontsize',16);

ROTD = zeros(4,3,10);

for m = 1:length(iidx)
    xi = dvlX(iidx(m));
    yi = dvlY(iidx(m));
    zi = dvlZ(iidx(m));
    
    
    
    %     if m > 1
    %         for n=1:length(hdl)
    %             set(hdl{n},'Visible','Off');
    %         end
    %     else
    %     end
    %
    
    %% Determine the new rotation of the dvl
    % Add a filter to this
    ROTD = circshift(ROTD,1,3);
    ROTD(:,:,1) = rotDvl(Bd,theta,psi);
    ROT = mean(ROTD,3);
    %%
    % Create a search vector that extends from the lowest point in the survey
    % to the current depth of the observation.
    
    Rz =minZ:dx:zi;
    
    %%
    % Solve for vectors that represent each beam:
    %  cos(pitch)*cos(yaw) = x
    %  cos(pitch)*sin(yaw) = y
    R1x = ROT(1,1).*(zi-Rz) + xi;
    R1y = ROT(1,2).*(zi-Rz) + yi;
    R1z = ROT(1,3).*(zi-Rz) + zi;
    
    R2x = ROT(2,1).*(zi-Rz) + xi;
    R2y = ROT(2,2).*(zi-Rz) + yi;
    R2z = ROT(2,3).*(zi-Rz) + zi;
    
    R3x = ROT(3,1).*(zi-Rz) + xi;
    R3y = ROT(3,2).*(zi-Rz) + yi;
    R3z = ROT(3,3).*(zi-Rz) + zi;
    
    R4x = ROT(4,1).*(zi-Rz) + xi;
    R4y = ROT(4,2).*(zi-Rz) + yi;
    R4z = ROT(4,3).*(zi-Rz) + zi;
    
    %%
    % Solve for closest point along beam vector
    
    R1 = mindist(R1x,R1y,R1z,X,Y,Z);
    R2 = mindist(R2x,R2y,R2z,X,Y,Z);
    R3 = mindist(R3x,R3y,R3z,X,Y,Z);
    R4 = mindist(R4x,R4y,R4z,X,Y,Z);
    %%
    % Now Create a two point matrix and a bottom position, then plot:
    
    R1m = [xi yi zi;
        xi+ROT(1,1)*(R1), ...
        yi+ROT(1,2)*(R1), ...
        zi+ROT(1,3)*(R1)];
    P1 = R1m(2,:);
    
    R2m = [xi yi zi;
        xi+ROT(2,1)*R2, ...
        yi+ROT(2,2)*R2, ...
        zi+ROT(2,3)*R2];
    P2 = R2m(2,:);
    
    R3m = [xi yi zi;
        xi+ROT(3,1)*R3, ...
        yi+ROT(3,2)*R3, ...
        zi+ROT(3,3)*R3];
    P3 = R3m(2,:);
    
    R4m = [xi yi zi;
        xi+ROT(4,1)*R4, ...
        yi+ROT(4,2)*R4, ...
        zi+ROT(4,3)*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)');
    Cen = mean(P);
    %determine the plane for plotting purposes
    xx = linspace(Cen(1)-mbswath/2,Cen(1)+mbswath/2,3);
    yy = linspace(Cen(2)-mbatrack/2,Cen(2)+mbatrack/2,3);
    [XX,YY] = meshgrid(xx,yy);
    ZZ = C(1)*XX + C(2)*YY + C(3);
    ZZmean = C(1)*Cen(1) + C(2)*Cen(2) + C(3);
    
    %% pitch offset for control loop
    Bb = -atan(-C(2));
    if loopclosed == true
        if (Bb)>0 && (Bb) < pi/2
            Bd = Bb;
        elseif Bb<0
            Bd = 0;
        end
    end
    
    if (m == 1)
        %% plot it
        %
        
        scale=0.75*alt ;
        subplot (3,3,[1 2 4 5 7 8]);
        hdl{1} = plot3(xi,yi,zi,'rs');
        hdl{1}.XDataSource = 'xi';
        hdl{1}.YDataSource = 'yi';
        hdl{1}.ZDataSource = 'zi';
        
        %hdl{10} = plot3(R1x,R1y,R1z,'b');
        %hdl{11} = plot3(R2x,R2y,R2z,'b');
        %hdl{12} = plot3(R3x,R3y,R3z,'b');
        %hdl{13} = plot3(R4x,R4y,R4z,'b');
        hdl{2} = plot3(R1m(:,1),R1m(:,2),R1m(:,3),'r.-');
        hdl{2}.XDataSource = 'R1m(:,1)';
        hdl{2}.YDataSource = 'R1m(:,2)';
        hdl{2}.ZDataSource = 'R1m(:,3)';
        hdl{3} = plot3(R2m(:,1),R2m(:,2),R2m(:,3),'g.-');
        hdl{3}.XDataSource = 'R2m(:,1)';
        hdl{3}.YDataSource = 'R2m(:,2)';
        hdl{3}.ZDataSource = 'R2m(:,3)';
        hdl{4} = plot3(R3m(:,1),R3m(:,2),R3m(:,3),'g.-');
        hdl{4}.XDataSource = 'R3m(:,1)';
        hdl{4}.YDataSource = 'R3m(:,2)';
        hdl{4}.ZDataSource = 'R3m(:,3)';
        hdl{5} = plot3(R4m(:,1),R4m(:,2),R4m(:,3),'r.-');
        hdl{5}.XDataSource = 'R4m(:,1)';
        hdl{5}.YDataSource = 'R4m(:,2)';
        hdl{5}.ZDataSource = 'R4m(:,3)';
        
        hdl{6} = surf(XX,YY,ZZ);
        hdl{6}.XDataSource = 'XX';
        hdl{6}.YDataSource = 'YY';
        hdl{6}.ZDataSource = 'ZZ';
        
        title('Vehicle Scene');
        axis equal
        set(gca,'FontSize',16);
        subplot(333);
        hdl{7} = plot(yi,zi,'rs');
        hdl{7}.XDataSource = 'yi';
        hdl{7}.YDataSource = 'zi';
        set(gca, 'XTickLabelMode','manual');
        hdl{8} = quiver(Cen(2),ZZmean,scale*-C(2),scale*1,'b');
        hdl{8}.XDataSource = 'Cen(2)';
        hdl{8}.YDataSource = 'ZZmean';
        hdl{8}.UDataSource = 'scale*-C(2)';
        hdl{8}.VDataSource = 'scale';
        
        set(hdl{8}, 'LineWidth',2);
        
        set(gca,'FontSize',16);
        subplot(3,3,6)
        hold on;
        grid on;
        for n = 1:4
            hdl{8+n} = quiver3(0,0,0,ROT(n,1),ROT(n,2),ROT(n,3),'b');
            str = sprintf('ROT(%i,1)',n);
            hdl{8+n}.UDataSource = str;
            str = sprintf('ROT(%i,2)',n);
            hdl{8+n}.VDataSource = str;
            str = sprintf('ROT(%i,3)',n);
            hdl{8+n}.WDataSource = str;
            
        end
        xlabel ('x')
        ylabel ('Along Track')
        zlabel ('Vertical Axis')
        title('DVL Position')
        set(gca,'FontSize',16);
        
        %axis equal;
        %
    end
    
    %% Adjust views
    subplot (3,3,[1 2 4 5 7 8]);
    win = 5;
    ylim([yi-win, yi+win]);
    xlim([xi-win, xi+win]);
    zlim([zi-2*win, zi+2*win]);
    view(47,50);
    subplot(333);
    xlim([yi-alt*4 yi+alt*4])
    ylim([zi-alt*1.5 zi+alt*1.5]);
    subplot(336)
    view(90,0);
    ylim([-1 1]);
    zlim([-1 0]);
    box on;
    
    refreshdata;
    F(m) = getframe(gcf);
    %drawnow;
    
    %pause(.05);
    
end

save frames.mat;

myVideo = VideoWriter('myfile.mp4','MPEG-4');
myVideo.FrameRate = 10;
myVideo.Quality = 100;
%myVideo.LosslessCompression = 1;
open(myVideo);
writeVideo(myVideo, F);
close(myVideo);

%     %% 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);

