close all; clear; clc;

%% ================= CONFIG =================

eventsFile  = "001rec_SI.csv";        % Events: time (ms), event name
dataFile    = "001rec_ADC_data.csv";  % ADC data: time (ms), channels...

PARTIAL_OPEN_FRACTION = 0.25;          % Valve position for *_OPEN (between 0 and 1)
IGNITION_DELAY        = 3.0;          % [s] after nitrogen open to assume ignition

% Valve ramp times [s] for each valve (you can tweak these)
rampConfig.LOX.open     = 0.2;   % CLOSED -> PARTIAL on LOX_OPEN
rampConfig.LOX.openFull = 1.5;   % -> FULL on LOX_OPEN_FULL
rampConfig.LOX.close    = 0.3;   % -> CLOSED on LOX_FULL_CLOSED

rampConfig.ETHANOL.open     = 0.5;
rampConfig.ETHANOL.openFull = 1.5;
rampConfig.ETHANOL.close    = 0.5;

rampConfig.NITROGEN.open     = 0.0;
rampConfig.NITROGEN.openFull = 0.0;
rampConfig.NITROGEN.close    = 0.5;


%% ================= LOAD DATA =================

events = readtable(eventsFile);
eventTime_ms = events{:,1};           % first column: time in ms
eventLabel   = string(events{:,2});   % second column: event name as string array

result = readmatrix(dataFile);        % ADC data
% result(:,1) assumed to be time in ms


%% ================= TIME ALIGNMENT =================
% Reference = NITROGEN_OPEN_FULL if available, else NITROGEN_OPEN, else first event.
% We set that reference event to t = -3 s.

idxN2full = find(eventLabel == "NITROGEN_OPEN_FULL", 1, 'first');
idxN2open = find(eventLabel == "NITROGEN_OPEN",      1, 'first');

if ~isempty(idxN2full)
    idxRef = idxN2full;
elseif ~isempty(idxN2open)
    idxRef = idxN2open;
else
    warning("No NITROGEN_OPEN[_FULL] event found. Using first event as reference.");
    idxRef = 1;
end

tRef_ms = eventTime_ms(idxRef);

% ADC time in seconds, such that nitrogen-open is at t = -3 s:
% t = (t_ms - tRef_ms)/1000 - 3  -> at t_ms = tRef_ms => t = -3
time = (result(:,1) - tRef_ms) ./ 1000 - 3;    % [s]

% Event times in the same reference
eventTime_s = (eventTime_ms - tRef_ms) ./ 1000 - 3;


%% ================= SYNTHETIC IGNITION EVENT =================
% Assume ignition occurs IGNITION_DELAY seconds after nitrogen open.

tN2open_s = eventTime_s(idxRef);           % should be -3
tIgn_s    = tN2open_s + IGNITION_DELAY;    % typically 0 s

eventTime_s(end+1) = tIgn_s;
eventLabel(end+1)  = "IGNITION";

% Sort events in time again
[eventTime_s, sortIdx] = sort(eventTime_s);
eventLabel = eventLabel(sortIdx);


%% ================= CALIBRATIONS =================

% Your original scaling on column 14
result(:,14) = result(:,14) ./ 2;

% Temperature calibration (columns 4–9)
temp = 0.000111 .* result(:,4:9) + 2.31991;

% Pressure calibration (columns 10–17)
pressure = result(:,10:17) .* 0.000015 - 6.565;

% Load cell channels
load_cell = result(:,2:3);

% Thrust / weight calibration (in kg)
weight = -1.166759307845543e-04 .* 0.5 .* (load_cell(:,1) + load_cell(:,2)) ...
         + 4.971416323340051e+02;


%% ================= IMPORTANT EVENTS FILTER =================
% Only show these in the event markers (others are hidden)

importantEvents = [ ... %    "ENTER_STATIC_FIRE", ..."EXIT_STATIC_FIRE", ... %"ENTER_ARM", ...
    "UPDATE_ARM", ...    "EXIT_ARM", ...
    "LOX_OPEN", ...
    "LOX_OPEN_FULL", ...
    "ETHANOL_OPEN", ...
    "ETHANOL_OPEN_FULL", ...
    "NITROGEN_OPEN", ...
    "NITROGEN_OPEN_FULL", ...
    "LOX_FULL_CLOSED", ...
    "ETHANOL_FULL_CLOSED", ..."NITROGEN_FULL_CLOSED", ...
    "ENTER_ABORT", ...
    "EXIT_ABORT", ...
    "IGNITION" ...
];

maskImportant = ismember(eventLabel, importantEvents);


%% ================= RECONSTRUCT VALVE SEQUENCES =================

loxCmd = buildValveCmd(time, eventTime_s, eventLabel, "LOX",      PARTIAL_OPEN_FRACTION, rampConfig.LOX);
ethCmd = buildValveCmd(time, eventTime_s, eventLabel, "ETHANOL",  PARTIAL_OPEN_FRACTION, rampConfig.ETHANOL);
n2Cmd  = buildValveCmd(time, eventTime_s, eventLabel, "NITROGEN", PARTIAL_OPEN_FRACTION, rampConfig.NITROGEN);


%% ================= PLOTTING: 3 STACKED SUBPLOTS =================

pressureNames = { ...
    'LOX tank pressure', ...
    'Ethanol tank pressure', ...
    'Ethanol pressure in injection plate', ...
    'Chamber pressure', ...
    'Ethanol pressure at inlet', ...
    'Pressure channel 6', ...
    'Pressure channel 7'};

figure;
tiledlayout(3,1,'TileSpacing','compact');

% ----- Top: reconstructed valve sequence -----
ax1 = nexttile; hold on;
plot(time, loxCmd, 'LineWidth', 1.2, 'DisplayName', 'LOX');
plot(time, ethCmd, 'LineWidth', 1.2, 'DisplayName', 'ETHANOL');
plot(time, n2Cmd,  'LineWidth', 1.2, 'DisplayName', 'NITROGEN');

ylabel('Valve Opening');
ylim([-0.1 1.1]);
yticks([0 PARTIAL_OPEN_FRACTION 1]);
yticklabels({'0', sprintf(' %.2g',PARTIAL_OPEN_FRACTION*100), '100%'});

title('Final Hot Fire');
legend('Location','eastoutside');
grid on;

addEventLines(eventTime_s, eventLabel, maskImportant);

% ----- Middle: thrust -----
ax2 = nexttile; hold on;
plot(time, weight);
ylabel('Thrust [kg]');
grid on;
addEventLines(eventTime_s, eventLabel, maskImportant);

% ----- Bottom: all pressures overlayed -----
ax3 = nexttile; hold on;
for i = 1:5
    plot(time, pressure(:,i), 'DisplayName', pressureNames{i});
end
ylabel('Pressure [bar]');
xlabel('Time [s]');
legend('Location','eastoutside');
grid on;
addEventLines(eventTime_s, eventLabel, maskImportant);

% Link x-axes for all subplots
linkaxes([ax1 ax2 ax3], 'x');
xlim([-4 10]);






%% ====== Make animation video (time-cursor moving in real time) ======
%{
videoName = 'nova2_run_animation.mp4';

v = VideoWriter(videoName, 'MPEG-4');  % use 'MPEG-4' for .mp4
v.FrameRate = 30;                      % frames per second
open(v);

% Time range of your data (already in seconds)
tMin = min(time);
tMax = max(time);

fps      = v.FrameRate;
duration = tMax - tMin;                % [s] actual test duration
nFrames  = ceil(duration * fps);       % so video length ≈ duration
tFrame   = linspace(tMin, tMax, nFrames);

% Create a vertical cursor line on each subplot
axAll    = [ax1 ax2 ax3];
hCursor  = gobjects(numel(axAll), 1);

for i = 1:numel(axAll)
    axes(axAll(i));                    %#ok<LAXES> to make it current
    hold(axAll(i), 'on');
    hCursor(i) = xline(axAll(i), tMin, 'k-', 'LineWidth', 1.5);
end

% Sweep cursor across time and record each frame
for k = 1:nFrames
    tCurr = tFrame(k);

    % Move cursor to current time on all axes
    for i = 1:numel(axAll)
        hCursor(i).Value = tCurr;
    end

    drawnow limitrate;                 % update figure
    frame = getframe(gcf);             % capture current figure
    writeVideo(v, frame);              % write to video
end

close(v);
disp("Saved video to: " + videoName);

%}
%% ============= LOCAL FUNCTIONS (same file) =================
function addEventLines(eventTime_s, eventLabel, mask)
% addEventLines Draw vertical event markers (lines + rotated text) on current axes.
%   addEventLines(eventTime_s, eventLabel)
%   addEventLines(eventTime_s, eventLabel, mask)
%
% eventTime_s : vector of event times in seconds
% eventLabel  : string/cellstr array of labels, same size as eventTime_s
% mask        : optional logical mask to select which events to draw

    if nargin < 3 || isempty(mask)
        mask = true(size(eventTime_s));
    end

    ax = gca;
    ylims  = ylim(ax);
    yrange = diff(ylims);

    % Put label a bit inside the top of the axis
    yLabel = ylims(2) - 0.02 * yrange;

    % Keep only selected events
    tAll      = eventTime_s(mask);
    labelsAll = eventLabel(mask);

    if isempty(tAll)
        return;
    end

    % Group by unique time
    [tUnique, ~, groupIdx] = unique(tAll);

    for g = 1:numel(tUnique)
        t = tUnique(g);

        % All events at this time
        inds        = find(groupIdx == g);
        groupLabels = labelsAll(inds);
        n           = numel(inds);

        % ---- Decide label text ----
        if n == 1
            % Single event → show its actual name
            lbl = char(groupLabels);
        else
            % Multiple events at same timestamp
            if any(endsWith(groupLabels, "FULL_CLOSED"))
                lbl = 'VALVES CLOSING           ';
            elseif any(endsWith(groupLabels, "OPEN_FULL"))
                lbl = 'VALVES OPEN FULL';
            else
                lbl = 'MULTIPLE EVENTS';
            end
        end

        % ---- Draw one vertical line ----
        xline(ax, t, 'r--', 'HandleVisibility', 'off');

        % ---- Draw one label on that line, inside the axis ----
        text(ax, t, yLabel, lbl, ...
            'Rotation', 90, ...
            'Interpreter', 'none', ...   % so "_" isn't treated as subscript
            'VerticalAlignment', 'top', ...
            'HorizontalAlignment', 'right', ...
            'FontSize', 8, ...
            'Clipping', 'on');           % keep it inside the axes
    end
end

function cmd = buildValveCmd(time, eventTime_s, eventLabel, prefix, partialFrac, rampCfg)
% buildValveCmd  Reconstruct valve command (0..1) for a given valve.
%
%   cmd = buildValveCmd(time, eventTime_s, eventLabel, "LOX", partialFrac, rampCfg)
%
% prefix      : "LOX", "ETHANOL", or "NITROGEN"
% partialFrac : value used for *_OPEN (e.g. 0.4)
% rampCfg     : struct with fields .open, .openFull, .close (in seconds)

    % Masks for events relevant to this valve
    maskOpen     = eventLabel == prefix + "_OPEN";
    maskOpenFull = eventLabel == prefix + "_OPEN_FULL";
    maskClosed   = eventLabel == prefix + "_FULL_CLOSED";

    timesAll  = [ eventTime_s(maskOpen); ...
                  eventTime_s(maskOpenFull); ...
                  eventTime_s(maskClosed) ];
    labelsAll = [ eventLabel(maskOpen); ...
                  eventLabel(maskOpenFull); ...
                  eventLabel(maskClosed) ];

    if isempty(timesAll)
        % No events for this valve: always closed
        cmd = zeros(size(time));
        return;
    end

    % Sort valve events by time
    [timesAll, idxSort] = sort(timesAll);
    labelsAll = labelsAll(idxSort);

    % Determine state right before the first time sample
    currentState = 0.0;   % start closed
    for k = 1:numel(timesAll)
        if timesAll(k) <= time(1)
            lbl = labelsAll(k);
            if lbl == prefix + "_OPEN"
                currentState = partialFrac;
            elseif lbl == prefix + "_OPEN_FULL"
                currentState = 1.0;
            elseif lbl == prefix + "_FULL_CLOSED"
                currentState = 0.0;
            end
        end
    end

    % Build event list (for this valve) starting at time(1)
    eTimes   = time(1);
    eTargets = currentState;
    eRampDur = 0;

    for k = 1:numel(timesAll)
        t = timesAll(k);
        if t <= time(1)
            continue;   % already accounted in initial state
        end
        lbl = labelsAll(k);

        if lbl == prefix + "_OPEN"
            target = partialFrac;
            ramp   = rampCfg.open;
        elseif lbl == prefix + "_OPEN_FULL"
            target = 1.0;
            ramp   = rampCfg.openFull;
        elseif lbl == prefix + "_FULL_CLOSED"
            target = 0.0;
            ramp   = rampCfg.close;
        else
            continue;
        end

        eTimes(end+1)   = t;
        eTargets(end+1) = target;
        eRampDur(end+1) = ramp;
    end

    % Convert these event + ramp definitions into a continuous command
    cmd = applyValveRamps(time, eTimes, eTargets, eRampDur);
end


function cmd = applyValveRamps(time, eTimes, eTargets, eRampDur)
% applyValveRamps  Build valve command with linear ramps between states.
%
% time     : time vector
% eTimes   : times of state changes
% eTargets : target state at each eTimes entry
% eRampDur : ramp duration after each event

    cmd = zeros(size(time));

    nEvents = numel(eTimes);
    currentState = eTargets(1);
    lastEndTime  = time(1);

    % Ensure row vectors
    time = time(:).';
    cmd  = cmd(:).';

    for i = 2:nEvents
        tEvent = eTimes(i);
        target = eTargets(i);
        ramp   = eRampDur(i);

        % Constant segment from lastEndTime up to the event time
        idxConst = time >= lastEndTime & time < tEvent;
        cmd(idxConst) = currentState;

        % Ramp segment from event time to event time + ramp
        rampStart = tEvent;
        rampEnd   = tEvent + ramp;

        if ramp > 0
            idxRamp = time >= rampStart & time < rampEnd;
            cmd(idxRamp) = currentState + (target - currentState) .* ...
                           (time(idxRamp) - rampStart) / ramp;
            currentState = target;
            lastEndTime  = rampEnd;
        else
            % Instantaneous change
            currentState = target;
            lastEndTime  = tEvent;
        end
    end

    % Final constant segment after the last ramp
    idxTail = time >= lastEndTime;
    cmd(idxTail) = currentState;

    % Return as column vector to match input style
    cmd = cmd(:);
end
