function DiagramOfVerticalDisparity_planes
% Does plots of vertical disparity as a funciton of position in the visual
% field, for two different values of Z. The first figure is for the
% approximate, first-order expressions; the second is for the exact
% numerical results.
% In this version, the viewed surface is a frontoparallel plane, and we consider 2
% different values of Z.


clear all
close all
I=6.4;
Zs=[60 1000];
Hc = 15*pi/180;
HD = 5.69637*pi/180;
fs = 16; % font size
zerocol = [1 1 0.99]; linew = 2; % color for plotting loci of zero


% Left eye:
eyes{1}.nodal = [I/2 0 0];
% Rotation matrix when V=T=0:
eyes{1}.Rot = [cos(Hc-HD/2) 0 sin(Hc-HD/2) ; 0 1 0 ; -sin(Hc-HD/2) 0 cos(Hc-HD/2)];
% Right eye:
eyes{2}.nodal = [-I/2 0 0];
eyes{2}.Rot = [cos(Hc+HD/2) 0 sin(Hc+HD/2) ; 0 1 0 ; -sin(Hc+HD/2) 0 cos(Hc+HD/2)];

% coordinates of the fixation point, for reference:
ZF = I / (tan(Hc+HD/2)-tan(Hc-HD/2))


for jdisp=1:2 % Two different object distacnes
    Z=Zs(jdisp);
    % Go over the whole visual field. This is a sphere but it will be
    % easier to lay out a square. Here, (X,Y,Z) are positions on the
    % cyclopean retina, satisfying X^2+Y^2+Z^2=1.
    Xret=[-1:0.005:1];
    Yret=[-1:0.005:1];
    for jX=1:length(Xret)
        for jY =1:length(Yret)
            % then if this is a physically possible position within the
            % cyclopean retina...
            if Xret(jX)^2+Yret(jY)^2<1
                % ... convert to azimuth-longitude/elevation-latitude
                % coordinates:
                kappac  = asin(Yret(jY));
                alphac = -asin(Xret(jX)/cos(kappac)); % postiive alpha points left
                betac = -asin(Xret(jX));
                etac = asin(Yret(jY)/cos(betac));
                % NB the above are all cyclopean coordinates
                % Calculate exact disparities numerically.
                % To this end, first seek the position in
                % space (X,Y,Z) in headcentered coordinates, where an
                % object projects to (alphaL,etaL), (alphaR,etaR) on the two retinae,
                % such that (alphaL+alphaR)/2 = alphac, (etaL+etaR)/2 = etac, and
                % X^2+Y^2+Z^2=R^2. Solve for this numerically:
                point = findpoint(Z,eyes,alphac,etac);
                R = norm(point);
                S = R*cos(alphac)*cos(kappac);
                % Use my first-order, approximate expressions:
                kappaDelApprox{jdisp}(jY,jX) = I/R.*sin(kappac).*sin(alphac-Hc);
                etaDelApprox{jdisp}(jY,jX) = sin(etac)*cos(etac)*(-I/S.*sin(Hc)+HD*tan(alphac));
                % Calculate exact disparity:
                % Project it into the two eyes:
                [AzLong,ElLong,ElLat] = project(point,eyes);
                kappaDelExact{jdisp}(jY,jX) = diff(ElLat);
                etaDelExact{jdisp}(jY,jX) = diff(ElLong);
            else
                kappaDelApprox{jdisp}(jY,jX) = NaN;
                etaDelApprox{jdisp}(jY,jX) = NaN;
                kappaDelExact{jdisp}(jY,jX) = NaN;
                etaDelExact{jdisp}(jY,jX) = NaN;
            end
        end
    end
end % bext distance
save tmp2

% Plot the difference between exact and approximate results:
figure('name','differences between exact and approx expressions')
subplot(2,2,1)
imagesc(Xret,Yret,etaDelExact{1}-etaDelApprox{1});
colorbar
subplot(2,2,2)
imagesc(Xret,Yret,kappaDelExact{1}-kappaDelApprox{1});
colorbar
subplot(2,2,3)
imagesc(Xret,Yret,etaDelExact{2}-etaDelApprox{2});
colorbar
subplot(2,2,4)
imagesc(Xret,Yret,kappaDelExact{2}-kappaDelApprox{2});
colorbar

% Now draw the results
% Set colormap same for all plots, based on limits of
% elevation-latitude disparity (kappaDel) for sphere at R=60.
mn=min(min([kappaDelExact{1} ]))*180/pi * 1.5;
mx=max(max([kappaDelExact{1} ]))*180/pi * 1.5;
for jfig=1:2
    figure('pos',[5   278   561   420]);
    for jdisp=1:2
        % Convert to deg and use ether approx or exact expressions:
        if jfig==1
            set(gcf,'name','approximate expressions');
            kappaDel = kappaDelApprox{jdisp}*180/pi;
            etaDel = etaDelApprox{jdisp}*180/pi;
        elseif jfig==2
            set(gcf,'name','exact calculations');
            kappaDel = kappaDelExact{jdisp}*180/pi;
            etaDel = etaDelExact{jdisp}*180/pi;
        end

        for j=1:2
            subplot(2,2,j+(jdisp-1)*2)
            % imagesc, except replace NaNs with white:
            if j==2
                im=kappaDel;
            else
                im=etaDel;
            end
            colim = GetColor(im,mn,mx);
            image(Xret,Yret,colim)
            axis equal tight off
            % Mark fovea
            hold on
            plot(0,0,'ko','markerfacecol','k','markersize',8)
        end

        % Mark on lines of lat and long:
        subplot(2,2,2+(jdisp-1)*2)
        if jdisp==1
            title('elevation latitude \kappa_\Delta','fonts',fs)
        end
        hold on
        alpha=[-90:15:90]*pi/180;
        kappa=[-90:90]*pi/180;
        for ja=1:length(alpha)
            X = cos(kappa).*sin(alpha(ja));
            Y = sin(kappa);
            plot(X,Y,'k')
        end
        alpha=[-90:90]*pi/180;
        kappa=[-90:15:90]*pi/180;
        for jk=1:length(kappa)
            X = cos(kappa(jk)).*sin(alpha);
            Y = sin(kappa(jk)).*ones(size(alpha));
            plot(X,Y,'k')
        end
        % Mark on loci of zero
        % kappaDel = 0 when alpha=Hc or kappa=0
        kappa=[-1:0.01:1]*pi/2;
        X = -cos(kappa).*sin(Hc);
        Y = sin(kappa);
        plot(X,Y,'col',zerocol,'linew',linew)
        X = -sin(alpha);
        Y = zeros(size(alpha));
        plot(X,Y,'col',zerocol,'linew',linew)

        subplot(2,2,1+(jdisp-1)*2)
        if jdisp==1
            txtlab = sprintf('Z=%2dcm',Zs(jdisp));
        else
            txtlab = sprintf('Z=%2dm',Zs(jdisp)/100);
        end
        text(-0.1,0.5,txtlab,'units','norm','rotation',90,'vertic','middle','horiz','cent','fonts',fs)
        if jdisp==1
            title('elevation longitude \eta_\Delta','fonts',fs)
        end
        hold on
        alpha=[-90:15:90]*pi/180;
        eta=[-90:90]*pi/180;
        for ja=1:length(alpha)
            X = tan(alpha(ja))./sqrt(1+tan(alpha(ja))^2+tan(eta).^2);
            Y = tan(eta)./sqrt(1+tan(alpha(ja))^2+tan(eta).^2);
            plot(X,Y,'k')
        end
        alpha=[-90:90]*pi/180;
        eta=[-90:15:90]*pi/180;
        for j=1:length(eta)
            X = tan(alpha)./sqrt(1+tan(alpha).^2+tan(eta(j)).^2);
            Y = tan(eta(j))./sqrt(1+tan(alpha).^2+tan(eta(j)).^2);
            plot(X,Y,'k')
        end
        % Mark on loci of zero
        % etaDel = 0 when tan(alpha) = I/S*sin(Hc)/HD or eta=0
        % etadel=0 when sin(alpha) = I/R/cos(kappa)*sin(Hc)/HD
        % etadel=0 when X = I/R*sin(Hc)/HD
        % In this version, R is changing all the time so
        contour(Xret,Yret,etaDel,[0 0],'col',zerocol,'linew',linew)

    end % next distance

    for j=1:4
        subplot(2,2,j)
        text(-0.12,0.9,char(64+j),'fonts',fs,'units','norm','fontw','bold')
    end
    subplot(2,2,1)
    set(gca,'pos',[0.01 0.51 0.45 0.4])
    subplot(2,2,3)
    set(gca,'pos',[0.01 0.05 0.45 0.4])
    subplot(2,2,2)
    set(gca,'pos',[0.37 0.51 0.45 0.4]);
    subplot(2,2,4)
    set(gca,'pos',[0.37 0.05 0.45 0.4]);
    % Do colorbar
    axes('pos',[0.9 0.11 0.05 0.7876])
    tmpim = [0:63]'/63*(mx-mn)+mn;
    colim = GetColor(tmpim,mn,mx);
    image(0,tmpim,colim);
    set(gca,'ydir','normal','xticklabel','','xtick',[],'fonts',fs);%,'ytick',[-5 -2.5 0 2.5 5])
    ylabel('vertical disparity (deg)','fonts',fs)

end % next figure
save DiagramOfVerticalDisparity_planes.mat
print -dtiff -zbuffer -r700 DiagramOfVerticalDisparity_planes

function colim = GetColor(im,mn,mx);
% Get pseudocolor
map = colormap(jet);
nc = 64;
scledim = 0.5 + sign(im).*(abs(im)/mx).^0.55/2;
% FInd which color to use
indx = round(scledim*(nc-1))+1;
% Saturate at top and bottom of colorscale:
indx(indx<1)=1;
indx(indx>64)=64;
[ny,nx]=size(im);
colim = zeros(ny,nx,3);
nancol =  get(gcf,'color');
for jy=1:ny
    for jx=1:nx
        if isnan(indx(jy,jx))
            colim(jy,jx,:) = [1 1 1];
        else
            colim(jy,jx,:) = map(indx(jy,jx),:);
        end
    end
end

function point = findpoint(Z,eyes,alphac,etac)
options=optimset('TolX',1e-4,'tolfun',1e-4,'maxiter',1000,'maxfuneval',1e6);
[XY, fval, exitflag] = fminsearch(@geterr, [0 0],options,Z,eyes,alphac,etac);
[XY, fval, exitflag] = fminsearch(@geterr, XY,options,Z,eyes,alphac,etac);
point = [XY Z];

function err = geterr(XY,Z,eyes,alphac,etac)
% eyes{1,2}.Rot is the rotation matrix for the two eyes.
% XY is a 1x2 vector specifying the estimated X and Y coordinates of the
% point.
point = [XY Z];
[AzLong,ElLong,ElLat] = project(point,eyes);
err = sqrt( (alphac-mean(AzLong)).^2 + (etac-mean(ElLong)).^2 );

function [AzLong,ElLong,ElLat] = project(point,eyes)
for jeye=1:2
    n = eyes{jeye}.nodal;
    nrm = dot(point-n , eyes{jeye}.Rot*[0;0;1]);
    xonf = - dot(point-n , eyes{jeye}.Rot*[1;0;0]) / nrm;
    yonf = - dot(point-n , eyes{jeye}.Rot*[0;1;0]) / nrm;

    AzLong(jeye) = atan(xonf);
    ElLong(jeye) = atan(yonf);
    ElLat(jeye) = atan(yonf/sqrt(xonf^2+1));
end
