function [fitResults, rawCurves] = fitTwoGaussianCurvesToAngularSpectralDistribution(S_theta_all, theta_vals, varargin)
%% fitTwoGaussianCurvesToAngularSpectralDistribution
% Fits a two-Gaussian model to multiple spectral curves from θ=0 up to
% the first secondary peak ≥50% of the primary peak, within 0–π/2 radians.
%
% Author: Karthik
% Date: 2025-10-06
% Version: 1.1
%
% Inputs:
%   S_theta_all - 1xN cell array of spectral curves
%   theta_vals  - vector of corresponding theta values (radians)
%
% Optional name-value pairs:
%   'MaxTheta'           - Maximum theta for search (default: pi/2)
%   'ResidualThreshold'     - Max relative deviation allowed (default: 0.3)
%   'PositionThreshold'  - Minimum angular separation between main and
%                          secondary peaks (default: 0.1 rad)
%   'AmplitudeThreshold' - Minimum fraction of primary peak amplitude for secondary peak (default: 0.5)
%
% Output:
%   fitResults - struct array with fields:
%       .pFit, .thetaFit, .xFit, .yFit, .thetaFine, .isValid, .fitMaxTheta

    % --- Parse optional inputs ---
    p = inputParser;
    addParameter(p, 'MaxTheta', pi/2, @(x) isnumeric(x) && isscalar(x));
    addParameter(p, 'ResidualThreshold', 0.3, @(x) isnumeric(x) && isscalar(x));
    addParameter(p, 'PositionThreshold', 0.1, @(x) isnumeric(x) && isscalar(x));
    addParameter(p, 'AmplitudeThreshold', 0.5, @(x) isnumeric(x) && isscalar(x));
    addParameter(p, 'RecenterCurves', true, @islogical);
    parse(p, varargin{:});
    opts = p.Results;

    Ncurves = numel(S_theta_all);
    fitResults = struct('pFit',[],'thetaFit',[],'xFit',[],'yFit',[],...
                    'thetaFine',[],'isValid',[], 'residual',[], 'maxAbs',[], 'R2',[], 'fitMaxTheta',[]);

    rawCurves = struct('x', cell(1, Ncurves), 'theta', cell(1, Ncurves));
    
    if opts.RecenterCurves
        % --- Preprocess curves: shift first peak to zero without wrapping ---
        S_theta_all_shifted = cell(size(S_theta_all));
        for k = 1:Ncurves
            curve = S_theta_all{k};
    
            idx_range = find(theta_vals >= 0 & theta_vals <= opts.MaxTheta);
            if isempty(idx_range)
                warning('Curve %d has no data in [0, MaxTheta]; skipping recentring.', k);
                S_theta_all_shifted{k} = curve;
                continue;
            end

            % --- Find all peaks in 0–MaxTheta range and pick largest ---
            [pks, locs] = findpeaks(curve(idx_range));
            if isempty(pks)
                [~, local_max_idx] = max(curve(idx_range));
            else
                [~, max_idx_in_peaks] = max(pks);
                local_max_idx = locs(max_idx_in_peaks);
            end
            peak_idx = idx_range(local_max_idx);

            % --- Determine cutoff index corresponding to π/2 beyond that peak ---
            theta_peak = theta_vals(peak_idx);
            theta_end  = theta_peak + pi/2;
            if theta_end > max(theta_vals)
                theta_end = max(theta_vals);
            end
            idx_end = find(theta_vals <= theta_end, 1, 'last');

            % --- Extract segment from main peak up to π/2 after ---
            S_theta_all_shifted{k} = curve(peak_idx:idx_end);
        end
    
        % --- Pad curves to same length ---
        Npoints_shifted = max(cellfun(@numel, S_theta_all_shifted));
        for k = 1:Ncurves
            len = numel(S_theta_all_shifted{k});
            if len < Npoints_shifted
                S_theta_all_shifted{k} = [S_theta_all_shifted{k}, nan(1, Npoints_shifted-len)];
            end
        end
    
        theta_recentered = linspace(0, pi/2, Npoints_shifted);
    else
        S_theta_all_shifted = S_theta_all;
        theta_recentered    = theta_vals;
    end
    
    for k = 1:Ncurves
        x        = S_theta_all_shifted{k};
        theta    = theta_recentered;
        
        if any(isnan(x))
            warning('Curve %d has NaNs, skipping.', k);
            fitResults(k)           = fillNaNStruct();
            continue;
        end
        
        mask     = theta>=0 & theta<=opts.MaxTheta;
        x        = x(mask);
        theta    = theta(mask);
        
        x        = x / x(1);
        
        % --- Store raw data for plotting ---
        rawCurves(k).x     = x;
        rawCurves(k).theta = theta;

        try
            % --- Filter 1: Ensure curve covers full 0–MaxTheta range ---
            thetaRange = max(theta) - min(theta);
            if thetaRange < 0.9 * opts.MaxTheta
                warning('Curve %d excluded: does not span full 0–MaxTheta range.', k);
                fitResults(k)           = fillZeroStruct(theta, opts.MaxTheta);
                continue;
            end

            if numel(theta) < 8
                warning('Curve %d too short (<8 points), skipping.', k);
                fitResults(k)           = fillNaNStruct();
                continue;
            end
    
            xSmooth        = smooth(x, 5, 'moving');
            curveAmp       = max(xSmooth) - min(xSmooth);
    
            %% 1) Attempt primary detection via findpeaks
            minProm        = opts.AmplitudeThreshold * curveAmp;
            [pk, locIdx]   = findpeaks(xSmooth, 'MinPeakProminence', minProm);
            thetaPeaks     = theta(locIdx);
    
            foundSecondary = false;
            fitMaxTheta    = [];
            secondaryAmp   = [];
    
            secondaryIdx = find(thetaPeaks > opts.PositionThreshold, 1, 'first');
            if ~isempty(secondaryIdx)
                foundSecondary = true;
                fitMaxTheta    = thetaPeaks(secondaryIdx);
                secondaryAmp   = pk(secondaryIdx);
            end
    
            %% 2) If secondary found, fit constrained two-Gaussian directly
            if foundSecondary
                twoGauss = @(p,theta) ...
                    1.0 * exp(-0.5*((theta - p(1))/max(p(2),1e-6)).^2) + ...
                    p(3) * exp(-0.5*((theta - p(4))/max(p(5),1e-6)).^2) + ...
                    p(6); % offset term
    
                mu1_guess    = 0;
                sigma1_guess = 0.1 * fitMaxTheta;
                A2_guess     = secondaryAmp;
                mu2_guess    = fitMaxTheta;
                sigma2_guess = fitMaxTheta / 3;
                p0           = [mu1_guess, sigma1_guess, A2_guess, mu2_guess, sigma2_guess, 0];
    
                lb           = [0, 1e-6, 0, max(fitMaxTheta - 0.1, 0), 1e-6, -0.5];
                ub           = [fitMaxTheta/4, fitMaxTheta, 2, min(fitMaxTheta + 0.1, opts.MaxTheta), fitMaxTheta, 0.5];
    
                optsLSQ      = optimoptions('lsqcurvefit','Display','off', ...
                                       'MaxFunctionEvaluations',1e4,'MaxIterations',1e4);
    
                try
                    pFit     = lsqcurvefit(twoGauss, p0, theta, x, lb, ub, optsLSQ);
                    
                    % --- Handle missing or weak secondary peak ---
                    if pFit(3) <= opts.AmplitudeThreshold
                        % amplitude too small → discard secondary
                        pFit(3) = 0;
                        pFit(4:5) = NaN;
                    end
                    
                    % --- Map to canonical format with primary amplitude = 1 ---
                    pCanonical = [1.0, pFit(1), pFit(2), pFit(3), pFit(4), pFit(5), pFit(6)];
            
                    thetaFine                 = linspace(0, opts.MaxTheta, 500);
                    yFit                      = 1.0*exp(-0.5*((thetaFine - pCanonical(2))/max(pCanonical(3),1e-6)).^2) + ...
                                                 pCanonical(4)*exp(-0.5*((thetaFine - pCanonical(5))/max(pCanonical(6),1e-6)).^2) + ...
                                                 pCanonical(7);
                    
                    % --- Localized residual diagnostics ---
                    yFit_raw = twoGauss(pFit, theta(:));
                    r = x(:) - yFit_raw;
                    
                    % --- Global RMS ---
                    fitResults(k).residual = sqrt(mean(r.^2));
                    
                    % --- Max absolute residual ---
                    fitResults(k).maxAbs = max(abs(r));
                    
                    % --- R^2 ---
                    SS_res           = sum(r.^2);
                    SS_tot           = sum((x(:) - mean(x(:))).^2);
                    fitResults(k).R2 = 1 - SS_res/SS_tot;
                    
                    % --- Check residual threshold: discard secondary peak if either exceeds ---
                    if pCanonical(1) > 1.25 || fitResults(k).residual > opts.ResidualThreshold || fitResults(k).maxAbs > opts.ResidualThreshold
                        % residual too large → discard secondary
                        pCanonical(4)             = NaN;
                        pCanonical(5:6)           = NaN;
                        fitResults(k).isValid     = false;
                    end

                    if ~isnan(pCanonical)
                        fitResults(k).pFit        = pCanonical;
                        fitResults(k).thetaFit    = theta;
                        fitResults(k).xFit        = x;
                        fitResults(k).thetaFine   = thetaFine;
                        fitResults(k).yFit        = yFit;
                        fitResults(k).isValid     = true;
                        fitResults(k).fitMaxTheta = fitMaxTheta;
                        continue; % success → next curve
                    end
                catch
                    warning('Curve %d constrained fit failed, switching to generic fit.', k);
                end
            end
    
            %% 3) Generic two-Gaussian fallback (no or failed secondary detection)
            try
                twoGaussFree = @(p,th) ...
                    p(1)*exp(-0.5*((th - p(2))/max(p(3),1e-6)).^2) + ...
                    p(4)*exp(-0.5*((th - p(5))/max(p(6),1e-6)).^2) + ...
                    p(7); % offset term
                
                % --- Original initial guess ---
                p0_base = [1.0, 0, 0.1, 0.3, 1.0, 0.1, 0];
                
                % --- Generate multiple initial guesses ---
                p0_candidates = [
                    p0_base;
                    [1.0, 0, 0.1, 0.3, 0.50, 0.1, 0];
                    [0.5, 0, 0.1, 0.3, 0.50, 0.1, 0.5];
                    [1.0, 0, 0.1, 0.3, 0.25, 0.1, 0];
                    [0.5, 0, 0.1, 0.3, 0.25, 0.1, 0.5];
                ];
            
                lb  = [0.0, 0, 1e-6, 0, 0, 1e-6, 0.0];
                ub  = [1.0, opts.PositionThreshold, opts.MaxTheta/2, 1.0, opts.MaxTheta, opts.MaxTheta/2, 0.5];
            
                optsLSQfree = optimoptions('lsqcurvefit', 'Display', 'off', ...
                    'MaxFunctionEvaluations', 2e4, 'MaxIterations', 2e4);
            
                bestResidual = Inf;
                bestMaxAbs   = Inf;
                pFreeBest    = nan(size(p0_base));
            
                for i = 1:size(p0_candidates,1)
                    try
                        pTry = lsqcurvefit(twoGaussFree, p0_candidates(i,:), theta(:), x(:), lb, ub, optsLSQfree);
                        rTry  = x(:) - twoGaussFree(pTry, theta(:));
                        residualTry = sqrt(mean(rTry.^2));
                        maxAbsTry   = max(abs(rTry));
            
                        % --- Select candidate with lowest RMS first, then max residual ---
                        if residualTry < bestResidual || (residualTry == bestResidual && maxAbsTry < bestMaxAbs)
                            bestResidual = residualTry;
                            bestMaxAbs   = maxAbsTry;
                            pFreeBest    = pTry;
                            rBest        = rTry;
                        end
                    catch
                        % ignore failed fits
                    end
                end
                
                pFree       = pFreeBest;
                r           = rBest;
            
                fitMaxTheta = max(pFree(5), pFree(2));
                thetaFine   = linspace(0, opts.MaxTheta, 500);
                yFit        = twoGaussFree(pFree, thetaFine);
            
                % --- Diagnostics ---
                fitResults(k).residual = bestResidual;
                fitResults(k).maxAbs   = bestMaxAbs;
                SS_res                 = sum(r.^2);
                SS_tot                 = sum((x(:) - mean(x(:))).^2);
                fitResults(k).R2       = 1 - SS_res/SS_tot;
                
                fitResults(k).thetaFit    = theta;
                fitResults(k).xFit        = x;
                fitResults(k).thetaFine   = thetaFine;
                fitResults(k).yFit        = yFit;
                fitResults(k).fitMaxTheta = fitMaxTheta;
                
                % --- Check residual threshold: discard secondary if exceeded ---
                if pFree(1) > 1.25 || fitResults(k).residual > opts.ResidualThreshold || fitResults(k).maxAbs > opts.ResidualThreshold
                    % 2-Gaussian fit failed → mark for 3-Gaussian fallback
                    pFree(4)              = NaN;
                    pFree(5:6)            = NaN;
                    fitResults(k).isValid = false;
                    fitResults(k).pFit    = pFree;
                    
                    % do NOT continue → next block can attempt 3-Gaussian fit
                
                else
                    % 2-Gaussian fit succeeded → store results and skip 3-Gaussian
                    fitResults(k).isValid = true;
                    fitResults(k).pFit    = pFree;
                    continue;
                end
            catch
                warning('Curve %d: generic two-Gaussian fit failed.', k);
                fitResults(k) = fillNaNStruct();
            end

            %% 4) Three-Gaussian fallback (if two-Gaussian residual too large)
            try
                % --- Define 3-Gaussian model ---
                threeGaussFree = @(p, th) ...
                    p(1)*exp(-0.5*((th - p(2))/max(p(3),1e-6)).^2) + ...
                    p(4)*exp(-0.5*((th - p(5))/max(p(6),1e-6)).^2) + ...
                    p(7)*exp(-0.5*((th - p(8))/max(p(9),1e-6)).^2) + ...
                    p(10); % offset term
            
                % --- Base initial guess ---
                p0_base3 = [1.0, 0, 0.1, 0.5, 0.5, 0.1, 0.3, 1.0, 0.1, 0];
            
                % --- Generate multiple initial guesses (slightly shifted) ---
                p0_candidates3 = [
                    p0_base3;
                    [1.0, 0, 0.1, 0.5, 0.5, 0.1, 0.3, 0.75, 0.1, 0];
                    [0.5, 0, 0.1, 0.5, 0.5, 0.1, 0.3, 1.25, 0.1, 0.5];
                    [1.0, 0, 0.1, 0.5, 0.25, 0.1, 0.3, 1.0, 0.1, 0];
                    [0.5, 0, 0.1, 0.5, 0.25, 0.1, 0.3, 1.0, 0.1, 0.5];
                ];
            
                % --- Bounds ---
                lb = [0.0, 0, 1e-6,  0.0,  0, 1e-6,  0.0,  0, 1e-6, 0.0];
                ub = [1.0, opts.PositionThreshold, opts.MaxTheta/2, ...
                       1.0, opts.MaxTheta/2,         opts.MaxTheta/2, ...
                       1.0, opts.MaxTheta,           opts.MaxTheta/2, 0.5];
            
                % --- Optimizer options ---
                optsLSQ3 = optimoptions('lsqcurvefit', 'Display', 'off', ...
                    'MaxFunctionEvaluations', 2e4, 'MaxIterations', 2e4);
            
                % --- Track best fit ---
                bestResidual3 = Inf;
                bestMaxAbs3   = Inf;
                pFreeBest3    = nan(size(p0_base3));
            
                % --- Loop over initial guesses ---
                for i = 1:size(p0_candidates3,1)
                    try
                        pTry3 = lsqcurvefit(threeGaussFree, p0_candidates3(i,:), theta(:), x(:), lb, ub, optsLSQ3);
                        rTry3 = x(:) - threeGaussFree(pTry3, theta(:));
                        residualTry3 = sqrt(mean(rTry3.^2));
                        maxAbsTry3   = max(abs(rTry3));
            
                        % --- Select best candidate (lowest RMS, then lowest max residual) ---
                        if residualTry3 < bestResidual3 || (residualTry3 == bestResidual3 && maxAbsTry3 < bestMaxAbs3)
                            bestResidual3 = residualTry3;
                            bestMaxAbs3   = maxAbsTry3;
                            pFreeBest3    = pTry3;
                            rBest3        = rTry3;
                        end
                    catch
                        % ignore failed fits
                    end
                end
            
                % --- Extract final best parameters ---
                pFree3 = pFreeBest3;
                r3     = rBest3;
            
                fitMaxTheta3 = max(pFree3([2,5,8]));
                thetaFine3   = linspace(0, opts.MaxTheta, 500);
                yFit3        = threeGaussFree(pFree3, thetaFine3);
            
                % --- Diagnostics ---
                fitResults(k).residual = bestResidual3;
                fitResults(k).maxAbs   = bestMaxAbs3;
                SS_res3 = sum(r3.^2);
                SS_tot3 = sum((x(:) - mean(x(:))).^2);
                fitResults(k).R2 = 1 - SS_res3/SS_tot3;
            
                % --- Check validity criteria ---
                if any(pFree3([1,4,7]) > 1.25) || ...
                   fitResults(k).residual > opts.ResidualThreshold || ...
                   fitResults(k).maxAbs > opts.ResidualThreshold
                    pFree3(4:9) = NaN; % invalidate secondary/tertiary
                    fitResults(k).isValid = false;
                else
                    fitResults(k).isValid = true;
                end
            
                % --- Store results ---
                fitResults(k).pFit        = pFree3;
                fitResults(k).thetaFit    = theta;
                fitResults(k).xFit        = x;
                fitResults(k).thetaFine   = thetaFine3;
                fitResults(k).yFit        = yFit3;
                fitResults(k).fitMaxTheta = fitMaxTheta3;
            
            catch
                warning('Curve %d: generic three-Gaussian fit failed.', k);
                fitResults(k) = fillNaNStruct();
            end
        catch
            warning('Curve %d fit failed completely.', k);
            fitResults(k)           = fillNaNStruct();
        end
    end
end

% --- Helper: return struct with NaNs (fit failed) ---
function s = fillNaNStruct()
    s = struct( ...
        'pFit',        nan(1,10), ...
        'thetaFit',    nan, ...
        'xFit',        nan, ...
        'yFit',        nan, ...
        'thetaFine',   nan, ...
        'isValid',     false, ...
        'residual',    nan, ...
        'maxAbs',      nan, ... 
        'R2',          nan, ...
        'fitMaxTheta', nan ...
     );
end

% --- Helper: return struct with zeros (no secondary peak found) ---
function s = fillZeroStruct(theta, fitMaxTheta)
    s = struct( ...
        'pFit',        zeros(1,10), ...
        'thetaFit',    theta, ...
        'xFit',        zeros(size(theta)), ...
        'yFit',        zeros(size(theta)), ...
        'thetaFine',   zeros(1,500), ...
        'isValid',     false, ...
        'residual',    zeros(size(theta)), ...
        'maxAbs',      zeros(size(theta)), ...
        'R2',          zeros(size(theta)), ...
        'fitMaxTheta', fitMaxTheta ...
    );
end