%This program implements fringe pattern demodulation using the continuous 
%wavelet transform algorithm and employs the phase estimation method and
%phase ridge extraction algorithms. The Morlet mother wavelet is used
%Install YAWTB toolbox to run this program. See the link --
%http://rhea.tele.ucl.ac.be/yawtb/
%This program is written By Dr. Abdulbasit Abid and Dr. Munther Gdeisat 
%at The General Engineering %Research Institute at 
%Liverpool John Moores University, Liverpool (UK) on 8th Febraury 2009.
clear; close all; clc;
S=256;
x=-S+1:1:S;
[X, Y] = meshgrid(x,x);
shape=peaks(2*S);
figure(1)
colormap(gray(256))
mesh(shape)
fo = 1./16;
fringes = cos(2.*pi.*fo.*X + shape);
figure(2)
colormap(gray(256))
imagesc(fringes)

unwrappedPhase = [];
for k=1:2*S
    k
    y = fringes(k,:);
    fsig=fft(fringes(k,:));
    c=cwt1d(fsig,'morlet',1:64);
    A=abs(c.data);
    psi=angle(c.data);
    c=c.data;
    init_scale = 15;
    old_scale = init_scale;
    for b=1:2*S-2
        phi1 = psi(old_scale, b+1);
        phi2 = psi(old_scale, b+2);
        omega = phi2 - phi1;
        if (omega > pi) omega=omega-2*pi; end
        if (omega < -pi) omega=omega+2*pi; end
        new_scale = round(6/abs(omega));
        
        while( (((new_scale - old_scale) / old_scale) > 0.01) && (new_scale <=64) )
            old_scale = new_scale;
            phi1 = psi(old_scale, b+1);
            phi2 = psi(old_scale, b+2);
            omega = phi2 - phi1;
            if (omega > pi) omega=omega-2*pi; end
            if (omega < -pi) omega=omega+2*pi; end
            new_scale = round(6/abs(omega));         
        end
        if( (new_scale <= 64) && (new_scale > 0) )
            scales_array(b) = new_scale;
            old_scale = new_scale;
        else
            scales_array(b+1) = scales_array(b);
        end
    end
    scales_array(b)=scales_array(b-1);
    for n=1:2*S-2
        wrappedPhase(k,n) = atan2( imag(c(scales_array(n),n)), real(c(scales_array(n),n)) );
    end
end
unwrappedPhase = unwrap(unwrap(wrappedPhase)')';
unwrappedPhase = (unwrappedPhase(:,1:510) - 2*pi*fo.*X(:,1:510));
error = shape(:,1:510) - unwrappedPhase(:,1:510);
figure(3);mesh(unwrappedPhase)
figure(4);mesh(error)