%Matlab script written by David Oakley for use with the program
%InvertTrishear. For noncommercial use. If using in a publication, please
%acknowledge.

function [ pts_out ] = parallel_FPF_func( pts,v0,total_slip,PoverS,...
    bendxy,ramp_angle,gamma,gamma1,gamma_star,kappa,beta1,beta2,tip_start )
%parallel_FPF_func Parallel Fault Propagation Fold
%   Moves points according to the rules of parallel fault propagation
%   folding, assuming initially flat beds.

bend = bendxy(:,1); %Only need this to be a 1D array.
ef_constant = sin(ramp_angle(1))*(sin(gamma_star-beta1)/(sin(gamma_star)*sin(2.*gamma_star-beta1))); %To be used in calculating the distance ef or the velocity of point e.
if (v0 >= 0) %determine the sign of the slip
    slip_sign = 1;
else
    slip_sign = -1;
end
%Calculate the slip ratios. All expresed relative to slip on the lowest segment.
R0 = sin(gamma1+ramp_angle(2))/sin(gamma1+ramp_angle(1));
R1 = R0*sin(gamma1+ramp_angle(1))/sin(gamma1+gamma);
R2=R0*sin(beta2)/sin(beta2-ramp_angle(1)+gamma);
%Calculate the starting position of point e.
if (slip_sign == 1) %Forwards slip
    pte_start = tip_start; %Note: This would not work for normal faults, if they were allowed.
else %Backwards slip
    pte_start = tip_start-PoverS*R0*total_slip*ef_constant*[cos(kappa);sin(kappa)]; %minus because total_slip is negative.
end
%Calculate velocities for each domain
v1 = v0*[cos(ramp_angle(2));sin(ramp_angle(2))];
v2 = R0*v0*[cos(ramp_angle(1));sin(ramp_angle(1))];
v3 = R1*v0*[cos(gamma);sin(gamma)];
v4 = R2*v0*[cos(gamma);sin(gamma)];
v5 = [0;0];
%Calculate velocities for each boundary
vb = [0;0]; %Velocity of the bend: doesn't move.
ve = R0*v0*PoverS*[cos(ramp_angle(1))+ef_constant*cos(kappa);sin(ramp_angle(1))+ef_constant*sin(kappa)]; %Velocity of point e.
vt = R0*v0*PoverS*[cos(ramp_angle(1));sin(ramp_angle(1))]; %Velocity of the fault tip.
%Calculate slopes for each boundary
gam1_slope = -tan(gamma1); %Slope of axes with dip gamma, dipping to the right.
gam_slope = tan(gamma); %Slope of axes with dip gamma1, dipping to the left.
kap_slope = tan(kappa); %Slope of boundary with dip kappa.
for j = 1:size(pts,3)
    for k = 1:size(pts,2)
        tip = tip_start; %Where the tip starts moving from. (Generally the final tip position for restoring a fault-propagation fold.)
        pt = pts(:,k,j); %The point to be moved.
        pte = pte_start; %Location e in Figure 11.6  of Allmendinger et al. (2012) and Fig. 16 of Suppe and Medwedeff (1990)
        slip = 0; %slip so far
        rem_slip = total_slip; %Remaining slip
        loc = 0; %location, zone numbers from Figure 11.6, Allmendinger et al. (2012), 5 = footwall, 0 = unknown
        loc_passed = zeros(1,5); %Tells if a point has already passed through one of the 5 zones.
        while (rem_slip*slip_sign>0. && isnan(pt(1))==0)
            %disp(rem_slip)
            tip = tip + (slip/v0)*vt;
            pte = pte + (slip/v0)*ve;
            if (loc == 1) %Domain 1
                loc_passed(1) = 1;
                if (slip_sign == 1) %Forward motion
                    slip2 = calc_slip(pt,bend,v0,v1,vb,gam1_slope); %Slip needed to reach domain 2
                    if (loc_passed(2)==0 && rem_slip > slip2) %Goes into domain 2
                        slip = slip2;
                        loc = 2;
                    else
                        slip = rem_slip; %Stays in domain 1
                    end
                else %Backwards motion
                    slip = rem_slip; %Stays in domain 1
                end
                pt = pt+(slip/v0)*v1;
            elseif (loc == 2) %Domain 2
                loc_passed(2) = 1;
                if (slip_sign == 1) %Forward motion
                    slip3 = calc_slip(pt,pte,v0,v2,ve,gam1_slope); %Slip needed to reach domain 3
                    if (loc_passed(3)==0 && rem_slip > slip3 && pt(2)+(slip3/v0)*v2(2) > pte(2)+(slip3/v0)*ve(2) && slip3>0) %Goes into domain 3. I changed 0 to 1e-13 to address an infinite loop bug probably caused by rounding.
                        slip = slip3;
                        loc = 3;
                    else
                        slip4 = calc_slip(pt,tip,v0,v2,vt,kap_slope); %Slip needed to reach domain 4
                        if (loc_passed(4)==0 && rem_slip>slip4 && pt(2)+(slip4/v0)*v2(2) < pte(2)+(slip4/v0)*ve(2) && slip4>0) %Goes into domain 4
                            slip = slip4;
                            loc = 4;
                        else
                            slip = rem_slip; %Stays in domain 2
                        end
                    end
                else %Backward motion
                    slip1 = calc_slip(pt,bend,v0,v2,vb,gam1_slope);
                    slip3 = calc_slip(pt,pte,v0,v2,ve,gam1_slope);
                    slip4 = calc_slip(pt,tip,v0,v2,vt,kap_slope);
                    if (loc_passed(4)==0 && rem_slip < slip4 && slip4<0 && pt(2)+(slip4/v0)*v2(2) < pte(2)+(slip4/v0)*ve(2) && slip1<slip4) %Goes into domain 4.
                        slip = slip4;
                        loc = 4;
                    elseif (loc_passed(3)==0 && rem_slip < slip3 && slip3<0 && pt(2)+(slip3/v0)*v2(2) > pte(2)+(slip3/v0)*ve(2) ...
                            && slip1<slip3) %Goes into domain 3.
                        slip = slip3;
                        loc = 3;
                    elseif (loc_passed(1)==0 && rem_slip < slip1) %Goes into domain 1. < because slips are negative
                        slip = slip1;
                        loc = 1;
                    else
                        slip = rem_slip; %Stays in domain 2.
                    end
                end
                pt = pt+(slip/v0)*v2;
            elseif (loc == 3) %Domain 3
                loc_passed(3) = 1;
                if (slip_sign == 1) %Forward motion
                    slip2 = calc_slip(pt,pte,v0,v3,ve,gam1_slope); %Slip needed to reach domain 2
                    slip4 = calc_slip(pt,pte,v0,v3,ve,gam_slope); %Slip needed to reach domain 4.
                    if (loc_passed(2)==0 && rem_slip > slip2 && (slip4>slip2 || slip4<0 || loc_passed(4)==1) && slip2 > 0) %Goes into domain 2. I changed 0 to 1e-13 to address an infinite loop bug probably caused by rounding.
                        slip = slip2;
                        loc = 2;
                    elseif (loc_passed(4)==0 && rem_slip > slip4 && slip4>0) %Goes into domain 4
                        slip = slip4;
                        loc = 4;
                    else
                        slip = rem_slip; %Stays in domain 3
                    end
                    %Slip in domain 3 is parallel to the domain 3-4 boundary, so the point cannot enter domain 4 if slip (and thus propagation) is forward.
                else %Backward motion
                    slip2 = calc_slip(pt,pte,v0,v3,ve,gam1_slope); %Slip needed to reach domain 2.
                    slip4 = calc_slip(pt,pte,v0,v3,ve,gam_slope); %Slip needed to reach domain 4.
                    if (loc_passed(2)==0 && rem_slip < slip2 && (slip4<slip2 || slip4>0 || loc_passed(4)==1) && slip2<0) %Goes into domain 2. I'm pretty sure this can only happen if P/S <1, otherwise material flows the other way.
                        slip = slip2;
                        loc = 2;
                    elseif (loc_passed(4)==0 && rem_slip < slip4 && slip4<0) %Goes into domain 4. Pte must move along a line less steep than gamma.) %Goes into domain 4
                        slip = slip4;
                        loc = 4;
                    else
                        slip = rem_slip;
                    end
                end
                pt = pt+(slip/v0)*v3;
            elseif (loc == 4) %Domain 4
                loc_passed(4) = 1;
                if (slip_sign == 1) %Forward motion
                    slip3 = calc_slip(pt,pte,v0,v4,ve,gam_slope); %Slip needed to reach domain 3.
                    if (loc_passed(3)==0 && rem_slip>slip3 && pt(2)+(slip3/v0)*v4(2) > pte(2)+(slip3/v0)*ve(2) && slip3 > 0)
                        slip = slip3;
                        loc = 3;
                    else
                        slip2 = calc_slip(pt,tip,v0,v4,vt,kap_slope); %Slip needed to reach domain 2.
                        if (loc_passed(2)==0 && rem_slip > slip2 && pt(2)+(slip2/v0)*v4(2) < pte(2)+(slip2/v0)*ve(2) && slip2 > 0)
                            slip = slip2;
                            loc = 2;
                        else
                            slip = rem_slip;
                        end
                    end
                else %Backward motion
                    slip2 = calc_slip(pt,tip,v0,v4,vt,kap_slope); %Slip needed to reach domain 2.
                    slip3 = calc_slip(pt,pte,v0,v4,ve,gam_slope); %Slip needed to reach domain 3.
                    slip5 = calc_slip(pt,tip,v0,v4,vt,gam_slope); %Slip needed to reach domain 5 (footwall).
                    if (loc_passed(2)==0 && rem_slip < slip2 && (slip5<slip2 || slip5>0 || loc_passed(5)==1) ...
                            && pt(2)+(slip2/v0)*v4(2) < pte(2)+(slip2/v0)*ve(2) && slip2<0)
                        slip = slip2;
                        loc = 2;
                    elseif (loc_passed(3)==0 && rem_slip < slip3 &&(slip5<slip3 || slip5>0 || loc_passed(5)==1) ...
                            && pt(2)+(slip3/v0)*v4(2) > pte(2)+(slip3/v0)*ve(2) && slip3<0)
                        slip = slip3;
                        loc = 3;
                    elseif (loc_passed(5)==0 && rem_slip < slip5 && slip5<0)
                        slip = slip5;
                        loc = 5;
                    else
                        slip = rem_slip;
                    end
                end
                pt = pt+(slip/v0)*v4;
            elseif (loc == 5) %Footwall (domain 5)
                loc_passed(5) = 1;
                if (slip_sign == 1) %Forward motion
                    slip4 = calc_slip(pt,tip,v0,v5,vt,gam_slope); %Slip needed to reach domain 4.
                    if (loc_passed(4)==0 && rem_slip > slip4 && pt(2)+(slip4/v0)*v5(2) > tip(2)+(slip4/v0)*vt(2))
                        slip = slip4;
                        loc = 4;
                    else
                        slip = rem_slip;
                    end
                else
                    slip = rem_slip;
                end
            else %Need to find out where the point is.
                slip = 0; %So no more slip gets subtracted from rem_slip while we're just deciding.
                if (loc_passed(5)==0 && (pt(1)<bend(1) && pt(2)-bend(2)<(pt(1)-bend(1))*tan(ramp_angle(2))) || ...
                        (pt(1)>bend(1) && pt(1)<tip(1) && pt(2)-bend(2)<(pt(1)-bend(1))*tan(ramp_angle(1))) || ...
                        (pt(1)>tip(1) && pt(2)-tip(2)<(pt(1)-tip(1))*gam_slope)) %Footwall
                    loc = 5;
                elseif (loc_passed(1)==0 && pt(2)-bend(2)<(pt(1)-bend(1))*gam1_slope) %Domain 1
                    loc = 1;
                elseif (loc_passed(2)==0 && (pt(2)<pte(2) && pt(1)-tip(1)<(pt(2)-tip(2))/kap_slope) || ...
                        (pt(2)>pte(2) && pt(1)-pte(1)<(pt(2)-pte(2))/gam1_slope)) %Domain 2
                    loc = 2;
                elseif (loc_passed(3)==0 && pt(2)>pte(2) && pt(1)-pte(1)<(pt(2)-pte(2))/gam_slope) %Domain 3
                    loc = 3;
                elseif (loc_passed(4)==0 && pt(1)-tip(1)<(pt(2)-tip(2))/gam_slope) %Domain 4
                    loc = 4;
                else
                    disp('Error: point is not in any of the domains') %This shouldn't happen.
                end
            end
            %disp(slip)
            rem_slip = rem_slip - slip; %remaining slip
        end
        pts(:,k,j) = pt; %Store the moved point back in the pts array.
    end
end
pts_out = pts;

    function slip = calc_slip(pt,ptb,v0,v1,vb,slope)
        %This function calculates the slip necessary for a point moving in a constant velocity domain to reach a boundary.
        %Everything with dimension(2) is x coordinate or x component followed by y coordinate or y component.
        slip = ((pt(1)-ptb(1))*slope-pt(2)+ptb(2))/((1./v0)*(v1(2)-vb(2)-(v1(1)-vb(1))*slope));
    end

end