function soITrans = findSoITransitions(initialState, maxSearchUT, soiSkipIds, massLoss, orbitDecay, celBodyData)
%findSoITransitions Summary of this function goes here
%   Detailed explanation goes here

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %findSoITransitions() returns a matrix of information that contains
    % data on upcoming SoI transitions for a given state.  SoI transition
    % information is given in the following format:
    %
    % [transType, transUT, fromSoI, toSoI, fromRx, fromRy, fromRz, fromVx, fromVy, fromVz, toRx, toRy, toRz, toVx, toVy, toVz]
    %
    % transType - type of SoI transition.  1 for upwards, -1 for downwards
    % transUT   - UT of the SoI transition
    % fromSoI   - ID number of the body transitioning out of
    % toSoI     - ID number of the body transitioning into
    %
    % fromRx    - The following three are the position components of the s/c prior to leaving, relative to the fromSoI body
    % fromRy
    % fromRz
    % fromVx    - The following three are the velocity components of the s/c prior to leaving, relative to the fromSoI body
    % fromVy
    % fromVz
    %
    % toRx    - The following three are the position components of the s/c after moving to new SoI, relative to the toSoI body
    % toRy
    % toRz
    % toVx    - The following three are the velocity components of the s/c after moving to new SoI, relative to the toSoI body
    % toVy
    % toVz
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    global num_SoI_search_revs use_selective_soi_search soi_search_tol num_soi_search_attempts_per_rev;

    if(isempty(use_selective_soi_search))
        use_selective_soi_search = 0;
    end

    if(isempty(num_SoI_search_revs))
        num_SoI_search_revs = 3;
    end

    if(isempty(soi_search_tol))
        soi_search_tol = 1E-12;
    end

    if(isempty(num_soi_search_attempts_per_rev))
        num_soi_search_attempts_per_rev = 1000;
    end

    ut = initialState(1);

    if(isempty(maxSearchUT))
        maxSearchUT = Inf;
    else
        if(maxSearchUT <= ut)
            maxSearchUT = Inf;
        end
    end

    bodyID = initialState(8);
    bodyInfo = getBodyInfoByNumber(bodyID, celBodyData);

    gmu = bodyInfo.gm;
    rVect = initialState(2:4)';
    vVect = initialState(5:7)';

    [sma, ecc, inc, raan, arg, truINI] = getKeplerFromState(rVect,vVect,gmu);
%     meanINI = computeMeanFromTrueAnom(truINI, ecc);
%     scBodyInfo = getBodyInfoStructFromOrbit([sma, ecc, inc, raan, arg, meanINI, ut]);

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %There are two cases to consider: moving up the heirarchy and moving
    % down the heirarchy.  For example:
    % Up Heirarchy: Kerbin -> Sun
    % Down Heirarchy Kerbin -> Mun
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %This segment looks for SoI transitions upwards.
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    upSoITrans = [];
    parentBodyInfo = bodyInfo.getParBodyInfo(celBodyData);
    if(~isempty(parentBodyInfo))
        soiRadius = getSOIRadius(bodyInfo, parentBodyInfo);
        if(ecc < 1)
            [rAp, ~] = computeApogeePerigee(sma, ecc);
        else
            rAp = Inf;
        end

        if(ecc >= 1 || rAp > soiRadius) %definite SoI transition upwards exists!
            if(abs(norm(rVect)-soiRadius)<0.001 && dot(rVect, vVect) > 0)
                truSoI = truINI;
                tempEventLog = initialState;
            else
                truSoI = computeTrueAFromRadiusEcc(soiRadius, sma, ecc);
                tempEventLog = ma_executeCoast_goto_tru(truSoI, initialState, -1, false, soiSkipIds, [], massLoss, orbitDecay, celBodyData);
            end

            upSoITransUT = tempEventLog(end,1);

            if(upSoITransUT <= maxSearchUT)
                [rVectUp, vVectUp] = convertRVVectOnUpwardsSoITransition(bodyInfo, celBodyData, upSoITransUT, tempEventLog(end,2:4), tempEventLog(end,5:7));

                upSoITrans = [1, upSoITransUT, bodyInfo.id, parentBodyInfo.id, ...
                              tempEventLog(end,2:7), ...
                              rVectUp', vVectUp'];
            else
                upSoITrans = [];
            end
        else
            upSoITrans = [];
        end
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %This segment looks for SoI transitions downwards.  This is the
    % trickier of the two algorithms.
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    parentBodyInfo = bodyInfo.getParBodyInfo(celBodyData);
    [childBodies] = getChildrenOfParentInfo(celBodyData, lower(bodyInfo.name));
    downSoITrans = [];

    searchableChildBodies = KSPTOT_BodyInfo.empty(1,0);
    childBodySoIRadii = [];
    for(child = childBodies)
        childBodyInfo = child{1};

        if(use_selective_soi_search && ismember(childBodyInfo.id,soiSkipIds))
            continue;
        else
            searchableChildBodies(end+1) = childBodyInfo; %#ok<AGROW>
            childBodySoIRadii(end+1) = getSOIRadius(childBodyInfo, bodyInfo);
        end
    end

    if(not(isempty(searchableChildBodies)))
        meanMotion = computeMeanMotion(sma, gmu);
        odefun = @(ut, mean) soiSearchOdeFun(ut, mean, meanMotion);

        [maxSearchUTComputed, maxStepSize] = getMaxSoISearchTime(ut, sma, ecc, truINI, bodyInfo, gmu, parentBodyInfo, num_SoI_search_revs, num_soi_search_attempts_per_rev);
        if(isempty(maxSearchUT) || not(isfinite(maxSearchUT)) || isnan(maxSearchUT))
            maxSearchUT = maxSearchUTComputed;
        end

        tspan = [ut, maxSearchUT];
        if(isempty(maxSearchUT))
            fprintf('%0.9f - %0.9f - %0.9f - %0.9f - %0.9f - %0.9f - %0.9f\n\n', ut, sma, ecc, truINI, gmu, num_SoI_search_revs, num_soi_search_attempts_per_rev);
        end

        meanIni = computeMeanFromTrueAnom(truINI, ecc);
        if(ecc < 1)
            meanIni = AngleZero2Pi(meanIni);
        end
        y0 = meanIni;

        odeEvtFcn = @(ut,mean) getSoITransitionOdeEvents(ut, mean, sma, ecc, inc, raan, arg, bodyInfo, gmu, searchableChildBodies, childBodySoIRadii, celBodyData);
        options = odeset('RelTol',soi_search_tol, 'AbsTol',soi_search_tol, 'Events',odeEvtFcn, 'MaxStep',maxStepSize, 'Refine',1, 'InitialStep',maxStepSize);

        [~,~,te,~,ie] = ode113(odefun,tspan,y0,options);

        if(not(isempty(ie)))
            [crossingUT, I] = min(te);
            childBodyInfo = searchableChildBodies(ie(I));

            tempEventLog = ma_executeCoast_goto_ut(crossingUT, initialState, -1, false, soiSkipIds, massLoss,  orbitDecay, celBodyData);

            [rVectDown, vVectDown] = convertRVVectOnDownwardsSoITransition(childBodyInfo, celBodyData, crossingUT, tempEventLog(end,2:4), tempEventLog(end,5:7));

            if(dot(rVectDown, vVectDown) < 0)
                downSoITrans(end+1,:) = [-1, crossingUT, bodyInfo.id, childBodyInfo.id, ...
                                         tempEventLog(end,2:7), ...
                                         rVectDown', vVectDown'];
            end
        end
    end

    soITrans = [upSoITrans;downSoITrans];
end

function [maxSearchUT, maxStep] = getMaxSoISearchTime(utINI, sma, ecc, truINI, bodyInfo, gmu, parentBodyInfo, num_SoI_search_revs, num_soi_search_attempts_per_rev)
    numStepsPer = num_soi_search_attempts_per_rev;

    if(ecc < 1)
        oPeriod = computePeriod(sma,gmu);

        maxSearchUT = utINI + num_SoI_search_revs .* oPeriod;

        maxStep = abs(maxSearchUT - utINI)/num_SoI_search_revs/numStepsPer;
    else
        soiRadius = getSOIRadius(bodyInfo, parentBodyInfo);
        maxHypTru = AngleZero2Pi(real(computeTrueAFromRadiusEcc(soiRadius, sma, ecc)));

        meanMotion = computeMeanMotion(sma, gmu);
        meanIni = computeMeanFromTrueAnom(truINI, ecc);
        maxMean = computeMeanFromTrueAnom(maxHypTru, ecc);

        maxSearchUT = utINI + (maxMean - meanIni)./meanMotion;

        maxStep = abs(maxSearchUT - utINI)./numStepsPer;
    end
end

function meandot = soiSearchOdeFun(~, ~, meanMotion)
    meandot = meanMotion;
end

function [value, isterminal, direction] = getSoITransitionOdeEvents(ut, mean, sma, ecc, inc, raan, arg, bodyInfo, gmu, childBodies, searchableChildBodies, celBodyData)
    value = [];
    isterminal = [];
    direction = [];

    tru = computeTrueAnomFromMean(mean, ecc);
    [rVect, vVect] = getStatefromKepler(sma, ecc, inc, raan, arg, tru, gmu, false);

    for(i=1:length(childBodies)) %#ok<*NO4LP>
        childBodyInfo = childBodies(i);

        dVect = getAbsPositBetweenSpacecraftAndBody(ut, rVect, bodyInfo, childBodyInfo, celBodyData);
        distToChild = norm(dVect);

        rSOI = searchableChildBodies(i);

        val = distToChild - rSOI;

        value(end+1) = val; %#ok<AGROW>
        direction(end+1) = -1; %#ok<AGROW>
        isterminal(end+1) = 1; %#ok<AGROW>
    end
end

%     for(child = childBodies)
%         childBodyInfo = child{1};
%
%         if(use_selective_soi_search && ismember(childBodyInfo.id,soiSkipIds))
%             continue;
%         end
%
%         soiRadius = getSOIRadius(childBodyInfo, bodyInfo);
%         if(~isempty(parentBodyInfo))
%             soiRadiusParent = getSOIRadius(bodyInfo, parentBodyInfo);
%         else
%             soiRadiusParent = Inf;
%         end
%
%         [rAp, rPe] = computeApogeePerigee(sma, ecc);
%         if(ecc > 1)
%             rAp = Inf;
%         end
%
%         [rApBody, rPeBody] = computeApogeePerigee(childBodyInfo.sma, childBodyInfo.ecc);
%
%         %%%%%%%%%%%
%         %These are the two cases in which an SoI transition downwards is
%         %impossible:
%         % 1) S/C apogee is less than the child body perigee minus the SoI
%         %    radius (we don't get up the child body).
%         % 2) S/C perigee is greater than the child body apogee plus the SoI
%         %    radius (we don't get down to the child body).
%         % 3) Position vectors at the asc/desc nodes must be within some
%         %    small number of SoI radii.
%         %%%%%%%%%%%
%         if(rAp < rPeBody-soiRadius)
%             continue;
%         end
%         if(rPe > rApBody+soiRadius)
%             continue;
%         end
%
%         if(strict_SoI_search)
%             sma1=sma;
%             ecc1=ecc;
%             inc1=inc;
%             raan1=raan;
%             arg1=arg;
%
%             sma2=childBodyInfo.sma;
%             ecc2=childBodyInfo.ecc;
%             inc2=deg2rad(childBodyInfo.inc);
%             raan2=deg2rad(childBodyInfo.raan);
%             arg2=deg2rad(childBodyInfo.arg);
%
%             [TF] = intersectCheck(sma1, ecc1, inc1, raan1, arg1,...
%                                   sma2, ecc2, inc2, raan2, arg2,...
%                                   gmu, soiRadius);
%
%             if(TF == false)
%                 continue;
%             end
%         end
%
%         soiSf = 0;
%
%         distScChildBody = norm(getAbsPositBetweenSpacecraftAndBody(ut, rVect, scBodyInfo, childBodyInfo, celBodyData));
%         if(distScChildBody < (soiRadius-2*soiSf)) %The spacecraft starts out inside the SoI for some reason!
%                 [rVectDown, vVectDown] = convertRVVectOnDownwardsSoITransition(childBodyInfo, celBodyData, ut, initialState(end,2:4), initialState(end,5:7));
%                 if(dot(rVectDown,vVectDown) < 0) %we're already headed down, so we can transition downwards.  But if we're headed up at the SoI boundary, don't transition!  Will cause SoI dithering.
%                     downSoITrans(end+1,:) = [-1, ut, bodyInfo.id, childBodyInfo.id, ...
%                                              initialState(end,2:7), ...
%                                              rVectDown', vVectDown']; %#ok<AGROW>
%                 end
%         end
%
%         n1 = computeMeanMotion(sma, gmu);
%         n2 = computeMeanMotion(childBodyInfo.sma, gmu);
%         maxSearchRadius = min(rApBody+soiRadius,rAp);
%         minSearchRadius = max(rPeBody-soiRadius,rPe);
%         if(ecc < 1)
%             sPeriod = computeSynodicPeriod(n1, n2);
%             oPeriod = computePeriod(sma,gmu);
%             if(sPeriod / oPeriod > 3)
%                 sPeriod = 3*computePeriod(sma,gmu);
%             end
%             numPeriods = ceil(sPeriod/oPeriod);
%             numPeriods = num_SoI_search_revs;
%
%             truMax1=real(computeTrueAFromRadiusEcc(maxSearchRadius, sma, ecc));
%             truMin1=real(computeTrueAFromRadiusEcc(minSearchRadius, sma, ecc));
%
%             truMax2 = 2*pi-truMin1;
%             truMin2 = 2*pi-truMax1;
%
%             %%%%%%%%%Set 1%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%             MA1 = computeMeanFromTrueAnom(truMin1, ecc);
%             MA2 = computeMeanFromTrueAnom(truMax1, ecc);
%
%             dt = (MA1 - meanINI)/n1;
%             if(dt < 0)
% %                 if(meanINI>=MA1 && meanINI<=MA2)
% %                     dt = (2*pi-meanINI+MA1)/n1;
% %                 else
% %                     dt = (2*pi-meanINI+MA1)/n1;
% %                 end
%                 dt = (2*pi-meanINI+MA1)/n1;
%             end
%
%             minT = ut+dt-100;
%             if(minT < ut)
%                 minT = ut;
%             end
%
%             dMA = MA2-MA1;
%             sPeriod = dMA/n1;
%             tArr = [];
%             for(i=1:numPeriods)
%                 t1 = minT + (i-1)*oPeriod;
%                 t2 = minT + (i-1)*oPeriod + sPeriod + 100;
%
%                 if(t1 <= maxSearchUT && t2 <= maxSearchUT)
%                     tArr(end+1) = t1;
%                     tArr(end+1) = t2;
%                 elseif(t1 <= maxSearchUT && t2 > maxSearchUT)
%                     tArr(end+1) = t1;
%                     tArr(end+1) = maxSearchUT; %we don't want to exceed maxSearchUT
%                 else
%                     continue; %both t1 and t2 are greater than maxSearchUT, don't add them
%                 end
%             end
%
%             if(truINI >= truMin1 && truINI <= truMax1)
%                 tArr(end+1) = ut;
%                 tArr(end+1) = minT;
%             end
%
%             %%%%%%%%%Set 2%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%             MA1 = computeMeanFromTrueAnom(truMin2, ecc);
%             MA2 = computeMeanFromTrueAnom(truMax2, ecc);
%
%             dt = (MA1 - meanINI)/n1;
%             if(dt < 0)
% %                 dt = 0;
%                 dt = (2*pi-meanINI+MA1)/n1;
%             end
%
%             minT = ut+dt-100;
%             if(minT < ut)
%                 minT = ut;
%             end
%
%             dMA = MA2-MA1;
%             sPeriod = dMA/n1;
%             for(i=1:numPeriods)
%                 t1 = minT + (i-1)*oPeriod;
%                 t2 = minT + (i-1)*oPeriod + sPeriod + 100;
%
%                 if(t1 <= maxSearchUT && t2 <= maxSearchUT)
%                     tArr(end+1) = t1;
%                     tArr(end+1) = t2;
%                 elseif(t1 <= maxSearchUT && t2 > maxSearchUT)
%                     tArr(end+1) = t1;
%                     tArr(end+1) = maxSearchUT; %we don't want to exceed maxSearchUT
%                 else
%                     continue; %both t1 and t2 are greater than maxSearchUT, don't add them
%                 end
%             end
%
%             if(truINI >= truMin2 && truINI <= truMax2)
%                 tArr(end+1) = ut;
%                 tArr(end+1) = minT;
%             end
%
%         else
%             iniHyTruMax1 = AngleZero2Pi(real(computeTrueAFromRadiusEcc(maxSearchRadius, sma, ecc)));
%             iniHyTruMin1 = AngleZero2Pi(real(computeTrueAFromRadiusEcc(minSearchRadius, sma, ecc)));
%
%             iniHyTruMax2 = -iniHyTruMin1;
%             iniHyTruMin2 = -iniHyTruMax1;
%
%             %%%%%%%%%Set 1%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%             MA1 = computeMeanFromTrueAnom(iniHyTruMin1, ecc);
%             MA2 = computeMeanFromTrueAnom(iniHyTruMax1, ecc);
%
%             dt = (MA1 - meanINI)/n1;
%             if(dt < 0)
%                 dt = 0;
%             end
%
%             dMA = MA2-MA1;
%             sPeriod = dMA/n1;
%
%             minT = ut+dt-100;
%             if(minT < ut)
%                 minT = ut;
%             end
%             tArr = linspace(minT, ut+dt+sPeriod+100, 2);
%
%             %%%%%%%%%Set 2%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%             MA1 = computeMeanFromTrueAnom(iniHyTruMin2, ecc);
%             MA2 = computeMeanFromTrueAnom(iniHyTruMax2, ecc);
%
%             dt = (MA1 - meanINI)/n1;
%             if(dt < 0)
%                 dt = 0;
%             end
%
%             dMA = MA2-MA1;
%             sPeriod = dMA/n1;
%
%             minT = ut+dt-100;
%             if(minT < ut)
%                 minT = ut;
%             end
%
%             tArrTmp = linspace(minT, ut+dt+sPeriod+100, 2);
%             tArr(end+1) = min(tArrTmp);
%             tArr(end+1) = max(tArrTmp);
%         end
%
%         %These two lines sort the SoI calculations so that they're in
%         %time-order, meaning that earlier SoI transitions will not be
%         %missed if there are multiple possible SoI transitions.
%         tArr = sortrows(reshape(tArr, [2,length(tArr)/2])', 1);
%         tArr = reshape(tArr',1,[]);
%
%         findChildSoITransFunc = @(ut) isSoICrossing(ut, scBodyInfo, childBodyInfo, bodyInfo, soiRadius-soiSf, true, celBodyData);
%         findChildSoITransFuncNoAbs = @(ut) isSoICrossing(ut, scBodyInfo, childBodyInfo, bodyInfo, soiRadius-soiSf, false, celBodyData);
%         findChildSoITransFuncRealSoI = @(ut) isSoICrossing(ut, scBodyInfo, childBodyInfo, bodyInfo, soiRadius-10/1000, false, celBodyData);
%
%         options = optimset('TolX',soi_search_tol);
%         tolX2 = soi_search_tol;
%         for(i=1:length(tArr)/2) %#ok<*NO4LP>
%             tInds = [2*(i-1)+1, 2*i];
%
%             if(tArr(tInds(1)) >= tArr(tInds(2)))
%                 continue;
%             end
%
%             %%%%%%%%%
%             %This block of code is designed to weed out extraneous calls to
%             %fminbnd by avoiding looking for SoI transitions where the
%             %child body in question is clearly off on the other side of the
%             %orbit.
%             %%%%%%%%%
%             [rVectSc1] = getStateAtTime(scBodyInfo, tArr(tInds(1)), gmu);
%             [rVectSc2] = getStateAtTime(scBodyInfo, tArr(tInds(2)), gmu);
%
%             [rVectC1] = getStateAtTime(childBodyInfo, tArr(tInds(1)), gmu);
%             [rVectC2] = getStateAtTime(childBodyInfo, tArr(tInds(2)), gmu);
%
%             distT1 = norm(rVectSc1-rVectC1)/childBodyInfo.sma;
%             distT2 = norm(rVectSc2-rVectC2)/childBodyInfo.sma;
%
%             if(strict_SoI_search)
%                 smaFrac = 0.5;
%             else
%                 smaFrac = 60.0; %used to be 0.9
%             end
%
%             if(min(distT1, distT2) > smaFrac)
%                 if(strict_SoI_search)
%                     fprintf('While propagating Mission Architect orbit, strict SoI search setting skipped over a possible SoI transition between UT = %f sec and UT = %f sec.\n', tArr(tInds(1)), tArr(tInds(2)))
%                 end
%                 continue;
%             end
%
%             try
%                 minUT = max(tArr(tInds(1)), initialState(1));
%                 maxUT = tArr(tInds(2));
%
%                 if(minUT > maxUT)
%                     continue;
%                 end
%
%                 if(num_soi_search_attempts_per_rev > 1)
%                     startPts = linspace(minUT,maxUT,num_soi_search_attempts_per_rev+1);
%                 else
%                     startPts = [minUT,maxUT];
%                 end
%                 crossingUTs = NaN(length(startPts)-1,1);
%                 minDistFromSOI2s = crossingUTs;
%                 for(j=1:length(startPts)-1)
%                     minUT_Opt = startPts(j);
%                     maxUT_Opt = startPts(j+1);
%
%                     [crossingUTs(j), ~,~,~] = fminbnd(findChildSoITransFunc,minUT_Opt,maxUT_Opt, options);
%                     minDistFromSOI2s(j) = findChildSoITransFuncRealSoI(crossingUTs(j));
%
%                     if(minDistFromSOI2s(j) < 1)
%                         break; %we found SoI transition, no need to keep going
%                     end
%                 end
%
%                 [minDistFromSOI2, minI] = min(minDistFromSOI2s);
%                 crossingUT = crossingUTs(minI);
%
%                 if(minDistFromSOI2 > 100)
%                     continue;
%                 end
%             catch ME %#ok<*NASGU>
%                 continue;
%             end
%
%             if(abs(minDistFromSOI2) > 0)
%                 minUT = crossingUT-1200;
%                 maxUT = crossingUT+1200;
%                 unrefinedCrossingUt = crossingUT;
%
%                 [crossingUT,minDistFromSOI2, exitFlag] = bisection(findChildSoITransFuncNoAbs, minUT, maxUT, 0.0, tolX2);
%                 if(minDistFromSOI2 > 0.01)
%                     continue;
%                 end
%
%                 if(minDistFromSOI2 < 0.01 && isnan(crossingUT))
%                     options = optimset('Display','off', 'TolX',tolX2);
%                     [crossingUT,minDistFromSOI2, exitFlag] = fzero(findChildSoITransFuncNoAbs, unrefinedCrossingUt, options);
%
%                     if(minDistFromSOI2 > 0.01)
%                         continue;
%                     end
%                 end
%             end
%
%             if(isempty(crossingUT) || isnan(crossingUT) || ~isreal(crossingUT) || ~isfinite(crossingUT))
%                 continue;
%             end
%
%             if(abs(crossingUT-initialState(1)) <= 0.1)
%                 crossingUT = initialState(1);
%             elseif(abs(crossingUT-initialState(1)) > 0.1)
%                 if(crossingUT < initialState(1))
%                     continue;
%                 end
%             end
%
%             if(abs(maxSearchUT - crossingUT) <= 0.1)
%                 crossingUT = maxSearchUT;
%             elseif(abs(maxSearchUT - crossingUT) > 0.1)
%                 if(crossingUT > maxSearchUT)
%                     continue;
%                 end
%             end
%
%             tempEventLog = ma_executeCoast_goto_ut(crossingUT, initialState, -1, false, soiSkipIds, massLoss,  orbitDecay, celBodyData);
%
%             [rVectDown, vVectDown] = convertRVVectOnDownwardsSoITransition(childBodyInfo, celBodyData, crossingUT, tempEventLog(end,2:4), tempEventLog(end,5:7));
%             if(dot(rVectDown, vVectDown) > 0) %we found the outgoing part of the SoI transition
%                 crossingUTTemp = crossingUT;
%
%                 [smaTd, eccTd, ~, ~, ~, truTd] = getKeplerFromState(rVectDown, vVectDown, childBodyInfo.gm);
%                 [meanTd] = computeMeanFromTrueAnom(truTd, eccTd);
%                 [meanMotionTd] = computeMeanMotion(smaTd, childBodyInfo.gm);
% %                 [~,vVectTd] = getStatefromKepler(smaTd, eccTd, incTd, raanTd, argTd, 0.0, childBodyInfo.gm);
%
%                 maxUT = crossingUT - meanTd/meanMotionTd+100;
%                 minUT = crossingUT - 6*meanTd/meanMotionTd;
%                 minUT = max(minUT,initialState(1));
%
%                 if(maxUT < initialState(1))
%                     continue;
%                 end
%
%                 try
%                     [crossingUT, ~,~,~] = fminbnd(findChildSoITransFunc,minUT,maxUT, options);
%                     minDistFromSOI2 = findChildSoITransFuncRealSoI(crossingUT);
%                     if(minDistFromSOI2 > 100)
%                         continue;
%                     end
%                 catch ME %#ok<*NASGU>
%                     continue;
%                 end
%
%                 if(minDistFromSOI2 > 0)
%                     minUT = crossingUT-1200;
%                     minUT = max(minUT,initialState(1));
%                     maxUT = crossingUT+1200;
%                     [crossingUT,minDistFromSOI2] = bisection(findChildSoITransFuncRealSoI, minUT, maxUT, 0.0, tolX2);
%                     if(minDistFromSOI2 > 0.01)
%                         continue;
%                     end
%                 end
%
%                 if(isempty(crossingUT) || isnan(crossingUT) || ~isreal(crossingUT) || ~isfinite(crossingUT))
%                     continue;
%                 end
%
%                 if(abs(crossingUT-initialState(1)) <= 1)
%                     crossingUT = initialState(1);
%                 elseif(abs(crossingUT-initialState(1)) > 1)
%                     if(crossingUT < initialState(1))
%                         continue;
%                     end
%                 end
%
%                 if(abs(maxSearchUT - crossingUT) <= 1)
%                     crossingUT = maxSearchUT;
%                 elseif(abs(maxSearchUT - crossingUT) > 1)
%                     if(crossingUT > maxSearchUT)
%                         continue;
%                     end
%                 end
%
%                 tempEventLog = ma_executeCoast_goto_ut(crossingUT, initialState, -1, false, soiSkipIds, massLoss, orbitDecay, celBodyData);
%                 [rVectDown, vVectDown] = convertRVVectOnDownwardsSoITransition(childBodyInfo, celBodyData, crossingUT, tempEventLog(end,2:4), tempEventLog(end,5:7));
%             end
% %             if(abs(norm(rVectDown) - soiRadius) >= 0.01)
% %                 continue; %something wierd happened, go around
% %             end
%             if(crossingUT <= maxSearchUT)
%                 downSoITrans(end+1,:) = [-1, crossingUT, bodyInfo.id, childBodyInfo.id, ...
%                                          tempEventLog(end,2:7), ...
%                                          rVectDown', vVectDown']; %#ok<AGROW>
%             end
%             break;
%         end
%     end
%
%     soITrans = [upSoITrans;downSoITrans];
% end
%


function [g1, g2] = disimXrbfKernGradient(disimKern, rbfKern, t1, t2, covGrad)

% DISIMXRBFKERNGRADIENT Compute gradient between the DISIM and RBF kernels.
% FORMAT
% DESC computes the gradient of an objective function with respect
% to cross kernel terms between DISIM and RBF kernels for
% the multiple output kernel.
% ARG disimKern : the kernel structure associated with the DISIM
% kernel.
% ARG rbfKern : the kernel structure associated with the RBF
% kernel.
% ARG t : inputs for which kernel is to be computed.
% RETURN g1 : gradient of objective function with respect to kernel
% parameters of DISIM kernel.
% RETURN g2 : gradient of objective function with respect to kernel
% parameters of RBF kernel.
%
% FORMAT
% DESC computes the gradient of an objective function with respect
% to cross kernel terms between DISIM and RBF kernels for
% the multiple output kernel.
% ARG disimKern : the kernel structure associated with the DISIM
% kernel.
% ARG rbfKern : the kernel structure associated with the RBF
% kernel.
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% RETURN g1 : gradient of objective function with respect to kernel
% parameters of DISIM kernel.
% RETURN g2 : gradient of objective function with respect to kernel
% parameters of RBF kernel.
%
% SEEALSO : multiKernParamInit, multiKernCompute, disimKernParamInit, rbfKernParamInit
%
% COPYRIGHT : Neil D. Lawrence, 2006
%
% COPYRIGHT : Antti Honkela, 2007-2009

% KERN

arg{1} = t1;
if nargin < 5
  covGrad = t2;
  t2 = t1;
else
  arg{2} = t2;
end
if size(t1, 2) > 1 | size(t2, 2) > 1
  error('Input can only have one column');
end
if disimKern.inverseWidth ~= rbfKern.inverseWidth
  error('Kernels cannot be cross combined if they have different inverse widths.')
end
if disimKern.rbf_variance ~= rbfKern.variance
  error('Kernels cannot be cross combined if they have different RBF variances.');
end

dim1 = size(t1, 1);
dim2 = size(t2, 1);
t1Mat = t1(:, ones(1, dim2));
t2Mat = t2(:, ones(1, dim1))';
diffT = (t1Mat - t2Mat);
l = sqrt(2/disimKern.inverseWidth);
l2 = l*l;
C_0 = sqrt(disimKern.di_variance);
C_i = sqrt(disimKern.variance);
C_j = rbfKern.variance;
D_i = disimKern.decay;
delta = disimKern.di_decay;

invLDiffT = 1/l*diffT;
halfLD_i = 0.5*l*D_i;
halfLDelta = 0.5*l*delta;

prefact = C_0 * C_i * C_j * sqrt(pi)/2 * l;

lnCommon = - log(delta - D_i);
[lnPart1, sign1] = lnDiffErfs(halfLDelta - invLDiffT, halfLDelta + t2Mat/l);
[lnPart2, sign2] = lnDiffErfs(halfLD_i + t2Mat/l, halfLD_i - invLDiffT);

lnFact1 = halfLDelta^2 - delta * diffT;
lnFact2 = halfLD_i^2 - D_i * diffT;

k = sign1 .* exp(lnCommon + lnFact1 + lnPart1) ...
    + sign2 .* exp(lnCommon + lnFact2 + lnPart2);

k = 0.5*sqrt(disimKern.variance)*sqrt(disimKern.di_variance)*rbfKern.variance*k*sqrt(pi)*l;

[dlnPart1, m1] = gradLnDiffErfs(halfLDelta - invLDiffT, halfLDelta + t2Mat/l, ...
				l/2, l/2);
[dlnPart2, m2] = gradLnDiffErfs(halfLD_i + t2Mat/l, halfLD_i - invLDiffT, ...
				l/2, l/2);

dK_dD = k .* (1./(delta-D_i)) ...
	+ prefact ...
	.* ((l*halfLD_i - diffT) .* sign2 .* exp(lnCommon + lnFact2 + lnPart2) ...
	    + dlnPart2 .* exp(lnCommon + lnFact2 - m2));
dk_dD = sum(sum(dK_dD.*covGrad));

dK_ddelta = k .* (-1./(delta-D_i)) ...
    + prefact ...
    .* ((l*halfLDelta - diffT) .* sign1 .* exp(lnCommon + lnFact1 + lnPart1) ...
	+ dlnPart1 .* exp(lnCommon + lnFact1 - m1));
dk_ddelta = sum(sum(dK_ddelta.*covGrad));

[dlnPart1, m1] = gradLnDiffErfs(halfLDelta - invLDiffT, halfLDelta + t2Mat/l, ...
				delta/2 + invLDiffT/l, delta/2 - t2Mat/l2);
[dlnPart2, m2] = gradLnDiffErfs(halfLD_i + t2Mat/l, halfLD_i - invLDiffT, ...
				D_i/2 - t2Mat/l2, D_i/2 + invLDiffT/l);

dK_dl = k/l ...
	+ prefact ...
	.* (delta*halfLDelta .* sign1 .* exp(lnCommon + lnFact1 + lnPart1) ...
	    + D_i*halfLD_i .* sign2 .* exp(lnCommon + lnFact2 + lnPart2) ...
	    + dlnPart1 .* exp(lnCommon + lnFact1 - m1) ...
	    + dlnPart2 .* exp(lnCommon + lnFact2 - m2));
dk_dl = sum(sum(dK_dl.*covGrad));

dk_dC_i = sum(sum(k.*covGrad))/C_i;
dk_dC_0 = sum(sum(k.*covGrad))/C_0;
dk_dRbfVariance = sum(sum(k.*covGrad))/rbfKern.variance;

dk_dinvWidth = -0.5*sqrt(2)/(disimKern.inverseWidth* ...
                             sqrt(disimKern.inverseWidth))*dk_dl;
dk_dDisimVariance = dk_dC_i*0.5/C_i;
dk_dDIVariance = dk_dC_0*0.5/C_0;

% only pass the gradient with respect to the inverse width to one
% of the gradient vectors ... otherwise it is counted twice.
g1 = real([dk_ddelta dk_dinvWidth dk_dDIVariance dk_dD dk_dDisimVariance 0]);
g2 = real([0 dk_dRbfVariance]);


function [results, success, raw] = opf_execute(om, mpopt)
%OPF_EXECUTE  Executes the OPF specified by an OPF model object.
%   [RESULTS, SUCCESS, RAW] = OPF_EXECUTE(OM, MPOPT)
%
%   RESULTS are returned with internal indexing, all equipment
%   in-service, etc.
%
%   See also OPF, OPF_SETUP.

%   MATPOWER
%   Copyright (c) 2009-2020, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

%% define named indices into data matrices
[PQ, PV, REF, NONE, BUS_I, BUS_TYPE, PD, QD, GS, BS, BUS_AREA, VM, ...
    VA, BASE_KV, ZONE, VMAX, VMIN, LAM_P, LAM_Q, MU_VMAX, MU_VMIN] = idx_bus;
[GEN_BUS, PG, QG, QMAX, QMIN, VG, MBASE, GEN_STATUS, PMAX, PMIN, ...
    MU_PMAX, MU_PMIN, MU_QMAX, MU_QMIN, PC1, PC2, QC1MIN, QC1MAX, ...
    QC2MIN, QC2MAX, RAMP_AGC, RAMP_10, RAMP_30, RAMP_Q, APF] = idx_gen;
[F_BUS, T_BUS, BR_R, BR_X, BR_B, RATE_A, RATE_B, RATE_C, ...
    TAP, SHIFT, BR_STATUS, PF, QF, PT, QT, MU_SF, MU_ST, ...
    ANGMIN, ANGMAX, MU_ANGMIN, MU_ANGMAX] = idx_brch;

%%-----  setup  -----
%% options
dc  = strcmp(upper(mpopt.model), 'DC');
alg = upper(mpopt.opf.ac.solver);
switch alg
    case {'PDIPM', 'TRALM', 'MINOPF', 'SDPOPF'}
        legacy_solver = 1;
    otherwise
        legacy_solver = 0;
end
sdp = strcmp(alg, 'SDPOPF');
vcart = ~dc && mpopt.opf.v_cartesian;

%% get indexing
[vv, ll, nne, nni] = om.get_idx();

if mpopt.verbose > 0
    v = mpver('all');
    fprintf('\nMATPOWER Version %s, %s', v.Version, v.Date);
end

if dc
    %%-----  run DC OPF solver  -----
    if mpopt.verbose > 0
        fprintf(' -- DC Optimal Power Flow\n');
    end
    [results, success, raw] = dcopf_solver(om, mpopt);
else
    %%-----  run AC OPF solver  -----
    if mpopt.verbose > 0
        fprintf(' -- AC Optimal Power Flow\n  AC OPF formulation: ');
        if sdp
            fprintf('SDP relaxation\n');
        else
            if vcart
                v = 'cartesian';
            else
                v = 'polar';
            end
            if mpopt.opf.current_balance
                v2 = 'current';
            else
                v2 = 'power';
            end
            fprintf('%s voltages, %s balance eqns\n', v, v2);
        end
    end

    %% ZIP loads?
    if legacy_solver && ( ...
            (~isempty(mpopt.exp.sys_wide_zip_loads.pw) && ...
              any(mpopt.exp.sys_wide_zip_loads.pw(2:3))) || ...
            (~isempty(mpopt.exp.sys_wide_zip_loads.qw) && ...
              any(mpopt.exp.sys_wide_zip_loads.qw(2:3))) )
        warning('opf_execute: ''%s'' solver does not support ZIP load model. Converting to constant power loads.', alg)
        mpopt = mpoption(mpopt, 'exp.sys_wide_zip_loads', ...
                          struct('pw', [], 'qw', []));
    end

    %% run specific AC OPF solver
    if legacy_solver
        switch alg
            case 'PDIPM'
                if mpopt.pdipm.step_control
                    if ~have_feature('scpdipmopf')
                        error('opf_execute: MPOPT.opf.ac.solver = ''%s'' requires SCPDIPMOPF (see http://www.pserc.cornell.edu/tspopf/)', alg);
                    end
                else
                    if ~have_feature('pdipmopf')
                        error('opf_execute: MPOPT.opf.ac.solver = ''%s'' requires PDIPMOPF (see http://www.pserc.cornell.edu/tspopf/)', alg);
                    end
                end
                [results, success, raw] = tspopf_solver(om, mpopt);
            case 'TRALM'
                if ~have_feature('tralmopf')
                    error('opf_execute: MPOPT.opf.ac.solver = ''%s'' requires TRALM (see http://www.pserc.cornell.edu/tspopf/)', alg);
                end
                [results, success, raw] = tspopf_solver(om, mpopt);
            case 'MINOPF'
                if ~have_feature('minopf')
                    error('opf_execute: MPOPT.opf.ac.solver = ''%s'' requires MINOPF (see http://www.pserc.cornell.edu/minopf/)', alg);
                end
                [results, success, raw] = mopf_solver(om, mpopt);
            case 'SDPOPF'
                if ~have_feature('yalmip')
                    error('opf_execute: MPOPT.opf.ac.solver = ''%s'' requires YALMIP (see https://yalmip.github.io)', alg);
                end
                [results, success, raw] = sdpopf_solver(om, mpopt);
            otherwise
                error('opf_execute: MPOPT.opf.ac.solver = ''%s'' is not a valid AC OPF solver selection', alg);
        end
    else    %% not legacy solver
        switch alg
            case 'IPOPT'
                if ~have_feature('ipopt')
                    error('opf_execute: MPOPT.opf.ac.solver = ''%s'' requires IPOPT (see https://github.com/coin-or/Ipopt)', alg);
                end
            case 'FMINCON'
                if ~have_feature('fmincon')
                    error('opf_execute: MPOPT.opf.ac.solver = ''%s'' requires FMINCON (Optimization Toolbox 2.x or later)', alg);
                end
            case 'KNITRO'
                if ~have_feature('knitro')
                    error('opf_execute: MPOPT.opf.ac.solver = ''%s'' requires Artelys Knitro (see https://www.artelys.com/solvers/knitro/)', alg);
                end
        end
        [results, success, raw] = nlpopf_solver(om, mpopt);
    end     %% if legacy_solver
end %% if dc
if ~isfield(raw, 'output') || ~isfield(raw.output, 'alg') || isempty(raw.output.alg)
    raw.output.alg = alg;
end

if success
  if ~dc && ~sdp
    %% copy bus voltages back to gen matrix
    results.gen(:, VG) = results.bus(results.gen(:, GEN_BUS), VM);

    %% cartesian voltage magnitude multipliers
    if vcart
        results.bus(:, MU_VMIN) = results.bus(:, MU_VMIN) .* results.bus(:, VM) * 2;
        results.bus(:, MU_VMAX) = results.bus(:, MU_VMAX) .* results.bus(:, VM) * 2;
    end

    %% gen PQ capability curve multipliers
    if ll.N.PQh > 0 || ll.N.PQl > 0
      mu_PQh = results.mu.lin.l(ll.i1.PQh:ll.iN.PQh) - results.mu.lin.u(ll.i1.PQh:ll.iN.PQh);
      mu_PQl = results.mu.lin.l(ll.i1.PQl:ll.iN.PQl) - results.mu.lin.u(ll.i1.PQl:ll.iN.PQl);
      Apqdata = om.get_userdata('Apqdata');
      results.gen = update_mupq(results.baseMVA, results.gen, mu_PQh, mu_PQl, Apqdata);
    end

    %% compute g, dg, f, df, d2f if requested by opf.return_raw_der = 1
    if mpopt.opf.return_raw_der
      %% move from results to raw if using v4.0 of MINOPF or TSPOPF
      if isfield(results, 'dg')
        raw.dg = results.dg;
        raw.g = results.g;
      end
      %% compute g, dg, unless already done by post-v4.0 MINOPF or TSPOPF
      if ~isfield(raw, 'dg')
        [g, geq, dg, dgeq] = nlp_consfcn(om, results.x);
        raw.g = [ geq; g];
        raw.dg = [ dgeq'; dg'];   %% true Jacobian organization
      end
      %% compute df, d2f
      [f, df, d2f] = nlp_costfcn(om, results.x);
      raw.df = df;
      raw.d2f = d2f;
    end
  end

  %% delete g and dg fields from results if using v4.0 of MINOPF or TSPOPF
  if isfield(results, 'dg')
    rmfield(results, 'dg');
    rmfield(results, 'g');
  end

  %% angle limit constraint multipliers
  iang = om.get_userdata('iang');
  if ~sdp && length(iang)
    if vcart
      iang = om.get_userdata('iang');
      results.branch(iang, MU_ANGMIN) = results.mu.nli(nni.i1.angL:nni.iN.angL) * pi/180;
      results.branch(iang, MU_ANGMAX) = results.mu.nli(nni.i1.angU:nni.iN.angU) * pi/180;
    else
      iang = om.get_userdata('iang');
      results.branch(iang, MU_ANGMIN) = results.mu.lin.l(ll.i1.ang:ll.iN.ang) * pi/180;
      results.branch(iang, MU_ANGMAX) = results.mu.lin.u(ll.i1.ang:ll.iN.ang) * pi/180;
    end
  end
else
  %% assign empty g, dg, f, df, d2f if requested by opf.return_raw_der = 1
  if ~dc && mpopt.opf.return_raw_der
    raw.dg = [];
    raw.g = [];
    raw.df = [];
    raw.d2f = [];
  end
end

if ~sdp
  %% assign values and limit shadow prices for variables
  om_var_order = om.get('var', 'order');
  for k = 1:length(om_var_order)
    name = om_var_order(k).name;
    if om.getN('var', name)
      idx = vv.i1.(name):vv.iN.(name);
      results.var.val.(name) = results.x(idx);
      results.var.mu.l.(name) = results.mu.var.l(idx);
      results.var.mu.u.(name) = results.mu.var.u(idx);
    end
  end

  %% assign shadow prices for linear constraints
  om_lin_order = om.get('lin', 'order');
  for k = 1:length(om_lin_order)
    name = om_lin_order(k).name;
    if om.getN('lin', name)
      idx = ll.i1.(name):ll.iN.(name);
      results.lin.mu.l.(name) = results.mu.lin.l(idx);
      results.lin.mu.u.(name) = results.mu.lin.u(idx);
    end
  end

  %% assign shadow prices for nonlinear constraints
  if ~dc
    om_nle_order = om.get('nle', 'order');
    for k = 1:length(om_nle_order)
      name = om_nle_order(k).name;
      if om.getN('nle', name)
        results.nle.lambda.(name) = results.mu.nle(nne.i1.(name):nne.iN.(name));
      end
    end

    om_nli_order = om.get('nli', 'order');
    for k = 1:length(om_nli_order)
      name = om_nli_order(k).name;
      if om.getN('nli', name)
        results.nli.mu.(name) = results.mu.nli(nni.i1.(name):nni.iN.(name));
      end
    end
  end

  %% assign values for components of quadratic cost
  om_qdc_order = om.get('qdc', 'order');
  for k = 1:length(om_qdc_order)
    name = om_qdc_order(k).name;
    if om.getN('qdc', name)
      results.qdc.(name) = om.eval_quad_cost(results.x, name);
    end
  end

  %% assign values for components of general nonlinear cost
  om_nlc_order = om.get('nlc', 'order');
  for k = 1:length(om_nlc_order)
    name = om_nlc_order(k).name;
    if om.getN('nlc', name)
      results.nlc.(name) = om.eval_nln_cost(results.x, name);
    end
  end

  %% assign values for components of legacy user cost
  om_cost_order = om.get('cost', 'order');
  for k = 1:length(om_cost_order)
    name = om_cost_order(k).name;
    if om.getN('cost', name)
      results.cost.(name) = om.eval_legacy_cost(results.x, name);
    end
  end

  %% if single-block PWL costs were converted to POLY, insert dummy y into x
  %% Note: The "y" portion of x will be nonsense, but everything should at
  %%       least be in the expected locations.
  pwl1 = om.get_userdata('pwl1');
  if ~isempty(pwl1) && ~strcmp(alg, 'TRALM') && ~(strcmp(alg, 'PDIPM') && mpopt.pdipm.step_control)
    %% get indexing
    vv = om.get_idx();
    if dc
      nx = vv.iN.Pg;
    else
      nx = vv.iN.Qg;
    end
    y = zeros(length(pwl1), 1);
    raw.xr = [ raw.xr(1:nx); y; raw.xr(nx+1:end)];
    results.x = [ results.x(1:nx); y; results.x(nx+1:end)];
  end
end


function [g1, g2] = simwhiteXrbfwhiteKernGradient(simKern, rbfKern, t1, varargin)

% SIMWHITEXRBFWHITEKERNGRADIENT Compute a cross gradient between a SIM-WHITE
% and an RBF-WHITE kernels.
% FORMAT
% DESC computes cross gradient of parameters of a cross kernel between a
% SIM-WHITE and an RBF-WHITE kernels for the multiple output kernel.
% ARG simKern : the kernel structure associated with the SIM-WHITE kernel.
% ARG rbfKern : the kernel structure associated with the RBF-WHITE kernel.
% ARG t1 : inputs for which kernel is to be computed.
% ARG covGrad : gradient of the objective function with respect to
% the elements of the cross kernel matrix.
% RETURN g1 : gradient of the parameters of the SIM-WHITE kernel, for
% ordering see simwhiteKernExtractParam.
% RETURN g2 : gradient of the parameters of the RBF-WHITE kernel, for
% ordering see rbfwhiteKernExtractParam.
%
% FORMAT
% DESC computes cross kernel terms between a SIM-WHITE and an RBF-WHITE
% kernels for the multiple output kernel.
% ARG simKern : the kernel structure associated with the SIM-WHITE kernel.
% ARG rbfKern : the kernel structure associated with the RBF-WHITE kernel.
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% ARG covGrad : gradient of the objective function with respect to
% the elements of the cross kernel matrix.
% RETURN g1 : gradient of the parameters of the SIM-WHITE kernel, for
% ordering see simwhiteKernExtractParam.
% RETURN g2 : gradient of the parameters of the RBF-WHITE kernel, for
% ordering see rbfwhiteKernExtractParam.
%
% SEEALSO : multiKernParamInit, multiKernCompute, simwhiteKernParamInit,
% rbfwhiteKernParamInit, simwhiteKernExtractParam, rbfwhiteKernExtractParam
%
% COPYRIGHT : David Luengo, 2009

% KERN

if nargin < 5
    t2 = t1;
else
    t2 = varargin{1};
end
covGrad = varargin{end};

if size(t1, 2) > 1 | size(t2, 2) > 1
  error('Input can only have one column');
end
if simKern.variance ~= rbfKern.variance
  error('Kernels cannot be cross combined if they have different variances.')
end
if simKern.isStationary ~= rbfKern.isStationary
  error('Stationary and non-stationary kernels cannot be cross combined.')
end

g1 = zeros(1, simKern.nParams);
g2 = zeros(1, rbfKern.nParams);

T1 = repmat(t1, 1, size(t2, 1));
T2 = repmat(t2.', size(t1, 1), 1);
deltaT = T1 - T2;
indT = double(deltaT >= 0);

% Parameters required for further computations
isStationary = simKern.isStationary;
variance = simKern.variance;
decay = simKern.decay;
sensitivity = simKern.sensitivity;
invWidth = rbfKern.inverseWidth;

% Computing a normalised (i.e. variance = 1 and sensitivity = 1) kernel
simKern.variance = 1;
simKern.sensitivity = 1;
rbfKern.variance = 1;
K = simwhiteXrbfwhiteKernCompute(simKern, rbfKern, t1, t2);

% Gradient w.r.t. the decay (simKern)
g1(1) = variance * sensitivity * sum(sum( ((-deltaT+decay/invWidth) .* K  ...
    + 1/sqrt(2*pi*invWidth) ...
        * exp(-decay*deltaT + 0.5*(decay^2)/invWidth) ...
        .* (exp(-0.5*invWidth*((T2+decay/invWidth).^2)) ...
        - exp(-0.5*invWidth*((abs(deltaT).*(1-indT)+decay/invWidth).^2)))) ...
    .* covGrad));

% Gradient w.r.t. the inverse width (rbfKern)
g2(1) = variance * sensitivity * sum(sum( (-0.5*((decay/invWidth)^2) .* K  ...
    + 1/sqrt(8*pi*invWidth) ...
        * exp(-decay*deltaT + 0.5*(decay^2)/invWidth) ...
        .* ((T2-decay/invWidth).*exp(-0.5*invWidth*((T2+decay/invWidth).^2)) ...
        - (abs(deltaT).*(1-indT)-decay/invWidth) ...
            .* exp(-0.5*invWidth*((abs(deltaT).*(1-indT)+decay/invWidth).^2)))) ...
    .* covGrad));

% Gradient w.r.t. sigma_r^2
g1(2) = sensitivity * sum(sum(K .* covGrad));
g2(2) = 0; % Otherwise it is counted twice

% Gradient w.r.t. sensitivity (only simKern)
g1(3) = variance * sum(sum(K .* covGrad));


classdef m_22_vic_10p_3s < MARRMoT_model
% Class for hydrologic conceptual model:  VIC

% Copyright (C) 2019, 2021 Wouter J.M. Knoben, Luca Trotter
% This file is part of the Modular Assessment of Rainfall-Runoff Models
% Toolbox (MARRMoT).
% MARRMoT is a free software (GNU GPL v3) and distributed WITHOUT ANY
% WARRANTY. See <https://www.gnu.org/licenses/> for details.

% Model references
% Clark, M. P., Slater, A. G., Rupp, D. E., Woods, R. A., Vrugt, J. A.,
% Gupta, H. V., � Hay, L. E. (2008). Framework for Understanding Structural
% Errors (FUSE): A modular framework to diagnose differences between
% hydrological models. Water Resources Research, 44(12), W00B02.
% http://doi.org/10.1029/2007WR006735
%
% Liang, X., Lettenmaier, D. P., Wood, E. F., & Burges, S. J. (1994). A
% simple hydrologically based model of land surface water and energy fluxes
% for general circulation models. Journal of Geophysical Research, 99,
% 14415�14428.

    properties
        % model-specific attributes
        aux_theta     % auxiliary parameter set
    end
    methods

        % creator method
        function obj = m_22_vic_10p_3s()
            obj.numStores = 3;                                             % number of model stores
            obj.numFluxes = 11;                                            % number of model fluxes
            obj.numParams = 10;

            obj.JacobPattern  = [1,0,0;
                                 1,1,0;
                                 1,1,1];                                   % Jacobian matrix of model store ODEs

            obj.parRanges = [   0.1 , 5;        % ibar,     Mean interception capacity [mm]
                                0   , 1;        % idelta,   Seasonal interception change as fraction of mean [-]
                                1   , 365;      % ishift,   Maximum interception peak timing [-]
                                1   , 2000;     % stot,    Maximum soil moisture capacity [mm]
                                0.01, 0.99;     % fsm, Fraction of stot that constitutes maximum soil moisture smmax [-]
                                0   ,10;        % b,        Infiltration excess shape parameter [-]
                                0   , 1;        % k1,       Percolation time parameter [d-1]
                                0   ,10;        % c1,       Percolation non-linearity parameter [-]
                                0   ,1;         % k2,       Baseflow time parameter [d-1]
                                1   , 5];       % c2,       Baseflow non-linearity parameter

            obj.StoreNames = {"S1", "S2" "S3"};                             % Names for the stores
            obj.FluxNames  = {"ei",   "peff", "iex", "qie",  "inf", "et1",...
                              "qex1", "pc",   "et2", "qex2", "qb"};        % Names for the fluxes

            obj.FluxGroups.Ea = [1 6 9];                                   % Index or indices of fluxes to add to Actual ET
            obj.FluxGroups.Q  = [4 7 10 11];                               % Index or indices of fluxes to add to Streamflow

        end

        % INITialisation function
        function obj = init(obj)
            %Parameters
            theta = obj.theta;
            stot    = theta(4);     % Total available storage [mm]
            fsm     = theta(5);     % Fraction of stot that constitutes maximum soil mositure storage [-]

            % Auxiliary parameter
            smmax   = fsm*stot;     % Maximum soil moisture capacity [mm]
            gwmax   = (1-fsm)*stot; % Maximum groundwater storage [mm]
            tmax    = 365.25;       % Length of one growing cycle [d]
            obj.aux_theta = [smmax; gwmax; tmax];

        end

        % MODEL_FUN are the model governing equations in state-space formulation
        function [dS, fluxes] = model_fun(obj, S)
            % parameters
            theta   = obj.theta;
            ibar    = theta(1);     % Mean interception capacity [mm]
            idelta  = theta(2);     % Seasonal interception change as fraction of mean [-]
            ishift  = theta(3);     % Maximum interception peak timing [-]
            b       = theta(6);     % Infiltration excess shape parameter [-]
            k1      = theta(7);     % Percolation time parameter [d-1]
            c1      = theta(8);     % Percolation non-linearity parameter [-]
            k2      = theta(9);     % Baseflow time parameter [d-1]
            c2      = theta(10);    % Baseflow non-linearity parameter

            aux_theta = obj.aux_theta;
            smmax = aux_theta(1);
            gwmax = aux_theta(2);
            tmax = aux_theta(3);

            % delta_t
            delta_t = obj.delta_t;

            % stores
            S1 = S(1);
            S2 = S(2);
            S3 = S(3);

            % climate input
            t = obj.t;                             % this time step
            climate_in = obj.input_climate(t,:);   % climate at this step
            P  = climate_in(1);
            Ep = climate_in(2);
            T  = climate_in(3);

            % fluxes functions
            aux_imax  = phenology_2(ibar,idelta,ishift,obj.t,tmax,delta_t);
            flux_ei   = evap_7(S1,aux_imax,Ep,delta_t);
            flux_peff = interception_1(P,S1,aux_imax);
            flux_iex  = excess_1(S1,aux_imax,delta_t);
            flux_qie  = saturation_2(S2,smmax,b,flux_peff+flux_iex);
            flux_inf  = effective_1(flux_peff+flux_iex,flux_qie);
            flux_et1  = evap_7(S2,smmax,max(0,Ep-flux_ei),delta_t);
            flux_qex1 = saturation_1(flux_inf,S2,smmax);
            flux_pc   = percolation_5(k1,c1,S2,smmax,delta_t);
            flux_et2  = evap_7(S3,gwmax,max(0,Ep-flux_ei-flux_et1),delta_t);
            flux_qex2 = saturation_1(flux_pc,S3,gwmax);
            flux_qb   = baseflow_5(k2,c2,S3,gwmax,delta_t);

            % stores ODEs
            dS1 = P - flux_ei - flux_peff - flux_iex;
            dS2 = flux_inf - flux_et1 - flux_qex1 - flux_pc;
            dS3 = flux_pc  - flux_et2 - flux_qex2 - flux_qb;

            % outputs
            dS = [dS1 dS2 dS3];
            fluxes = [flux_ei  flux_peff flux_iex  flux_qie ...
                      flux_inf flux_et1  flux_qex1 flux_pc ...
                      flux_et2 flux_qex2 flux_qb];
        end

        % STEP runs at the end of every timestep
        function obj = step(obj)
        end
    end
end


function f = constructor(f, op, varargin)
%CONSTRUCTOR   CHEBFUN3 constructor.
%   Given a function OP of three variables, this code represents it as a
%   CHEBFUN3 object. A CHEBFUN3 object is a low-rank representation
%   expressing a function as a trilinear product of a discrete core tensor
%   and three quasimatrices consisting of univariate functions.
%
%
%   Since March 2023, chebfun3f is used by default to construct CHEBFUN3
%   objects. The legacy constructor chebfun3classic can be accessed by
%   providing the 'classic' flag as additional input argument.
%
% See also CHEBFUN2, CHEBFUN3T and CHEBFUN3V.

% Copyright 2023 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Parse the inputs:
[op, dom, pref, fiberDim, vectorize, isEqui, fixedRank] = ...
    parseInputs(op, varargin{:});

% The 'equi' flag can be used only with numeric data:
if ( isEqui && ~isa(op, 'double') )
    error('CHEBFUN:CHEBFUN3:constructor:equi', ...
        'The EQUI flag is valid only when constructing from numeric data');
end

if ( isa(op, 'chebfun3') )     % CHEBFUN3( CHEBFUN3 )
    f = op;
    return
elseif ( isa(op, 'double') )   % CHEBFUN3( DOUBLE )
    f = chebfun3.chebfun3double(f, op, dom, pref, isEqui);
elseif ( strcmpi(pref.cheb3Prefs.constructor, 'classic') )
    f = chebfun3.chebfun3classic(f, op, pref, dom, vectorize, fiberDim);
else
    f = chebfun3.chebfun3f(f, op, pref, dom, vectorize);
end

if ( fixedRank )
    % Simplify the rank if requested.
    f = fixTheRank(f , fixedRank);
end
end
%% End of constructor

%%
function [op, dom, pref, fiberDim, vectorize, isEqui, ...
    fixedRank] = parseInputs(op, varargin)
vectorize = 0;
isEqui = 0;
isCoeffs = 0;
fixedRank = 0;
pref = chebfunpref();
fiberDim = [];
dom = [-1 1 -1 1 -1 1];

% Preferences structure given?
isPref = find(cellfun(@(p) isa(p, 'chebfunpref'), varargin));
if ( any(isPref) )
    pref = varargin{isPref};
    varargin(isPref) = [];
end

if ( isa(op, 'char') )     % CHEBFUN3( CHAR )
    op = str2op(op);
end

for k = 1:length(varargin)
    if any(strcmpi(varargin{k}, {'trig', 'periodic'}))
        pref.tech = @trigtech;
    elseif strcmpi(varargin{k}, 'eps')
        pref.cheb3Prefs.chebfun3eps = varargin{k+1};
    elseif strcmpi(varargin{k}, 'classic')
        pref.cheb3Prefs.constructor = 'classic';
    elseif strcmpi(varargin{k}, 'chebfun3f')
        pref.cheb3Prefs.constructor = 'chebfun3f';
    elseif strcmpi(varargin{k}, 'rank') % rank is specified.
        fixedRank = varargin{k+1};
    elseif ( isnumeric(varargin{k}) )
        if ( numel(varargin{k}) == 6 ) % domain is specified.
            dom = varargin{k};
        elseif ( numel(varargin{k}) == 3 ) % length is specified.
            if ( k > 1 && strcmpi(varargin{k-1}, 'rank') )
                % Rank is specified, not length. Don't confuse them.
                continue
            else
                % Interpret this as the user wants a fixed degree chebfun3
                % on the domain DOM.
                len = varargin{k};
                [xx, yy, zz] = chebpts3(len(1), len(2), len(3), dom);
                op = op(xx, yy, zz);
            end
        end
    elseif strcmpi(varargin{k}, {'fiberDim'})
        fiberDim = varargin{k+1};
    elseif any(strcmpi(varargin{k}, {'vectorize', 'vectorise'}))
        vectorize = true;
    elseif strcmpi(varargin{k}, 'coeffs')
        isCoeffs = 1;
    elseif strcmpi(varargin{k}, 'equi')
        isEqui = 1;
    end
end

if ( isCoeffs )
    op = chebfun3.coeffs2vals(op);
end

% If the vectorize flag is off, do we need to give user a warning?
if ( ~vectorize && ~isnumeric(op) ) % another check
    [vectorize, op] = vectorCheck(op, dom);
end

end

%%
function [vectorize, op] = vectorCheck(op, dom)
% Check for cases like op = @(x,y,z) x*y^2*z

vectorize = false;
[xx, yy, zz] = ndgrid(dom(1:2), dom(3:4), dom(5:6));
try
    A = feval(op, xx, yy, zz);
catch
    throwVectorWarning();
    vectorize = true;
    return
end

A = feval(op, xx, yy, zz);
if ( any(isinf(A(:) ) ) )
    error('CHEBFUN:CHEBFUN3:constructor:inf', ...
        'Function returned INF when evaluated');
elseif ( any(isnan(A(:)) ) )
    error('CHEBFUN:CHEBFUN3:constructor:nan', ...
        'Function returned NaN when evaluated');
end

if ( isscalar(A) )
    op = @(x,y,z) op(x,y,z) + 0*x + 0*y + 0*z;
end

end

%%
function throwVectorWarning()
warning('CHEBFUN:CHEBFUN3:constructor:vectorize',...
    ['Function did not correctly evaluate on an array.\n', ...
    'Turning on the ''vectorize'' flag. Did you intend this?\n', ...
    'Use the ''vectorize'' flag in the CHEBFUN3 constructor\n', ...
    'call to avoid this warning message.']);
end
%%
function op = str2op(op)
% OP = STR2OP(OP), finds independent variables in a string and returns an
% op handle than can be evaluated.

vars = symvar(op);        % Independent variables
numVars = numel(vars);
if ( numVars == 0 )
    op = @(x,y,z) eval (op);

elseif ( numVars == 1 )
    op = eval(['@(' vars{1} ', myVarBeta, myVarGamma)' op]);

elseif ( numVars == 2 )
    op = eval(['@(' vars{1} ',' vars{2} ', myVarGamma)' op]);

elseif ( numVars == 3 )
    op = eval(['@(' vars{1} ',' vars{2} ',' vars{3} ')' op]);

else
    error('CHEBFUN:CHEBFUN3:constructor:str2op:depvars', ...
        'Too many independent variables in string input.');
end

end

%%
function f = fixTheRank(f , fixedRank)
% Fix the rank of a CHEBFUN3. Used for calls to constructor with specified
% rank.

if ( any(fixedRank) < 0 )
    error('CHEBFUN:CHEBFUN3:constructor:fixTheRank:negative', ...
        'Ranks should all be nonnegative.')
elseif ( all(fixedRank) )
    [r1, r2, r3] = rank(f);
    t1 = fixedRank(1);
    t2 = fixedRank(2);
    t3 = fixedRank(3);

    % What to do with cols?
    if ( r1 > t1 )
        % Truncate cols:
        f.cols = f.cols(:, 1:t1);
        f.core = f.core(1:t1, :, :);
        r1 = t1; % New size of core
    elseif ( r1 < t1 )
        % Pad cols with approprate number of zero cols:
        zCols = chebfun(0, f.cols.domain);
        for jj = r1 : t1 - 1
            f.cols = [f.cols zCols];
        end
        % Pad mode 1 of the core tensor with zeros:
        tempCore = zeros(t1, r2, r3);
        tempCore(1:r1, :, :) = f.core;
        f.core = tempCore;
        r1 = t1; % New size of core
    end

    % What to do with rows?
    if ( r2 > t2 )
        % Truncate rows:
        f.rows = f.rows(:,1:t2);
        f.core = f.core(:, 1:t2, :);
        r2 = t2; % New size of core
    elseif ( r2 < t2 )
        % Pad rows with approprate number of zero rows:
        zRows = chebfun(0, f.rows.domain);
        for jj = r2 : t2 - 1
            f.rows = [f.rows zRows];
        end
        % Pad mode 2 of the core tensor with zeros:
        tempCore = zeros(r1, t2, r3);
        tempCore(:, 1:r2, :) = f.core;
        f.core = tempCore;
        r2 = t2; % New size of core
    end

    % What to do with tubes?
    if ( r3 > t3 )
        % Truncate tubes:
        f.tubes = f.tubes(:, 1:t3);
        f.core = f.core(:, :, 1:t3);
    elseif ( r3 < t3 )
        % Pad tubes with approprate number of zero tubes:
        zTubes = chebfun(0, f.tubes.domain);
        for jj = r3 : t3 - 1
            f.tubes = [f.tubes zTubes];
        end
        % Pad mode 3 of the core tensor with zeros:
        tempCore = zeros(r1, r2, t3);
        tempCore(:, :, 1:r3) = f.core;
        f.core = tempCore;
    end
end

end


%% Tilt and Twist Boundaries
%
%%
% If a material deforms through the movement of dislocations, rearrangement
% of dislocations to a low-energy configuration may happen during
% deformation (i.e. in slow, geologic deformation) or or afterwards (in
% many metals). In any case, the arrangement of dislocation walls can lead
% to so-called subgrains boundaries. If such a boundary is composed of edge
% dislocations, it is called a tilt boundary and the rotation axis relating
% both parts of the grain at each side can be expected to be within the
% boundary plane (ideally parallel to the edge dislocation line). If the
% boundary is composed of screw dislocations, the rotation axis should be
% normal to the boundary. Between those end-members, there are general
% boundaries where the rotation axis is not easily related to the type of
% dislocations unless further information is available.
%
% In this chapter we discuss the computation of the misorientation axes at
% subgrain boundaries and discuss whether they vote for twist or tilt
% boundaries. We start by importing an sample EBSD data set and computing
% all subgrain boundaries as it is described in more detail in the chapter
% <subGrainBoundaries.html Subgrain Boundaries>.

% load some test data
mtexdata forsterite silent

% remove one pixel grains
[grains,ebsd.grainId] = calcGrains(ebsd('indexed'));
ebsd(grains(grains.grainSize<5)) = [];

% compute subgrain boundaries with 1.5 degree threshold angle
[grains,ebsd.grainId] = calcGrains(ebsd('indexed'),'threshold',[1*degree, 15*degree]);

% lets smooth the grain boundaries a bit
grains = smooth(grains,5);

% set up the ipf coloring
cKey = ipfColorKey(ebsd('fo').CS.properGroup);
cKey.inversePoleFigureDirection = xvector;
color = cKey.orientation2color(ebsd('fo').orientations);

% plot the forsterite phase
plot(ebsd('fo'),color,'faceAlpha',0.5,'figSize','large')

% init override mode
hold on

% plot grain boundares
plot(grains.boundary,'linewidth',2)

% compute transparency from misorientation angle
alpha = grains('fo').innerBoundary.misorientation.angle / (5*degree);

% plot the subgrain boundaries
plot(grains('fo').innerBoundary,'linewidth',1.5,'edgeAlpha',alpha,'linecolor','b');

% stop override mode
hold off

%%
% In the above plot we have marked all subgrain boundaries in blue and
% adjusted the transparency value according to the misorientation angle.
%
%% Misorientation Axes
%
% When analysing the misorientation axes of the subgrain boundary
% misorientations we need to distinguish whether we look at the
% misorientation axes in crystal coordinates or in specimen coordinates.
% Lets start with the misorientation axes in crystal coordinates which can
% directly be computed by the command <orientation.axis.html |axis|>.

% extract the Forsterite subgrain boundaries
subGB = grains('fo').innerBoundary;

% plot the misorientation axes in the fundamental sector
plot(subGB.misorientation.axis,'fundamentalRegion','figSize','small')

%%
% Obviously from the above plot it is not easy to judge about prefered
% misorientation axes. We get more insight if we <DensityEstimation.html
% compute the density distribution> of the misorientation axes and look for
% <S2FunOperations.html#4 local extrema>.

% compute the density distribution of misorientation axes
density = calcDensity(subGB.misorientation.axis,'halfwidth',3*degree);

% plot them
plot(density,'figSize','small')
mtexColorbar

% find the two prefered misorientation axes
[~,hkl] = max(density,'numLocal',2); round(hkl)

%%
% We find two preferred misorientation axes - (001) and (071). *TODO*: can
% this be interpreted?
%
%% The misorientation axis in specimen coordinates
%
% The computation of the misorientation axis in specimen coordinates is a
% little bit more complicated as it is impossible using only the
% misoriention. In fact we require the adjacent orientations on both sides
% of the subgrain boundaries. We can find those by making use of the
% |ebsdId| stored in the grain boundaries. The command

oriGB = ebsd('id',subGB.ebsdId).orientations

%%
% results in a $N \times 2$ matrix of orientations with rows corresponding
% to the boundary segments and two columns for both sides of the boundary.
% The misorientation axis in specimen coordinates is again computed by the
% command <orientation.axis.html |axis|>

axS = axis(oriGB(:,1),oriGB(:,2),'antipodal')

% plot the misorientation axes
plot(axS,'MarkerAlpha',0.2,'MarkerSize',2,'figSize','small')

%%
% We have used here the option |antipodal| as we have no fixed ordering of
% the grains at the two sides of the grain boundaries. For a more
% quantitative analysis we again compute the corresponding density
% distribution and find the preferred misorientation axes in specimen
% coordinates

density = calcDensity(axS,'halfwidth',5*degree);
plot(density,'figSize','small')
mtexColorbar

[~,pos] = max(density)
annotate(pos)

%% Tilt and Twist Boundaries
%
% Subgrain boundaries are often assumed to form during deformation by the
% accumulation of edge or screw dislocations. In the first extremal case of
% exclusive edge dislocations the misorientation axis is parallel to the
% deformation line and within the boundary plane. Such boundaries are
% called *tilt boundaries*. In the second extremal case of exclusive screw
% dislocations the misorientation axis is the screw axis and is parallel to
% the boundary normal. Such boundaries are called *twist boundaries*.
%
% In the case of 2d EBSD data one usually has not the full boundary
% information, but only the trace of the boundary with the measurement
% surface. Hence, it is impossible to distinguish tilt and twist
% boundaries. However, for twist boundaries misorientation axis must be normal
% to the boundary trace. This means, if the misorientation axis lays in the
% measurement plane and normal to the boundary trace, the boundary is quite
% likely to be a twist boundary. At the other hand, if the misorientation axis
% is parallel to the trace of a boundary, the boundary is quite likely to be a
% tilt boundary.
% We can be easily check the latter situation from our EBSD data, which allows us to
% exclude certain boundaries to be twist boundaries and to be most likely tilt
% boundaries To do so, we colorize in the following plot all subgrain boundaries
% according to the angle between the boundary trace and the misorientation axis.
% Blue subgrain boundaries are very likely tilt boundaries, while red subgrain
% boundaries are can be either tilt or twist boundaries.

plot(ebsd('fo'),color,'faceAlpha',0.5,'figSize','large')

% init override mode
hold on

% plot grain boundares
plot(grains.boundary,'linewidth',2)

% colorize the subgrain boundaries according the angle between boundary
% trace and misorientation axis
plot(subGB,angle(subGB.direction,axS)./degree,'linewidth',2)
mtexColorMap blue2red
mtexColorbar

hold off


%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled non-linear
% 	fluid-structure interaction models. This version of the code is based off of
%	Peskin's Immersed Boundary Method Paper in Acta Numerica, 2002.
%
% Author: Nicholas A. Battista
% Email:  nick.battista@unc.edu
% Date Created: May 27th, 2015
% Institution: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs)
% 	3. Target Points
%	4. Muscle-Model (combined Force-Length-Velocity model, "HIll+(Length-Tension)")
%
% One is able to update those Lagrangian Structure parameters, e.g., spring constants, resting %%	lengths, etc
%
% There are a number of built in Examples, mostly used for teaching purposes.
%
% If you would like us %to add a specific muscle model, please let Nick (nick.battista@unc.edu) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the CHANNEL_CHANNEL-EXAMPLE geometry and prints associated input files
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function Channel_Channel()

%
% Grid Parameters (MAKE SURE MATCHES IN input2d !!!)
%
Nx =  512;       % # of Eulerian Grid Pts. in x-Direction (MUST BE EVEN!!!)
Ny =  512;       % # of Eulerian Grid Pts. in y-Direction (MUST BE EVEN!!!)
Lx = 1.0;        % Length of Eulerian Grid in x-Direction
Ly = 1.0;        % Length of Eulerian Grid in y-Direction

% Immersed Structure Geometric / Dynamic Parameters %
ds= min(Lx/(2*Nx),Ly/(2*Ny));  % Lagrangian spacing
L = 0.9*Lx;                    % Length of Channel
w = 0.2*Ly;                    % Width of Channel
x0 = 0.3;                      % x-Center for Cylinder
y0 = 0.5;                      % y-Center for Cylinder
r = w/10;                      % Radii of Cylinder
struct_name = 'channel'; % Name for .vertex, .spring, etc files.

% Call function to construct geometry
[xLag,yLag] = give_Me_Channel_Immsersed_Boundary_Geometry(ds,L,w,Lx,Ly);
[xLag_C,yLag_C] = give_Me_Cylinder_Immsersed_Boundary_Geometry(ds,r,x0,y0);
[xLag_V,yLag_V] = give_Me_Piston_IB_Geometry(ds,w,Lx,Ly);

Npts = length(xLag_C);
NPistonStart = length(xLag);
fprintf('\n\n          POSSIBLE WARNING!!!      \n\n');
fprintf('# of pts in a cell: %d\n',Npts);
fprintf('If it is not EVEN...there will be a problem :)\n');
fprintf('Change radius, r, or ds slightly to try to make it even\n\n\n');

% Compute Curvatures
C =  compute_Curvatures(xLag_C,yLag_C);

% Plot Geometry to test
plot(xLag(1:end/2),yLag(1:end/2),'r-'); hold on;
plot(xLag(end/2+1:end),yLag(end/2+1:end),'r-'); hold on;
plot(xLag_C,yLag_C,'r-'); hold on;
plot(xLag_V,yLag_V,'k.-'); hold on;

plot(xLag,yLag,'*'); hold on;
plot(xLag_C,yLag_C,'g*'); hold on;
xlabel('x'); ylabel('y');
axis([0 Lx 0 Ly]);

% Create different Bubbles
xLag_C1 = xLag_C; yLag_C1 = yLag_C;              % Bubble 1
xLag_C2 = xLag_C+33.5*r; yLag_C2 = yLag_C+2*r;      % Bubble 2
xLag_C3 = xLag_C+1.5*r; yLag_C3 = yLag_C-2.5*r;  % Bubble 3

% Combine Geometry Files into ONE Vector
%xLag = [xLag_C1 xLag_C2 xLag_C3 xLag xLag_V];
%yLag = [yLag_C1 yLag_C2 yLag_C3 yLag yLag_V];
xLag = [xLag_C1 xLag_C2 xLag_C3 xLag_V];
yLag = [yLag_C1 yLag_C2 yLag_C3 yLag_V];
yLag = yLag - 0.375;

first_Index = 3*length(xLag_C1)+1;
Ncell_Lag_Pts = [length(xLag_C1) length(xLag_C2) length(xLag_C3)];

% Combine CELL Geometry files into ONE vector for springs/beams (not necessary...just easier to count)
xLag_Cells = [xLag_C1 xLag_C2 xLag_C3];
yLag_Cells = [yLag_C1 yLag_C2 yLag_C3];

plot(xLag_Cells,yLag_Cells,'k*'); hold on;

%
% Prints .vertex file!
%
print_Lagrangian_Vertices(xLag,yLag,struct_name);

%
% Prints .spring file!
%
k_Spring = 2.5e7;
NPistonStart = first_Index; % Gives starting index of piston instead of num of channel pts
NPiston = length(xLag_V);
print_Lagrangian_Springs(xLag_C,yLag_C,xLag,yLag,k_Spring,ds,r,struct_name,NPistonStart,NPiston);
plot(xLag(NPistonStart),yLag(NPistonStart),'r*'); hold on;
plot(xLag(NPistonStart+NPiston-1),yLag(NPistonStart+NPiston-1),'g*'); hold on

%
% Prints .beam file!
%
k_Beam = 1e12;
print_Lagrangian_Beams(xLag_C,yLag_C,k_Beam,C,struct_name);

%
% Prints .coagulation file!
%
starting_index = 1;         % index of first cellular Lagrangian Pt.
threshold_radius = 0.075;   % how close do cells need to be to form a bond
bond_strength = 1e3;        % bond strength
fracture_force = 1e2;       % how strong of force to break bond
print_Lagrangian_Coagulation_Inputs(struct_name,Ncell_Lag_Pts,starting_index,threshold_radius,fracture_force,bond_strength);

%
% Prints .target file!
%
k_Target = 1e7;
fprintf('For update_target_pts: last target pt index before piston: %d\n\n\n',0);
print_Lagrangian_Target_Pts(xLag,k_Target,struct_name,first_Index);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints VERTEX points to a file called struct.vertex
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Vertices(xLag,yLag,struct_name)

    N = length(xLag);

    vertex_fid = fopen([struct_name '.vertex'], 'w');

    fprintf(vertex_fid, '%d\n', N );

    %Loops over all Lagrangian Pts.
    for s = 1:N
        X_v = xLag(s);
        Y_v = yLag(s);
        fprintf(vertex_fid, '%1.16e %1.16e\n', X_v, Y_v);
    end

    fclose(vertex_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints COAGULATION Parameters to the file struct.vertex
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Coagulation_Inputs(struct_name,Ncell_Lag_Pts,starting_index,threshold_radius,fracture_force,bond_strength)

    % Ncell_Lag_Pts:    -gives # of Lag. Pts in each cell (# of Lag. Pts that compose each cell)
    % starting_index:   -gives first index of first cell Lag. Pt. in vertex file
    %                   -NOTE: IB2d Assumes all cell Lag. Pts are next to each other in .vertex file
    % threshold_radius: -how close do cells need to be to form a bond
    % fracture_force:   -how strong of force to break bond between cells

    Ncells = length(Ncell_Lag_Pts);   % gives total number of cells

    coag_fid = fopen([struct_name '.coagulation'], 'w');

    fprintf(coag_fid, '%d\n', starting_index );   % Prints first index of a Lag. Pt. associated to a Cell
    fprintf(coag_fid, '%d\n', threshold_radius ); % Prints threshold radii
    fprintf(coag_fid, '%d\n', bond_strength );    % Prints bond strength
    fprintf(coag_fid, '%d\n', fracture_force );   % Prints fracture_force

    %Loops over all CELLs.
    for s = 1:Ncells
        fprintf(coag_fid, '%1.16e\n', Ncell_Lag_Pts(s) );
    end

    fclose(coag_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints Target points to a file called struct.target
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Target_Pts(xLag,k_Target,struct_name,first_index)

    N = length(xLag) - first_index + 1;  % # of target points (e.g., the walls of channel)

    target_fid = fopen([struct_name '.target'], 'w');

    fprintf(target_fid, '%d\n', N );

    %Loops over all Lagrangian Pts.
    for s = 0:N-1                   % Loops over N target pts...just starts at zero bc of how first_index is defined
        fprintf(target_fid, '%d %1.16e\n', s+first_index, k_Target);
    end

    fclose(target_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints SPRING points to a file called struct.spring
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Springs(xLag,yLag,xLagAll,yLagAll,k_Spring,ds,r,struct_name,NPistonStart,NPiston)

    N = length(xLag);

    spring_fid = fopen([struct_name '.spring'], 'w');

    fprintf(spring_fid, '%d\n', 3*N + 3/2*N + (NPiston-1) ); %N MUST BE EVEN!

    %spring_force = kappa_spring*ds/(ds^2);

    %SPRINGS BETWEEN VERTICES
    for s = 1:3*N
            if s < N
                x1 = xLag(s);    y1 = yLag(s);
                x2 = xLag(s+1);  y2 = yLag(s+1);
                ds_Rest = sqrt( (x1-x2)^2 + (y1-y2)^2 );
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1, k_Spring, ds_Rest);
            elseif s==N
                %Case s=N
                x1 = xLag(s);    y1 = yLag(s);
                x2 = xLag(1);    y2 = yLag(1);
                ds_Rest = sqrt( (x1-x2)^2 + (y1-y2)^2 );
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, 1,   k_Spring, ds_Rest);
            elseif ( ( s>N ) && ( s<2*N ) )
                x1 = xLagAll(s);    y1 = yLagAll(s);
                x2 = xLagAll(s+1);  y2 = yLagAll(s+1);
                ds_Rest = sqrt( (x1-x2)^2 + (y1-y2)^2 );
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1, k_Spring, ds_Rest);
            elseif s==2*N
                x1 = xLagAll(s);    y1 = yLagAll(s);
                x2 = xLagAll(N+1);  y2 = yLagAll(N+1);
                ds_Rest = sqrt( (x1-x2)^2 + (y1-y2)^2 );
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, N+1,   k_Spring, ds_Rest);
            elseif ( ( s>2*N ) && ( s<3*N ) )
                x1 = xLagAll(s);    y1 = yLagAll(s);
                x2 = xLagAll(s+1);  y2 = yLagAll(s+1);
                ds_Rest = sqrt( (x1-x2)^2 + (y1-y2)^2 );
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1, k_Spring, ds_Rest);
            elseif s==3*N
                x1 = xLagAll(s);      y1 = yLagAll(s);
                x2 = xLagAll(2*N+1);  y2 = yLagAll(2*N+1);
                ds_Rest = sqrt( (x1-x2)^2 + (y1-y2)^2 );
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, 2*N+1,   k_Spring, ds_Rest);
            end
    end

    for s=1:N/2
        x1 = xLagAll(s);      y1 = yLagAll(s);
        x2 = xLagAll(s+N/2);  y2 = yLagAll(s+N/2);
        ds_Rest = sqrt( (x1-x2)^2 + (y1-y2)^2 );
        fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+N/2, k_Spring/5000, ds_Rest);
    end

    for s=1:N/2
        x1 = xLagAll(N+s);      y1 = yLagAll(N+s);
        x2 = xLagAll(N+s+N/2);  y2 = yLagAll(N+s+N/2);
        ds_Rest = sqrt( (x1-x2)^2 + (y1-y2)^2 );
        fprintf(spring_fid, '%d %d %1.16e %1.16e\n', N+s, N + s+N/2, k_Spring/5000, 2*r);
    end

    for s=1:N/2
        x1 = xLagAll(2*N+s);      y1 = yLagAll(2*N+s);
        x2 = xLagAll(2*N+s+N/2);  y2 = yLagAll(2*N+s+N/2);
        ds_Rest = sqrt( (x1-x2)^2 + (y1-y2)^2 );
        fprintf(spring_fid, '%d %d %1.16e %1.16e\n', 2*N+s, 2*N + s+N/2, k_Spring/5000, 2*r);
    end

    for s=NPistonStart:NPistonStart+NPiston-2
        fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1, k_Spring, ds);
    end
    fclose(spring_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints BEAM (Torsional Spring) points to a file called struct.beam
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Beams(xLag,yLag,k_Beam,C,struct_name)

    % k_Beam: beam stiffness
    % C: beam curvature

    N = length(xLag); % NOTE: Total number of beams = Number of Total Lag Pts. - 2

    beam_fid = fopen([struct_name '.beam'], 'w');

    fprintf(beam_fid, '%d\n', 3*N );

    %spring_force = kappa_spring*ds/(ds^2);

    %BEAMS BETWEEN VERTICES
    for s = 1:N
            if s==1
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',N, s, s+1,   k_Beam, C(s) );
            elseif  s <= N-1
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, s+1, k_Beam, C(s) );
            elseif s==N
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, 1,   k_Beam, C(s) );
            end
    end

    for s = N+1:2*N
            if s==N+1
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',2*N, s, s+1,   k_Beam, C(s-N) );
            elseif  s <= 2*N-1
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, s+1, k_Beam, C(s-N) );
            elseif s==2*N
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, N+1,   k_Beam, C(s-N) );
            end
    end

    for s = 2*N+1:3*N
            if s==2*N+1
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',3*N, s, s+1,   k_Beam, C(s-2*N) );
            elseif  s <= 3*N-1
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, s+1, k_Beam, C(s-2*N) );
            elseif s==3*N
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, 2*N+1,   k_Beam, C(s-2*N) );
            end
    end
    fclose(beam_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: computes "curvature" of ellipse
%
% NOTE: not curvature in the traditional geometric sense, in the 'discrete'
% sense through cross product.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function C = compute_Curvatures(xLag,yLag)

%a-x component (rmin)
%b-y component (rmax)
%C = ab / ( sqrt( a^2*sin(t)^2 + b^2*cos(t)^2  )  )^3

N = length(xLag);
C = zeros( N );

%Note: needs to be done same order as you print .beam file!
for i=1:N

   % Pts Xp -> Xq -> Xr (same as beam force calc.)

   if ( (i > 1) && (i < N) )

        Xp = xLag(i-1); Xq = xLag(i); Xr = xLag(i+1);
        Yp = yLag(i-1); Yq = yLag(i); Yr = yLag(i+1);

   elseif (i==1)

        Xp = xLag(N); Xq = xLag(i); Xr = xLag(i+1);
        Yp = yLag(N); Yq = yLag(i); Yr = yLag(i+1);

   elseif (i==N)

        Xp = xLag(N-1); Xq = xLag(N); Xr = xLag(1);
        Yp = yLag(N-1); Yq = yLag(N); Yr = yLag(1);

   end

   C(i) = (Xr-Xq)*(Yq-Yp) - (Yr-Yq)*(Xq-Xp); %Cross product btwn vectors

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the Lagrangian structure geometry for channel
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLag,yLag] = give_Me_Channel_Immsersed_Boundary_Geometry(ds,L,w,Lx,Ly)

% The immsersed structure is a channel %
x = (Lx-L)/2:ds:(L+(Lx-L)/2);  %xPts
yBot = (Ly-w)/2;               %yVal for bottom of Channel
yTop = Ly - (Ly-w)/2;          %yVal for top of Channel

xLag = [x x];
yLag = [yBot*ones(1,length(x)) yTop*ones(1,length(x))];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: gives Lag. Pts for Piston
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLag,yLag] = give_Me_Piston_IB_Geometry(ds,w,Lx,Ly)

yBot = (Ly-w)/2;               %yVal for bottom of Channel
yTop = Ly - (Ly-w)/2;          %yVal for top of Channel

yLag = Ly/2-0.05:ds:Ly/2+0.05;
xLag = 0.25*ones(1,length(yLag));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the Lagrangian structure geometry for cylinder
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLag,yLag] = give_Me_Cylinder_Immsersed_Boundary_Geometry(ds,r,x0,y0)

% The immsersed structure is a cylinder %

dtheta = ds/ (2.08*r);
theta = 0; i=1;
while theta < 2*pi
   xLag(i) = x0 - r*cos(theta);
   yLag(i) = y0 - r*sin(theta);
   theta = theta + dtheta;
   i=i+1;
end

%ds_Rest = sqrt( ( xLag(2) - xLag(3) )^2 + ( xLag(2) - xLag(3) )^2 );


% Copyright 2017 Lime Microsystems Ltd.
%
% Licensed under the Apache License, Version 2.0 (the "License");
% you may not use this file except in compliance with the License.
% You may obtain a copy of the License at
%
%    http://www.apache.org/licenses/LICENSE-2.0
%
% Unless required by applicable law or agreed to in writing, software
% distributed under the License is distributed on an "AS IS" BASIS,
% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
% See the License for the specific language governing permissions and
% limitations under the License.
%
% provide EVM for random sequences
% W-CDMA does not enforce equal amplitude signals, so we need AGC for each signal
% we assume we are dealing with BPSK or QAM, so rms amplitude should be unity
function evm=WCDMAULevm(x,qam)
  agc=1/sqrt(sum(x.*conj(x))/length(x));
  y=x*agc;
	evmref2=sum(abs(qam))/length(qam);
	[evm,loc]=min(abs(repmat(conj(qam'),1,length(y))-repmat(y,length(qam),1))); % decode QAM symbols
%	evm=evm./abs(qam(loc)); % some EVMs define relative to desired symbol
	evm/=evmref2; % some EVMs define relative to rms of QAM symbol table
end


function f = compose(f, op)
%COMPOSE   Compose command for SPHEREFUNV objects.
%   F = COMPOSE(F, G) returns the composition G(F) of the SPHEREFUNV object
%   F and a CHEBFUN3 or CHEBFUN3V with three components G.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Deal with empty CHEBFUN objects:
if ( isempty(f) || isempty(op) )
    if ( isa(op, 'chebfun3v') )
        f = spherefunv();
    else
        f = spherefun();
    end
end

% F is real and has three components, so we can compose with a CHEBFUN3 or
% CHEBFUN3V.
% TODO? compose when op is a spherefun object?

if ( isa(op, 'chebfun3') )
    % Get components of F:
    f1 = f.components{1};
    f2 = f.components{2};
    f3 = f.components{3};

    % Check that image(f) is in domain(op):
    rangef = minandmax2est(f);              % Estimate of image(f).
    tol = 100 * chebfun2eps * max(vscale(f), vscale(op)) * ...
            norm(f1.domain, inf);           % Tolerance.
    if ( ~isSubset(rangef, op.domain, tol) )
        error('CHEBFUN:SPHEREFUNV:COMPOSE:DomainMismatch3', ...
            'OP(F) is not defined, since image(F) is not contained in domain(OP).')
    end

    % Call constructor:
    f = spherefun(@(x,y,z) op(feval(f1, x, y, z), feval(f2, x, y, z), ...
        feval(f3, x, y, z)));

elseif ( isa(op, 'chebfun3v') )
    % Check that OP has three components:
    if ( op.nComponents ~= 3 )
        error('CHEBFUN:SPHEREFUNV:compose:nComponents', ...
            'CHEBFUN3V(SPHEREFUNV) is defined for CHEBFUN3V objects with 3 components.')
    end

    % Get components of op:
    op1 = op(1);
    op2 = op(2);
    op3 = op(3);

    % Get components of f:
    f1 = f.components{1};
    f2 = f.components{2};
    f3 = f.components{3};

    % Check that image(f) is in domain(op):
    rangef = minandmax2est(f);              % Estimate of image(f).
    tol = 100 * chebfun2eps * max(vscale(f), vscale(op)) * ...
            norm(f1.domain, inf);           % Tolerance.
    if ( ~isSubset(rangef, op1.domain, tol) )
        error('CHEBFUN:SPHEREFUNV:COMPOSE:DomainMismatch3v', ...
            'OP(F) is not defined, since image(F) is not contained in domain(OP).')
    end

    % Calling directly the constructor for [ op1(f), op2(f), op3(f) ] as below
    % seems to be slightly faster than building a spherefun for each component
    % and then calling the spherefunv constructor on the three spherefuns.

    % Call constructor:
    f = spherefunv(@(x,y,z) op1(feval(f1, x, y, z), feval(f2, x, y, z), ...
        feval(f3, x, y, z)), ...
        @(x,y,z) op2(feval(f1, x, y, z), feval(f2, x, y, z), ...
        feval(f3, x, y, z)), ...
        @(x,y,z) op3(feval(f1, x, y, z), feval(f2, x, y, z), ...
        feval(f3, x, y, z)));

else
    error('CHEBFUN:SPHEREFUNV:COMPOSE:OP', ...
        'Can compose a SPHEREFUNV with a CHEBFUN3 or CHEBFUN3V.')
end

end


function op = linop_handles( sz, Af, At, cmode )
%LINOP_HANDLES Linear operator from user-supplied function handles.
%OP = LINOP_HANDLES( SZ, AF, AT, CMODE )
%    Constructs a TFOCS-compatible linear operator from separate function
%    handles that compute the forward and adjoint operations. The first
%    argument, SZ, gives the size of the linear operator; and the forward
%    and adjoint handles are AF and AT, respectively.
%
%    If the inputs and outputs are simple vectors, then SZ can take the
%    standard Matlab form [M,N], where N is the input length and M is the
%    output length. If the input or output is a matrix or array, then SZ
%    should take the form { S_in, S_out }, where S_in is the size of the
%    input and S_out is the size of the output.
%
%    If the input or output space of the operator is complex, then the
%    CMODE string must be supplied, which describes the forward operation:
%       'R2R': real input and output
%       'R2C': real input, complex output
%       'C2R': complex input, real output
%       'C2C': complex input, complex output
%    If CMODE is not supplied, then 'R2R' is assumed. If the operator
%    detects a complex input or output when it is not expected, then an
%    error results. Therefore, you must make sure that your operators
%    return real values when they are expected to do so.

error(nargchk(3,4,nargin));
if numel( sz ) ~= 2,
    error( 'Size must have two elements.' );
elseif isnumeric( sz ),
    sz = { [sz(2),1], [sz(1),1] };
elseif ~isa( sz, 'cell' ),
    error( 'Invalid operator size specification.' );
end
if ~isa( Af, 'function_handle' ) || ~isa( At, 'function_handle' ),
    error( 'Second and third arguments must be function handles.' );
end
if nargin < 4 || isempty( cmode ),
    cmode = 'R2R';
elseif ~ischar( cmode ) || size( cmode, 1 ) ~= 1,
    error( 'Fourth argument must be a string.' );
else
    cmode = upper( cmode );
end

switch cmode,
    case 'R2R', op = @(x,mode)linop_handles_r2r( sz, Af, At, x, mode );
    case {'R2C','R2CC'}, op = @(x,mode)linop_handles_r2c( sz, Af, At, x, mode );
    case {'C2R','CC2R'}, op = @(x,mode)linop_handles_c2r( sz, Af, At, x, mode );
    case 'C2C', op = @(x,mode)linop_handles_c2c( sz, Af, At, x, mode );
    otherwise,
        error( 'Invalid complex mode: %s', cmode );
end

function y = linop_handles_r2r(sz, Af, At, x, mode )
switch mode,
    case 0, y = sz;
    case 1, y = realcheck( Af( realcheck( x ) ) );
    case 2, y = realcheck( At( realcheck( x ) ) );
end

function y = linop_handles_r2c(sz, Af, At, x, mode )
switch mode,
    case 0, y = sz;
    case 1, y = Af( realcheck( x ) );
    case 2, y = realcheck( At( x ) );
end

function y = linop_handles_c2r(sz, Af, At, x, mode )
switch mode,
    case 0, y = sz;
    case 1, y = realcheck( Af( x ) );
    case 2, y = At( realcheck( x ) );
end

function y = linop_handles_c2c(sz, Af, At, x, mode )
switch mode,
    case 0, y = sz;
    case 1, y = Af( x );
    case 2, y = At( x );
end

function y = realcheck( y )
if ~isreal( y ),
    error( 'Unexpected complex value in linear operation.' );
end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


function [nb] = find_triangle_neighbours(pnt, tri)

% FIND_TRIANGLE_NEIGHBOURS determines the three neighbours for each triangle
% in a mesh. It returns NaN's if the triangle does not have a neighbour on
% that particular side.
%
% [nb] = find_triangle_neighbours(pnt, tri)

% Copyright (C) 2003, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

npnt = size(pnt,1);
ntri = size(tri,1);

% each triangle has maximally three neighbours, assuming that the
% surface mesh is not degenerate
nb = nan(size(tri));

% for i=1:ntri
%   for j=setdiff(1:ntri, i)
%     if length(intersect(tri(i,[1 2]), tri(j,:)))==2
%       nb(i,1) = j;
%       continue;
%     end
%     if length(intersect(tri(i,[2 3]), tri(j,:)))==2
%       nb(i,2) = j;
%       continue;
%     end
%     if length(intersect(tri(i,[3 1]), tri(j,:)))==2
%       nb(i,3) = j;
%       continue;
%     end
%   end
%   if all(~isnan(nb(i,:)))
%     continue;
%   end
% end

for i=1:ntri
  % find all neighbouring triangles
  tmp1 = (tri==tri(i,1));
  tmp2 = (tri==tri(i,2));
  tmp3 = (tri==tri(i,3));
  tmp  = (tmp1|tmp2|tmp3);
  sel = find(sum(tmp,2)==2);

  % ensure that each neighbour is assigned to the proper edge
  if length(sel)>3
    ft_error('more than three neighbours found for triangle %d', i);
  else
    for j=1:length(sel)
      if isempty(setdiff(intersect(tri(i,:), tri(sel(j),:)), tri(i,[1 2])))
        nb(i,1) = sel(j);
      elseif isempty(setdiff(intersect(tri(i,:), tri(sel(j),:)), tri(i,[2 3])))
        nb(i,2) = sel(j);
      elseif isempty(setdiff(intersect(tri(i,:), tri(sel(j),:)), tri(i,[3 1])))
        nb(i,3) = sel(j);
      end
    end
  end
end