clear
format compact
tic
Par = csvread('GParameter.csv');
Ped = csvread('Pedigree&Record.csv');
nt = size(Par,1);
na = size(Ped,1);
k0 = Par(:,1); WVP = Par(:,2); GP = Par(:,3:2+nt);
for i=1:nt
  for j=1:nt
    if i==j
      G0(i,i) = GP(i,i); R0(i,i) = WVP(i)-G0(i,i);
    elseif i>=j
      G0(i,j) = GP(i,j); G0(j,i) = G0(i,j);
    elseif i<j
      R0(i,j) = GP(i,j); R0(j,i) = R0(i,j);
    end
  end
end
P0 = G0+R0; P0I = inv(P0);
id0 = Ped(:,1); id1 = Ped(:,2); id2 = Ped(:,3);
Cand = Ped(:,4); Gend = Ped(:,5);
for i=1:na
  wkid = id1(i);
  wkid(wkid~=id1(i)) = 0;
  id1(i) = sum(wkid);
  wkid = id2(i);
  wkid(wkid~=id2(i)) = 0;
  id2(i) = sum(wkid);
end
base1 = id1==0; base2 = id2==0;
nb = min(sum(base1==1),sum(base2==1));
A0 = eye(na);
for i=nb+1:na
  if (base1(i)==1)
    A0(i,1:i-1) = A0(id2(i),1:i-1)/2;
    A0(1:i-1,i) = A0(i,1:i-1);
  elseif (base2(i)==1)
    A0(i,1:i-1) = A0(id1(i),1:i-1)/2;
    A0(1:i-1,i) = A0(i,1:i-1);
  else
    A0(i,i) = 1+A0(id1(i),id2(i))/2;
    A0(i,1:i-1) = (A0(id1(i),1:i-1)+A0(id2(i),1:i-1))/2;
    A0(1:i-1,i) = A0(i,1:i-1);
  end
end
for i=1:nt
  Nonr(i,:) = Ped(:,5+i);
end
ZZ = zeros(sum(sum(Nonr)),na*nt);
no = 0;
for i=1:nt
  for j=1:na
    if Nonr(i,j)==1
      no = no+1;
      ZZ(no,(i-1)*na+j) = 1;
    end
  end
end
GG = kron(G0,A0);
RR = kron(R0,eye(na));
PP = ZZ*(GG+RR)*ZZ';
PPI0 = ZZ'*inv(PP)*ZZ;
vv1 = 0; vv2 = 0; ns1 = 0; ns2 = 0;
for i=1:na
  if Cand(i)==1
    WGA = kron(G0,A0(i,:));
    LHS = WGA*PPI0*WGA'; RHS = k0;
    vv = LHS\RHS;
    if Gend(i)==1
      vv1 = vv1+vv;
      ns1 = ns1+1;
    else
      vv2 = vv2+vv;
      ns2 = ns2+1;
    end
  end
end
v0 = inv(G0'*P0I*G0)*k0;
vv1 =vv1/ns1;
vv2 = vv2/ns2;
vv0 = (vv1+vv2)/2;
for i=1:nt
  if vv0(i)~=0&&v0(i)~=0
    ratio = v0(i)/vv0(i);
    v0 = v0/ratio;
    break
  end
end
nanimal = na
nrecord = sum(sum(Ped(:,6:6+nt-1)))
vv = [vv1 vv2 vv0 v0]
toc
csvwrite('Weighted_value.csv',vv)
