function [Dx_phase,C_phase,M_phase,minphase,Dx_maxcorr,C_maxcorr,M_maxcorr,Dx_maxE] = FindDisparity(x2,y2,imL,imR,SF,RFSD,theta,RFphase,xpref,ypref,Dxprefs)
% Estimate the disparity of the stereogram imL,imR at position (xpref,ypref) using the RFs specified.
% I have only written it for isotropic Gaussian envelopes

% All cells have the same preferred cyclopean position
% (xpref,ypref) and preferred vertical disparity zero, but differ in their
% preferred horizontal disparity and phase disparity.

% Dxprefs is an array of position disparities first used to find maxima.

% First off, if not already done, let's make x2, y2, imL, imR 1d arrays: imL(j) is the value of the left image at x2(j) and y2(j).
imL = reshape(imL,[1 prod(size(imL))]);
imR = reshape(imR,[1 prod(size(imR))]);
x2 = reshape(x2,[1 prod(size(x2))]);
y2 = reshape(y2,[1 prod(size(y2))]);

halfSDsq=0.5/RFSD^2;
twopiF = 2*pi*SF;
nDx = length(Dxprefs);
costh = cos(theta);
sinth = sin(theta);
options=optimset('tolx',1e-6);

for jdx=1:nDx
    [zerophasedispE(jdx),zerophaseC(jdx)] = EnergyModel(Dxprefs(jdx),x2,y2,imL,imR,twopiF,halfSDsq,costh,sinth,RFphase,xpref,ypref,0);
end
% Locate turning points:
df=diff(zerophasedispE);
jlocmax = find((df<0&shiftx(df,1)>0) | (df==0 & shiftx(df,-1)<0&shiftx(df,1)>0));
jlocmin = find((df>0&shiftx(df,1)<0) | (df==0 & shiftx(df,-1)>0&shiftx(df,1)<0));
% Now look for these turning points more accurately:
Dxprefmin = [];
for j=1:length(jlocmin)
    [Dxprefmin(j),fval,exitflag] = fminsearch( @EnergyModel,Dxprefs(jlocmin(j)),options, x2,y2,imL,imR,twopiF,halfSDsq,costh,sinth,RFphase,xpref,ypref,0 );
    %[Dxprefmin(j),fval,exitflag] = fminsearch( @EnergyModel,Dxprefmin(j),options, x2,y2,imL,imR,twopiF,halfSDsq,costh,sinth,RFphase,xpref,ypref,0 );
    if exitflag<0
        disp('failed when finding minimum E as a function of pos-disp')
    end
end
Dxprefmax=[];
minusEpeak=[];
for j=1:length(jlocmax)
    [Dxprefmax(j),minusEpeak(j),exitflag] = fminsearch( @MinusEnergyModel,Dxprefs(jlocmax(j)),options, x2,y2,imL,imR,twopiF,halfSDsq,costh,sinth,RFphase,xpref,ypref,0 );
    %[Dxprefmax(j),minusEpeak(j),exitflag] = fminsearch( @MinusEnergyModel,Dxprefmax(j),options, x2,y2,imL,imR,twopiF,halfSDsq,costh,sinth,RFphase,xpref,ypref,0 );
    if exitflag<0
        disp('failed when finding maximum E as a function of pos-disp')
    end
end
% Reject TPs outside the range it was told to look for:
Dxprefmin = Dxprefmin(Dxprefmin>=min(Dxprefs) & Dxprefmin<=max(Dxprefs)); 
Dxprefmax = Dxprefmax(Dxprefmax>=min(Dxprefs) & Dxprefmax<=max(Dxprefs)); 
minusEpeak = minusEpeak(Dxprefmax>=min(Dxprefs) & Dxprefmax<=max(Dxprefs)); 

% I'm alos going to find accurately the peak in corrtelation , for comparison
% Peak in C is estimated:
[mx,j] = max(zerophaseC);
if j==1 | j==nDx
    % then the maximum in C was the last value in the array, ie is not a true maximum, so ignore:
    Dx_maxcorr = NaN;
    E_maxcorr = NaN;
    C_maxcorr = NaN;
    M_maxcorr = NaN;
else
    Dx_maxcorr = Dxprefs(j);
    [Dx_maxcorr,fval,exitflag] = fminsearch( @MinusCorrelation,Dx_maxcorr,options, x2,y2,imL,imR,twopiF,halfSDsq,costh,sinth,RFphase,xpref,ypref,0 );
    % Finally record value of monoc responses at this fitted disparity:
    [E_maxcorr,C_maxcorr,M_maxcorr] = EnergyModel(Dx_maxcorr,x2,y2,imL,imR,twopiF,halfSDsq,costh,sinth,RFphase,xpref,ypref,0);
end

% If there was at least 1 local maximum in E, record it for the maxE method:
if length(Dxprefmax)>0
    % Find biggest peak:
    [mx,j] = min(minusEpeak);
    Dx_maxE = Dxprefmax(j);
else
    Dx_maxE = max(zerophasedispE);
end

% Amalgamate all TPs:
DxprefTP = sort([Dxprefmin Dxprefmax]);
nTP = length(Dxprefmin)+length(Dxprefmax);
% If no TPs at all, return NaN:    
if nTP==0
   [Dx_phase,minphase,E_phase,C_phase,M_phase]  = deal(NaN);
    return
end
DxprefTP = sort([Dxprefmin Dxprefmax]);

% For each turning-point on the zero-phase-disp line, calculate the response of phase disparity detectors:
for jTP = 1:nTP
    % Find phase disparity at which E peaks, keeping position disparity constant:
    [fittedphase(jTP),fval,exitflag] = fminsearch(@negEasfnofphase,0,options,x2,y2,imL,imR,twopiF,halfSDsq,costh,sinth,RFphase,xpref,ypref,DxprefTP(jTP));
    if exitflag<0
        disp('failed when finding maximum E as a function of phase-disp')
    end
end

% Now return the disparity of the TP with the smallest absolute phase:
[minphase,jfit]=min(abs(fittedphase));
Dx_phase = DxprefTP(jfit);
% Finally record value of monoc responses at this fitted place:
[E_phase,C_phase,M_phase] = EnergyModel(Dx_phase,x2,y2,imL,imR,twopiF,halfSDsq,costh,sinth,RFphase,xpref,ypref,0);



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function E = MinusEnergyModel(Dxpref,x2,y2,imL,imR,twopiF,halfSDsq,costh,sinth,RFphase,xpref,ypref,Dphipref)
E = - EnergyModel(Dxpref,x2,y2,imL,imR,twopiF,halfSDsq,costh,sinth,RFphase,xpref,ypref,Dphipref);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [E,varargout] = EnergyModel(Dxpref,x2,y2,imL,imR,twopiF,halfSDsq,costh,sinth,RFphase,xpref,ypref,Dphipref)
% Energy model as function of postion disparity
xL = xpref - Dxpref/2;
x2rotL = costh.*(x2-xL) + sinth.*(y2-ypref);    
xR = xpref + Dxpref/2;
x2rotR = costh.*(x2-xR) + sinth.*(y2-ypref);

% RF 1
RFL = exp( - halfSDsq*((x2-xL).^2+(y2-ypref).^2) ) .* cos(twopiF.*x2rotL - RFphase - Dphipref/2);
RFR = exp( - halfSDsq*((x2-xR).^2+(y2-ypref).^2) ) .* cos(twopiF.*x2rotR - RFphase + Dphipref/2);    
L1 = sum(RFL.*imL);
R1 = sum(RFR.*imR);
% RF 2
RFL = exp( - halfSDsq*((x2-xL).^2+(y2-ypref).^2) ) .* sin(twopiF.*x2rotL - RFphase - Dphipref/2);
RFR = exp( - halfSDsq*((x2-xR).^2+(y2-ypref).^2) ) .* sin(twopiF.*x2rotR - RFphase + Dphipref/2);    
L2 = sum(RFL.*imL);
R2 = sum(RFR.*imR);

E = (L1+R1)^2+(L2+R2)^2;
if nargout>1
    varargout{1} = 2*(L1*R1+L2*R2)/(L1^2+L2^2+R1^2+R2^2);
end
if nargout>2
    varargout{2} = (L1^2+L2^2+R1^2+R2^2);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function E = negEasfnofphase(Dphipref,x2,y2,imL,imR,twopiF,halfSDsq,costh,sinth,RFphase,xpref,ypref,Dxpref)
% minus Energy model ouput as function of phase disparity
xL = xpref - Dxpref/2;
x2rotL = costh.*(x2-xL) + sinth.*(y2-ypref);    
xR = xpref + Dxpref/2;
x2rotR = costh.*(x2-xR) + sinth.*(y2-ypref);

% RF 1
RFL = exp( - halfSDsq*((x2-xL).^2+(y2-ypref).^2) ) .* cos(twopiF.*x2rotL - RFphase - Dphipref/2);
RFR = exp( - halfSDsq*((x2-xR).^2+(y2-ypref).^2) ) .* cos(twopiF.*x2rotR - RFphase + Dphipref/2);    
L1 = sum(RFL.*imL);
R1 = sum(RFR.*imR);
% RF 2
RFL = exp( - halfSDsq*((x2-xL).^2+(y2-ypref).^2) ) .* sin(twopiF.*x2rotL - RFphase - Dphipref/2);
RFR = exp( - halfSDsq*((x2-xR).^2+(y2-ypref).^2) ) .* sin(twopiF.*x2rotR - RFphase + Dphipref/2);    
L2 = sum(RFL.*imL);
R2 = sum(RFR.*imR);

E = -(L1+R1)^2-(L2+R2)^2;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function mC = MinusCorrelation(Dxpref,x2,y2,imL,imR,twopiF,halfSDsq,costh,sinth,RFphase,xpref,ypref,Dphipref)
xL = xpref - Dxpref/2;
x2rotL = costh.*(x2-xL) + sinth.*(y2-ypref);    
xR = xpref + Dxpref/2;
x2rotR = costh.*(x2-xR) + sinth.*(y2-ypref);
% RF 1
RFL = exp( - halfSDsq*((x2-xL).^2+(y2-ypref).^2) ) .* cos(twopiF.*x2rotL - RFphase - Dphipref/2);
RFR = exp( - halfSDsq*((x2-xR).^2+(y2-ypref).^2) ) .* cos(twopiF.*x2rotR - RFphase + Dphipref/2);    
L1 = sum(RFL.*imL);
R1 = sum(RFR.*imR);
% RF 2
RFL = exp( - halfSDsq*((x2-xL).^2+(y2-ypref).^2) ) .* sin(twopiF.*x2rotL - RFphase - Dphipref/2);
RFR = exp( - halfSDsq*((x2-xR).^2+(y2-ypref).^2) ) .* sin(twopiF.*x2rotR - RFphase + Dphipref/2);    
L2 = sum(RFL.*imL);
R2 = sum(RFR.*imR);
mC = -2*(L1*R1+L2*R2)/(L1^2+L2^2+R1^2+R2^2);
