function GICOVspline
% First use GICOV with ellipse, then refine it by GICOV with spline
% uses (mean/sigma)^2 for directional gradients
close all;
clc;

% input image
I=double(imread('seqt_0007.bmp'));
figure(1); imagesc(I);colormap(gray);axis image;

% possible user interaction - commented out for now
% title('Input upper left and lower right points of a rectangle.');
% [P,Q]=ginput(2);
% P(:)=round(P); Q(:)=round(Q);

% example input
P = [158;169];
Q = [163;172];

figure(1),hold on; plot([P(1) P(2) P(2) P(1) P(1)],[Q(1) Q(1) Q(2) Q(2) Q(1)]); hold off; drawnow;

disp('Time to find out best ellipse:');
tic;
% range of radii
R1=12:1.25:21;
R2=8:1.25:13;

Rmax = round(max(R1));

npoints=90; % number of points on ellipse
ntheta=3;   % number of orientations of the ellipse
thetavec = linspace(-pi/6,pi/6,ntheta); % centered at horizontal direction
tvec = linspace(0,2*pi,npoints+1);

[Rx,Ry,theta,T] = ndgrid(R1,R2,thetavec(1:end),tvec(1:end-1));
X = Rx.*cos(T).*cos(theta)-Ry.*sin(T).*sin(theta);
Y = Rx.*cos(T).*sin(theta)+Ry.*sin(T).*cos(theta);
NX = Ry.*cos(T).*cos(theta)-Rx.*sin(T).*sin(theta);
NY = Ry.*cos(T).*sin(theta)+Rx.*sin(T).*cos(theta);
x=reshape(X,[length(R1)*length(R2)*ntheta npoints]);
y=reshape(Y,[length(R1)*length(R2)*ntheta npoints]);
nx=reshape(NX,[length(R1)*length(R2)*ntheta npoints]);
ny=reshape(NY,[length(R1)*length(R2)*ntheta npoints]);
clear X; clear Y; clear NX; clear NY;
dmag = sqrt(nx.^2+ny.^2);
nx = nx./dmag;
ny = ny./dmag;

nellipse = size(nx,1);
% now create ellipse templates
NX = zeros(2*Rmax+1,2*Rmax+1,nellipse);
NY = zeros(2*Rmax+1,2*Rmax+1,nellipse);
NX2 = zeros(2*Rmax+1,2*Rmax+1,nellipse);
NY2 = zeros(2*Rmax+1,2*Rmax+1,nellipse);
NXY = zeros(2*Rmax+1,2*Rmax+1,nellipse);
TX = zeros(2*Rmax+1,2*Rmax+1);
TY = zeros(2*Rmax+1,2*Rmax+1);
T = zeros(2*Rmax+1,2*Rmax+1);
for n=1:nellipse,
    ind = (2*Rmax+1)*(round(x(n,:))+Rmax) + round(y(n,:))+Rmax+1;
    T(ind)=1;
    tsum = sum(sum(T));
    TX(ind) = -nx(n,:);
    TY(ind) = -ny(n,:);
    NX(:,:,n) = TX/tsum;
    NY(:,:,n) = TY/tsum;
    NX2(:,:,n) = TX.^2/tsum;
    NY2(:,:,n) = TY.^2/tsum;
    NXY(:,:,n) = TX.*TY/tsum;
    TX(:)=0;TY(:)=0;T(:)=0;
end


% zero padding
[Ih,Iw]=size(I);
loff = max(0,Rmax-P(1)+1);
roff = max(0,Rmax+P(2)-Iw);
uoff = max(0,Rmax-Q(1)+1);
doff = max(0,Rmax+Q(2)-Ih);
I = [zeros(Ih,loff) I zeros(Ih,roff)];
[Ih,Iw]=size(I);
I = [zeros(uoff,Iw); I; zeros(doff,Iw)];
P = P + loff; Q = Q + uoff;

smlI = I(Q(1)-Rmax:Q(2)+Rmax,P(1)-Rmax:P(2)+Rmax);
[Ix,Iy] = gradient(smlI);
Ix2 = Ix.^2;
Iy2 = Iy.^2;
Ixy = Ix.*Iy;

h = Q(2)-Q(1)+1;
w = P(2)-P(1)+1;
MeanVal = zeros(h,w);
VarVal  = MeanVal;
CellVal  = zeros(h,w,nellipse);

for n=1:nellipse,
    MeanVal(:) = conv2(Ix,NX(:,:,n),'valid') +  conv2(Iy,NY(:,:,n),'valid');
    MeanVal(:) = MeanVal.^2;
    VarVal(:) = conv2(Ix2,NX2(:,:,n),'valid') + conv2(Iy2,NY2(:,:,n),'valid') + 2*conv2(Ixy,NXY(:,:,n),'valid')- MeanVal;
    CellVal(:,:,n) = MeanVal./VarVal;
%     disp(n);
end
toc;
disp(' ');


[maxCellVal,maxind]=max(CellVal,[],3);

Cells=uint16(65535*(maxCellVal-min(maxCellVal(:)))/(max(maxCellVal(:))-min(maxCellVal(:))));
[drow,dcol]=find(Cells==max(Cells(:)));
% undo zeropadding
[Ih,Iw]=size(I);
I = I(uoff+1:Ih-doff,loff+1:Iw-roff);
P = P - loff; Q = Q - uoff;

figure(1); hold on;
    plot(x(maxind(drow(1),dcol(1)),:)+dcol(1)+P(1),y(maxind(drow(1),dcol(1)),:)+drow(1)+Q(1),'y');
    plot(dcol(1)+P(1),drow(1)+Q(1),'r+');
hold off; drawnow;

disp('Time for B-sline snake computation:');
tic;
% Now refine the solution spline computation
X = x(maxind(drow(1),dcol(1)),:)+dcol(1)+P(1); X=X(:);
Y = y(maxind(drow(1),dcol(1)),:)+drow(1)+Q(1); Y=Y(:);

[h,w]=size(I);
fromx = max(round(mean(X)-2*Rmax),1);
fromy = max(round(mean(Y)-2*Rmax),1);
tox = min(round(mean(X)+2*Rmax),w);
toy = min(round(mean(Y)+2*Rmax),h);

[Ix,Iy]=gradient(double(I(fromy:toy,fromx:tox)));

ncontrol=15;npoints=6;
A=Tmatrix(ncontrol,npoints);
cx=A*X;
cy=A*Y;

% maxval = max(maxCellVal(:));
% disp(maxval);
TX = getfdriv(cx,npoints);
TY = getfdriv(cy,npoints);
tmp=sqrt(TX.^2+TY.^2);
TX = TX./tmp;
TY = TY./tmp;
dirgrad = interp2(Ix,X-fromx+1,Y-fromy+1).*TY - interp2(Iy,X-fromx+1,Y-fromy+1).*TX;
maxval = (mean(dirgrad)/std(dirgrad))^2;
% disp(maxval);
for itr=1:10,
    for i=1:ncontrol,
        breakflag=0;
        for sx=-1:1,
            for sy=-1:1,
                cx(i) = cx(i) + sx;
                cy(i) = cy(i) + sy;
                X(:)=getsampling(cx,npoints);
                Y(:)=getsampling(cy,npoints);
                TX(:) = getfdriv(cx,npoints);
                TY(:) = getfdriv(cy,npoints);
                tmp(:)=sqrt(TX.^2+TY.^2);
                TX(:) = TX./tmp;
                TY(:) = TY./tmp;
                dirgrad(:) = interp2(Ix,X-fromx+1,Y-fromy+1).*TY - interp2(Iy,X-fromx+1,Y-fromy+1).*TX;
                val = (mean(dirgrad)/std(dirgrad))^2;
                if val<maxval,
                    cx(i) = cx(i) - sx;
                    cy(i) = cy(i) - sy;
                else
                    maxval=val;
                    breakflag=1;
                end
                if breakflag, break; end
            end
            if breakflag, break; end
        end
    end
end
toc;
disp(' ');
figure(1);hold on;plot(X,Y,'g'); hold off; drawnow;
%disp(maxval);

disp('Seacred for an ellipse (yellow) with center of ellipse within the blue rectangular area');
disp(' ');
disp('Red cross shows the center of the best ellipse');
disp(' ');
disp('Green contour is the B-spline snake that started from the best ellipse (yellow)');

function y=Tmatrix(N,M)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% M: number of sampling points in each segment
% N: number of segment of curve
%e.g. N=6 M=4
%   |b c d 0 0 a|                               |B1|
%   |a b c d 0 0|       |b11 c11 d11 0 0 a11|   |B2|
%B'=|0 a b c d 0|  B1=  |b12 c12 d12 0 0 a12| B=|B3| a NM*M matrix
%   |0 0 a b c d|       |b13 c13 d13 0 0 a13|   |..|
%   |d 0 0 a b c|       |b14 c14 d14 0 0 a14|   |..|
%   |c d 0 0 a b|                               |BN|
% Least Square Error transform matrix Q could be
% Q=[invert(B'*B)]*B'
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%N=6;M=4;
B=zeros(N*M,N);

aindex=[N 1:N-1];
bindex=[1:N];
cindex=[2:N 1];
dindex=[3:N 1 2];

for i=1:N
    LB=zeros(M,N);
    for j=1:M
        % uniform knots vector
        s=(j-1)/M;
        
        a=(-s^3+3*s^2-3*s+1)/6;
        b=(3*s^3-6*s^2+4)/6;
        c=(-3*s^3+3*s^2+3*s+1)/6;
        d=(s^3)/6;
        
        LB(j,aindex(i))=a;
        LB(j,bindex(i))=b;
        LB(j,cindex(i))=c;
        LB(j,dindex(i))=d;
    end
    B((i-1)*M+1:i*M,:)=LB;
end
%least square error solution
y=inv(B'*B)*B';

function y = getsampling(x,M)
% get sampling points in the segment of curve.
% x: control points
% M: number of sampling points in each segment

N=length(x);
aindex=[N 1:N-1];
bindex=[1:N];
cindex=[2:N 1];
dindex=[3:N 1 2];

for i=1:N
    for j=1:M
        s=(j-1)/M;
        a=x(aindex(i))*(-s^3+3*s^2-3*s+1);
        b=x(bindex(i))*(3*s^3-6*s^2+4);
        c=x(cindex(i))*(-3*s^3+3*s^2+3*s+1);
        d=x(dindex(i))*(s^3);
      	y((i-1)*M+j)=(a+b+c+d)/6;
    end
end
y=y';

function y = getfdriv(x,M)
% get first derivative of sampling points in the segment of curve.
% x: control points
% M: number of sampling points in each segment
% to calculate slope of curve, use dy/dx=(dy/ds)/(dx/ds)
% that is, use getfd twice.

N=length(x);
aindex=[N 1:N-1];
bindex=[1:N];
cindex=[2:N 1];
dindex=[3:N 1 2];

for i=1:N
    for j=1:M
        s=(j-1)/M;
        a=x(aindex(i))*(-3*s^2+6*s-3);
        b=x(bindex(i))*(9*s^2-12*s);
        c=x(cindex(i))*(-9*s^2+6*s+3);
        d=x(dindex(i))*(3*s^2);
      	y((i-1)*M+j)=(a+b+c+d)/6;
    end
end
y=y'; 