function [LFM,R,Ch]=CalcLFM(P,MEG2HEAD,ChInform,Center,vm);
wm=1; // weight ratio of magnetometer/gradiometer
Ch=ConversionCoordination(MEG2HEAD,ChInform);
R=CalcTangentialVectors(P,Center);
LFM=CalcVectorViewLFM(R,Ch,Center,wm);

function LFM=CalcVectorViewLFM(R,Ch,Center,wm);
N=size(R.P,1);
LFM=zeros(N*2,306);
P=R.P;
V2=R.V2;
V3=R.V3;
Q1=[P,V2];
Q2=[P,V3];
C1=3:3:306;
C2=1:3:306;
C3=2:3:306;
P1=Ch.P(:,C2)+8.4/1000*Ch.X(:,C2)+0.3/1000*Ch.Z(:,C2);
P2=Ch.P(:,C2)-8.4/1000*Ch.X(:,C2)+0.3/1000*Ch.Z(:,C2);
P3=Ch.P(:,C3)+8.4/1000*Ch.X(:,C3)+0.3/1000*Ch.Z(:,C3);
P4=Ch.P(:,C3)-8.4/1000*Ch.X(:,C3)+0.3/1000*Ch.Z(:,C3);
P5=Ch.P(:,C1)+12.9/1000*Ch.X(:,C1)+12.9/1000*Ch.Y(:,C1)+0.3/1000*Ch.Z(:,C1);
P6=Ch.P(:,C1)+12.9/1000*Ch.X(:,C1)-12.9/1000*Ch.Y(:,C1)+0.3/1000*Ch.Z(:,C1);
P7=Ch.P(:,C1)-12.9/1000*Ch.X(:,C1)+12.9/1000*Ch.Y(:,C1)+0.3/1000*Ch.Z(:,C1);
P8=Ch.P(:,C1)-12.9/1000*Ch.X(:,C1)-12.9/1000*Ch.Y(:,C1)+0.3/1000*Ch.Z(:,C1);
PP=[P1,P2,P3,P4,P5,P6,P7,P8];
Z=Ch.Z;
Z=[Z(:,C2),Z(:,C2),Z(:,C3),Z(:,C3),Z(:,C1),Z(:,C1),Z(:,C1),Z(:,C1)];
wd=1/1.68; // weight of gradiometers
for x=1:N;
    LQ1=sum(Sarvas(Q1(x,:),PP',Center).*Z',2)';
    LQ2=sum(Sarvas(Q2(x,:),PP',Center).*Z',2)';
    // gradiometers
    L12=wd*(LQ1(:,1:102)-LQ1(:,103:204));
    L22=wd*(LQ2(:,1:102)-LQ2(:,103:204));
    L13=wd*(LQ1(:,205:306)-LQ1(:,307:408));
    L23=wd*(LQ2(:,205:306)-LQ2(:,307:408));
    // magnetometers
    L11=wm*(LQ1(:,409:510)+LQ1(:,511:612)+LQ1(:,613:714)+LQ1(:,715:816))/4;
    L21=wm*(LQ2(:,409:510)+LQ2(:,511:612)+LQ2(:,613:714)+LQ2(:,715:816))/4;
    LFM(x,:) =[L11,L12,L13];
    LFM(x+N,:)=[L21,L22,L23];
end;
LFM=LFM'*1e+15;
M=zeros(1,306);M([C1,C2,C3])=1:306;
LFM=LFM(M,:);
endfunction;

function R=CalcTangentialVectors(P,Center);
// P:matrix consists of (N x 3) elements
// Center=[0,0,40]/1000;
N=size(P,1);
V1=P-ones(N,1)*Center;V1=V1';
V1=real(V1./(ones(3,1)*sum(V1.^2,1).^0.5));
SinPhi=V1(3,:);
CosPhi=real((1-SinPhi.^2).^0.5);
check=find(CosPhi==0);
CosPhi(check)=1;
CosTheta=V1(1,:)./CosPhi;
SinTheta=V1(2,:)./CosPhi;
V2=[-CosTheta.*SinPhi;-SinTheta.*SinPhi;CosPhi];
V2(:,check)=[1,0,0]'*ones(1,length(check));
V2=V2./(ones(3,1)*sum(V2.^2,1));
V3=[V1(2,:).*V2(3,:)-V1(3,:).*V2(2,:);...
    V1(3,:).*V2(1,:)-V1(1,:).*V2(3,:);...
    V1(1,:).*V2(2,:)-V1(2,:).*V2(1,:)];
V3(:,check)=[0,1,0]'*ones(1,length(check));
V3=V3./(ones(3,1)*sum(V2.^2,1));
R.P=P;   //location
R.V1=V1';//radial vector
R.V2=V2';//tantential vector (phi)
R.V3=V3';//tangential vector (radial x phi)
endfunction;

function Ch=ConversionCoordination(MEG2HEAD,ChInform);
MTX=MEG2HEAD(1:3,1:3);
Ch.P=MTX*ChInform(1:3,1:306)+MEG2HEAD(1:3,4)*ones(1,306);
Ch.X=MTX*ChInform(4:6,1:306);
Ch.Y=MTX*ChInform(7:9,1:306);
Ch.Z=MTX*ChInform(10:12,1:306);
endfunction;

function B=Sarvas(Q,S,p);
//Q = (Nq,[x,y,z,qx,qy,qz]);//dipole position & moment
//S = (Ns,[x,y,z]);         //Sensor position
//p = [x,y,z];              //sphere center
Ns=size(S,1);
if size(Q,1)==1;
    Q=ones(Ns,1)*Q;
end;
P=ones(Ns,1)*p;
R=S-P;
R0=Q(:,1:3)-P;
A=R-R0;
r=real(sum(R.^2,2).^0.5);
r0=real(sum(R0.^2,2).^0.5);
a=real(sum(A.^2,2).^0.5);
ar=sum(A.*R,2);
F=a.*(r.*a+ar);
n=a+2*r+ar./a;
nn=(a.^2)./r+a+n;
n=n*ones(1,3);
nn=nn*ones(1,3);
NablaF=nn.*R-n.*R0;//vector!
Q=Q(:,4:6);
QR0=zeros(Q);
QR0(:,1)=Q(:,2).*R0(:,3)-Q(:,3).*R0(:,2);
QR0(:,2)=Q(:,3).*R0(:,1)-Q(:,1).*R0(:,3);
QR0(:,3)=Q(:,1).*R0(:,2)-Q(:,2).*R0(:,1);
B=zeros(Q);
F=F*ones(1,3);
QR0R=sum(QR0.*R,2)*ones(1,3);//vector!
B=QR0-(QR0R.*NablaF)./F;
B=real(B./F*1e-7);
endfunction;


