% N = 8901;       % Number of elements, maybe take as input from file?
addpath '/gpfs/fs1/home/aanand6/Documents/MATLAB/psyche'
% We are currently excluding the normalization to the dipole moment of each
% dipole element/cell
R = 111; % 125
n = 101;
a_e = [111, 111*0.9]; %[125; 125*0.9];
b_e = [111, 111*0.9]; %[85; 85*0.9];
theta_=0; %0.9553166181245d0*180/pi; 
phi_=0; %0.9553166181245d0*180/pi; 

% [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:length(rs)
    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=rs(1:end-1); % 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:length(rs)-1
    rrc(ii,:,:)=0.25*(rr(2:end,2:end,2:end).^4-rr(1:end-1,2:end,2:end).^4)./ ...
        ((1.0/3.0)*(rr(2:end,2:end,2:end).^3-rr(1:end-1,2:end,2:end).^3));
end
% All angles 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));

%

%% Calculate dipole field at zone centers
pos_rd = cat(2,xxc(:),yyc(:),zzc(:))';  % Creates a 3xN array of zone center positions
AM = [0;0;1e14];            % Dipole moment of the core
r=pos_rd-[0;0;0];
rm = sum(r.^2,1).^(1/2);
m_rd=3*r.*sum(AM.*r,1)./rm.^5-AM./rm.^3;

%

% 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

%% Plot the dipole locations
% figure,
% % quiver3(pos_rd(1,1:1:end),pos_rd(2,1:1:end),pos_rd(3,1:1:end),m_rd(1,1:1:end), ...
% %     m_rd(2,1:1:end),m_rd(3,1:1:end));
% scatter3(pos_rd(1,1:1:end),pos_rd(2,1:1:end),pos_rd(3,1:1:end),3,volc(1:1:end));
% axis equal;
% xlabel('X'); ylabel('Y'); zlabel('Z');

%

%% Define the grid
npoints=101;
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+4*dr)^2;
r2=0;
pos_mesh = cat(2,X(:),Y(:),Z(:))'; 
%% So we have two 3D meshes - and we want to calculate the position vector between each pair
%dx=xx1(:)-pos_rd(1,:);
%dy=yy1(:)-pos_rd(2,:);
%dz=zz1(:)-pos_rd(3,:);
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
volc=vc(:);

%

%% Calculate the multiple dipole field
bd1d=zeros(size(pos_mesh));
for i=1:size(pos_rd,2) % Loop over dipoles
%     pos_rd(:,i)=[250;0;0];
%     m_rd(:,i)=[0;1e14;0];
    r=pos_mesh-pos_rd(:,i);   
    rm=sum(r.^2,1).^(1/2);
    fc = 1;
    if ttc(i)<(pi/2+0.1) && ttc(i)>(pi/2-0.1)
        fc = 2;
    end
    bd1d=bd1d+fc*(3*r.*sum(m_rd(:,i).*r,1)./rm.^5-m_rd(:,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
% bd1d(:,mask)=0;

%


%%
writebov3dvector('BD_circ111',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

