%% computeLoadPositionStatus Function
function [pumpPosition, pumpLoad, status, topPosArray, topLoadArray, ...
    count] = pocAlgorithm(polishedRodPosition, lastPolishedRodPosition, ...
    polishedRodLoad, count, dt, a, c, factorArray, rodLengths, lagIndex, ...
    rodYMs, area, lengthRequired, centerPoint, rodWeightFluidTotal, ...
    stuffingBoxFriction, force, topPosArray, topLoadArray)
    
    pumpPosition = -1.0;
    pumpLoad = -1.0;
    status = -1;
    loadMult = 1.0;
    tapersAllowed = 1;
    
    numTapers = length(rodLengths);

    for ii = 2:lengthRequired(1)
        topPosArray(1, ii-1) = topPosArray(1, ii);
        topLoadArray(1, ii-1) = topLoadArray(1, ii);
    end

    topPosArray(1, lengthRequired(1)) = -1.0 * polishedRodPosition / 12.0;

    if polishedRodPosition > lastPolishedRodPosition
        topLoadArray(1, lengthRequired(1)) = loadMult * ...
            (polishedRodLoad - rodWeightFluidTotal) - stuffingBoxFriction;
    elseif polishedRodPosition < lastPolishedRodPosition
        topLoadArray(1, lengthRequired(1)) = loadMult * ...
            (polishedRodLoad - rodWeightFluidTotal) + stuffingBoxFriction;
    else
        topLoadArray(1, lengthRequired(1)) = loadMult * ...
            (polishedRodLoad - rodWeightFluidTotal);
    end

    tap=1;
    while tap <= tapersAllowed
        count(tap) = count(tap) + 1;
        if count(tap) >= lengthRequired(tap)
            if tap+1 <= numTapers
                % working our way down to the bottom of the well
                for pti = 2:lengthRequired(tap+1)+1
                    topPosArray(tap+1, pti-1) = topPosArray(tap+1, pti);
                    topLoadArray(tap+1, pti-1) = topLoadArray(tap+1, pti);
                end
                [pumpPosition, pumpLoad] = positionLoadI(dt, a(tap), ...
                    c(tap), factorArray(tap), rodLengths(tap), ...
                    lagIndex(tap), rodYMs(tap), area(tap), ...
                    centerPoint(tap), topPosArray(tap,:), topLoadArray(tap,:));
                status = 0;
                topPosArray(tap+1, lengthRequired(tap+1)) = pumpPosition;
                topLoadArray(tap+1, lengthRequired(tap+1)) = pumpLoad;
            else
                [position, load] = positionLoadI(dt, a(tap), ...
                    c(tap), factorArray(tap), rodLengths(tap), ...
                    lagIndex(tap), rodYMs(tap), area(tap), ...
                    centerPoint(tap), topPosArray(tap,:), topLoadArray(tap,:));
                pumpPosition = -12.0 * position;
                pumpLoad = load + force(numTapers);
                status = 1;
            end
            count(tap) = count(tap) - 1;
            tapersAllowed = tapersAllowed + 1;
            if tapersAllowed > numTapers
                tapersAllowed = numTapers;
            end
        end
        tap = tap + 1;
    end
end


%% Position and Load Function
% This function calculates the position and load for a given taper
function [pumpPosition, pumpLoad] = positionLoadI(dt, ai, ci, factori, ...
    lengthi, lagi, yi, areai, centeri, topPosArrayI, topLoadArrayI)
    
    % FUNCTION PARAMETERS
    % dt [seconds] = amount of time between measurements
    % ai [?] = a value for the specific taper
    % ci [?] = damping factor for the taper
    % factori [?] = factor value for the taper
    % lengthi [feet] = length of the taper
    % lagi [?] = lag index of the taper
    % yi [?] = youngs modulus for the taper 
    %   (SHOULD BE 7.2 * 10^6 or 30.6 * 10^6)
    % areai [in^2] = annulus area of the taper
    % centeri [?] = centerpoint of the taper
    % topPosArrayI [] = array of position values
    % topLoadArrayI [] = array of load values
    

    iBefore = centeri - lagi;
    iAfter = centeri + lagi;

    %% Position Calculation
    pumpPosition = exp(ci * lengthi / (2.0 * ai)) * (topPosArrayI(iAfter) + ...
        factori * (topPosArrayI(iAfter+1) - topPosArrayI(iAfter)));

    pumpPosition = pumpPosition + exp(-ci * lengthi / (2.0 * ai)) * ...
        (topPosArrayI(iBefore) + factori * (topPosArrayI(iBefore-1) - ...
        topPosArrayI(iBefore)));

    pumpPosition = 0.5 * pumpPosition;
    insideIntegral = 0.0;

    for jj = 1:2*lagi-1
        insideIntegral = insideIntegral + dt / (yi * areai) * ...
            (exp((-ci * (lagi - jj) * dt) / 2.0) * ...
            topLoadArrayI(iBefore + jj));
    end

    insideIntegral = insideIntegral + 0.5 * dt /(yi * areai) * ...
        (exp((-ci * lagi * dt) / 2.0) * topLoadArrayI(iBefore) + ...
        exp((-ci * -lagi * dt) / 2.0) * topLoadArrayI(iAfter));

    loadBefore = exp((-ci * lagi * dt) / 2.0) * topLoadArrayI(iBefore) + ...
        factori * (exp((-ci * (lagi + 1) * dt) / 2.0) * topLoadArrayI(iBefore-1) - ...
        exp((-ci * lagi * dt) / 2.0) * topLoadArrayI(iBefore));

    loadAfter = exp((-ci * -lagi * dt) / 2.0) * topLoadArrayI(iAfter) + ...
        factori * (exp((-ci * (-lagi - 1) * dt) / 2.0) * topLoadArrayI(iAfter+1) - ...
        exp((-ci * -lagi * dt) / 2.0) * topLoadArrayI(iAfter));

    insideIntegral = insideIntegral + 0.5 * factori * dt / (yi * areai) * ...
        (loadBefore + exp((-ci * lagi * dt) / 2.0) * topLoadArrayI(iBefore));

    insideIntegral = insideIntegral + 0.5 * factori * dt / (yi * areai) * ...
        (loadAfter + exp((-ci * -lagi * dt) / 2.0) * topLoadArrayI(iAfter));

    insideIntegral = 0.5 * ai * insideIntegral;
    pumpPosition = pumpPosition + insideIntegral;

    insideIntegral = 0.0;

    for jj = 1:2*lagi-1
        insideIntegral = insideIntegral + dt * (exp((-ci * (lagi - jj) * dt) / 2.0) * ...
            topPosArrayI(iBefore+jj));
    end

    insideIntegral = insideIntegral + 0.5 * dt * (exp((-ci * lagi * dt) / 2.0) * ...
        topPosArrayI(iBefore) + exp((-ci * -lagi * dt) / 2.0) * ... 
        topPosArrayI(iAfter));

    loadBefore3 = exp((-ci * lagi * dt) / 2.0) * topPosArrayI(iBefore) + ...
        factori * (exp((-ci * (lagi+1) * dt) / 2.0) * topPosArrayI(iBefore-1) - ...
        exp((-ci * lagi * dt) / 2.0) * topPosArrayI(iBefore));

    loadAfter3 = exp((-ci * -lagi * dt) / 2.0) * topPosArrayI(iAfter) + ...
        factori * (exp((-ci * (-lagi-1) * dt) / 2.0) * topPosArrayI(iAfter+1) - ...
        exp((-ci * -lagi * dt) / 2.0) * topPosArrayI(iAfter));

    insideIntegral = insideIntegral + 0.5 * factori * dt * (loadBefore3 + ...
        exp((-ci * lagi * dt) / 2.0) * topPosArrayI(iBefore));

    insideIntegral = insideIntegral + 0.5 * factori * dt * (loadAfter3 + ...
        exp((-ci * -lagi * dt) / 2.0) * topPosArrayI(iAfter));

    insideIntegral = -(ci * lengthi / 4) * 0.5 * (ci / (2.0 * ai)) * insideIntegral;
    pumpPosition = pumpPosition + insideIntegral;
    
    %% Load Calculation
    pumpLoad = 0.5 * (ai / (yi * areai)) * (1 / ai) * (loadBefore + loadAfter);
    pumpLoad = pumpLoad - (ci * lengthi / 4) * (0.5 * (ci / (2.0 * ai))) * (1 / ai) * ...
        (loadBefore3 + loadAfter3);

    ptAfter = (topPosArrayI(iAfter+1) - topPosArrayI(iAfter-1)) / (2.0 * dt);
    ptBefore = (topPosArrayI(iBefore+1) - topPosArrayI(iBefore-1)) / (2.0 * dt);
    firstPart = (exp((ci * lengthi) / (2.0 * ai)) * ptAfter - exp((-ci * lengthi) / ...
        (2.0 * ai)) * ptBefore) / (2.0 * ai);

    firstPart = firstPart + (ci * exp((ci * lengthi) / (2.0 * ai)) * ... 
        topPosArrayI(iAfter) - ci * exp((-ci * lengthi) / (2.0 * ai)) * ...
        topPosArrayI(iBefore)) / (4 * ai);

    pumpLoad = yi * areai * (firstPart + pumpLoad);
end