%N = 25636;       % Number of elements, maybe take as input from file?
A = load('fort.45_0.txt');
pos = A(:,1:3);             % Postition of dipole's center
m = A(:,4:6);               % Magnetic field at dipole
% We are currently excluding the normalization to the dipole moment of each
% dipole element/cell
R = 125;
n = 101;
a_e = [125; 125*0.9];
b_e = [85; 85*0.9];
theta_=45; phi_=0; 

% [X,Y,Z] = sphere(n);
th = linspace(0,pi,n);
phi = linspace(0,2*pi,n);
rs = linspace(R-10,R,2);

[rr,tt,pp] = ndgrid(rs,th,phi);
for ii =1:2
    rr(ii,:,:)=1./sqrt((sin(tt(ii,:,:)).*cos(pp(ii,:,:))./a_e(ii)).^2+ ...
        (sin(tt(ii,:,:)).*sin(pp(ii,:,:))./a_e(ii)).^2+(cos(tt(ii,:,:))./b_e(ii)).^2);
end

% rr=rotz(phi_)*roty(theta_)*rr;
% tt=rotz(phi_)*roty(theta_)*tt;
% pp=rotz(phi_)*roty(theta_)*pp;

% xx=rr.*sin(tt).*cos(pp);
% yy=rr.*sin(tt).*sin(pp);
% zz=rr.*cos(tt);
%rsc=0.5*(rs(1:end-1)+rs(2:end));

%

rsc=0.25*(rs(2:end).^4-rs(1:end-1).^4)./((1.0/3.0)*(rs(2:end).^3-rs(1:end-1).^3));
phic=0.5*(phi(1:end-1)+phi(2:end));
thc=acos(0.5*(cos(th(1:end-1))+cos(th(2:end))));

[rrc,ttc,ppc] = ndgrid(rsc,thc,phic);
for ii =1:1
    rrc(ii,:,:)=1./sqrt((sin(ttc(ii,:,:)).*cos(ppc(ii,:,:))./a_e(ii)).^2+ ...
        (sin(ttc(ii,:,:)).*sin(ppc(ii,:,:))./a_e(ii)).^2+(cos(ttc(ii,:,:))./b_e(ii)).^2);
end
xxc=rrc.*sin(ttc).*cos(ppc);
yyc=rrc.*sin(ttc).*sin(ppc);
zzc=rrc.*cos(ttc);

% xxc=rotz(phi_)*roty(theta_)*(rrc.*sin(ttc).*cos(ppc));
% yyc=rotz(phi_)*roty(theta_)*(rrc.*sin(ttc).*sin(ppc));
% zzc=rotz(phi_)*roty(theta_)*(rrc.*cos(ttc));

%

%za = 45; ya = 90;
za = 0; ya = 0; xa = 0;
% pos_r = roty(-ya)*rotz(-za)*pos';
% m_r = roty(-ya)*rotz(-za)*m';
%pos_r = rotx(xa)*rotz(-za)*pos';
pos_r = roty(-theta_)*rotz(-phi_)*pos';
m_r = roty(-theta_)*rotz(-phi_)*m';

F1 = scatteredInterpolant(pos_r',m_r(1,:)','linear','nearest');
F2 = scatteredInterpolant(pos_r',m_r(2,:)','linear','nearest');
F3 = scatteredInterpolant(pos_r',m_r(3,:)','linear','nearest');
mx = reshape(F1(xxc(:),yyc(:),zzc(:)),size(xxc));
my = reshape(F2(xxc(:),yyc(:),zzc(:)),size(xxc));
mz = reshape(F3(xxc(:),yyc(:),zzc(:)),size(xxc));

%

% vc=(-1/3)*(rr(2:end,1:end-1,1:end-1).^3-rr(1:end-1,1:end-1,1:end-1).^3).* ...
%     (cos(tt(1:end-1,2:end,1:end-1))-cos(tt(1:end-1,1:end-1,1:end-1))).* ...
%     (pp(1:end-1,1:end-1,2:end)-pp(1:end-1,1:end-1,1:end-1));    % Volume of cells

figure,
%scatter3(xx(:),yy(:),zz(:),'r.');
scatter3(pos_r(1,1:2:end),pos_r(2,1:2:end),pos_r(3,1:2:end),'r.');
hold on;
scatter3(xxc(:),yyc(:),zzc(:),'b.'); %,vc(:));
hold off;
axis equal;

%

figure,
% quiver3(xxc(:),yyc(:),zzc(:),mx(:),my(:),mz(:));
% hold on;
quiver3(pos_r(1,1:4:end),pos_r(2,1:4:end),pos_r(3,1:4:end),m_r(1,1:4:end),m_r(2,1:4:end),m_r(3,1:4:end));
% quiver3(pos(1:1:end,1),pos(1:1:end,2),pos(1:1:end,3),m(1:1:end,1),m(1:1:end,2),m(1:1:end,3));
% axis equal;
xlabel('X'); ylabel('Y'); zlabel('Z');

%

figure,
% qcp = quiver3(pos(1:4:end,1),pos(1:4:end,2),pos(1:4:end,3),m(1:4:end,1),m(1:4:end,2),m(1:4:end,3));
% qcp = quiver3(pos(1:3:end,1),pos(1:3:end,2),pos(1:3:end,3),m(1:3:end,1),m(1:3:end,2),m(1:3:end,3));
qcp = quiver3(xxc(:),yyc(:),zzc(:),mx(:),my(:),mz(:));

%// Compute the magnitude of the vectors
mags = sqrt(sum(cat(2, qcp.UData(:), qcp.VData(:), ...
            reshape(qcp.WData, numel(qcp.UData), [])).^2, 2));

%// Get the current colormap
currentColormap = colormap(gca);

%// Now determine the color to make each arrow using a colormap
[~, ~, ind] = histcounts(mags, size(currentColormap, 1));

%// Now map this to a colormap to get RGB
cmap = uint8(ind2rgb(ind(:), currentColormap) * 255);
cmap(:,:,4) = 255;
cmap = permute(repmat(cmap, [1 3 1]), [2 1 3]);

%// We repeat each color 3[666.7754 2.0313e+03 2.4064e+03] times (using 1:3 below) 
% because each arrow has 3 vertices
set(qcp.Head, 'ColorBinding', 'interpolated', 'ColorData', reshape(cmap(1:3,:,:), [], 4).');   %'

%// We repeat each color 2 times (using 1:2 below) because each tail has 2 vertices
set(qcp.Tail, 'ColorBinding', 'interpolated', 'ColorData', reshape(cmap(1:2,:,:), [], 4).');
% Colorbar of the inside sphere is messing with the plot colorbar
% hold on
% [Xsp,Ysp,Zsp] = sphere(n);
% hsp = surf(R*0.95*Xsp,R*0.95*Ysp,R*0.95*Zsp);
% set(hsp,'edgecolor','none','facecolor',[.3 .3 .3]);
% colormap bone; shading interp;
axis equal; colorbar;
xlabel('X'); ylabel('Y'); zlabel('Z');

%

m_sph=zeros(numel(mx),3);
for ii=1:numel(mx)
    m_sph(ii,:) = cart2sphvec([mx(ii);my(ii);mz(ii)],atan2d(yyc(ii),xxc(ii)),...
        asind(zzc(ii)/rrc(ii)));
end

%

%% Averaging (How to handle arbitrary spin? Weights?)
% Azimuthal averaging - average over phi
nt=n-1; np=n-1;
tmp = 0;
m_sphr = reshape(m_sph(:,3),size(xxc));
m_sphth = reshape(m_sph(:,2),size(xxc));
m_sphp = reshape(m_sph(:,1),size(xxc));

mra = repmat(mean(m_sphr,3),[1,1,size(m_sphr,3)]);
% mra = m_sphr, try mean along 2nd dim, [1, size(m_sphr,2), 1]
mtha = repmat(mean(m_sphth,3),[1,1,size(m_sphth,3)]);
mpa = repmat(mean(m_sphp,3),[1,1,size(m_sphp,3)]);

% mra = repmat(mean(m_sphr,2),[1, size(m_sphr,2), 1]);
% mtha = repmat(mean(m_sphth,2),[1, size(m_sphr,2), 1]);
% mpa = repmat(mean(m_sphp,2),[1, size(m_sphr,2), 1]);

%

m_c=zeros(numel(mx),3);
for ii=1:numel(mx)
%     m_c(ii,:) = sph2cartvec([m_sph(ii,1);m_sph(ii,2);m_sph(ii,3)],atan2d(yyc(ii),xxc(ii)),...
%         asind(zzc(ii)/rrc(ii)));
    m_c(ii,:) = sph2cartvec([mpa(ii);mtha(ii);mra(ii)],atan2d(yyc(ii),xxc(ii)),...
        asind(zzc(ii)/rrc(ii)));
end

%

figure,
surf(reshape(xxc,[100 100]),reshape(yyc,[100 100]),...
    reshape(zzc,[100 100]),reshape(mpa,[100 100]));%reshape(m_c(:,1),size(xxc)));
shading interp;
axis equal; colorbar;
xlabel('X'); ylabel('Y'); zlabel('Z');

%

figure,
surf(reshape(xxc,[100 100]),reshape(yyc,[100 100]),...
    reshape(zzc,[100 100]),reshape(m_c(:,1),[100 100]));%reshape(m_c(:,1),size(xxc)));
shading interp;
axis equal; colorbar;

figure,
surf(reshape(xxc,[100 100]),reshape(yyc,[100 100]),...
    reshape(zzc,[100 100]),reshape(m_c(:,2),[100 100]));%reshape(m_c(:,1),size(xxc)));
shading interp;
axis equal; colorbar;

figure,
surf(reshape(xxc,[100 100]),reshape(yyc,[100 100]),...
    reshape(zzc,[100 100]),reshape(m_c(:,3),[100 100]));%reshape(m_c(:,1),size(xxc)));
shading interp;
axis equal; colorbar;

%

figure,
% qcp = quiver3(pos(1:4:end,1),pos(1:4:end,2),pos(1:4:end,3),m(1:4:end,1),m(1:4:end,2),m(1:4:end,3));
% qcp = quiver3(pos(1:3:end,1),pos(1:3:end,2),pos(1:3:end,3),m(1:3:end,1),m(1:3:end,2),m(1:3:end,3));
qcp = quiver3(xxc(:),yyc(:),zzc(:),m_c(:,1),m_c(:,2),m_c(:,3));

%// Compute the magnitude of the vectors
mags = sqrt(sum(cat(2, qcp.UData(:), qcp.VData(:), ...
            reshape(qcp.WData, numel(qcp.UData), [])).^2, 2));

%// Get the current colormap
currentColormap = colormap(gca);

%// Now determine the color to make each arrow using a colormap
[~, ~, ind] = histcounts(mags, size(currentColormap, 1));

%// Now map this to a colormap to get RGB
cmap = uint8(ind2rgb(ind(:), currentColormap) * 255);
cmap(:,:,4) = 255;
cmap = permute(repmat(cmap, [1 3 1]), [2 1 3]);
cb=colorbar;
% cb.Limits=([min(mags) max(mags)]);  
%// We repeat each color 3[666.7754 2.0313e+03 2.4064e+03] times (using 1:3 below) because each arrow has 3 vertices
set(qcp.Head, 'ColorBinding', 'interpolated', 'ColorData', reshape(cmap(1:3,:,:), [], 4).');   %'

%// We repeat each color 2 times (using 1:2 below) because each tail has 2 vertices
set(qcp.Tail, 'ColorBinding', 'interpolated', 'ColorData', reshape(cmap(1:2,:,:), [], 4).');
% hold on
% [Xsp,Ysp,Zsp] = sphere(n);
% hsp = surf(R*0.95*Xsp,R*0.95*Ysp,R*0.95*Zsp);
% set(hsp,'edgecolor','none','facecolor',[.3 .3 .3]);
% colormap bone; shading interp;
axis equal
xlabel('X'); ylabel('Y'); zlabel('Z');

%

%% Define the grid
npoints=151;
xx1=linspace(-2.5*R,2.5*R,npoints);
yy1=xx1;
zz1=xx1;
sz=length(xx1);
[X,Y,Z]=ndgrid(xx1,yy1,zz1);
%b=zeros(sz,sz,sz,3);  
mask=(X.^2+Y.^2+Z.^2)<(R+10)^2;     % dr=5km
pos_mesh = cat(2,X(:),Y(:),Z(:))'; 
vc=(-1/3)*(rr(2:end,1:end-1,1:end-1).^3-rr(1:end-1,1:end-1,1:end-1).^3).* ...
    (cos(tt(1:end-1,2:end,1:end-1))-cos(tt(1:end-1,1:end-1,1:end-1))).* ...
    (pp(1:end-1,1:end-1,2:end)-pp(1:end-1,1:end-1,1:end-1));    % Volume of cells
pos_rc = cat(2,xxc(:),yyc(:),zzc(:))';

%

bd1d=zeros(size(pos_mesh));
volc=vc(:);
for i=1:size(pos_rc,2) % Loop over dipoles
%     pos_rd(:,i)=[250;0;0];
%     m_rd(:,i)=[0;1e14;0];
    r=pos_mesh-pos_rc(:,i);   
    rm=sum(r.^2,1).^(1/2);
    bd1d=bd1d+(3*r.*sum(m_c(i,:)'.*r,1)./rm.^5-m_c(i,:)'./rm.^3).*volc(i);   
    %bd1d=bd1d+(3*r.*sum(m_rd(:,i).*r,1)./rm.^3-m_rd(:,i)./rm).*volc(i);
    %bd1d=r;    
end
% Take vector moments
% b = k*b;

%

% figure,
% pcolor(squeeze(X),squeeze(Z),squeeze(log(sqrt(sum(b(:,1,:,:).^2,4))))); colorbar;
% figure,
% pcolor(squeeze(X),squeeze(Z),squeeze(log(sqrt(sum(b(:,1,:,1:1).^2,4))))); colorbar;
% figure,
% pcolor(squeeze(X),squeeze(Z),squeeze(log(sqrt(sum(b(:,1,:,2:2).^2,4))))); colorbar;
% figure,
% pcolor(squeeze(X),squeeze(Z),squeeze(log(sqrt(sum(b(:,1,:,3:3).^2,4))))); colorbar;

%
%% BOV File

%%
writebov3dvector('B_45_0_e125_2.5R',min(pos_mesh,[],2), max(pos_mesh,[],2), 0, reshape(bd1d.',[sz,sz,sz,3]), 'B');

%

function writebov3dvector(name, lower, upper, tnow, data, varname)
    f=fopen([name,'.bov'],'w');
    datafile=[name,'.dat'];
    fprintf(f, 'TIME: %f\n', tnow);
    fprintf(f, 'DATA_FILE: %s\n', datafile);
    fprintf(f, 'DATA_SIZE: %d %d %d\n', size(data,1), size(data, 2), size(data,3));
    fprintf(f, 'DATA_FORMAT: DOUBLE\n');
    fprintf(f, 'VARIABLE: %s\n',varname);
    fprintf(f, 'DATA_ENDIAN: LITTLE\n');
    fprintf(f, 'CENTERING: zonal\n');
    fprintf(f, 'BRICK_ORIGIN: %e %e %e\n',lower);
    fprintf(f, 'BRICK_SIZE: %e %E %e\n', upper-lower);
    fprintf(f, 'BYTE_OFFSET: 0\n');
    fprintf(f, 'DATA_COMPONENTS: %d', size(data, 4));
    fclose(f);
    f=fopen(datafile, 'wb');
    fwrite(f, permute(data,[4,1,2,3]),'double');
    fclose(f);
end