function TestWithNoise_smalldisp
% Compare performance of different algorithms on constant disparity noise
% stimulus.
% For this example, the true disparity is within a quarter-cycle.
% This simulation is designed to address the possible criticism that it was
% not fair to test phase-disp detectors on a stimulus outside the
% half-cycle limit.
% I am now testing both position and phase disparity detectors, looking for
% matches up to half a cycle.

clear all
close all
minDspace = 0.01;

nims = 1e4;
trueDpix = 2;
nx=100;
ny=100;
x= ([1:nx]-nx/2)*3/nx;
y= x;
Dxtrue = trueDpix*min(diff(x));

SFBW=1.5;
SF = 2;
xpref = 0;
ypref = 0;
SD = sqrt(log(2))./(2*pi*SF).*(2^SFBW+1)/(2^SFBW-1);
halfSDsq=0.5/SD^2;
minD = -1/SF/2; % found by looking at truedisp
maxD = 1/SF/2;
Dxprefs = [minD : min([minDspace 1/SF/5]) : maxD];
Dphiprefs = [-1:0.05:1]*pi;

Dx_phase = NaN*zeros(1,nims);
C_phase = Dx_phase;
M_phase = Dx_phase;
minphase = Dx_phase;
C_maxcorr = Dx_phase;
Dx_maxcorr = Dx_phase;
M_maxcorr = Dx_phase;
Dx_maxE_TE = Dx_phase;
Dx_maxE_all = Dx_phase;
Dx_maxE_pureph = Dx_phase;

[x2,y2]=meshgrid(x,y);
jdp0 = find(Dphiprefs==0);
jdx0 = find(Dxprefs==0);

figure('pos',[16   484   560   232])
imagesc(x,y, exp( - halfSDsq*(x2.^2+y2.^2)).*cos(2*pi*SF*x2) )
drawnow
f=figure;
f2=figure('pos',[44          40        1090         248]);
for jim=1:nims
    randn('state',jim)
    imL = randn(ny,nx);
    imR(:,(trueDpix+1):nx)=imL(:,1:(nx-trueDpix));
    imR(:,1:trueDpix)=randn(ny,trueDpix);

    immx=max([max(max(imL)) max(max(imR))]);
    immn=min([min(min(imL)) min(min(imR))]);

    dx = min(diff(x))
    dy = min(diff(y))

    % First look at the whole popn response:
    [E,C] = PopResponse(x2,y2,imL,imR,SF,halfSDsq,xpref,ypref,Dxprefs,Dphiprefs);
    figure(f)
    subplot(1,1,1)
    hold off
    imagesc(Dxprefs,Dphiprefs,E)
    hold on
    drawnow
    
    zerophasedispE = E(jdp0,:)
    zerophaseC = C(jdp0,:);
    % Find maximum-energy
    [mx,jdx,jdp] = max2d(E);
    Dx_maxE_all(jim) = Dxprefs(jdx) - Dphiprefs(jdp)/2/pi/SF;
    % Find maximum-energy along pure phase disparity
    [mx,jdp2] = max(E(:,jdx0));
    Dx_maxE_pureph(jim) = - Dphiprefs(jdp2)/2/pi/SF;
        
    % do the integral in physical space
    [Dx_phase(jim),C_phase(jim),M_phase(jim),minphase(jim),...
        Dx_maxcorr(jim),C_maxcorr(jim),M_maxcorr(jim),Dx_maxE_TE(jim)] ...
        = LocalFindDisparity(x2,y2,imL,imR,SF,SD,0,0,xpref,ypref,Dxprefs,zerophasedispE,zerophaseC)
    %     [Dx_phase(jim),C_phase(jim),M_phase(jim),minphase(jim),...
    %         Dx_maxcorr(jim),C_maxcorr(jim),M_maxcorr(jim),Dx_maxE_TE(jim)] ...
    %         = FindDisparity(x2,y2,imL,imR,SF,SD,0,0,xpref,ypref,Dxprefs)
    save TestWithNoise_smalldisp

     plot(Dxprefs,Dxprefs*0,'k-')
    plot(Dphiprefs*0,Dphiprefs,'k-')
    plot(Dxtrue,0,'ks','markerfacecol','c')
    plot(Dx_phase(jim),0,'rx')
    plot(Dx_maxcorr(jim),0,'b+')
    plot(Dx_maxE_TE(jim),0,'go')
    plot([0 Dx_maxE_pureph(jim)],[Dphiprefs(jdp2) 0],'ms-')
    plot([Dxprefs(jdx) Dx_maxE_all(jim)],[Dphiprefs(jdp) 0],'gs-')
    title(sprintf('image #%d',jim))
    set(gca,'ydir','norm')
    drawnow
    
    figure(f2)
    nbins=20;    
    dxb = [minD : (maxD-minD)/nbins : maxD];
    [freq,dx] = hist(Dx_phase,dxb);
    subplot(1,4,1)
    bar(dx,freq)
    title('Our phase algorithm')
    [freq,dx] = hist(Dx_maxE_TE,dxb);
    subplot(1,4,2)
    bar(dx,freq)
    title('Max-E (pure pos cells only)')
    [freq,dx] = hist(Dx_maxE_pureph,dxb);
    subplot(1,4,3)
    bar(dx,freq)
    title('Max-E (pure phase cells)')
    [freq,dx] = hist(Dx_maxE_all,dxb);
    subplot(1,4,4)
    bar(dx,freq)
    title('Max-E (full population)')
    for jj=1:4
        subplot(1,4,jj)
        xlim([min(Dxprefs) max(Dxprefs)])
        hold on
        plot(Dxtrue*[1 1],ylim,'r')
        hold off
    end
    drawnow
    
    
end % Next noise image

function [E,C] = PopResponse(x2,y2,imL,imR,SF,halfSDsq,xpref,ypref,Dxprefs,Dphiprefs)
% Calculate the response of a population of energy-model cells to a stereogram
% imL,imR. 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.
% SF,SFBW,theta,RFphase,xpref,ypref specify the RF.
% This version is only for vertical RFs, and outputs the energy for complex
% cells.

nDx = length(Dxprefs);
nDp = length(Dphiprefs);

for jdx=1:nDx
    
    xL = xpref - Dxprefs(jdx)/2;
    xR = xpref + Dxprefs(jdx)/2;
    envL = exp( - halfSDsq*((x2-xL).^2+(y2-ypref).^2) );
    envR = exp( - halfSDsq*((x2-xR).^2+(y2-ypref).^2) );
    
    for jdp=1:nDp
        RFLE = envL .* cos(2*pi*SF.*(x2-xL) - Dphiprefs(jdp)/2);
        RFRE = envR .* cos(2*pi*SF.*(x2-xR) + Dphiprefs(jdp)/2);
        RFLO = envL .* sin(2*pi*SF.*(x2-xL) - Dphiprefs(jdp)/2);
        RFRO = envR .* sin(2*pi*SF.*(x2-xR) + Dphiprefs(jdp)/2);
        
        LE = (sum(sum(RFLE.*imL)));
        RE = (sum(sum(RFRE.*imR)));
        LO = (sum(sum(RFLO.*imL)));
        RO = (sum(sum(RFRO.*imR)));
        
        E(jdp,jdx) = (LE+RE)^2+(LO+RO)^2;        
        C(jdp,jdx) = 2*(LE*RE+LO*RO)/(LE^2+RE^2+LO^2+RO^2);        
    end
end


function [Dx_phase,C_phase,M_phase,minphase,Dx_maxcorr,C_maxcorr,M_maxcorr,Dx_maxE] =LocalFindDisparity(x2,y2,imL,imR,SF,RFSD,theta,RFphase,xpref,ypref,Dxprefs,zerophasedispE,zerophaseC)
% 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);

% 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
    [mx,j] = max(zerophasedispE);
    Dx_maxE = Dxprefs(j);
end

if abs(Dx_maxE) > max(abs(Dxprefs))
    stop
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);


