clc;
clear;

% Load data from the file
filename = 'Trial0002_6D.xls';
data = readtable(filename);

% Define constants and variables
global armLength frame time SMPTE westHeight centralHeight centralPitch outputArray rowCounter startFrame endFrame;

armLength = 1.8 * 1000; % Arm length in millimeters
startFrame = 6000;
endFrame = 12000;

frame = data{:, 1};
time = data{:, 2};
SMPTE = data{:, 3};
westHeight = data{:, 6};
centralHeight = data{:, 23};
centralPitch = data{:, 25};

outputArray = {}; % To store results
rowCounter = 1;   % Row counter for output
stop = false;     % Flag to stop processing
nextPosition = 0;     % Initialize position (1 = peak, -1 = trough)

% Function to calculate rotation and other properties
function Calculation(pre, cur, direction)
    global armLength frame time SMPTE westHeight centralHeight centralPitch outputArray rowCounter;

    preHeight = abs(westHeight(pre) - centralHeight(pre));
    curHeight = abs(westHeight(cur) - centralHeight(cur));

    preAngle = asin(preHeight / armLength);
    curAngle = asin(curHeight / armLength);
    radian = preAngle + curAngle;
   

    pitchDiff = (centralPitch(cur) - centralPitch(pre)) * (pi) / 180;

    if direction == 1
        adjustedRadian = radian - pitchDiff;
        location = 'peak';
    else
        adjustedRadian= radian + pitchDiff;
        location = 'trough';
    end

    period = time(cur) - time(pre);

    % Append results to output array
    outputArray{ rowCounter, 1} = frame(cur);
    outputArray{ rowCounter, 2} =  time(cur);
    outputArray{ rowCounter, 3} = SMPTE(cur);
    outputArray{ rowCounter, 4} = westHeight(cur);
    outputArray{ rowCounter, 5} = centralHeight(cur);
    outputArray{ rowCounter, 6} = centralPitch (cur);
    outputArray{ rowCounter, 7} = adjustedRadian;
    outputArray{ rowCounter, 8} = period;
    outputArray{ rowCounter, 9} = location;
 
    rowCounter = rowCounter + 1; % Increment row counter

  
end

% Function to find the next peak or trough

        

function maxIndex = findMax(start, n)
global westHeight position stop endFrame;
    maxIndex = start;
    for i = start:1:(start + n -1)
         if i >= endFrame
            stop = false;
            break;
        end
        if westHeight(i + 1) > westHeight(maxIndex) &&  westHeight(i + 1) ~= 0
            maxIndex = i + 1;
            position = -1;
        

        end
    end
end

function minIndex = findMin(start, n)
 global westHeight position stop endFrame;
    minIndex = start;
    for i = start:1:(start + n)
        if i >= endFrame
            stop = false;
            break;
        end
        if westHeight(i + 1) < westHeight(minIndex) && westHeight(i + 1) ~= 0
            minIndex = i + 1;
            position = 1;
        end
    end
end
% Main processing loop

    outputArray{1, 1} = frame(startFrame);
    outputArray{1, 2} =  time(startFrame);
    outputArray{1, 3} = SMPTE(startFrame);
    outputArray{1, 4} = westHeight(startFrame);
    outputArray{1, 5} = centralHeight(startFrame);
    outputArray{1, 6} = centralPitch (startFrame);
    outputArray{1, 7} = 0;
    outputArray{1, 8} = 0;
    outputArray{1, 9} = 'Start';
    
    rowCounter = rowCounter + 1; % Increment row counter

    prevFrame = startFrame;

fprintf('Starting main loop...\n');
maxFrame = findMax(startFrame,20);
minFrame = findMin(startFrame,20);
if  abs(westHeight(maxFrame) - westHeight(startFrame)) > abs(westHeight(startFrame) - westHeight(minFrame)) 
    nextPosition = 1;
    fprintf('up'    );
else 
    nextPosition = -1;
     fprintf('down'    );

end 

while ~stop
  
    if nextPosition == -1
        nextFrame = findMin(prevFrame, 110); % Find next trough
    else
        nextFrame = findMax(prevFrame, 110);  % Find next peak
    end
%{
    % Check if nextFrame is advancing
    if nextFrame == prevFrame
        fprintf('Warning: nextFrame == prevFrame. No movement detected.\n');
        break;  % Exit the loop to prevent infinite looping
    end
%}
    % Calculate and record values
    Calculation(prevFrame, nextFrame, nextPosition);

    % Update for next iteration
    prevFrame = nextFrame;
    nextPosition = - nextPosition; % Alternate between peak and trough

    % Check stop condition
    if nextFrame == endFrame
        stop = true;
    end
end


% Check outputArray before writing to Excel
if ~isempty(outputArray) && rowCounter > 1
    
    % Write data to Excel
    outputTable = cell2table(outputArray, 'VariableNames', {'Frame', 'time','SMPTE','WestFloat Height', 'CentralFloat Height','CentralFloat Pitch','Radian', 'Period', 'Location'});
    writetable(outputTable, 'output_data2.xlsx');
    disp('Processing complete. Output written to output_data1.xlsx.');
else
    disp('No data processed. Output file not created.');
end





