# Single Trait Animal Model - Date : 2020-11-11

# Reliability, Accuracy and Standard Error Prediction

# Partitioning Breeding Values (PA, PC, YD)

# Linear Models for the Prediction of Animal Breeding Values, Raphael Mrode, 3rd Edition

#Julius Mugambe 



# install.packages('MASS', dependencies = TRUE)
library(MASS)

# make design matrix 
desgn <- function(v) {
  if (is.numeric(v)) {
    va = v
    mrow = length(va)
    mcol = max(va)
  }
  if (is.character(v)) {
    vf = factor(v)
    # Save the index value of each level
    va = as.numeric(vf)
    mrow = length(va)
    mcol = length(levels(vf))
  }
  
  # Initializing X-matrix
  X = matrix(data = c(0), nrow = mrow, ncol = mcol)
  
  for (i in 1:mrow) {
    ic = va[i]
    X[i, ic] = 1
  }
  return(X)
}

# function to make inverse of numerator relationship matrix
ainv = function(ped) {
  n = nrow(ped)
  Ainv = matrix(c(0), nrow = n, ncol = n)
  
  for (i in 1:n) {
    animal = ped[i, 1]
    sire = ped[i, 2]
    dam = ped[i, 3]
    
    if (sire == 0 & dam == 0) {
      # both parents unknown
      alpha = 1
      Ainv[animal, animal] = alpha + Ainv[animal, animal]
    } else if (sire != 0 & dam == 0) {
      # sire known
      alpha = 4/3
      Ainv[animal, animal] = alpha + Ainv[animal, animal]
      Ainv[animal, sire] = -alpha/2 + Ainv[animal, sire]
      Ainv[sire, animal] = -alpha/2 + Ainv[sire, animal]
      Ainv[sire, sire] = alpha/4 + Ainv[sire, sire]
    } else if (sire == 0 & dam != 0) {
      # dam known
      alpha = 4/3
      Ainv[animal, animal] = alpha + Ainv[animal, animal]
      Ainv[animal, dam] = -alpha/2 + Ainv[animal, dam]
      Ainv[dam, animal] = -alpha/2 + Ainv[dam, animal]
      Ainv[dam, dam] = alpha/4 + Ainv[dam, dam]
    } else {
      # both parents known
      alpha = 2
      Ainv[animal, animal] = alpha + Ainv[animal, animal]
      Ainv[animal, sire] = -alpha/2 + Ainv[animal, sire]
      Ainv[sire, animal] = -alpha/2 + Ainv[sire, animal]
      Ainv[animal, dam] = -alpha/2 + Ainv[animal, dam]
      Ainv[dam, animal] = -alpha/2 + Ainv[dam, animal]
      Ainv[sire, sire] = alpha/4 + Ainv[sire, sire]
      Ainv[sire, dam] = alpha/4 + Ainv[sire, dam]
      Ainv[dam, sire] = alpha/4 + Ainv[dam, sire]
      Ainv[dam, dam] = alpha/4 + Ainv[dam, dam]
    }
  }
  return(Ainv)
}