function floatTrack3()

clear all;
close all;
delete(instrfindall);

bathy = getBathy();
%iUSBL = getIUSBL();
utm = initUTM();
chart = initChart();
paragon = initParagon();
stripChart = initStripChart();
controls = initControls();

gps = initGPS();
iUSBL = initIUSBL();

fprintf('Starting run loop\n');

run = true;
fopen(gps.SP);
fopen(iUSBL.SP);

while(run)
    pause(2);
    fprintf(iUSBL.SP,'POS V7,4');
    %simIUSBLCallback();
end

%Cleanup before exit
fclose(gps.SP);
fclose(iUSBL.SP);
close all;

%-----------------------
    function gps = initGPS()
        fprintf('Init GPS\n');
        gps = struct();
        
        gps.SP = serial('COM19', ...
            'BaudRate',9600, ...
            'Parity','none', ...
            'DataBits', 8, ...
            'StopBits',1, ...
            'InputBufferSize',8192);
        
        gps.SP.BytesAvailableFcnMode = 'terminator';
        gps.SP.BytesAvailableFcn = @GPSCallback;
        
        %closeSP = onCleanup(@() fclose(gps.SP));
    end

%-----------------------
    function iUSBL = initIUSBL()
        fprintf('Init iUSBL\n');
        iUSBL = struct();
        
        iUSBL.SP = serial('COM10', ...
            'BaudRate',9600, ...
            'Parity','none', ...
            'DataBits', 8, ...
            'StopBits',1, ...
            'InputBufferSize',8192);
        
        iUSBL.SP.BytesAvailableFcnMode = 'terminator';
        iUSBL.SP.BytesAvailableFcn = @iUSBLCallback;
        
        closeSP = onCleanup(@() fclose(iUSBL.SP));
    end

%-----------------------
    function GPSCallback(obj, event)
        
        msg = fgetl(gps.SP);
        
        gpsFields = strsplit(msg,',');
        
        if strcmp(gpsFields(1), '$GPGGA')
            temp = char(gpsFields(3));
            latDeg =  temp(1:2);
            latMin = temp(3:end);
            lat = str2double(latDeg) + (str2double(latMin)/60.00);
            if( strcmp(gpsFields(4),'S') )
                lat = -1.0 * lat;
            end
            temp = char(gpsFields(5));
            lonDeg = temp(1:3);
            lonMin = temp(4:end);
            lon = str2double(lonDeg) + (str2double(lonMin)/60.00);
            if( strcmp(gpsFields(6),'W') )
                lon = -1.0 * lon;
            end
            
            set(controls.shipLatEdit, 'String', [latDeg, ' ', latMin, char(gpsFields(4))]);
            set(controls.shipLonEdit, 'String', [lonDeg, ' ', lonMin, char(gpsFields(6))]);
            
            [easting, northing] = mfwdtran(utm.struct, lat, lon);
            fprintf('Easting = %f   Northing = %f\n', easting, northing);
            
            heading = 90;
            M = makehgtform('zrotate', degtorad(heading));
            M = M(1:2, 1:2);
            shipRot = M * paragon.XY;
            shipXY = [easting + shipRot(1,:); northing + shipRot(2,:)];
            
            paragon.Patch.XData = shipXY(1,:);
            paragon.Patch.YData = shipXY(2,:);
            drawnow;
        end
    end

%-----------------------
    function [] = iUSBLCallback(varargin)
        persistent msgIndex
        if isempty(msgIndex)
            msgIndex = 1;
        end
        
        iUSBL.msg = fgetl(iUSBL.SP);
        
        %iUSBL.msg(msgIndex,:);
        
        fields = strsplit(iUSBL.msg(1:end-2), {'X','Y','Z',','});
        if(length(fields) > 4)
            set(controls.floatEastingEdit, 'String', fields{3});
            set(controls.floatNorthingEdit, 'String', fields{4});
            set(controls.floatDepthEdit, 'String', fields{5});
            
            msgIndex = msgIndex + 1;
            
            stripChart.time(1:stripChart.arrayLength-1) = stripChart.time(2:end);
            stripChart.y(1:stripChart.arrayLength-1) = stripChart.y(2:end);
            stripChart.time(stripChart.arrayLength) = (now - stripChart.startTime) * 24.0 * 3600.0;
            stripChart.y(stripChart.arrayLength) = -1.0 * str2double(fields{5});
            
            plotX = stripChart.time(end-stripChart.width + 1:end);
            plotY = stripChart.y(end-stripChart.width+1:end);
            
            set(stripChart.hPlot, 'XData', plotX, 'YData', plotY);
            set(stripChart.hAxes, 'XLim', [(stripChart.time(end) - stripChart.width) stripChart.time(end)], ...
                'YLim', [stripChart.startDepth - stripChart.depthRange stripChart.startDepth]);
            drawnow
        end
    end

%-----------------------
    function [] = simIUSBLCallback()
        persistent simDepth
        if isempty(simDepth)
            simDepth = 1;
        end
        
 
            set(controls.floatEastingEdit, 'String', '0');
            set(controls.floatNorthingEdit, 'String', '1');
            set(controls.floatDepthEdit, 'String', int2str(simDepth));
            
            simDepth = simDepth + 1;
            
            stripChart.time(1:stripChart.arrayLength-1) = stripChart.time(2:end);
            stripChart.y(1:stripChart.arrayLength-1) = stripChart.y(2:end);
            stripChart.time(stripChart.arrayLength) = (now - stripChart.startTime) * 24.0 * 3600.0;
            stripChart.y(stripChart.arrayLength) = -1.0 * simDepth;
            
            plotX = stripChart.time(end-stripChart.width + 1:end);
            plotY = stripChart.y(end-stripChart.width+1:end);
            
            simSpeed = polyfit(plotX(end-10:end), plotY(end-10:end), 1)
            
            set(stripChart.hPlot, 'XData', plotX, 'YData', plotY);
            set(stripChart.hAxes, 'XLim', [(stripChart.time(end) - stripChart.width) stripChart.time(end)], ...
                'YLim', [stripChart.startDepth - stripChart.depthRange stripChart.startDepth]);
            title = sprintf('Depth = %.1f meters     Velocity = %.2f m/sec', simDepth, simSpeed(1));
            set(stripChart.hAxes.Title, 'String', title);
            drawnow
    end

%-----------------------
    function stripChart = initStripChart()
        fprintf('Init Strip Chart\n');
        stripChart = struct();
        
        stripChart.hFigure = figure( 'Name', 'Float Depth', ...
            'NumberTitle', 'off');
        set(stripChart.hFigure,'DoubleBuffer', 'on');
        
        %One bigbox for the whole figure split into 2 slider boxes and one
        %chart box.  Then split the chart box into a chart box and a slider
        %box
        bigBox = uix.HBox('Parent', stripChart.hFigure, 'Padding', 5, 'Spacing', 5);
        leftBox = uix.VBox('Parent', bigBox);
        rightBox = uix.VBox('Parent', bigBox, 'Padding', 5, 'Spacing', 5);
        set(bigBox, 'Widths', [50 -1]);
        
        vertSliderBox = uix.HBox('Parent', leftBox, 'Padding', 5, 'Spacing', 5);
        vertSliderTextBox = uix.VBox('Parent', leftBox, 'Padding', 5, 'Spacing', 5);
        set(leftBox, 'Heights', [-.8 -.1]);
        
        stripChart.depthRange = 100;
        depthRangeSlider = uicontrol('Parent', vertSliderBox, 'Style', 'slider', ...
            'Min', 1, 'Max', 500, 'Value', stripChart.depthRange, ...
            'Callback', @setDepthRange);
        stripChart.startDepth = 0;
        startDepthSlider = uicontrol('Parent', vertSliderBox, 'Style', 'slider', ...
            'Min', -500, 'Max', 0, 'Value', stripChart.startDepth, ...
            'Callback', @setStartDepth);
        
        rangeText = uicontrol('Parent', vertSliderTextBox, 'Style', 'text', ...
            'HorizontalAlignment', 'left', 'String', 'Range');
        startDepthText = uicontrol('Parent', vertSliderTextBox, 'Style', 'text', ...
            'HorizontalAlignment', 'right', 'String', 'Start');
        
        stripChart.hAxes = axes('Parent', rightBox);
        horizSliderBox = uix.HBox('Parent', rightBox, 'Padding', 5, 'Spacing', 5);
        
        widthText = uicontrol('Parent',horizSliderBox, 'Style', 'text', ...
            'String', 'Chart Width (sec)');
        stripChart.width = 100;
        widthSlider = uicontrol('Parent', horizSliderBox, 'Style', 'slider', ...
            'Min', 30, 'Max', 300, 'Value', stripChart.width, ...
            'Callback', @setSCWidth);
        set(rightBox, 'Heights', [-1 25]);
        set(horizSliderBox, 'Widths', [-.3 -.4]);
        
        stripChart.startTime = now;
        stripChart.index = 1;
        
        stripChart.arrayLength = 1000;
        stripChart.time = NaN * ones(1,stripChart.arrayLength);
        stripChart.y = NaN * ones(1,stripChart.arrayLength);
        
        plotX = stripChart.time(end-stripChart.width + 1:end);
        plotY = stripChart.y(end-stripChart.width+1:end);
        stripChart.hPlot = plot(plotX, plotY);
        grid;
    end
%-----------------------
    function setStartDepth(hObject, ~)
        stripChart.startDepth = hObject.Value;
        set(stripChart.hAxes, 'XLim', [(stripChart.time(end) - stripChart.width) stripChart.time(end)], ...
            'YLim', [stripChart.startDepth - stripChart.depthRange stripChart.startDepth]);
        drawnow;
    end

%-----------------------
    function setDepthRange(hObject, ~)
        stripChart.depthRange = hObject.Value;
        set(stripChart.hAxes, 'XLim', [(stripChart.time(end) - stripChart.width) stripChart.time(end)], ...
            'YLim', [stripChart.startDepth - stripChart.depthRange stripChart.startDepth]);
        drawnow;
    end

%-----------------------
    function setSCWidth(hObject, ~)
        stripChart.width = hObject.Value;
        set(stripChart.hAxes, 'XLim', [(stripChart.time(end) - stripChart.width) stripChart.time(end)], ...
            'YLim', [stripChart.startDepth - stripChart.depthRange stripChart.startDepth]);
        drawnow;
    end

%-----------------------
    function iUSBL = getIUSBL()
        iUSBL = load('iUSBLMsg.mat');
    end



%-----------------------
    function bathy = getBathy()
        fprintf('Loading mc15 data set \n');
        bathy = load('mc15.mat');
    end

%------------------------
    function chart = initChart()
        chart = struct();
        
        screenSize = get(groot,'ScreenSize');
        chart.hFigure = figure( ...
            'Name', 'Float Track', ...
            'NumberTitle', 'off', ...
            'OuterPosition', [0.1*screenSize(3) 0.2*screenSize(4) 0.5*screenSize(3) 0.75*screenSize(4)]);
        
        chart.chartAxes = axes('Parent', chart.hFigure);
        surf(bathy.mc15.x, bathy.mc15.y, bathy.mc15.z, 'Parent', chart.chartAxes, ...
            'FaceColor', 'interp', ...
            'LineStyle', 'none', ...
            'FaceLighting', 'gouraud');
        cm = haxby(256);
        colormap(cm);
        daspect([1, 1, .25]);
        axis tight;
        camlight left;
    end

%--------------------------------
    function controls = initControls()
        controls = struct();
        
        screenSize = get(groot,'ScreenSize');
        controls.hFigure = figure( ...
            'Name', 'Float Track', ...
            'NumberTitle', 'off', ...
            'MenuBar', 'none', ...
            'Toolbar', 'none', ...
            'Position', [0.7*screenSize(3) 0.4*screenSize(4) 0.1*screenSize(3) 0.4*screenSize(4)]);
        
        controlsBox = uix.VBox('Parent', controls.hFigure, 'Padding', 5);
        
        
        controls.shipLatEdit = uicontrol('Parent', controlsBox, 'Style', 'edit', ...
            'FontSize', 15, 'FontWeight', 'bold');
        uicontrol('Parent', controlsBox, 'Style', 'text', ...
            'FontSize', 10, ...
            'String', 'Ship Latitude');
        
        controls.shipLonEdit = uicontrol('Parent', controlsBox, 'Style', 'edit', ...
            'FontSize', 15, 'FontWeight', 'bold');
        uicontrol('Parent', controlsBox, 'Style', 'text', ...
            'FontSize', 10, ...
            'String', 'Ship Longitude')
        
        controls.floatNorthingEdit = uicontrol('Parent', controlsBox, 'Style', 'edit', ...
            'FontSize', 15, 'FontWeight', 'bold');
        uicontrol('Parent', controlsBox, 'Style', 'text', ...
            'FontSize', 10, ...
            'String', 'Float Northing');
        
        controls.floatEastingEdit = uicontrol('Parent', controlsBox, 'Style', 'edit', ...
            'FontSize', 15, 'FontWeight', 'bold');
        uicontrol('Parent', controlsBox, 'Style', 'text', ...
            'FontSize', 10, ...
            'String', 'Float Easting')
        
        controls.floatDepthEdit = uicontrol('Parent', controlsBox, 'Style', 'edit', ...
            'FontSize', 15, 'FontWeight', 'bold');
        uicontrol('Parent', controlsBox, 'Style', 'text', ...
            'FontSize', 10, ...
            'String', 'Float Depth')
        
        controls.exitButton = uicontrol('Parent', controlsBox, 'Style', 'pushbutton', ...
            'String', 'Exit', ...
            'Callback', @exitCallback);
    end

%-----------------------
    function utm = initUTM()
        fprintf('Init UTM\n');
        utm = struct();
        
        utmstruct = defaultm('utm');
        utmstruct.zone = '10S';
        wgs84 = wgs84Ellipsoid;
        ellipsoid = [wgs84.SemimajorAxis wgs84.Eccentricity];
        utmstruct.geoid = ellipsoid;
        utm.struct = defaultm(utmstruct);
    end

%-----------------------
    function paragon = initParagon()
        fprintf('Init Paragon\n');
        paragon = struct();
        
        LOA = 100;
        beam = 50;
        Y = [0, beam/2 beam/2 -beam/2 -beam/2   0];
        X = [0 -(.3*LOA) -LOA,   -LOA     -(.3*LOA) 0];
        
        tempY = NaN * ones(length(Y));
        tempX = NaN * ones(length(X));
        paragon.XY = [tempX; tempY];
        
        paragon.Patch = patch(tempX(1,:), tempY(2,:), ...
            'r', 'Parent', chart.chartAxes);
        
        %  load('paragonPatch.mat');
        %set(paragon.Patch, 'Parent', chart.chartAxes);
        
        paragon.XY = [X; Y];
    end

    function exitCallback(source, callbackData)
        run = false;
    end
end