## Copyright (C) 2015   Thomas D. Dean
##
## This file is part of the Octave Geometry Package.
##
## All rights reserved.
## 
## Redistribution and use in source and binary forms, with or without
## modification, are permitted provided that the following conditions are met:
## 
##     1 Redistributions of source code must retain the above copyright notice,
##       this list of conditions and the following disclaimer.
##     2 Redistributions in binary form must reproduce the above copyright
##       notice, this list of conditions and the following disclaimer in the
##       documentation and/or other materials provided with the distribution.
## 
## THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ''AS IS''
## AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
## IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
## ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE FOR
## ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
## DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
## SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
## CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
## OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
## OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
##
## You should have received a copy of the GNU General Public License
## along with this file.  If not, see <http://www.gnu.org/licenses/>.

## -*- texinfo -*- 
## @deftypefn{Function File} {} brg_only_tma
## @deftypefnx{Function File} {@var{result} =} brg_only_tma (@var{verbose})
##
## Calculate the location, course, and speed of a target based on two
## sets of {time, sensor platform course and speed and three bearings}.
## The target is assumed to be on a constant course and speed. The
## sensor platform changes course and speed in the middle of the bearing
## sequence.
##
## The Octave Geometry Package functions are used extensively to
## calculate the target course, speed, and location.
##
## This example presents several 'screen' pages of documentation of the
## calculations and some reasoning about why.  The procedure is
## developed using Euclidian Geometry constructs.  Plots of the results
## are presented in most cases.
##
## The process is to display a 'screen' page of text followed by the
## calculation and a 'Press return to continue' message.  To proceed in
## the example, press return.  ^C to exit.
##
## @end deftypefn

## Author: Thomas D. Dean <tomdean@wavecable.com>
## Created: June 2015
## Version: 0.1

function [] = brg_only_tma(verbose = true)

  if (nargin > 1)
    print_usage ();
  endif

  more off;
  clc;
  disp ("---- Bearings Only Target Motion Analysis ----");
  if (verbose)
    disp ("");
    disp ("This example develops a solution to the bearings only target motion");
    disp ("analysis problem that is similar to the solution found in the");
    disp ("Maneuvering Board Manual.  The bearing  only problem arises from");
    disp ("many situations where a sensor platform wants to determine the");
    disp ("position, direction of motion and speed of a moving target.");
    disp ("Examples include ships detecting objects via passive RF receivers,");
    disp ("submarines detecting objects via passive sonar, and, taking compass");
    disp ("bearings on strange lights over Area 51, etc.");
    disp ("");
    disp ("Determining target location, direction of motion and speed can be");
    disp ("very complicated.  For generalized target motions, complex models");
    disp ("and advanced procedures like lsqlin or statistical methods like lqe");
    disp ("must be used.  This approach simplifies the process by assuming");
    disp ("Euclidian motion, constant direction of motion and constant speeds.");
    disp ("The sensor platform is assumed to change course and speed after the");
    disp ("third bearing, but, its course and speed are constant otherwise.");
    disp ("");
    disp ("Consider a sensor platform starting from some known geographic");
    disp ("location and moving in a known direction at a known speed.");
    disp ("Also, consider a target starting at some different known geographic");
    disp ("location and moving in a known direction at a known speed.  The");
    disp ("locations of the sensor and target may be plotted on a map or");
    disp ("nautical chart.  Most, if not all maps have North up and East to");
    disp ("the right.  Compass (or true) bearings start at 000 at North,");
    disp ("increasing clock wise through 090 at East, 180 at South, 270 at");
    disp ("West, back to 000 at North.");
    disp ("");
    disp ("Without loss of generality, Cartesian coordinates may be placed on");
    disp ("the map with the origin at the starting point of the sensor");
    disp ("platform. Cartesian coordinates have angles starting at 000 along");
    disp ("the +X axis, increasing counter clockwise to 090 at the +Y axis,");
    disp ("etc. To make direction and position correspond to the map, consider");
    disp ("North along the +X axis, East along the +Y axis, South along the -X");
    disp ("axis, and, West along the -Y axis.");
    disp ("");
    disp ("Time is also be adjusted so the starting time is zero.");
    disp ("");
    disp ("Simplifying assumptions for the problem include, the target moves");
    disp ("in a constant direction at a constant speed.  No time or position");
    disp ("offset occurs when the sensor changes direction or speed.");
    disp ("");
    input ("Press Return to continue:");
    blanks ();
    disp ("An example situation has a sensor platform and target starting from");
    disp ("known positions with known directions of motion and speeds.");
    disp ("Consider the sensor platform starting at the origin and the target");
    disp ("starting at some point (x,y);  A numerical example:");
    disp ("");
  endif;
  disp ("The sensor starts from the origin on course 090, speed 15 mph.");
  disp ("After 3 hours, the sensor changes course to 050 and speed 22mph.");
  disp ("The target starts from point (95,16)  on course 170, speed 10mph.");
  disp ("");
  disp ("Load the geometry package:");
  show ("pkg load geometry;");
  disp ("");
  disp ("Clock Time:");
  show ("clock = [1300, 1430, 1600, 1630, 1730, 1830]';");
  disp ("");
  disp ("Convert to relative hours, starting at zero:");
  show ("t = floor(clock/100)+(clock-floor(clock/100)*100)/60;");
  show ("t = t - t(1);");
  disp ("");
  disp ("Number of elements in the problem statement:");
  show ("n = numel(t);");
  disp ("");
  disp ("Delta time:");
  show ("dt=[0;t(2:n)-t(1:n-1)];");
  disp ("");
  disp ("Sensor Platform Direction of Movement and Speed:");
  show ("Cse = deg2rad([90, 90, 90, 50, 50, 50]');");
  show ("Spd = [15, 15, 15, 22, 22, 22]';");
  disp ("");
  disp ("Target Location, Direction of Movement and Speed:");
  show ("tgtLoc = [95,16];");
  show ("tgtCse = deg2rad(170);");
  show ("tgtSpd = 10;");
  disp ("");
  input ("Press Return to continue the calculations:");
  blanks ();
  disp ("Sensor Platform Motion:");
  show ("Posit = cumsum([dt .* Spd .* cos(Cse), dt .* Spd .* sin(Cse)]);");
  disp ("Remember, the sensor changes course and speed.");
  show ("Leg = createLine([Posit(1,:),Posit(3,:); Posit(3,:),Posit(6,:)]);");
  disp ("");
  disp ("Target Motion:");
  show ("tgtPosit = tgtLoc + [t * tgtSpd * cos(tgtCse), t * tgtSpd * sin(tgtCse)];");
  disp ("The target does not change direction or speed.");
  show ("tgtLeg=createLine([tgtPosit(1,:),tgtPosit(6,:)]);");
  disp ("");
  disp ("Calculate Plot Limits:");
  show ("Prange = [Posit;tgtPosit];");
  show ("axis_limit = [min(Prange(:,1))-10,max(Prange(:,1))+10,min(Prange(:,2))-10,max(Prange(:,2))+10];");
  disp ("");
  input ("Press Return to Plot the Geographical situation:");
  blanks ();
  disp ("Plot the Geographical situation:");
  show ("GrProb = figure; hold on; axis(axis_limit);");
  show ("drawEdge(Leg, 'color', 'b');");
  show ("drawPoint(Posit, 'color', 'b');");
  show ("drawEdge(tgtLeg, 'color', 'r');");
  show ("drawPoint(tgtPosit, 'color', 'r');");
  show ("grid;");
  show ("text(Posit(1,1)+2,Posit(1,2),' S1', 'color', 'b');");
  show ("text(Posit(2,1)+2,Posit(2,2),' S2', 'color', 'b');");
  show ("text(Posit(3,1)+2,Posit(3,2),' S3', 'color', 'b');");
  show ("text(Posit(4,1)+2,Posit(4,2),' S4', 'color', 'b');");
  show ("text(Posit(5,1)+2,Posit(5,2),' S5', 'color', 'b');");
  show ("text(Posit(6,1)+2,Posit(6,2),' S6', 'color', 'b');");
  show ("text(tgtPosit(1,1)+1,tgtPosit(1,2)+1,' T1', 'color', 'r');");
  show ("text(tgtPosit(2,1)+1,tgtPosit(2,2)+1,' T2', 'color', 'r');");
  show ("text(tgtPosit(3,1)+1,tgtPosit(3,2)+1,' T3', 'color', 'r');");
  show ("text(tgtPosit(4,1)+1,tgtPosit(4,2)+1,' T4', 'color', 'r');");
  show ("text(tgtPosit(5,1)+1,tgtPosit(5,2)+1,' T5', 'color', 'r');");
  show ("text(tgtPosit(6,1)+1,tgtPosit(6,2)+1,' T6', 'color', 'r');");
  show ("title('Bearing Only TMA - Geographical Situation');");
  show ("xlabel('Miles  + North');");
  show ("ylabel('Miles  + East');");
  disp ("");
  disp ("From t(1) to t(n), the Sensor Platform moves S1 -> S6");
  disp ("and the target moves from T1 -> T6.");
  disp ("");
  disp ("At sensor position S1, the target bearing line is S1 -> T1.");
  disp ("At sensor position S2, the target bearing line is S2 -> T2.");
  disp ("etc.");
  disp ("");
  disp ("Using the geometry package, it is very easy to calculate lines between");
  disp ("two points and to plot them.");
  disp ("");
  input ("Press Return to calculate the bearing lines:");
  disp ("");
  show ("calcBrgs = angle2Points(Posit,tgtPosit);");
  disp ("");
  input ("Press Return to add the bearing lines to the plot:");
  disp ("");
  show ("drawEdge(Posit, tgtPosit, 'color', 'g');");
  show ("hold off;");
  disp ("");
  disp ("The next part of the process will be plotted in a different figure.");
  disp ("");
  input ("Press Return to continue:");
  blanks ();
  disp ("From this point on, the problem will take somewhat larger steps.");
  disp ("");
  disp ("The next step in the process changes from a geographical");
  disp ("representaton to a relative representation where the ");
  disp ("Sensor Platform is always at the origin.");
  disp ("");
  disp ("This presents the bearing lines to the target as originating from");
  disp ("the origin.  Target motion is relative to the Sensor Platform.  At");
  disp ("first, target relative position points are plotted on the bearing");
  disp ("lines.");
  disp ("");
  input ("Press Return to plot the Problem as a relative motion problem:");
  blanks ();
  show ("ReProb = figure; hold on;");
  show ("relTgtPosit = tgtPosit-Posit;");
  disp ("");
  show ("relTgtLeg = createLine([relTgtPosit(1,:),relTgtPosit(3,:);relTgtPosit(3,:),relTgtPosit(6,:)]);");
  show ("relBrg = createLine([zeros([6,2]),tgtPosit-Posit]);");
  show ("drawEdge([zeros([6,2]),1.2*relTgtPosit], 'color', 'g');");
  show ("relTgtEdge = [relTgtPosit(1,:),relTgtPosit(3,:); relTgtPosit(3,:),relTgtPosit(6,:)];");
  show ("drawEdge(relTgtEdge, 'color', 'r');");
  show ("drawPoint(relTgtPosit, 'color', 'r');");
  show ("grid;");
  show ("text(relTgtPosit(1,1)+1,relTgtPosit(1,2)+1,' T1', 'color', 'r');");
  show ("text(relTgtPosit(2,1)+1,relTgtPosit(2,2)+1,' T2', 'color', 'r');");
  show ("text(relTgtPosit(3,1)+1,relTgtPosit(3,2)+1,' T3', 'color', 'r');");
  show ("text(relTgtPosit(4,1)+1,relTgtPosit(4,2)+1,' T4', 'color', 'r');");
  show ("text(relTgtPosit(5,1)+1,relTgtPosit(5,2)+1,' T5', 'color', 'r');");
  show ("text(relTgtPosit(6,1)+1,relTgtPosit(6,2)+1,' T6', 'color', 'r');");
  show ("title('Bearing Only TMA - Sensor Relative Situation');");
  show ("xlabel('Miles  + North');");
  show ("ylabel('Miles  + East');");
  disp ("");  
  disp ("As before, from t(1) to t(6), the target moves from T1 -> T6.  But,");
  disp ("since this is motion relative to the Sensor Platform, the motion of");
  disp ("the target appears linear T1 -> T3, but, the relative motion changes");
  disp ("after T3 because the direction of motion of Sensor Platform changes ");
  disp ("at t(3). The motion of the target appears linear from T3 to T6.");
  disp ("");
  input ("Press Return to continue:");
  blanks ();
  disp ("Now, use geometry to construct a solution to the bearing only");
  disp ("target motion problem.");
  disp ("");  
  disp ("Notation: O is the origin, OT1 is a line defined by O and T1, T1T2");
  disp ("is a line defined by points T1 and T2.  Since we are using the");
  disp ("geometry, the word define will be used instead of the geometrical");
  disp ("word construct, and calculate instead of measure.");
  disp ("");  
  disp ("The relative motion line, T1T3 consists of two equal segments,");
  disp ("since the target vector is constant and the time interval t2-t1");
  disp ("equals t3-t2.");
  disp ("");  
  disp ("Note: if the time intervals are not equal we must use proportional");
  disp ("distances for dist(P1,P2) and dist(P2,X1):");
  disp ("  (t2-t1)/dist(P1,P2) == (t3-t2)/DIST(p2,x1)");
  disp ("");  
  input ("Press Return to select and plot a point on the 2nd bearing line:");
  blanks ();
  disp ("Select any point, P2 on 2nd bearing line.");
  disp ("");
  show ("P2 = 0.7*relTgtPosit(2,:);");
  disp ("");
  show ("drawPoint(P2, 'color', 'm');");
  show ("text(P2(1), P2(2)+1,' P2', 'color', 'm');");
  disp ("");  
  input ("Press Return to continue:");
  blanks ();
  disp ("Define a line parallel to T1T3 through P2, intersecting OT1");
  disp ("at point Z1 and OT3 at Y1.  This line, Y1Z1 is parallel to the");
  disp ("relative motion of the target.  By properties of parallel lines,");
  disp ("line Y1Z1 consists of two equal segments, Y1P2 and Y1Z1.");
  disp ("Define lineP2 parallel to T1T3, through P2");
  disp ("");
  show ("x=relTgtPosit(1,1);");
  show ("y=relTgtPosit(1,2);");
  show ("dx=relTgtPosit(3,1)-relTgtPosit(1,1);");
  show ("dy=relTgtPosit(3,2)-relTgtPosit(1,2);");
  disp ("");
  disp ("Define lineP2 parallel to T1T3, through P2");
  disp ("Do this in two steps so it fits on the screen.");
  disp ("");
  show ("lineP2 = createLine(x,y,dx,dy);");
  show ("lineP2 = parallelLine(lineP2,P2);");
  show ("drawLine(lineP2, 'color', 'c');");
  disp ("");  
  input ("Press Return to draw and label the points:");

  show ("Z1 = intersectLines(relBrg(1,:), lineP2);");
  show ("Y1 = intersectLines(relBrg(3,:), lineP2);");
  show ("drawPoint(Y1, 'color', 'b');");
  show ("drawPoint(Z1, 'color', 'b');");
  show ("text(Y1(1), Y1(2)+1,' Y1', 'color', 'b');");
  show ("text(Z1(1), Z1(2)+1,' Z1', 'color', 'b');");

  disp ("Define a line through P2 perpendicular to line OT1, labeling the");
  disp ("intersection with OT1 as point P1 and the intersection with OT3 as");
  disp ("P3. ");
  disp ("");  
  input ("Press Return to define the line:");

  show ("P1P3 = orthogonalLine(relBrg(1,:), P2);");
  show ("P1 = intersectLines(relBrg(1,:),P1P3);");
  show ("P3 = intersectLines(relBrg(3,:),P1P3);");
  disp ("");  
  input ("Press Return to add the line and points to the display:");

  show ("drawPoint(P1, 'color', 'm');");
  show ("drawPoint(P3, 'color', 'm');");
  show ("drawEdge(P1,P3, 'color', 'm');");
  show ("text(P1(1), P1(2)+1,' P1', 'color', 'm');");
  show ("text(P3(1), P3(2)+1,' P3', 'color', 'm');");
  disp ("");  
  input ("Press Return to continue:");
  disp ("");  
  disp ("Define a line through Y1 perpendicular to line P1P3,");
  disp ("labeling the intersection with line P1P3 as X1.");
  disp ("");  
  show ("X1Y1 = orthogonalLine(P1P3, Y1);");
  show ("X1 = intersectLines(P1P3, X1Y1);");
  show ("drawEdge(Y1,X1, 'color', 'b');");
  show ("drawPoint(X1, 'color', 'b');");
  show ("text(X1(1), X1(2)+1,' X1', 'color', 'b');");
  disp ("");  
  disp ("Since Z1P2 equals Y1P2,");
  disp (" angle P1P2Z1 equals angle Y1P2P3,");
  disp (" triangle P1P2Z1 and triangle Y1P2X1 are right triangles,");
  disp (" THEY ARE CONGRUENT TRIANGLES.");
  disp ("Therefore, dist(P1,P2) equals dist(P2,X1).");
  disp ("");  
  disp ("Depending on the ratio of horizontal to vertical plot sizes,");  
  disp ("the triangle may not look like a right triangle, but, it is.");  
  disp ("The maximum angle between two lines is 180 degrees or pi radians.");
  disp ("");
  show ("thisAngle = mod(lineAngle(P1P3,relBrg(1,:))*180/pi,180)");
  printf("The angle between the lines is %d\n",thisAngle);
  disp ("");  
  input ("Press Return to continue:");
  blanks ();
  disp ("Using the fact that we have congruent right triangles,");  
  disp ("we can reverse this process, removing knowledge of the");
  disp ("target distance and the line of relative movement.");
  disp ("Removing these two facts, leaves us with the bearings");  
  disp ("only problem.");  
  disp ("");  
  disp ("");  
  input ("Press Return to Plot the lines of bearing.");
  disp (""); 
  show ("axis_limit = [min(1.6*relTgtPosit)-10 max(1.6*relTgtPosit)+10];");
  show ("axis_limit = axis_limit(1,[1,3,2,4]);");
  show ("ovRelSol = figure; hold on; axis (axis_limit);");
  show ("drawEdge([zeros([6,2]),1.6*relTgtPosit], 'color', 'g');");
  show ("grid;");
  show ("title('Bearing Only TMA - Sensor Relative Overall Solution');");
  show ("xlabel('Miles  + North');");
  show ("ylabel('Miles  + East');");
  disp ("");  
  input ("Press Return to continue:");
  disp ("");  
  disp ("Select any point, P2 on line relBrg(2,:) that is not on");
  disp ("line T1T3.");
  disp ("");  
  show ("P2 = 1.2*relTgtPosit(2,:);");
  show ("drawPoint(P2, 'color', 'm');");
  show ("text(P2(1), P2(2)+1,' P2', 'color', 'm');");
  disp ("");  
  input ("Press Return to continue:");
  blanks ();  
  disp ("Define a line through P2 orthogonal to the line relBrg(1,:).");
  disp ("Label the intersection of this line with relBrg(1,:) as P1 and the");
  disp ("intersection with relBrg(3,:) as P3.");
  disp ("");  
  show ("P1P3 = orthogonalLine(relBrg(1,:), P2);");
  show ("P1 = intersectLines(relBrg(1,:),P1P3);");
  show ("P3 = intersectLines(relBrg(3,:),P1P3);");
  show ("drawPoint(P1, 'color', 'm');");
  show ("drawPoint(P3, 'color', 'm');");
  show ("drawEdge(P1,P3, 'color', 'm');");
  show ("text(P1(1), P1(2)+1,' P1', 'color', 'm');");
  show ("text(P3(1), P3(2)+1,' P3', 'color', 'm');");
  disp ("");  
  input ("Press Return to continue:");
  blanks ();  
  disp ("Redefine P1P3 to make sure it goes the right way.");  
  disp ("");  
  show ("P1P3 = createLine(P1,P3);");
  disp ("");  
  disp ("Calculate the distance dist(P1,P2) on line P1P3.");
  disp ("Define a point, X1 on edge P2P3 from point P2 toward");
  disp ("P3 so dist(P2,X1) equals dist(P1,P2).");
  disp ("");  
  disp ("Note: If the time intervals are not equal we must use proportional");
  disp ("distances for dist(P1,P2) and dist(P2,X1):");
  disp ("  (t2-t1)/dist(P1,P2) == (t3-t2)/dist(P2,X1)");
  disp ("");
  disp ("Use 2 * distance between P1 and P2 along the line P1P3 as X1.");
  show ("X1 = pointOnLine(P1P3,2*sqrt(sum((P2-P1).^2)));");
  show ("drawPoint(X1, 'color', 'b');");
  show ("text(X1(1), X1(2)+1,' X1', 'color', 'b');");
  disp ("");  
  input ("Press Return to continue:");
  blanks ();
  disp ("Define a line, X1Y1, perpendicular to line P1P3 through X1.  Label");
  disp ("the intersection of line X1Y1 with line OT3 as Y1.");
  disp ("OT1, labeling this intersection as Z1.");
  show ("X1Y1 = orthogonalLine(P1P3,X1);");
  show ("Y1 = intersectLines(relBrg(3,:),X1Y1);");
  show ("drawPoint(Y1, 'color', 'b');");
  show ("text(Y1(1), Y1(2)+1,' Y1', 'color', 'b');");
  show ("drawEdge(X1, Y1, 'color', 'b');");
  disp ("");  
  disp ("Define a line Y1Z1 from point Y1 through P2, intersecting");
  disp ("line relBrg(3,:) at Z1");
  disp ("");
  show ("Y1Z1 = createLine(Y1,P2);");
  show ("Z1 = intersectLines(relBrg(1,:),Y1Z1);");
  show ("drawPoint(Z1, 'color', 'c');");
  show ("text(Z1(1), Z1(2)+1,' Z1', 'color', 'c');");
  show ("drawEdge(Y1, Z1, 'color', 'c');");
  disp ("");  
  input ("Press Return to continue:");
  disp ("");  
  disp ("Triangle P1P2Z1 is congruent to triangle Y1P2X1.  And, as shown");
  disp ("previously, line Y1Z1 is parallel to any line of target ");
  disp ("relative motion  resulting from the sensor platform motion.");
  disp ("during this time interval, t1 through t3.");
  disp ("");
  input ("Press Return to continue:");
  blanks()
  disp ("");
  disp ("By similar logic, it can be shown that the bearing lines 0T4, OT5,");
  disp ("and, OT6 with the target relative motion T4 -> T5 -> T6 will form");
  disp ("congruent triangles with the same construction techniques.");
  disp ("");
  disp ("This construction will be done in one block without intersperced.");
  disp ("connents.  If this is confusing, read the previous comments, changing");
  disp ("1 to 4, 2 to 5, 3 to 6, etc.");
  disp ("");
  disp ("");
  input ("Press Return to perform the calculations and continue:");
  disp ("");
  show ("P5 = 1.2*relTgtPosit(5,:);");
  show ("drawPoint(P5, 'color', 'm');");
  show ("text(P5(1), P5(2)+1,' P5', 'color', 'm');");
  show ("P4P6 = orthogonalLine(relBrg(4,:), P5);");
  show ("P4 = intersectLines(relBrg(4,:),P4P6);");
  show ("P6 = intersectLines(relBrg(6,:),P4P6);");
  show ("drawPoint(P4, 'color', 'm');");
  show ("drawPoint(P6, 'color', 'm');");
  show ("drawEdge(P4,P6, 'color', 'm');");
  show ("text(P4(1), P4(2)+1,' P4', 'color', 'm');");
  show ("text(P6(1), P6(2)+1,' P6', 'color', 'm');");
  show ("P4P6 = createLine(P4,P6);");
  show ("X2 = pointOnLine(P4P6,2*sqrt(sum((P5-P4).^2)));");
  show ("drawPoint(X2, 'color', 'b');");
  show ("text(X2(1), X2(2)+1,' X2', 'color', 'b');");
  show ("X2Y2 = orthogonalLine(P4P6,X2);");
  show ("Y2 = intersectLines(relBrg(6,:),X2Y2);");
  show ("drawPoint(Y2, 'color', 'b');");
  show ("text(Y2(1), Y2(2)+1,' Y2', 'color', 'b');");
  show ("drawEdge(X2, Y2, 'color', 'b');");
  show ("Y2Z2 = createLine(Y2,P5);");
  show ("Z2 = intersectLines(relBrg(4,:),Y2Z2);");
  show ("drawPoint(Z2, 'color', 'c');");
  show ("text(Z2(1), Z2(2)+1,' Z2', 'color', 'c');");
  show ("drawEdge(Y2, Z2, 'color', 'c');");
  disp ("");  
  disp ("Triangle P4P5Z2 is congruent to triangle Y2P5X2.  And, by the");
  disp ("previous logic, line Y2Z2 is parallel to any line of target");
  disp ("relative motion resulting from the sensor platform motion.");
  disp ("during this time interval, t4 through t6");
  disp ("");
  input ("Press Return to continue:");
  blanks ();  
  disp ("Using this later procedure with the bearing lines to the target, we");
  disp ("can define a line parallel to the target's relative motion.  By");
  disp ("adding a vector parallel to this line, of length equal to the");
  disp ("relative speed to the sensor platform vector of course and speed,");
  disp ("we can determine the course and speed of the target.  Any line of");
  disp ("relative motion from the end of the sensor platform course and");
  disp ("speed vector passes through the point OT, the vector of the target");
  disp ("course and speed, since the target course and speed are constant.");
  disp ("Using two lines of relative motion and two corresponding but");
  disp ("different sensor platform course and speed vectors, the");
  disp ("intersection of the two lines of relative motion passes through the");
  disp ("point OT, the vector of the target course and speed.");
  disp ("");
  disp ("Calculate the Sensor Platform motion vector.")
  disp ("");
  show ("endIdx = [3;6]; ## index to select the end of the sensor vector;");
  show ("sensVec = Spd(endIdx) .* [zeros(2,2),[cos(Cse(endIdx)),sin(Cse(endIdx))]];");
  show ("RM1 = parallelLine(Y1Z1,sensVec(1,3:4));");
  show ("RM2 = parallelLine(Y2Z2,sensVec(2,3:4));");
  show ("tgtVec = [0,0,intersectLines(RM1,RM2)];");
  disp ("");
  disp ("Calculate Plot Limits:");
  disp ("");  
  input ("Press Return to plot the motion vectors");
  disp ("");
  show ("Prange = [sensVec(:,3:4);tgtVec(:,3:4)];");
  show ("axis_limit = [min(Prange(:,1))-10,max(Prange(:,1))+10,min(Prange(:,2))-10,max(Prange(:,2))+10];");
  show ("cseSpdSol = figure; hold on; axis(axis_limit);");
  disp ("");
  show ("drawEdge(sensVec, 'color', 'b');");
  show ("drawPoint(sensVec(1,3:4), 'color', 'b'); ## if do both, they are connected;");
  show ("drawPoint(sensVec(2,3:4), 'color', 'b'); ## if do both, they are connected;");
  show ("drawLine(RM1, 'color', 'r');");
  show ("drawLine(RM2, 'color', 'r');");
  show ("drawEdge(tgtVec, 'color', 'r');");
  show ("drawPoint(tgtVec(1,3:4), 'color', 'r');");
  show ("text(1,0,' O', 'color', 'b');");
  show ("text(sensVec(1,3),sensVec(1,4)+1,' S1', 'color', 'b');");
  show ("text(sensVec(2,3),sensVec(2,4)+1,' S2', 'color', 'b');");
  show ("text(tgtVec(1,3),tgtVec(1,4)+1,' T1', 'color', 'r');");
  show ("title('Bearing Only TMA - Graphical Solution, Course and Speed');");
  show ("xlabel('Miles  + North');");
  show ("ylabel('Miles  + East');");
  show ("grid;");

  disp ("");
  blanks ();
  disp ("Now, we plot the solution on the earlier relative bearing plot.");
  disp ("From time t4 to time t6, the target moved at a constant speed.");
  disp ("Calculate the relative speed and distance the target moved along");
  disp ("the relative movement line Z2Y2.");
  disp ("");
  input ("Press Return to recall the relative movement plot:");  
  disp ("");
  show ("figure(ovRelSol); refresh ();");
  disp ("");
  input ("Press Return to Calculate the relative motion distance:");  
  disp ("");
  show ("distRM2 = (t(6) - t(4))*sqrt(sum((tgtVec(3:4)-sensVec(2,3:4)).^2));");
  disp ("");  
  disp ("Redefine the line Y2Z2 so it is from Z2 to Y2 at a rate");  
  disp ("corresponding to the target speed.");  
  disp ("");  
  show ("tmp = Y2 - Z2;");
  show ("tmpBrg = atan2(tmp(2),tmp(1));");
  show ("tmpLine = [Z2, [cos(tmpBrg),sin(tmpBrg)]];");
  disp ("");  
  disp ("Calculate points A and B on lines Z2Y2 and OT6, respectively.");  
  show ("A = pointOnLine(tmpLine, distRM2);");
  show ("AB = parallelLine(relBrg(4,:),A);");
  show ("B = intersectLines(relBrg(6,:), AB);");
  input ("Press Return to plot these values on the relative bearing plot:");
  show ("drawPoint(A, 'color', 'r');");
  show ("drawPoint(B, 'color', 'r');");
  show ("text(A(1),A(2)+1, ' A', 'color', 'r');");
  show ("text(B(1),B(2)+1, ' B', 'color', 'r');");
  show ("drawEdge(A,B, 'color', 'r');");

  show ('printf("Target speed actual: %.1f calc: %.1f\n",tgtSpd, sqrt(sum(tgtVec.^2)));');

  show ('printf("Target course actual: %.1f calc: %.1f\n",tgtCse*180/pi, atan2(tgtVec(4),tgtVec(3))*180/pi);');
  show ('printf("Target speed actual: %.1f calc: %.1f\n",tgtSpd, sqrt(sum(tgtVec.^2)));');
  show ('printf("At time  %d, the target bears %.1f, %.1f miles\n",clock(6), mod(lineAngle(relBrg(6,:))*180/pi,360), sqrt(sum(B.^2)));');

  blanks ();
  disp ("");
  input ("Press Return to continue:");
  disp ("");  
  disp ("This process left 4 plots open.");
  disp ("   1. Geograhical Situation.  The problem as it would appear when");
  disp ("      plotted on a Cartesian coordinates using true bearings.");
  disp ("   2. Relative Situation.  The problem as it would appear when");
  disp ("      plotted on Cartesian  coordinates using relative bearings.");
  disp ("      This plot includes the target location, direction and speed.");
  disp ("      This plot demonstrates the compass and straightedge method");
  disp ("      of solving the problem.  To solve the problem without");
  disp ("      knowledge of the target location, direction and speed,");
  disp ("      reverse the construction steps used here.");
  disp ("   3. Relative Bearings.  The problem plotted on Cartesian");
  disp ("      coordinates without the target location, direction and speed.");
  disp ("      This plot was generated before plot 4 and re-focused for");
  disp ("      addition of the calculated target position.");
  disp ("   4. Solution.  The target course and speed plotted on Cartesian ");
  disp ("      coordinates from the head of the sensor platform vectors.");
  disp ("");
  if (yes_or_no("Do you wish to close all the plots?"))
    show("close all");
  else
    blanks();
    disp ("Use the octave command close all to close the plots.");
  endif

endfunction;

## support function to display a command and then
## execute it in the caller's environment.
function show (str)

  disp ([">> ", str]);
  evalin ("caller", str);

endfunction

## support function to insert blank lines in the display
function blanks (n = 5)

  for idx = 1:n
    disp ("");
  endfor

endfunction
