function[] = newton_wedge()
clc;
d=15; p=5; c=5;
[d1, d2, t1, t2] = gen_example2(d, p, c);
[ep, M] = find_offset(d1, d2, t1, t2);
end

function [d1, d2, t1, t2] = gen_example2(d, p, c)

%Harcoded theta values for M=2.0
t_2_05=34.301574994;
t_2_10=39.313931845;
t_2_15=45.343616762;
t_2_20=53.422940527;

if d+p==5
    t1=t_2_05;
elseif d+p==10
    t1=t_2_10;
elseif d+p==15
    t1=t_2_15;
elseif d+p==20
    t1=t_2_20;    
end

if d-p==5
    t2=t_2_05;
elseif d-p==10
    t2=t_2_10;
elseif d-p==15
    t2=t_2_15;
elseif d-p==20
    t2=t_2_20;    
end

d1=d+p-c;
d2=d-p+c;
t1=t1-c;
t2=t2+c;

[d1,d2,t1,t2]
end

function [ep, M] = find_offset(d1,d2,t1,t2) %returns offset (ep) and Mach number
y=1.4;
tol=1e-9; %difference required to break loop
maxiterations=1000;
ep=0; %initial guess for offset epsilon
 
for i=1:maxiterations
    %1 corresponds to top edge, 2 corresponds to bottom edge
    M1=2*(cotd(t1+ep)+tand(d1+ep))/(sind(2*(t1+ep))-tand(d1+ep)*(y+cosd(2*(t1+ep))));
    M2=2*(cotd(t2-ep)+tand(d2-ep))/(sind(2*(t2-ep))-tand(d2-ep)*(y+cosd(2*(t2-ep))));
    
    dM1=-(2*cotd(ep+t1)^2-2*tand(ep+d1)^2)/...
        (sind(2*ep+2*t1)-tand(ep+d1)*(cosd(2*ep+2*t1)+7/5)) ...
        -((2*cotd(ep+t1)+2*tand(ep+d1))*(2*cosd(2*ep+2*t1)+...
        2*sind(2*ep+2*t1)*tand(ep+d1)-(tand(ep+d1)^2+1)*(cosd(2*ep+2*t1)+7/5)))...
        /(sind(2*ep+2*t1)-tand(ep+d1)*(cosd(2*ep+2*t1)+7/5))^2;
    
    dM2=(2*cotd(t2-ep)^2-2*tand(d2-ep)^2)...
        /(sind(2*t2-2*ep)-tand(d2-ep)*(cosd(2*t2-2*ep)+7/5))+...
        ((2*cotd(t2-ep)+2*tand(d2-ep))*(2*cosd(2*t2-2*ep)+...
        2*sind(2*t2-2*ep)*tand(d2-ep)-(cosd(2*t2-2*ep)+7/5)*(tand(d2-ep)^2+1)))...
        /(sind(2*t2-2*ep)-tand(d2-ep)*(cosd(2*t2-2*ep)+7/5))^2;
    
    %Newton's Method
    Mdiff=M1-M2;
    dMdiff=dM1-dM2;
    diff=Mdiff/dMdiff;
    ep=ep-diff;
    
    %Check for early break
    if abs(diff)<tol
        break
    end
    %fprintf('%f %f %f\n', M1, M2, ep);
end
 
%Output
M=sqrt((M1+M2)/2); %Best guess for M is average of top and bottom
fprintf('M=%f\noffset=%f deg\niterations: %g\n' ... 
    , M, ep,i)

end %function

function [d1, d2, t1, t2] = gen_example(M, tfull, a, c)
%Constandts
y=1.4;
 
%Inputs
offset=c*pi/180;
tfull=tfull*pi/180;
a=a*pi/180;
 
t1=tfull/2-a;
t2=tfull/2+a;
 
d1=acotd(tand(t1)*((y+1)*M^2/(2*(M^2*sind(t1)^2-1))-1));
d2=acotd(tand(t2)*((y+1)*M^2/(2*(M^2*sind(t2)^2-1))-1));
 
t1=t1-offset;
t2=t2+offset;
d1=d1-offset;
d2=d2+offset;

fprintf('%f %f %f %f %f\n%f %f %f %f\n', M, tfull, a, offset, a+offset, ...
t1, d1, t2, d2);

end