function [U] = simchony_dirichlet(p,q,ub)
% An implementation of the method from Simchony et Al for integration 
% of a normal field with Dirichlet boundary condition
% Ref : Direct Analytical Methods for Solving Poisson Equation in 
% Computer Vision problems - PAMI 1990
%
% Usage : 
% u=simchony_dirichlet(p,q) 
% where p and q are MxN matrices, solves in the least square sense 
% \nabla u = [p,q] , assuming homogeneous Dirichlet boundary 
% condition u = 0 on the boundary
%
% u=simchony_dirichlet(p,q,ub) 
% where p,q and ub are NxM matrix, such as ub(1,:) contains the values 
% on the first line, ub(end,:) contains the values on the last line etc.
% Apart from the boundary ub can be anything
%
% Example (weird)
% p = zeros(100,100); 
% q = zeros(100,100);
% ub = zeros(size(p));
% ub(1,:)=1:1OO; 
% u = simchony(p,q,ub);
% surfl(u)
%
% This performs the least square solution to \nabla u = [p,q], i.e. :
% min \int_\Omega \| \nablua U - [p,q] \|^2
% where \Omega is square and the Dirichlet boundary condition 
% u = ub on the boundary of \Omega. 
%
% Axis : O->y
%        |
%        x
%
% Fast solution is provided by Discrete Sine Transform
%
% Implementation : Yvain Queau
% Universite de Toulouse, IRIT, UMR CNRS 5505
% yvain.queau@enseeiht.fr
% See other codes at http://ubee.enseeiht.fr/photometricstereo/

if(nargin<3)
	ub=zeros(size(p));
end


% Define the 1st order derivative operators : 
Fx= zeros(size(p,1));
Fx=Fx+diag(ones(size(p,1)-1,1),1);
Fx=Fx-diag(ones(size(p,1)-1,1),-1);
Fx=0.5*Fx;
% Change fist and last line
Fx(1,1)=-1.5;Fx(1,2)=2;Fx(1,3)=-0.5;
Fx(end,end)=1.5;Fx(end,end-1)=-2;Fx(end,end-2)=0.5;

Fy= zeros(size(q,2));
Fy=Fy+diag(ones(size(q,2)-1,1),1);
Fy=Fy-diag(ones(size(q,2)-1,1),-1);
Fy=0.5*Fy;
% Change fist and last line
Fy(1,1)=-1.5;Fy(1,2)=2;Fy(1,3)=-0.5;
Fy(end,end)=1.5;Fy(end,end-1)=-2;Fy(end,end-2)=0.5;
Fy=Fy';

% Define the 2nd order derivative operators : 
Fxx= -2*diag(ones(size(p,1),1));
Fxx=Fxx+diag(ones(size(p,1)-1,1),1);
Fxx=Fxx+diag(ones(size(p,1)-1,1),-1);
% Change fist and last line
Fxx(1,1)=2;Fxx(1,2)=-5;Fxx(1,3)=4;Fxx(1,4)=-1;
Fxx(end,end)=-2;Fxx(end,end-1)=5;Fxx(end,end-2)=-4;Fxx(end,end-3)=1;

Fyy= -2*diag(ones(size(q,2),1));
Fyy=Fyy+diag(ones(size(q,2)-1,1),1);
Fyy=Fyy+diag(ones(size(q,2)-1,1),-1);
% Change fist and last line
Fyy(1,1)=2;Fyy(1,2)=-5;Fyy(1,3)=4;Fyy(1,4)=-1;
Fyy(end,end)=-2;Fyy(end,end-1)=5;Fyy(end,end-2)=-4;Fyy(end,end-3)=1;
Fyy=Fyy';

% Div(p,q)
f = Fx*p + q*Fy - Fxx*ub - ub*Fyy;
f = f(2:end-1,2:end-1);

% Sine transform of f
fsin=dst2(f);
% Denominator
[x,y] = meshgrid(0:size(p,2)-1,0:size(p,1)-1);
denom = (2*cos(pi*x/(size(p,2)))-2) + (2*cos(pi*y/(size(p,1))) - 2);
Z = fsin./denom(2:end-1,2:end-1);

% Inverse Sine transform :
U=ub;
U(2:end-1,2:end-1) = idst2(Z);
return


function y = dst2(x)
y = dst(dst(x)')';
return

function Y=idst2(X);
Z=idst(X');
Y=idst(Z');
return


