%
% This script file tests the functions of the Chapter 4 Progamming
% Exercises.  The {TOOL} frame offset TrelW and the {STATION} frame offset
% SrelB are defined, as are the four positions for which we are two find
% the joint coordinates.  The "current" joint angles are initially zero,
% and are reset to each "near" position in turn.
%
% This version of the script uses a MATLAB "structure" to keep track of all
% the information.  Not really necessary, but it's a useful thing to know.
% For example, each trajectory point could have a different {TOOL} offset,
% which could be stored in a "tool" field of the "point" structure.

TrelW = utoi([0.1 0.2 30]');    % Define tool offset and
SrelB = utoi([-0.1 0.3 0]');    % station offset.

DEG = 180/pi;   % Conversion factors
RAD = pi/180;

point(1).TrelS = [0.0 0.0 -90]';    % Define a structure
point(2).TrelS = [0.6 -0.3 45]';    % with the user form of
point(3).TrelS = [-0.4 0.3 120]';   % the four desired goal
point(4).TrelS = [0.8 1.4 30]';     % positions.

current = zeros(3,1);  % Current joint angle vector initially zero.

% Now move from position to position, displaying results as we go...

for i = 1:4 
    [point(i).sol,point(i).near,point(i).far] = ...     % Solve for the
    solve(utoi(point(i).TrelS),TrelW,SrelB,current);    % joint angles.
    if point(i).sol == 0
        str = sprintf('Point %1d: No solutions!',i);    % No solution,
        disp(str);      % break out and try go to next position
    elseif point(i).sol == 1        % One solution, so we "move" there
        current = point(i).near;    % and set the current joint angles
        str = sprintf('Point %1d: One solution:',i);    % to those
        disp(str);       % values; then display the single solution.
        str = sprintf('  Near: %.2f   %.2f   %.2f degrees',point(i).near*DEG);
        disp(str);
    elseif point(i).sol == 2        % Two solutions, so we "move" to the
        current = point(i).near;    % near solution and set the current
        str = sprintf('Point %1d: Two solutions:',i);   % joint angles to
        disp(str);	% those near values; then display both solutions.
        str = sprintf('  Near: %.2f   %.2f   %.2f degrees',point(i).near*DEG);
        disp(str);
        str = sprintf('  Far : %.2f   %.2f   %.2f degrees',point(i).far*DEG);
        disp(str);
    end;
end;