%
% function fitD2()
%
% Fit 2-dimensional diffusion tensor
%
%
function fitD2()

    close all

    imsize = 500;
    fontsize = 18;

    text_fig = figure('name','notes','position', ...
                      [250,250,imsize,imsize],...
                      'visible','off') ;

    data_fig = figure('name','Measurements','position', ...
                      [250,250+imsize,2*imsize,imsize],...
                      'visible','off') ;

    est_fig = figure('name','Estimates','position', ...
                     [250+2*imsize,250+imsize,2*imsize,imsize],...
                     'visible','off') ;

    pdf_fig = figure('name','PDF','position', ...
                     [250,250,2*imsize,imsize],...
                     'visible','off') ;


    % ----- Units conversion --------- %

    sec2ms = 1e3;                       % seconds to milliseconds
    ms2sec = 1./sec2ms;                 % milliseconds to seconds 
    mm2um = 1e3;                        % millimeter to micrometer (micron)
    um2mm = 1./mm2um;                   % micrometer (micron) to millimeter
    cm2mm = 1e1;                        % centimeter to millimeter
    mm2cm = 1./cm2mm;                   % millimeter to centimeter
    cm2um = 1e4;                        % centimeter to micrometer (micron)
    um2cm = 1./cm2um;                   % micrometer (micron) to centimeter 

    % gyromagnetic ratio (Rad/s/G)
    Gam = 4258*2*pi;         

    % ----- User input --------- %

    fprintf('--------- User input ---------\n');

    n = input('number of directions: [50] ');
    if isempty(n),
        n = 50;
    end

    snr_def = 10;  
    istr = sprintf('snr: (0=inf) [%g] ',snr_def);
    snr = input(istr);
    if isempty(snr),
        snr = snr_def;
    end
    if snr==0,
        nvar = 0;
    else
        nvar = 1./snr;
    end

    % ----- Construct diffusion tensor ----- %

    rotang_def = 30;
    istr = sprintf('Fiber angle (degrees) (degrees): [%g] ',rotang_def);
    rotang = input(istr);
    if isempty(rotang),
        rotang = rotang_def;
    end
    rotang = deg2rad(rotang);

    lamy = 1;
    lamrat_def = 10;
    istr = sprintf('Ratio of largest to smallest eigenvalue: [%g] ',lamrat_def);
    lamrat = input(istr);
    if isempty(lamrat)
        lamrat = 10;
    end
    lamx = lamrat*lamy;

    lam = [lamx lamy];
    D = diag(lam);
    fprintf('True eigenvalues: (lamx,lamy) = (%g,%g)\n',lamx,lamy);

    fprintf('Fiber angle (degrees) = %g\n',rad2deg(rotang));

    rotmat = [ cos(rotang) sin(rotang); ...
               -sin(rotang) cos(rotang)];
    % 
    % Problem 3a
    % What does this next line represent and why does it have this
    % form? 
    %
    D = rotmat'*D*rotmat;

    if 0
        % ----- text test --------- %

        Eqn_str = sprintf('\t signal: s(b) = s(0) e^{-b D}');
        descr1 = {' ';
                  ' ';
                  ' ';
                  ' ';
                  'Fitting the diffusion coefficient';
                  Eqn_str;
                 ' '};

        figure(text_fig)
        fontsize = 18;
        fontweight = 'bold';
        text(.1,.9,descr1,'Fontsize',fontsize,'FontWeight',fontweight);
        axis off
    end

    % -------- Sample -----------%

    figure(data_fig)

    origin = [0 0];
    svec = [1 0];
    theta = mylinspace(0,2*pi,n,false);

    subplot(121)
    axlim = 1.1;
    axis(axis)
    arrow(origin,svec);
    grid on
    grid minor
    box on
    set(gca,'xlim',[-axlim,axlim]);
    set(gca,'ylim',[-axlim,axlim]);
    axis equal
    xlabel('x')
    ylabel('y')
    title('S(b)')
    hold on

    subplot(122)
    axlim = 1.1;
    axis(axis)
    arrow(origin,svec);
    grid on
    grid minor
    box on
    set(gca,'xlim',[-axlim,axlim]);
    set(gca,'ylim',[-axlim,axlim]);
    axis equal
    xlabel('x')
    ylabel('y')
    title('D_{app}')
    hold on

    bval = .1;
    S0 = 5.;
    sig = zeros(n,1);
    Drot = zeros(n,1);
    bmat = zeros(n,3);
    fprintf('Sampling ... ')
    for i=1:n
        rotmat = [ cos(theta(i)) sin(theta(i)); ...
                   -sin(theta(i)) cos(theta(i))];
        % 
        % Problem 3b
        % What does this next line represent and why does it have this
        % form? 
        %
        svec_rot = rotmat*svec';
        % 
        % Problem 3c
        % What does this next line represent and why does it have this
        % form? 
        %
        Drot(i) = svec_rot'*D*svec_rot;
        % 
        % Problem 3d
        % What does this next line represent and why does it have this
        % form? 
        %
        sig(i) = S0*exp(-bval*Drot(i)) + nvar*randn;

        subplot(121)
        axis(axis)
        arrow(origin,svec_rot);
        sigx = sig(i)*svec_rot(1)/S0;
        sigy = sig(i)*svec_rot(2)/S0;
        plot(sigx,sigy,'bo','markerfacecolor','r')

        subplot(122)
        axis(axis)
        arrow(origin,svec_rot);
        dx = Drot(i)*svec_rot(1)/max(lam);
        dy = Drot(i)*svec_rot(2)/max(lam);
        plot(dx,dy,'bo','markerfacecolor','r')

        ssmat = svec_rot*svec_rot';
        bmat(i,:) = [ssmat(1,1) 2*ssmat(1,2) ssmat(2,2)];
    end
    fprintf('done\n')
    subplot(121)
    axis equal
    hold off
    subplot(122)
    axis equal
    hold off

    % Estimate tensor components 
    fprintf('Estimating tensor ...')
    y = -log(sig/S0)/bval;
    % 
    % Problem 3e
    % What is this function doing? 
    %
    dest = lsqnonneg(bmat,y);
    fprintf('done\n')

    % Rerrange back into tensor form
    Dxx = dest(1);
    Dxy = dest(2);
    Dyy = dest(3);
    fprintf('Estimated diffusion tensor:\n');
    %
    % Problem 3f
    % What is being constructed here? 
    %
    Dest = [Dxx Dxy; Dxy Dyy]

    % Tensor eigensystem
    %
    % Problem 3g
    % What is going on here? 
    %
    [evecs,evals] = eig(Dest);
    evals = diag(evals);
    % Sort in descending order
    [evals,ii] = sort(evals,'descend');
    evecs = evecs(:,ii);

    evec1 = [evecs(1,1),evecs(2,1)];
    evec2 = [evecs(1,2),evecs(2,2)];

    fprintf('Estimated eigenvalues:\n');
    fprintf('(lamx,lamy) = (%g,%g)\n',evals(1),evals(2));
    fprintf('Estimated eigenvectors:\n');
    fprintf('ev1 = (%g,%g)\n',evec1(1),evec1(2));
    fprintf('ev2 = (%g,%g)\n',evec2(1),evec2(2));

    % Find angle of principal eigenvector
    %
    % Problem 3h
    % What is going on here? 
    %
    evec1_angle = atan2(evec1(2),evec1(1));
    fprintf('Estimated angle:\n');
    fprintf('evec1 angle with positive x-axis = %g (degrees)\n',rad2deg(evec1_angle));

    arrow_width = 3;
    arrow_length = [];
    arrow_tipangle = [];

    figure(data_fig)
    subplot(121)

    axis(axis)
    arrow(origin,evec1,'FaceColor','g','Width',arrow_width, ...
          'Length',arrow_length,'TipAngle',arrow_tipangle); 
    axis(axis)
    arrow(origin,evec2,'FaceColor','r','Width',arrow_width, ...
          'Length',arrow_length,'TipAngle',arrow_tipangle); 

    figure(est_fig)

    arrow_width = 50;
    arrow_length = 250;
    arrow_tipangle = [];

    axis(axis)
    arrow(origin,evals(1)*evec1,'FaceColor','g','Width', ...
          arrow_width,'Length',arrow_length,'TipAngle',arrow_tipangle); 
    axis(axis)
    arrow(origin,-evals(1)*evec1,'FaceColor','g','Width', ...
          arrow_width,'Length',arrow_length,'TipAngle',arrow_tipangle); 
    axis(axis)
    arrow(origin,evals(2)*evec2,'FaceColor','r','Width', ...
          arrow_width,'Length',arrow_length,'TipAngle',arrow_tipangle); 
    axis(axis)
    arrow(origin,-evals(2)*evec2,'FaceColor','r','Width', ...
          arrow_width,'Length',arrow_length,'TipAngle',arrow_tipangle); 
    axis equal tight
    grid on
    grid minor
    box on

    %
    % Problem 3i
    % What is going on here? 
    %
    ellipse(evals(1),evals(2),evec1_angle,0,0,'b');
    xlabel('x')
    ylabel('y')
    title('Diffusion ellipsoid')

    set(gca,'xlim',1.5*xlim);
    set(gca,'ylim',1.5*ylim);

    % 2D Gaussian distribution

    figure(pdf_fig)
    mu = [0 0];
    Sigma = sqrt(Dest);
    x1max = 5; x1min = -x1max;
    x2max = 5; x2min = -x2max;
    x1 = linspace(x1min,x1max);
    x2 = linspace(x2min,x2max);
    fprintf('x1 range: (%g,%g)\n',min(x1),max(x1));
    fprintf('x2 range: (%g,%g)\n',min(x2),max(x2));

    [X1,X2] = meshgrid(x1,x2);
    %
    % Problem 3i
    % What is this function?
    %
    F = mvnpdf([X1(:) X2(:)],mu,Sigma);
    F = reshape(F,length(x2),length(x1));

    subplot(121)
    surf(x1,x2,F);
    xlabel('x1');
    ylabel('x2');
    zlabel('pdf');

    subplot(122)
    ncont = 5;
    contour(x1,x2,F,ncont);
    caxis([min(F(:))-.5*range(F(:)),max(F(:))]);
    grid minor
    xlabel('x1');
    ylabel('x2');
    title('pdf');

    return

%
% function x = mylinspace(xmin,xmax,nx,lastpt)
%
% Create vector of nx linearly spaced point from xmin to xmax
%
% Similar to matlab's linspace but with slight differences:
% 1) if nx=1, returns 'xmin' (linspace returns 'xmax')
% 2) with optional argument 'lastpt=true', behave just like
% linspace
%    with optional argument 'lastpt=false', covers interval from
%    xmin,xmax-dx
%    where dx = (xmax-xmin)/nx
%    (very useful for angles where you don't want to repeat the
%    last point, ie phi = linspace(0,2*pi,n,false)
%
function x = mylinspace(xmin,xmax,nx,lastpt)

    if ~exist('lastpt','var'), 
        lastpt = true;
    end

    if nx==1,
        x = xmin;
        return;
    end

    if (~lastpt) & ~(xmin==xmax),
        x = linspace(xmin,xmax,nx+1);
        x = x(1:end-1);
    else
        x = linspace(xmin,xmax,nx);
    end

    return