%% GAUSSIANA 3D
close all;
clear all;
clc;


u = dlmread('samples_50.txt');
u = u(:,2:end);



lambda_1 = 0.3e-03;
lambda_2 = 0.3e-03;
lambda_3 = 1.7e-03;

angle = [0,45];

D1 = create_diffusion_tensor([lambda_1,lambda_2,lambda_3],angle);


angle = [0,0];

D2 = create_diffusion_tensor([lambda_1,lambda_2,lambda_3],angle);

l = length(u);

P = zeros(l,1);
r = 50e-03;
tau=0.04;
for i=1:l
    P(i) = 0.5/(sqrt((4*pi*tau)^3*(lambda_1*lambda_2*lambda_3))) *exp(-r^2 * u(i,:)*inv(D1)*u(i,:)')+0.5/(sqrt((4*pi*tau)^3*(lambda_1*lambda_2*lambda_3))) *exp(-r^2 * u(i,:)*inv(D2)*u(i,:)'); 
end


N=8;

nc = (N+1)*(N+2)/2;

Y = zeros(l,nc);
[theta,phi,r] = cart2sph(u(:,1),u(:,2),u(:,3));
c=1;
for l=0:2:N
    for m=-l:l
        
        Y(:,c) = real_sh(pi/2-phi,theta,l,m);
        c=c+1;
    end
end


C = pinv(Y'*Y+ 1e-08*eye(nc))*Y'*P;


n= 100;
[x,y,z] = sphere(n);

X = x(:);
Y = y(:);
Z = z(:);

[theta,phi,r] = cart2sph(X,Y,Z);

Y_ = zeros((n+1)*(n+1),nc);

c=1;
for l=0:2:N
    for m=-l:l
        Y_(:,c) = real_sh(pi/2-phi,theta,l,m);
        c=c+1;
    end
end

P_ = Y_ * C;

[X2,Y2,Z2] = sph2cart(theta,phi,P_);

x2 = reshape(X2,[n+1,n+1]);
y2 = reshape(Y2,[n+1,n+1]);
z2 = reshape(Z2,[n+1,n+1]);
p = reshape(P_,[n+1,n+1]);


clf
sc=surf(x2,y2,z2,p)
set(sc,'FaceAlpha',0.5, 'EdgeAlpha', 0,'edgecolor','none')
light
lighting phong
axis tight equal off
view(40,30)
camzoom(1.5)
