clear
close

shipLOA = 25;
shipBeam = 7;
shipShapeY = [0, shipBeam/2,  shipBeam/2, -shipBeam/2, -shipBeam/2,   0];
shipShapeX = [0 -(.3*shipLOA) -shipLOA,   -shipLOA     -(.3*shipLOA) 0];
shipShapeXY = [shipShapeX; shipShapeY];

shipLat = 36.8020317;
shipLon = -121.7872017;
shipHeading = 10;
shipHeadingXY = -(shipHeading - 90);

zone = utmzone(shipLat, shipLon);
wgs84 = wgs84Ellipsoid;
ellipsoid = [wgs84.SemimajorAxis wgs84.Eccentricity];

axesm utm
setm(gca,'zone', zone, 'geoid', ellipsoid, ...
    'grid','on','meridianlabel','on','parallellabel','on');
c = load('coastline.dat');
plotm(c(:,2), c(:,1), 'Color', 'cyan')

[shipE, shipN] = mfwdtran(shipLat, shipLon);
floatE = shipE - 10000;
floatN = shipN - 10000;
[floatLat, floatLon] = minvtran(floatE, floatN);

[rng, brg] = distance(shipLat, shipLon, floatLat, floatLon, ellipsoid);
plotm([shipLat floatLat], [shipLon floatLon], 'Color', 'red');

M = makehgtform('zrotate', degtorad(shipHeadingXY));
M = M(1:2, 1:2);
shipShapeXYRot = M * shipShapeXY;

stop = -1;
i = 0;
while stop < 0
    offset = -i * 10.0;
    shipShapeXYNew = [shipE + offset + shipShapeXYRot(1,:); shipN + shipShapeXYRot(2,:)];
    [shipShapeLat, shipShapeLon] = minvtran(shipShapeXYNew(1,:), shipShapeXYNew(2,:));
   
    if stop == -1
        stop = -2;
    else
        linem(oldShipShapeLat, oldShipShapeLon, 'EraseMode', 'xor', 'Color', 'green');
        text(textX, textY, oldStr, 'EraseMode', 'xor');
    end
    
    linem(shipShapeLat, shipShapeLon, 'EraseMode','xor', 'Color', 'green');
    
    str = sprintf('Range = %6.0f\nBearing = %3.0f\nShip Lat = %9.6f\nShip Lon = %10.6f', rng, brg, shipShapeLat(1), shipShapeLon(1));
    fig = get(gca);
    textX = fig.XLim(1) + 0.05 * (fig.XLim(2) - fig.XLim(1));
    textY = fig.YLim(1) + 0.2 * (fig.YLim(2) - fig.YLim(1));
    text(textX, textY, str, 'EraseMode', 'xor');
    
    pause(1);
    
    oldShipShapeLat = shipShapeLat;
    oldShipShapeLon = shipShapeLon;
    oldStr = str;
    
    i = i + 1;
end





