clear all
close all
global L R CYC DELTA HALFDELTA ALL
L = 1; R = 2; CYC = 3; DELTA = 4; HALFDELTA = 5; ALL=6;

% Specify a function relating Z to X and Y for the viewed surface:
dZfn = inline('0.01 + 0.05.*dX + 0.02.*dY - 0.01.*dX.^2 + 0.02.*dY.^2 - 0.005.*dX.*dY ','dX','dY');
% NB the Z-coordinate of fixation is added onto this

% Specify what eccentricities count as parafoveal:
parafoveal = 0.5*pi/180; % we work in radians
% Only points within [parafoveal] deg of the fovea will be fitted and used
% to estimate vertical disparity derivatives and hence eye position

% Specify interocular distance
I = 6.5; % cm
for jrun = 1:1e3
    jrun

    % Pick random eye position
    Tc(jrun) = (rand(1)-0.5)*2*2*pi/180;
    VD(jrun) = (rand(1)-0.5)*2*2*pi/180;
    TD(jrun) = (rand(1)-0.5)*2*2*pi/180;
    HD(jrun) = rand(1)*15*pi/180;

    Hc(jrun) = (rand(1)-0.5)*2*15*pi/180;
    Vc(jrun) = (rand(1)-0.5)*2*15*pi/180;

    HL = Hc(jrun)-HD(jrun)/2;
    HR = Hc(jrun)+HD(jrun)/2;
    fixX = I/2*(tan(HL)+tan(HR))/(tan(HR)-tan(HL));
    fixY = -I*sin(Vc(jrun))/(tan(HR)-tan(HL));
    fixZ = I*cos(Vc(jrun))/(tan(HR)-tan(HL));
    fixation = [fixX;fixY;fixZ];

    % Work out rotation matrices etc, given the Cyclopean and Vergence angles:
    eyes = GetEyesGivenCV(I,Hc(jrun)*180/pi,Vc(jrun)*180/pi,Tc(jrun)*180/pi,HD(jrun)*180/pi,VD(jrun)*180/pi,TD(jrun)*180/pi);

    % Generate some random points around fixation
    npts = 1e3;
    % Generate difference between objects and fixation:
    dX = (rand(1,npts)-0.5)*2*2*fixZ*tan(parafoveal);
    dY = (rand(1,npts)-0.5)*2*2*fixZ*tan(parafoveal);
    %dZ = dZfn(dX,dY);
    %('0.01 + 0.05.*dX + 0.02.*dY - 0.01.*dX.^2 + 0.02.*dY.^2 - 0.005.*dX.*dY ','dX','dY')
    a = (rand(4,4)-0.5)*2*0.02;
    dZ = 0;
    for zi=0:3
        for zj=0:3
            dZ = dZ + a(zi+1,zj+1) .*dX.^zi.*dY.^zj;
        end
    end
    
    X = fixation(1)+dX;
    Y = fixation(2)+dY;
    Z = fixation(3)+dZ;

    points = [X;Y;Z];
    % Project these points onto the retina
    imagecoords = ProjectToRetina(eyes,points);
    % Select only those within the desired vicinity of the fovea:
    jparafoveal = find(imagecoords.Eccentricity(CYC,:)<parafoveal);
    nptsused(jrun) = length(jparafoveal);
    % Record vertical disparity of parafoveal image-points as afunction of
    % their cyclopean location:
    alpha_c = imagecoords.AzLong(CYC,jparafoveal);
    kappa_c = imagecoords.ElLat(CYC,jparafoveal);
    kappa_Del = imagecoords.ElLat(DELTA,jparafoveal);

    % Now fit this with a parabolic function:
    % once with a very poor initial guess
    [params,fval,exitflag] = fminsearch(@ToFitParabola,[-VD(jrun) -TD(jrun) -Hc(jrun)*HD(jrun) 0 HD(jrun) -Tc(jrun)*HD(jrun)],[],alpha_c,kappa_c,kappa_Del);
    % now again using this as the new starting point:
    [params,fval,exitflag] = fminsearch(@ToFitParabola,params,[],alpha_c,kappa_c,kappa_Del);
    if exitflag~=1 % ie fit failed for some reason
        params = NaN*params;
    end
    % Read off fitted vertical disparity and derivatives at the fovea
    fit_kappa_Delta = params(1);
    fit_kappa_Deltaalpha = params(2);
    fit_kappa_Deltakappa = params(3);
    fit_kappa_Deltaalpha2 = params(4);
    fit_kappa_Deltaalphakappa = params(5);
    fit_kappa_Deltakappa2 = params(6);
    % Deduce eye position:
    fit_VD(jrun) = -fit_kappa_Delta;
    fit_TD(jrun) = -fit_kappa_Deltaalpha;
    fit_HD(jrun) = fit_kappa_Deltaalphakappa;
    fit_Tc(jrun) = -fit_kappa_Deltakappa2/fit_kappa_Deltaalphakappa;
    fit_Hc(jrun) = -fit_kappa_Deltakappa/fit_kappa_Deltaalphakappa;

    % For checking
    if jrun==1
        figure
        subplot(1,2,1)
        plotvector(points(:,jparafoveal),'g.')
        hold on
        plotvector(fixation,'ko')

        subplot(1,2,2)
        plot3(alpha_c,kappa_c,kappa_Del,'g.')
        hold on
        plot3(0,0,0,'ko')
        coarsegrid = [-1:0.1:1]*parafoveal;
        finegrid = [-1:0.001:1]*parafoveal;
        % Mark on fitted parabola for comparison:
        for j1=1:length(coarsegrid)
            x = coarsegrid(j1)*ones(size(finegrid));
            y = finegrid;
            fitz = params(1) + params(2).*x + params(3).*y + params(4).*0.5*x.^2 + params(5).*x.*y  + params(6).*0.5*y.^2;
            plot3(x,y,fitz,'b-')
            y = coarsegrid(j1)*ones(size(finegrid));
            x = finegrid;
            fitz = params(1) + params(2).*x + params(3).*y + params(4).*0.5*x.^2 + params(5).*x.*y  + params(6).*0.5*y.^2;
            plot3(x,y,fitz,'b-')
        end
    end

end
% Only use runs where the fit succeeded
j = find(~isnan(fit_VD));
fit_VD = fit_VD(j);
fit_HD = fit_HD(j);
fit_TD = fit_TD(j);
fit_Tc = fit_Tc(j);
fit_Hc = fit_Hc(j);
VD = VD(j);
HD = HD(j);
TD = TD(j);
Tc = Tc(j);
Hc = Hc(j);
Vc = Vc(j);

figure('pos',[5         479        1381         319])
fs = 16;
fs1=14;
for j=1:5
    ax(j)=subplot(1,5,j);
    plot(0,0)
    hold on
    switch j
        case 1
            x = Hc;y=fit_Hc;
            title('gaze azimuth H_c','fonts',fs)
        case 2
            x = Tc;y=fit_Tc;
            title('torsion T_c','fonts',fs)
        case 3
            x = HD;y=fit_HD;
            title('convergence H_\Delta','fonts',fs)
        case 4
            x = TD;y=fit_TD;
            title('cyclovergence T_\Delta','fonts',fs)
        case 5
            x = VD;y=fit_VD;
            title('vertical vergence V_\Delta','fonts',fs)
    end
    % convert to deg for display
    x=x*180/pi;y=y*180/pi;
    plot(x,y,'r.')
    axis tight
    MarkIdentity(gca,'k-');
    text(0.07,0.93,sprintf('median |err|=%2.1f deg',median(abs(x-y))),'units','norm','fonts',fs1)

end
w = 0.17;
s = 0.2;
for j=1:5
    set(ax(j),'pos',[0.02+s*(j-1) 0.1 w 0.77])
end
save ExtractEyePosition
set(gcf,'paperpositionmode','auto')
print -dtiff -r700 ExtractEyePosition
