close all;
clc;

%data = csvread('/Users/rob/windows_shared/auv-qnx/auv/altex/onboard/Octree/theObstacles_1m.csv');
%data = csvread('/Users/rob/windows_shared/auv-qnx/auv/altex/onboard/Octree/tripleIsland.csv');
%data = csvread('/Users/rob/windows_shared/auv-qnx/auv/altex/onboard/Octree/tripleIsland10.csv');

data = csvread(fileName);

V = [];
FVCD = [];
for x = 0:1
    for y = 0:1
        for z = 0:1
            V(end+1,:) = [x y z];
            FVCD(end+1,:) = [0.5 0.5 z];
        end
    end
end

F =[1 3 4 2 1;
    5 7 8 6 5;
    1 2 6 5 1;
    3 4 8 7 3;
    1 5 7 3 1;
    4 8 6 2 4];

minX = min(data(:,1));
minY = min(data(:,3));
minZ = min(data(:,5));
maxZ = max(data(:,6));
total_faces = 0;
cHaxby = haxby(256);


%%
for i = 1:length(data(:,1))
    V_out = V;
    V_out(:,1) = data(i,1)+V_out(:,1)*(data(i,2)-data(i,1))-minX;
    V_out(:,2) = data(i,3)+V_out(:,2)*(data(i,4)-data(i,3))-minY;
    V_out(:,3) = data(i,5)+V_out(:,3)*(data(i,6)-data(i,5));
    F_out = F(data(i,7:12)'==1,:);

    FVCD_out = cHaxby(1+floor((V_out(:,3)-minZ)*255/(maxZ-minZ)),:);
    
    total_faces = total_faces + length(F_out(:,1));
    patch('Vertices',V_out,'Faces',F_out,'FaceVertexCData',FVCD_out,'FaceColor','interp','EdgeColor','k','LineStyle',':')
    hold on;
        
    
end
axis equal
currAz = 0;
currEl = 0;
view(currAz,currEl)
xlabel('Local Northings (m)')
ylabel('Local Eastings (m)')
zlabel('Down (m)')
title(strcat('North East Offset = [',num2str(minX),',',num2str(minY),']'))
colorbar();

%%
k = 0;
drawnow;
step = 30;
while(k~=5)
    k = input('Menu:\n5 close/quit, 1 quit\n 8 up, 2 down, 4 left, 6 right\n 7 custom view, 9 change step size\n');
    if(isnumeric(k))
        switch(k)
            case 8
                currEl = currEl + step;
            case 2
                currEl = currEl - step;
            case 4
                currAz = mod(currAz - step,360);
            case 6
                currAz = mod(currAz + step,360);
            case 7
                az = input('Azumith?\n');
                currAz = mod(az,360);
                el = input('Elevation?\n');
                currEl = mod(el,360);
            case 9
                inStep = input('Step Size (deg)?\n');
                if(isnumeric(inStep))
                    step = inStep;
                else
                    display('Invalid Entry\n');
                end
            case 5
                close all;
                break;
            case 1
                break;
            otherwise
                disp('Invalid Entry\n')
        end
        currEl = min(90,currEl);
        currEl = max(-90,currEl);
        display(strcat('Curr Azimuth = ',num2str(currAz),', Curr Elevation = ',num2str(currEl)));
        view(currAz,currEl);
    else
        disp('Invalid Entry\n')
    end
    drawnow;
end
            
