function floatTrack4()

clear all;
close all;
delete(instrfindall);

controls = initControls();
wayPts = initWayPts();
bathy = getBathy();
utm = initUTM();
chart = initChart();

boat = initBoat('Paragon');
float = initFloat3D();
stripChart = initStripChart();
meters = initMeters();

controls.simGPS.Value = 1;
controls.simIUSBL.Value = 1;

if (controls.simGPS.Value == 0)
    gps = initGPS();
end
if (controls.simIUSBL.Value == 0)
    iUSBL = initIUSBL();
end

if (controls.simGPS.Value == 0)
    fopen(gps.SP);
end
if (controls.simIUSBL.Value == 0)
    fopen(iUSBL.SP);
end


fid = fopen('gpsUSBL.log', 'w');

fprintf('Starting run loop\n');
run = true;
while(run)
    pause(2);
    
    if (controls.simGPS.Value == 1)
        simGPSCallback();
    end
    writeKMLFile(boat.lat, boat.lon, controls.simBoatHeading.Value, controls.GEZoom.Value);
    wayPtRangeBearing(boat.lat(end), boat.lon(end));
    
    if (controls.simIUSBL.Value == 1)
        simIUSBLCallback();
    else
        fprintf(iUSBL.SP,'POS V7,4');
    end
    
 %   fprintf(fid, 'usbl: %s   gps: %s',  gps.msg, iUSBL.msg);
    
end

%Cleanup before exit
if (controls.simGPS.Value == 0)
    fclose(gps.SP);
end
if (controls.simIUSBL.Value == 0)
    fclose(iUSBL.SP);
end

fclose(fid);

close all;
%--------------------------------

%--------------------------------
    function float = initFloat3D()
        fprintf('Init float');
        float = struct();
        
        screenSize = get(groot,'ScreenSize');
        float.hFigure = figure( ...
            'Name', 'Float 3D Track', ...
            'NumberTitle', 'off', ...
            'OuterPosition', [0.5*screenSize(3) 0.05*screenSize(4) 0.3*screenSize(3) 0.4*screenSize(4)]);
        
        float.plotAxes = axes('Parent', float.hFigure);
        
        float.x = nan(1,100);
        float.y = nan(1,100);
        float.z = nan(1,100);
        
        float.hPlot = plot3(float.x, float.y, float.z, 'Parent', float.plotAxes);
        grid;
        xlabel('Easting');
        ylabel('Northing');
        zlabel('Depth');
        axis([-20 20 -20 20 -100 0]);
        axis equal;
     end
%--------------------------------

%--------------------------------
    function wayPtRangeBearing(shipLat, shipLon)
         wpLat = wayPts.wpStruct(wayPts.list.Value).Lat;
         wpLon = wayPts.wpStruct(wayPts.list.Value).Lon;
         
         [arclen, az] = distance(shipLat, shipLon, wpLat,wpLon);
         range = deg2km(arclen);
         set(wayPts.Range, 'String', num2str(range, '%.3f'));
         set(wayPts.Bearing, 'String', num2str(az,'%.1f'));
         
         drawnow;
    end
%--------------------------------

%--------------------------------
    function wayPts = initWayPts()
        wayPts = struct();
        wayPts.wpStruct = kml2struct('Waypoints.kml');
        
        screenSize = get(groot,'ScreenSize');
        wayPts.hFigure = figure( ...
            'Name', 'Way Points', ...
            'NumberTitle', 'off', ...
            'MenuBar', 'none', ...
            'Toolbar', 'none', ...
            'Position', [0.8*screenSize(3) 0.05*screenSize(4) 0.15*screenSize(3) 0.4*screenSize(4)]);
        
        wayPtsBox = uix.VBox('Parent', wayPts.hFigure, 'Padding', 5);
        
        wayPtsListPanel = uix.BoxPanel('Parent', wayPtsBox, 'Title', 'Way Points', 'Padding', 5);
        listBox = uix.VBox('Parent', wayPtsListPanel, 'Padding', 5);
        wayPts.list = uicontrol('Parent', listBox, 'Style', 'listBox', ...
            'FontSize', 10, 'Max', 2, 'Min', 1, ...
            'String', {wayPts.wpStruct(1:end).Name});
        
        metersPanel = uix.BoxPanel('Parent', wayPtsBox, 'Title', 'Float', 'Padding', 5);
        metersBox = uix.VBox('Parent', metersPanel, 'Padding', 5);
        wayPts.Range = uicontrol('Parent', metersBox, 'Style', 'edit', ...
            'FontSize', 15, 'FontWeight', 'bold');
        uicontrol('Parent', metersBox, 'Style', 'text', ...
            'FontSize', 10, ...
            'String', 'Range to Way Point (km)')

        wayPts.Bearing = uicontrol('Parent', metersBox, 'Style', 'edit', ...
            'FontSize', 15, 'FontWeight', 'bold');
        uicontrol('Parent', metersBox, 'Style', 'text', ...
            'FontSize', 10, ...
            'String', 'Bearing to Way Point')

        wayPts.minutesToWP = uicontrol('Parent', metersBox, 'Style', 'edit', ...
            'FontSize', 15, 'FontWeight', 'bold');
        uicontrol('Parent', metersBox, 'Style', 'text', ...
            'FontSize', 10, ...
            'String', 'Minutes to Way Point');
    end
%--------------------------------

%--------------------------------
    function controls = initControls()
        controls = struct();
        
        screenSize = get(groot,'ScreenSize');
        controls.hFigure = figure( ...
            'Name', 'Controls', ...
            'NumberTitle', 'off', ...
            'MenuBar', 'none', ...
            'Toolbar', 'none', ...
            'Position', [0.8*screenSize(3) 0.5*screenSize(4) 0.15*screenSize(3) 0.4*screenSize(4)]);
        
        controlsBox = uix.VBox('Parent', controls.hFigure, 'Padding', 5);
        
        realTimePanel = uix.BoxPanel('Parent', controlsBox, 'Title', 'Real Time Controls', 'Padding', 5);
        realTimeBox = uix.VBox('Parent', realTimePanel, 'Padding', 5);
        controls.GEZoom = uicontrol('Parent', realTimeBox, 'Style', 'slider', ...
            'Min', 0, 'Max', 100000, 'SliderStep', [0.002 0.1]);
        controls.GEZoom.Value = 3000;
        uicontrol('Parent', realTimeBox, 'Style', 'text', ...
            'FontSize', 11, ...
            'String', 'Google Earth Zoom');
 
        controls.iUSBLSpeedSmooth = uicontrol('Parent', realTimeBox, 'Style', 'slider', ...
            'Min', 1, 'Max', 100, 'SliderStep', [0.01 0.1]);
        controls.iUSBLSpeedSmooth.Value = 10;
        uicontrol('Parent', realTimeBox, 'Style', 'text', ...
            'FontSize', 11, ...
            'String', 'iUSBL Speed Smoothing');
        set(realTimeBox, 'Heights', [20 35 20 45]);
        
        simPanel = uix.BoxPanel('Parent', controlsBox, 'Title', 'Simulation Controls', 'Padding', 5);
        simBox = uix.VBox('Parent', simPanel, 'Padding', 5);
        controls.simGPS = uicontrol('Parent', simBox, 'Style', 'checkbox', ...
            'String', 'Simulate GPS', 'FontSize', 11);

        controls.simIUSBL = uicontrol('Parent', simBox, 'Style', 'checkbox', ...
            'String', 'Simulate iUSBL', 'FontSize', 11);
        
        controls.simBoatSpeed = uicontrol('Parent', simBox, 'Style', 'slider', ...
            'Min', 1, 'Max', 100, 'SliderStep', [0.01 0.1]);
        controls.simBoatSpeed.Value = 10;
        uicontrol('Parent', simBox, 'Style', 'text', ...
            'FontSize', 11, ...
            'String', 'Sim Boat Speed');

        controls.simBoatHeading = uicontrol('Parent', simBox, 'Style', 'slider', ...
            'Min', -180, 'Max', 180, 'SliderStep', [0.01 0.1], 'String', 'test');
        controls.simBoatHeading.Value = -90;
        uicontrol('Parent', simBox, 'Style', 'text', ...
            'FontSize', 11, ...
            'String', '180   270   0   90   180');
        uicontrol('Parent', simBox, 'Style', 'text', ...
            'FontSize', 11, ...
            'String', 'Sim Boat Heading');
        
        set(simBox, 'Heights', [25 40 20 30 20 17 25]);
        set(controlsBox, 'Heights', [-.4 -.6]);
    end
%-----------------------

%-----------------------
    function gps = initGPS()
        fprintf('Init GPS\n');
        gps = struct();
        
        gps.SP = serial('COM15', ...
            '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)
        
        gps.msg = fgetl(gps.SP);
       % fprintf(1, '%s', gps.msg);
        
        gpsFields = strsplit(gps.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(meters.shipLatEdit, 'String', [latDeg, ' ', latMin, char(gpsFields(4))]);
            set(meters.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 * boat.XY;
            shipXY = [easting + shipRot(1,:); northing + shipRot(2,:)];
            
            boat.Patch.XData = shipXY(1,:);
            boat.Patch.YData = shipXY(2,:);
            drawnow;
        end
    end
%-----------------------

%-----------------------
    function simGPSCallback()
        arclen = nm2deg(controls.simBoatSpeed.Value * (2.0 / 3600.0));
        [lat, lon] = reckon(boat.lat(end), boat.lon(end), arclen, controls.simBoatHeading.Value);
        lat = lat + 0.0002 * (rand - 0.5);
        lon = lon + 0.0002 * (rand - 0.5);
        boat.lat = [boat.lat(2:end) lat];
        boat.lon = [boat.lon(2:end) lon];
        
        latDeg =  floor(lat);
        latMin = (lat - latDeg) * 60.0;
        
        lonDeg = ceil(lon);
        lonMin = (lonDeg - lon) * 60;
        
        set(meters.shipLatEdit, 'String', [num2str(latDeg), ' ', num2str(latMin, '%2.4f'), ' N']);
        set(meters.shipLonEdit, 'String', [num2str(-lonDeg), ' ', num2str(lonMin, '%2.4f'), ' W']);
        
        [easting, northing] = mfwdtran(utm.struct, lat, lon);
        %fprintf('Easting = %f   Northing = %f\n', easting, northing);
        
        M = makehgtform('zrotate', degtorad(controls.simBoatHeading.Value));
        M = M(1:2, 1:2);
        shipRot = M * boat.XY;
        shipXY = [easting + shipRot(1,:); northing + shipRot(2,:)];
        
        boat.Patch.XData = shipXY(1,:);
        boat.Patch.YData = shipXY(2,:);
        drawnow;
    end
%-----------------------

%-----------------------
    function [] = iUSBLCallback(varargin)
        persistent msgIndex
        if isempty(msgIndex)
            msgIndex = 1;
        end
        
        iUSBL.msg = '';
        iUSBL.msg = fgetl(iUSBL.SP);
        fprintf(1, '%s', iUSBL.msg);
        
        fields = strsplit(iUSBL.msg(msgIndex,1:end-2), {' ', ','});
        
        if( length(fields) == 4 )
            tempRange = fields{4};
            set(meters.floatRangeEdit, 'String', tempRange(2:end));
        end
        
        if(length(fields) == 7)
            tempRange = fields{4};
            set(meters.floatRangeEdit, 'String', tempRange(2:end));
            tempEast = fields{5};
            set(meters.floatEastingEdit, 'String', tempEast(2:end));
            tempNorthing = fields{6};
            set(meters.floatNorthingEdit, 'String', tempNorthing(2:end));
            tempDepth = fields{7};
            set(meters.floatDepthEdit, 'String', tempDepth(2:end));
            
            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(tempDepth(2:end));
            
            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
        
        easting = randn(1);
        northing = randn(1);
        set(meters.floatEastingEdit, 'String', num2str(easting));
        set(meters.floatNorthingEdit, 'String', num2str(northing));
        set(meters.floatDepthEdit, 'String', int2str(simDepth));
        
        simDepth = simDepth + 1;
        
        float.x = [easting float.x(1:end-1)];
        float.y = [northing float.y(1:end-1)];
        float.z = [-simDepth float.z(1:end-1)];
        
        set(float.hPlot, 'XData', float.x, 'YData', float.y, 'ZData', float.z);
        set(float.plotAxes, 'XLim', [-20 20], 'YLim', [-20 20], 'ZLim', [(-simDepth-10) (-simDepth+90)]);
 
        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-controls.iUSBLSpeedSmooth.Value:end), plotY(end-controls.iUSBLSpeedSmooth.Value: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();
        
       screenSize = get(groot,'ScreenSize');
       stripChart.hFigure = figure( 'Name', 'Float Depth vs Time', ...
            'NumberTitle', 'off', ...
            'Position', [0.05*screenSize(3) 0.05*screenSize(4) 0.5*screenSize(3) 0.4*screenSize(4)]);

        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 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.05*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 meters = initMeters()
        meters = struct();
        
        screenSize = get(groot,'ScreenSize');
        meters.hFigure = figure( ...
            'Name', 'Positions', ...
            'NumberTitle', 'off', ...
            'MenuBar', 'none', ...
            'Toolbar', 'none', ...
            'Position', [0.6*screenSize(3) 0.5*screenSize(4) 0.15*screenSize(3) 0.5*screenSize(4)]);
        
        metersBox = uix.VBox('Parent', meters.hFigure, 'Padding', 5);
        
        boatPanel = uix.BoxPanel('Parent', metersBox, 'Title', 'Ship', 'Padding', 5);
        boatBox = uix.VBox('Parent', boatPanel, 'Padding', 5);
        meters.shipLatEdit = uicontrol('Parent', boatBox, 'Style', 'edit', ...
            'FontSize', 15, 'FontWeight', 'bold');
        uicontrol('Parent', boatBox, 'Style', 'text', ...
            'FontSize', 10, ...
            'String', 'Ship Latitude');
        
        meters.shipLonEdit = uicontrol('Parent', boatBox, 'Style', 'edit', ...
            'FontSize', 15, 'FontWeight', 'bold');
        uicontrol('Parent', boatBox, 'Style', 'text', ...
            'FontSize', 10, ...
            'String', 'Ship Longitude')
        
        floatPanel = uix.BoxPanel('Parent', metersBox, 'Title', 'Float', 'Padding', 5);
        floatBox = uix.VBox('Parent', floatPanel, 'Padding', 5);
        meters.floatNorthingEdit = uicontrol('Parent', floatBox, 'Style', 'edit', ...
            'FontSize', 15, 'FontWeight', 'bold');
        uicontrol('Parent', floatBox, 'Style', 'text', ...
            'FontSize', 10, ...
            'String', 'Float Northing');
        
        meters.floatEastingEdit = uicontrol('Parent', floatBox, 'Style', 'edit', ...
            'FontSize', 15, 'FontWeight', 'bold');
        uicontrol('Parent', floatBox, 'Style', 'text', ...
            'FontSize', 10, ...
            'String', 'Float Easting')
        
        meters.floatDepthEdit = uicontrol('Parent', floatBox, 'Style', 'edit', ...
            'FontSize', 15, 'FontWeight', 'bold');
        uicontrol('Parent', floatBox, 'Style', 'text', ...
            'FontSize', 10, ...
            'String', 'Float Depth')
        
        meters.floatRangeEdit = uicontrol('Parent', floatBox, 'Style', 'edit', ...
            'FontSize', 15, 'FontWeight', 'bold');
        uicontrol('Parent', floatBox, 'Style', 'text', ...
            'FontSize', 10, ...
            'String', 'Range')
        
        meters.exitButton = uicontrol('Parent', floatBox, 'Style', 'pushbutton', ...
            'String', 'Exit', ...
            'Callback', @exitCallback);
        set(metersBox, 'Heights', [-1 -2]);
    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 boat = initBoat(boatName)
        fprintf('Init Paragon\n');
        boat = struct();
        
        boat.tailLength = 10;
        boat.lat = nan(1, boat.tailLength);
        boat.lon = nan(1, boat.tailLength);
        
        boat.lat(boat.tailLength) = 36.805;
        boat.lon(boat.tailLength) = -121.792;
        
        switch boatName
            case 'Paragon'
                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));
                boat.XY = [tempX; tempY];
                
                boat.Patch = patch(tempX(1,:), tempY(2,:), ...
                    'r', 'Parent', chart.chartAxes);
        end
        
        boat.XY = [X; Y];
    end
%-----------------------

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

%-----------------------
function writeKMLFile(lat, lon, heading, GEZoom)
    fid = fopen('kmlTrackTest.kml', 'w');
    
    fprintf(fid,'<?xml version="1.0" encoding="UTF-8"?>\n');
    fprintf(fid,'<kml xmlns="http://www.opengis.net/kml/2.2"\n');
    fprintf(fid,' xmlns:gx="http://www.google.com/kml/ext/2.2">\n');
   
    fprintf(fid,'<Document>\n');

    fprintf(fid,'   <Placemark>\n');
    fprintf(fid,'      <gx:Track>\n');
    for i=1:length(lon)
        fprintf(fid,'         <gx:coord>%3.9f %2.9f %3f </gx:coord>\n',lon(i), lat(i), 10);
    end
    
    fprintf(fid,'      </gx:Track>\n');
    fprintf(fid,'  </Placemark>\n\n');
    
    fprintf(fid,'  <Placemark>\n');
    fprintf(fid,'     <name>Paragon</name>\n');
    fprintf(fid,'     <Model id="Paragon">\n');
    fprintf(fid,'        <Location id="Paragon Model Location">\n');
    fprintf(fid,'           <longitude>%3.9f</longitude>\n', lon(end));
    fprintf(fid,'           <latitude>%3.9f</latitude>\n', lat(end));
    fprintf(fid,'           <altitude>0</altitude>\n');
    fprintf(fid,'        </Location>\n');
    fprintf(fid,'        <Orientation>\n');
    fprintf(fid,'           <heading>%f</heading>\n', mod(heading + 180, 360));
    fprintf(fid,'           <tilt>0</tilt>\n');
    fprintf(fid,'           <roll>0</roll>\n');
    fprintf(fid,'        </Orientation>\n');
    fprintf(fid,'        <Scale>\n');
    fprintf(fid,'           <x>1</x>\n');
    fprintf(fid,'           <y>1</y>\n');
    fprintf(fid,'           <z>1</z>\n');
    fprintf(fid,'        </Scale>\n');
    fprintf(fid,'        <Link>\n');
    fprintf(fid,'           <href>Paragon.dae</href>\n');
    fprintf(fid,'        </Link>\n');
    fprintf(fid,'     </Model>\n');
    fprintf(fid,'  </Placemark>\n\n');

    fprintf(fid,'       <LookAt>\n');
    fprintf(fid,'          <longitude>%3.9f</longitude>\n', lon(end));
    fprintf(fid,'          <latitude>%3.9f</latitude>\n', lat(end));
    fprintf(fid,'          <altitude>%f</altitude>\n', GEZoom);
 %  fprintf(fid,'          <tilt>0</tilt>\n');
 %  fprintf(fid,'          <heading>0</heading>\n');
    fprintf(fid,'          <altitudeMode>relativeToGround</altitudeMode>\n');
    fprintf(fid,'       </LookAt>\n\n');

    fprintf(fid,'</Document>\n');
    fprintf(fid,'</kml>\n');
    
    fclose(fid);
end
%-----------------------





