# Methodology of Rebonato and Jackel (2000) to create a
# positive definite matrix out of a non-positive definite matrix.
# Reference: Rebonato and Jackel, "The most general methodology for creating a
# valid correlation matrix for risk management and option pricing purposes",
# Journal of Risk, Vol 2, No 2, 2000
fixNonPositiveDefiniteMatrix <- function(origMat) {
    cholStatus <- try(u <- chol(origMat), silent = TRUE)
    cholError <- ifelse(class(cholStatus) == "try-error", TRUE, FALSE)
    newMat <- origMat

    iter <- 0
    while (cholError) {

        iter <- iter + 1
        cat("iteration ", iter, "\n")

        # replace -ve eigen values with small +ve number
        newEig <- eigen(newMat)
        newEig2 <- ifelse(newEig$values < 0, 1e-10, newEig$values)

        # create modified matrix eqn 5 from Brissette et al 2007,
		# inv = transp for eig vectors
        newMat <- newEig$vectors %*% diag(newEig2) %*% t(newEig$vectors)

        # normalize modified matrix eqn 6 from Brissette et al 2007
        newMat <- newMat #/sqrt(diag(newMat) %*% t(diag(newMat)))

        # try chol again
        cholStatus <- try(u <- chol(newMat), silent = TRUE)
        cholError <- ifelse(class(cholStatus) == "try-error", TRUE, FALSE)
    }
    newMat
}

library(kinship2)
getKinshipList <- function(pedigrees) {
  famids = unique(pedigrees$famid)

  Phi_list <- list()
  for (famid in famids) {
    ped <- pedigrees[pedigrees$famid == famid,]
    Phi_list[[famid]] <- kinship(id=ped$id, dadid=ped$dadid, momid=ped$momid, sex=ped$sex)
  }

  return(Phi_list)
}


library(MASS)
simulatePhenotypes <- function(Phi_list, fam.nf, phen.p, phen.mu0, phen.Sigma_g, phen.Sigma_e) {
  fen_df <- data.frame()
  for (f in 1:length(fam.nf)) {
    nf = fam.nf[f]
    EYf <- as.numeric(kronecker(rep(1, nf), phen.mu0)) # (nf.p) x 1
    CovYf <- kronecker(2*Phi_list[[f]], phen.Sigma_g) + kronecker(diag(1,nf),phen.Sigma_e) #(nf.p) x (nf.p)
    Sigma_f = fixNonPositiveDefiniteMatrix(CovYf)
    fen_f <- mvrnorm(1, mu=EYf, Sigma=Sigma_f)
    fen_f <- matrix(fen_f, byrow=T, ncol=phen.p)
    fen_df <- rbind(fen_df, fen_f)
  }

  colnames(fen_df) <- paste0("fen_", 1:phen.p)
  return(fen_df)
}

addPCLine <- function(PC, mu0_barra, out, color="black") {
  # reta do PC: y = ax+b, com a = PC_2/PC_1 e b = mu_2 - a mu_1
  slope_PC <- PC[2]/PC[1]
  intercept_PC <- mu0_barra[2] - slope_PC * mu0_barra[1]

  out <- out + geom_abline(intercept = intercept_PC, slope = slope_PC, colour=color)
  return(out)
}

addPCEllipse <- function(PC1, eigenvalue1, eigenvalue2, mu0_barra, out, gColor, color, alpha) {
  # General parametric form:
  # An ellipse in general position can be expressed parametrically 
  # as the path of a point (X(t),Y(t)), where

  # X(t)=X_{c}+a\,\cos t\,\cos \varphi -b\,\sin t\,\sin \varphi 
  # Y(t)=Y_{c}+a\,\cos t\,\sin \varphi +b\,\sin t\,\cos \varphi 

  # as the parameter t varies from 0 to 2\pi. Here \varphi is the angle between
  # the X-axis and the major axis of the ellipse.

  # This equation means an ellipse scaled by
  # a factor of "a" in the x direction and 
  # a factor of "b" in the y direction.

   # reta do PC: y = ax+b, com a = PC_2/PC_1 e b = mu_2 - a mu_1
  slope_PC1 <- PC1[2]/PC1[1]
  varphi <- atan(slope_PC1) 

  a <- (eigenvalue1)/2
  b <- (eigenvalue2)/2
  Xc <- mu0_barra[1]
  Yc <- mu0_barra[2]

  t <- seq(0,2*pi,length.out=1000)

  Xt <- Xc + a * cos(t) * cos(varphi) - b * sin(t) * sin(varphi)
  Yt <- Yc + a * cos(t) * sin(varphi) + b * sin(t) * cos(varphi)
  colors <- rep(gColor, length(t))

  datapoly <- data.frame(x=Xt, y=Yt, group=colors)

  if (is.na(color))
    out <- out + geom_polygon(data=datapoly, mapping=aes(x=x, y=y, colour=group), alpha=alpha)
  else
    out <- out + geom_polygon(data=datapoly, mapping=aes(x=x, y=y, colour=group), fill = color, colour = color, alpha=alpha)

  return(out)
}

PCbiplot <- function(scores1, scores2, loadingsPC1, loadingsPC2, obsnames, varnames) {
	div <- max(abs(scale(scores1)), abs(scale(scores2)))*(1/max(loadingsPC1, loadingsPC2))
	data <- data.frame(scores1=scale(scores1)/div, scores2=scale(scores2)/div)
	out <- ggplot(data=data, aes(x=scores1, y=scores2)) + geom_text(alpha=.4, size=3, aes(label=obsnames))
	datapc <- data.frame(loadingsPC1=loadingsPC1, loadingsPC2=loadingsPC2)
	rownames(datapc) <- varnames
	out <- out + geom_segment(data=datapc, aes(x=0, y=0, xend=loadingsPC1, yend=loadingsPC2), arrow=arrow(length=unit(0.2,"cm")), alpha=0.75, color="red")
	out <- out + coord_equal() + geom_text(data=datapc, aes(x=loadingsPC1, y=loadingsPC2, label=varnames), size = 3, vjust=1, color="red")
	return(out)
}