% TestTrackRotationAngle_MultiSpan.m
% Script to test calculation of trackRotationAngle and track adjustment with more spans and plot vectors

clc; clear; close all;

spanCount = 10; % More spans

% Create dummy spans with varying data
for i = 1:spanCount
    spans(i).LeftEnd.Stagger = 0.02 * sin(i/2);      % stagger oscillates
    spans(i).RightEnd.Stagger = 0.03 * cos(i/3);
    spans(i).LeftEnd.ContactHeight = 0.15 + 0.005*i; % slowly increasing contact height
    spans(i).RightEnd.ContactHeight = 0.15 + 0.005*i;
    spans(i).LeftEnd.TrackZ = 0.05 * i;              % linearly increasing Z offset
    spans(i).RightEnd.TrackZ = 0.05 * i;
    spans(i).LeftEnd.Cant = 0.01 * sin(i/4);         % cant oscillates slowly
    spans(i).RightEnd.Cant = 0.02 * cos(i/5);
    spans(i).SpanType = 1;                            % normal span
    spans(i).StructureIDstart = sprintf('Span_%d', i);
end

% Dummy registrations - create registration points along a simple curve
registrations = cell(spanCount,1);
CW = cell(spanCount,1);
for i = 1:spanCount
    % Generate 3 points for each registration forming a small curve segment
    t = linspace(0, 1, 3)';
    x = i + 0.5 * t;           % X increments along i with small segment length
    y = sin(i/2) + 0.3*t;     % Y oscillates slowly + small increments
    z = zeros(3,1);           % flat for simplicity
    registrations{i} = [x y z];
    CW{i} = [x y z];
end

trackOrigin = [0 0 0];
instancePosition = [];

% Helper functions
CantRotationMatrix = @(cant) [cos(cant) -sin(cant) 0; sin(cant) cos(cant) 0; 0 0 1];
SpanRotationMatrix = @(angle) [cos(angle) -sin(angle) 0; sin(angle) cos(angle) 0; 0 0 1];
CantAngle = @(cant) cant;

track = zeros(spanCount, 6);
RAname = cell(spanCount,1);

figure; hold on; axis equal;
title('Track Registration Directions and Adjusted Positions');
xlabel('X');
ylabel('Y');

colors = lines(spanCount);

for regIndex = 1:spanCount
    registration = registrations{regIndex};
    
    if regIndex == 1
        cw = CW{1}(1, 1:3);
        spanEnd = spans(1).LeftEnd;
    else
        cw = CW{regIndex - 1}(end, 1:3);
        spanEnd = spans(regIndex - 1).RightEnd;
    end
    
    switch regIndex
        case 1
            regDirection = [0 1];
        case spanCount
            delta = CW{regIndex - 1}(end, 1:3) - CW{regIndex - 1}(1, 1:3);
            regDirection = [-delta(2), delta(1)];
        otherwise
            regDirection = registration(3, 1 : 2) - registration(1, 1 : 2);
    end
    
    if ~isempty(instancePosition)
        inPlaneTrackAdjustment = [0 -spanEnd.Stagger -instancePosition];
    else
        inPlaneTrackAdjustment = [0 -spanEnd.Stagger -spanEnd.ContactHeight + spanEnd.TrackZ];
    end
    
    cantRotation = CantRotationMatrix(spanEnd.Cant);
    trackRotationAngle = atan2(regDirection(1), regDirection(2));
    
    if abs(trackRotationAngle) > pi / 2
        trackRotationAngle = trackRotationAngle - sign(trackRotationAngle) * pi;
    end
    
    if isnan(trackRotationAngle)
        trackRotationAngle = 0;
    end
    
    trackRotation = SpanRotationMatrix(trackRotationAngle);
    trackAdjustment = inPlaneTrackAdjustment * trackRotation * cantRotation;
    
    track(regIndex, :) = [cw + trackAdjustment + trackOrigin, CantAngle(spanEnd.Cant), 0, trackRotationAngle];
    RAname{regIndex,1} = spans(regIndex).StructureIDstart;
    
    % Plot CW point
    plot(cw(1), cw(2), 'o', 'Color', colors(regIndex,:), 'MarkerFaceColor', colors(regIndex,:), 'DisplayName', sprintf('CW %d', regIndex));
    
    % Plot regDirection vector starting at cw
    quiver(cw(1), cw(2), regDirection(1), regDirection(2), 0.3, 'Color', colors(regIndex,:), 'LineWidth', 2, 'MaxHeadSize', 2, 'DisplayName', sprintf('Dir %d', regIndex));
    
    % Plot adjusted track position
    plot(track(regIndex,1), track(regIndex,2), '*', 'Color', colors(regIndex,:), 'MarkerSize', 10, 'DisplayName', sprintf('Adj Pos %d', regIndex));
end

% Connect CW points and adjusted track with lines for clarity
plot(cellfun(@(c) c(1,1), CW), cellfun(@(c) c(1,2), CW), '--k', 'DisplayName', 'CW Track');
plot(track(:,1), track(:,2), '-g', 'LineWidth', 1.5, 'DisplayName', 'Adjusted Track');

legend('Location', 'bestoutside');
grid on;