# list of functions


#' The bivariate von Mises cosine model
#' @inheritParams rvmsin
#' @param mu1,mu2 vectors of mean parameters.
#' @param kappa1,kappa2,kappa3 vectors of concentration parameters; \code{kappa1, kappa2 > 0}.
#' @param ... additional arguments to be passed to dvmcos. See details.
#'
#' @details
#' The bivariate von Mises cosine model density at the point \eqn{x = (x_1, x_2)} is given by
#' \deqn{f(x) = C_c (\kappa_1, \kappa_2, \kappa_3) \exp(\kappa_1 \cos(T_1) + \kappa_2 \cos(T_2) + \kappa_3 \cos(T_1 - T_2))}
#' where
#' \deqn{T_1 = x_1 - \mu_1;  T_2 = x_2 - \mu_2}
#' and \eqn{C_c (\kappa_1, \kappa_2, \kappa_3)} denotes the normalizing constant for the cosine model.
#'
#' Because \eqn{C_c} involves an infinite  alternating series with product of Bessel functions,
#' if \code{kappa3 < -5} or \code{max(kappa1, kappa2, abs(kappa3)) > 50}, \eqn{C_c} is evaluated
#' numerically via (quasi) Monte carlo method for
#' numerical stability. These (quasi) random numbers can be provided through the
#' argument \code{qrnd}, which must be a two column matrix, with each element being
#' a  (quasi) random number between 0 and 1. Alternatively, if \code{n_qrnd} is
#' provided (and \code{qrnd} is missing), a two dimensional sobol sequence of size \code{n_qrnd} is
#' generated via the function \link[qrng]{sobol} from the R package \code{qrng}. If none of \code{qrnd}
#' or \code{n_qrnd} is available, a two dimensional sobol sequence of size 1e4 is used. By default Monte
#' Carlo approximation is used only if \code{kappa3 < -5} or \code{max(kappa1, kappa2, abs(kappa3)) > 50}.
#' However, a forced Monte Carlo approximation can be made (irrespective of the choice of \code{kappa1, kappa2} and
#' \code{kappa3}) by setting \code{force_approx_const = TRUE}. See examples.
#'
#' @return \code{dvmcos} gives the density  and \code{rvmcos} generates random deviates.
#'
#' @examples
#' kappa1 <- c(1, 2, 3)
#' kappa2 <- c(1, 6, 5)
#' kappa3 <- c(0, 1, 2)
#' mu1 <- c(1, 2, 5)
#' mu2 <- c(0, 1, 3)
#' x <- diag(2, 2)
#' n <- 10
#'
#' # when x is a bivariate vector and parameters are all scalars,
#' # dvmcos returns single density
#' dvmcos(x[1, ], kappa1[1], kappa2[1], kappa3[1], mu1[1], mu2[1])
#'
#' # when x is a two column matrix and parameters are all scalars,
#' # dmvsin returns a vector of densities calculated at the rows of
#' # x with the same parameters
#' dvmcos(x, kappa1[1], kappa2[1], kappa3[1], mu1[1], mu2[1])
#'
#' # if x is a bivariate vector and at least one of the parameters is
#' # a vector, all parameters are recycled to the same length, and
#' # dvmcos returns a vector with ith element being the density
#' # evaluated at x with parameter values kappa1[i], kappa2[i],
#' # kappa3[i], mu1[i] and mu2[i]
#' dvmcos(x[1, ], kappa1, kappa2, kappa3, mu1, mu2)
#'
#' # if x is a two column matrix and at least one of the parameters is
#' # a vector, rows of x and the parameters are recycled to the same
#' # length, and dvmcos returns a vector with ith element being the
#' # density evaluated at ith row of x with parameter values kappa1[i],
#' # kappa2[i], # kappa3[i], mu1[i] and mu2[i]
#' dvmcos(x, kappa1, kappa2, kappa3, mu1, mu2)
#'
#' # when parameters are all scalars, number of observations generated
#' # by rvmcos is n
#' rvmcos(n, kappa1[1], kappa2[1], kappa3[1], mu1[1], mu2[1])
#'
#' # when at least one of the parameters is a vector, all parameters are
#' # recycled to the same length, n is ignored, and the number of
#' # observations generated by rvmcos is the same as the length of the
#' # recycled vectors
#' rvmcos(n, kappa1, kappa2, kappa3, mu1, mu2)
#'
#'
#' \donttest{
#' ## Visualizing (quasi) Monte Carlo based approximations of
#' ## the normalizing constant through density evaluations.
#'
#' # "good" setup, where the analytic formula for C_c can be
#' # calculated without numerical issues
#' # kappa1 = 1, kappa2 = 1, kappa3 = -2, mu1 = pi, mu2 = pi
#'
#' n_qrnd <-  (1:500)*20
#' # analytic
#' good.a <- dvmcos(c(3,3), 1, 1, -2, pi, pi, log=TRUE)
#' # using quasi Monte Carlo
#' good.q <- sapply(n_qrnd,
#'                  function(j)
#'                    dvmcos(c(3,3), 1, 1, -2, pi, pi,
#'                           log=TRUE, n_qrnd = j,
#'                           force_approx_const = TRUE))
#' # using ordinary Monte Carlo
#' set.seed(1)
#' good.r <- sapply(n_qrnd,
#'                  function(j)
#'                    dvmcos(c(3,3), 1, 1, -2, pi, pi,
#'                           log=TRUE,
#'                           qrnd = matrix(runif(2*j), ncol = 2),
#'                           force_approx_const = TRUE))
#'
#'
#' plot(n_qrnd, good.q, ylim = range(good.a, good.q, good.r),
#'      col = "orange", type = "l",
#'      ylab = "",
#'      main = "dvmcos(c(3,3), 1, 1, -2, pi, pi, log = TRUE)")
#' points(n_qrnd, good.r, col = "skyblue", type = "l")
#' abline(h = good.a, lty = 2, col = "grey")
#' legend("topright",
#'        legend = c("Sobol", "Random", "Analytic"),
#'        col = c("orange", "skyblue", "grey"),
#'        lty = c(1, 1, 2))
#'
#'
#' # "bad" setup, where the calculating C_c
#' # numerically using the analytic formula is problematic
#' # kappa1 = 100, kappa2 = 100, kappa3 = -200, mu1 = pi, mu2 = pi
#'
#' n_qrnd <-  (1:500)*20
#'
#' # using quasi Monte Carlo
#' bad.q <- sapply(n_qrnd,
#'                 function(j)
#'                   dvmcos(c(3,3), 100, 100, -200, pi, pi,
#'                          log=TRUE, n_qrnd = j,
#'                          force_approx_const = TRUE))
#' # using ordinary Monte Carlo
#' set.seed(1)
#' bad.r <- sapply(n_qrnd,
#'                 function(j)
#'                   dvmcos(c(3,3), 100, 100, -200, pi, pi,
#'                          log=TRUE,
#'                          qrnd = matrix(runif(2*j), ncol = 2),
#'                          force_approx_const = TRUE))
#'
#'
#' plot(n_qrnd, bad.q, ylim = range(bad.q, bad.r),
#'      col = "orange", type = "l",
#'      ylab = "",
#'      main = "dvmcos(c(3,3), 100, 100, -200, pi, pi, log = TRUE)")
#' points(n_qrnd, bad.r, col = "skyblue", type = "l")
#' legend("topright",
#'        legend = c("Sobol", "Random"),
#'        col = c("orange", "skyblue"), lty = 1)
#'}
#'
#' @export
rvmcos <- function(n, kappa1=1, kappa2=1,
                   kappa3=0, mu1=0, mu2=0,
                   method="naive")
{
  if(any(c(kappa1, kappa2) < 0)) stop("kappa1 and kappa2 must be nonnegative")
  if(any(mu1 < 0 | mu1 >= 2*pi)) mu1 <- prncp_reg(mu1)
  if(any(mu2 < 0 | mu2 >= 2*pi)) mu2 <- prncp_reg(mu2)
  if(n < 0) stop("invalid n")


  # if (is.null(method)) {
  #   if (n > 1e5) method <- "vmprop"
  #   else method <- "naive"
  # }

  if (!method %in% c("naive", "vmprop"))
    stop("method must be either \'naive\' or \'vmprop\'")

  if(max(length(kappa1), length(kappa2), length(kappa3),
         length(mu1), length(mu2)) > 1) {
    expanded <- expand_args(kappa1, kappa2, kappa3, mu1, mu2)
    k1 <- expanded[[1]]; k2 <- expanded[[2]]; k3 <- expanded[[3]]
    mu1 <- expanded[[4]]; mu2 <- expanded[[5]]

    t(vapply(1:length(k1),
             function(j) rvmcos_1par(1, k1[j], k2[j], k3[j],
                                     mu1[j], mu2[j], "naive"), c(0, 0)))
  } else {
    # if (is.null(method) & max(kappa1, kappa2, abs(kappa3)) < 0.1)
    #   method <- "vmprop"

    rvmcos_1par(n, kappa1, kappa2, kappa3, mu1, mu2, method)
  }

}


# rvmcos <- function(n, kappa1=1, kappa2=1, kappa3=0,
#                    mu1=0, mu2=0) {
#   if(any(c(kappa1, kappa2) < 0)) stop("kappa1 and kappa2 must be nonnegative")
#   if(any(mu1 < 0 | mu1 >= 2*pi)) mu1 <- prncp_reg(mu1)
#   if(any(mu2 < 0 | mu2 >= 2*pi)) mu2 <- prncp_reg(mu2)
#
#   opt_obj <- function(k1=1, k2=1, k3=0, mu1=0, mu2=0) {
#     # for numerical stability, if k3 < 0, fabs(k1+k3) < 1e-5 or fabs(k2+k3) < 1e-5
#     # make k3 = k3 * (1+1e-5)
#     if (k3 < 0) {
#       while (abs(k1 + k3) < 1e-5 || abs(k2 + k3) < 1e-5) {
#         k3 = k3*(1+1e-5)
#       }
#     }
#     obj <- optim(c(0,0), fn = function(x) -(k1*cos(x[1]-mu1)+k2*cos(x[2]-mu2)+k3*cos(x[1]-x[2]-mu1+mu2)),
#                  gr = function(x)  -c(-k1*sin(x[1]-mu1)-k3*sin(x[1]-x[2]-mu1+mu2),
#                                       -k2*sin(x[2]-mu2)+k3*sin(x[1]-x[2]-mu1+mu2)))
#     -obj$value
#
#   }
#
#   if(max(length(kappa1), length(kappa2), length(kappa3), length(mu1), length(mu2)) > 1) {
#     expanded <- expand_args(kappa1, kappa2, kappa3, mu1, mu2)
#     k1 <- expanded[[1]]; k2 <- expanded[[2]]; k3 <- expanded[[3]]
#     mu1 <- expanded[[4]]; mu2 <- expanded[[5]]
#     upper_bd_all <- vapply(1:length(k1),
#                            function(h) opt_obj(k1[h], k2[h], k3[h], mu1[h], mu2[h]),
#                            0)
#     rcos_manypar(k1, k2, k3, mu1, mu2, upper_bd_all)
#   } else {
#     upper_bd <- opt_obj(kappa1, kappa2, kappa3, mu1, mu2)
#     qrnd_grid <- sobol(1e4, 2, FALSE)
#     cat(exp(log(const_vmcos(kappa1, kappa2, kappa3,
#                             qrnd_grid))
#             - upper_bd)/(4*pi^2))
#     rcos_onepar(n, kappa1, kappa2, kappa3, mu1, mu2, upper_bd)
#   }
# }




#' @rdname rvmcos
#' @importFrom qrng sobol
#' @export

dvmcos <- function(x, kappa1=1, kappa2=1, kappa3=0, mu1=0,
                   mu2=0, log=FALSE, ...) {

  if (any(c(kappa1, kappa2) < 0)) stop("kappa1 and kappa2 must be nonnegative")
  if (any(mu1 < 0 | mu1 >= 2*pi)) mu1 <- prncp_reg(mu1)
  if (any(mu2 < 0 | mu2 >= 2*pi)) mu2 <- prncp_reg(mu2)
  if ((length(dim(x)) < 2 && length(x) != 2) || (length(dim(x)) == 2 && tail(dim(x), 1) != 2)
      || (length(dim(x)) > 2)) stop("x must either be a bivariate vector or a two-column matrix")

  ell <- list(...)

  if (!is.null(ell$qrnd)) {
    qrnd_grid <- ell$qrnd
    dim_qrnd <- dim(qrnd_grid)
    if (!is.matrix(qrnd_grid) | is.null(dim_qrnd) |
        dim_qrnd[2] != 2)
      stop("\'qrnd\' must be a two column matrix")
    n_qrnd <- dim_qrnd[1]
  } else if (!is.null(ell$n_qrnd)){
    n_qrnd <- round(ell$n_qrnd)
    if (n_qrnd < 1)
      stop("n_qrnd must be a positive integer")
    qrnd_grid <- sobol(n_qrnd, 2, FALSE)
  } else {
    n_qrnd <- 1e4
    qrnd_grid <- sobol(n_qrnd, 2, FALSE)
  }


  if(max(length(kappa1), length(kappa2), length(kappa3), length(mu1), length(mu2)) > 1) {
    expanded <- expand_args(kappa1, kappa2, kappa3, mu1, mu2)
    kappa1 <- expanded[[1]]
    kappa2 <- expanded[[2]]
    kappa3 <- expanded[[3]]
    mu1 <- expanded[[4]]
    mu2 <- expanded[[5]]
  }

  par.mat <- rbind(kappa1, kappa2, kappa3, mu1, mu2)
  n_par <- ncol(par.mat)
  if (length(x) == 2) x <- matrix(x, nrow = 1)
  n_x <- nrow(x)



  if (is.null(ell$force_approx_const)) {
    force_approx_const <- FALSE
  } else if (!is.logical(ell$force_approx_const)) {
    stop("\'force_approx_const\' must be logical.")
  } else {
    force_approx_const <- ell$force_approx_const
  }


  l_const_all <- rep(0, n_par)

  for(j in 1:n_par) {
    if (all(!force_approx_const,
            ((kappa3[j] >= -5 & max(kappa1[j], kappa2[j],
                                    abs(kappa3[j])) <= 50) |
             abs(kappa3[j]) < 1e-4))) {

      l_const_all[j] <- log(const_vmcos_anltc(kappa1[j], kappa2[j],
                                              kappa3[j]))
    } else {
      l_const_all[j] <- log(const_vmcos_mc(kappa1[j], kappa2[j],
                                           kappa3[j], qrnd_grid));
    }
  }

  if (n_par == 1) {
    log_den <- c(ldcos_manyx_onepar(x, kappa1, kappa2, kappa3,
                                    mu1, mu2, l_const_all))

  } else if (n_x == 1) {
    log_den <- c(ldcos_onex_manypar(c(x), kappa1, kappa2, kappa3,
                                    mu1, mu2, l_const_all))
  } else {
    x_set <- 1:nrow(x)
    par_set <- 1:n_par
    expndn_set <- expand_args(x_set, par_set)
    x_set <- expndn_set[[1]]
    par_set <- expndn_set[[2]]
    log_den <- c(ldcos_manyx_manypar(x[x_set, ], kappa1[par_set],
                                     kappa2[par_set], kappa3[par_set],
                                     mu1[par_set], mu2[par_set],
                                     l_const_all[par_set]))
  }

  if (log) log_den
  else exp(log_den)

}

# dvmcos <- function(x, kappa1=1, kappa2=1, kappa3=0, mu1=0, mu2=0) {
#
#   if(any(c(kappa1, kappa2) < 0)) stop("kappa1 and kappa2 must be nonnegative")
#   if(any(mu1 < 0 | mu1 >= 2*pi)) mu1 <- prncp_reg(mu1)
#   if(any(mu2 < 0 | mu2 >= 2*pi)) mu2 <- prncp_reg(mu2)
#   if((length(dim(x)) < 2 && length(x) != 2) || (length(dim(x)) == 2 && tail(dim(x), 1) != 2)
#      || (length(dim(x)) > 2)) stop("x must either be a bivariate vector or a two-column matrix")
#
#
#   if(max(length(kappa1), length(kappa2), length(kappa3), length(mu1), length(mu2)) > 1) {
#     expanded <- expand_args(kappa1, kappa2, kappa3, mu1, mu2)
#     k1 <- expanded[[1]]; k2 <- expanded[[2]]; k3 <- expanded[[3]]
#     mu1 <- expanded[[4]]; mu2 <- expanded[[5]]
#     sobol_grid <- sobol_2d_1e4_from_seed_1
#     par.mat <- rbind(k1,k2,k3,mu1,mu2)
#     l_const_all <- log_const_vmcos_all(par.mat, sobol_grid, ncores = 1)
#     if(length(x) != 2) {
#       x_set <- 1:nrow(x)
#       par_set <- 1:length(kappa1)
#       expndn_set <- expand_args(x_set, par_set)
#       x_set <- expndn_set[[1]]
#       par_set <- expndn_set[[2]]
#       as.vector(dcos_manyx_manypar(x[x_set, ], k1[par_set], k2[par_set], k3[par_set], mu1[par_set], mu2[par_set], l_const_all[par_set]))
#     } else{
#       as.vector(dcos_onex_manypar(x, k1, k2, k3, mu1, mu2, l_const_all))
#     }
#
#   } else {
#     sobol_grid <- sobol_2d_1e4_from_seed_1
#     const.vmcos <- const_vmcos(kappa1, kappa2, kappa3, sobol_grid, ncores = 1)
#     if(length(x) != 2){
#       as.vector(dcos_manyx_onepar(x, kappa1, kappa2, kappa3, mu1, mu2, log(const.vmcos)))
#     } else{
#       exp(ldcosnum(x[1], x[2], c(kappa1, kappa2, kappa3, mu1, mu2)))/const.vmcos
#     }
#   }
#
#
#
# }

#' The bivariate von Mises cosine model mixtures
#' @inheritParams rvmsinmix
#' @inheritParams rvmcos
#' @param mu1,mu2 vectors of mean parameters.
#' @param kappa1,kappa2,kappa3 vectors of concentration parameters; \code{kappa1, kappa2 > 0} for each component.
#'
#' @details All the argument vectors \code{pmix, kappa1, kappa2, kappa3, mu1} and \code{mu2} must be of
#' the same length ( = component size of the mixture model), with \eqn{j}-th element corresponding to the
#' \eqn{j}-th component of the mixture distribution.
#' @details The bivariate von Mises cosine model mixture distribution with component size \code{K = \link{length}(pmix)} has density
#' \deqn{g(x) = \sum p[j] * f(x; \kappa_1[j], \kappa_2[j], \kappa_3[j], \mu_1[j], \mu_2[j])}
#' where the sum extends over \eqn{j}; \eqn{p[j]; \kappa_1[j], \kappa_2[j], \kappa_3[j]}; and \eqn{\mu_1[j], \mu_2[j]} respectively denote the mixing proportion,
#' the three concentration parameters and the two mean parameter for the \eqn{j}-th cluster, \eqn{j = 1, ..., K},
#' and \eqn{f(. ; \kappa_1, \kappa_2, \kappa_3, \mu_1, \mu_2)} denotes the density function of the von Mises cosine model
#' with concentration parameters \eqn{\kappa_1, \kappa_2, \kappa_3} and  mean parameters \eqn{\mu_1, \mu_2}.
#'
#' @return \code{dvmcosmix} computes the density  and \code{rvmcosmix} generates random deviates from the mixture density.
#'
#' @examples
#' kappa1 <- c(1, 2, 3)
#' kappa2 <- c(1, 6, 5)
#' kappa3 <- c(0, 1, 2)
#' mu1 <- c(1, 2, 5)
#' mu2 <- c(0, 1, 3)
#' pmix <- c(0.3, 0.4, 0.3)
#' x <- diag(2, 2)
#' n <- 10
#'
#' # mixture densities calculated at the rows of x
#' dvmcosmix(x, kappa1, kappa2, kappa3, mu1, mu2, pmix)
#'
#' # number of observations generated from the mixture distribution is n
#' rvmcosmix(n, kappa1, kappa2, kappa3, mu1, mu2, pmix)
#'
#' @export

rvmcosmix <- function(n, kappa1, kappa2, kappa3,
                      mu1, mu2, pmix, method = "naive", ...)
{
  allpar <- list(kappa1=kappa1, kappa2=kappa2, kappa3=kappa3,
                 mu1=mu1, mu2=mu2, pmix=pmix)

  allpar_len <- listLen(allpar)
  if(min(allpar_len) != max(allpar_len))
    stop("component size mismatch: number of components of the input parameter vectors differ")

  if(any(allpar$pmix < 0)) stop("\'pmix\' must be non-negative")
  sum_pmix <- sum(allpar$pmix)
  if(signif(sum_pmix, 5) != 1) {
    if(sum_pmix <= 0) stop("\'pmix\' must have at least one positive element")
    allpar$pmix <- allpar$pmix/sum_pmix
    warning("\'pmix\' is rescaled to add up to 1")
  }

  if(any(c(allpar$kappa1, allpar$kappa2) < 0)) stop("kappa1 and kappa2 must be non-negative")
  if(any(allpar$mu1 < 0 | allpar$mu1 >= 2*pi)) allpar$mu1 <- prncp_reg(allpar$mu1)
  if(any(allpar$mu2 < 0 | allpar$mu2 >= 2*pi)) allpar$mu2 <- prncp_reg(allpar$mu2)

  # opt_I_k13 <- function(k1, k2, k3, mu1, mu2) {
  #   I_k13 <- function(y) BESSI0_C(sqrt(k1 * k1 + k3 * k3 + 2 * k1 * k3 * cos(y - mu2)))
  #   optimize(I_k13, c(0, 2*pi), maximum = TRUE)$maximum
  # }
  #
  # upper_bd_all <- vapply(1:length(kappa1),
  #                        function(h) opt_I_k13(kappa1[h], kappa2[h], kappa3[h],
  #                                              mu1[h], mu2[h]),
  #                        0)
  #
  # clus_label <- cID(t(replicate(allpar$pmix, n = n)), length(allpar$pmix), runif(n))
  # rcos_manypar(allpar$kappa1[clus_label], allpar$kappa2[clus_label], allpar$kappa3[clus_label],
  #              allpar$mu1[clus_label], allpar$mu2[clus_label], upper_bd_all[clus_label])

  out <- matrix(0, n, 2)
  ncomp <- allpar_len[1] # number of components
  comp_ind <- cID(tcrossprod(rep(1, n), allpar$pmix), ncomp, runif(n))
  # n samples from multinom(ncomp, pmix)
  for(j in seq_len(ncomp)) {
    obs_ind_j <- which(comp_ind == j)
    n_j <- length(obs_ind_j)
    if(n_j > 0) {
      out[obs_ind_j, ] <- rvmcos(n_j, kappa1[j], kappa2[j],
                                 kappa3[j], mu1[j], mu2[j], method)
    }
  }

  out

}


#' @rdname rvmcosmix
#' @export
dvmcosmix <- function(x, kappa1, kappa2, kappa3,
                      mu1, mu2, pmix, log=FALSE, ...)
{
  allpar <- list("kappa1"=kappa1, "kappa2"=kappa2, "kappa3"=kappa3,
                 "mu1"=mu1, "mu2"=mu2, "pmix"=pmix)

  allpar_len <- listLen(allpar)
  if(min(allpar_len) != max(allpar_len)) stop("component size mismatch: number of components of the input parameter vectors differ")

  if(any(allpar$pmix < 0)) stop("\'pmix\' must be non-negative")
  sum_pmix <- sum(allpar$pmix)
  if(signif(sum_pmix, 5) != 1) {
    if(sum_pmix <= 0) stop("\'pmix\' must have at least one positive element")
    allpar$pmix <- allpar$pmix/sum_pmix
    warning("\'pmix\' is rescaled to add up to 1")
  }

  if(any(c(allpar$kappa1, allpar$kappa2) <= 0)) stop("kappa1 and kappa2 must be positive")
  if(any(allpar$mu1 < 0 | allpar$mu1 >= 2*pi)) allpar$mu1 <- prncp_reg(allpar$mu1)
  if(any(allpar$mu2 < 0 | allpar$mu2 >= 2*pi)) allpar$mu2 <- prncp_reg(allpar$mu2)
  if((length(dim(x)) < 2 && length(x) != 2) || (length(dim(x)) == 2 && tail(dim(x), 1) != 2)
     || (length(dim(x)) > 2)) stop("x must either be a bivariate vector or a two-column matrix")

  ncomp <- length(kappa1)

  if (length(x) == 2) x <- matrix(x, nrow=1)

  allcompden <- vapply(1:ncomp,
                       function(j) dvmcos(x, kappa1[j], kappa2[j],
                                          kappa3[j], mu1[j], mu2[j], FALSE, ...),
                       rep(0, nrow(x)))

  mixden <- c(allcompden %*% pmix)

  if (log) {
    log(mixden)
  } else {
    mixden
  }
}


#' Fitting bivariate von Mises cosine model mixtures using MCMC
#' @inheritParams fit_vmsinmix
#'
#' @details
#' Wrapper for \link{fit_angmix} with \code{model = "vmcos"}.
#'
#'
#' @examples
#' # illustration only - more iterations needed for convergence
#' fit.vmcos.10 <- fit_vmcosmix(tim8, ncomp = 3, n.iter =  10,
#'                              n.chains = 1)
#' fit.vmcos.10
#'
#' @export

fit_vmcosmix <- function(...)
{
  fit_angmix(model="vmcos", ...)
}




vmcos_var_cor_singlepar_numeric <- function(kappa1, kappa2, kappa3, qrnd_grid) {
  # N <- 1e4

  # browser()

  fn_log_vmcos_const <- function(pars) {
    const_vmcos(pars[1], pars[2], pars[3], uni_rand = qrnd_grid, return_log = TRUE)
  }

  # const <- fn_log_vmcos_const(c(kappa1, kappa2, kappa3))
  grad_over_const <- numDeriv::grad(fn_log_vmcos_const, c(kappa1, kappa2, kappa3))
  names(grad_over_const) <- c("k1", "k2", "k3")
  hess_over_const <- numDeriv::hessian(fn_log_vmcos_const, c(kappa1, kappa2, kappa3)) +
    tcrossprod(grad_over_const)
  dimnames(hess_over_const) <- list(c("k1", "k2", "k3"), c("k1", "k2", "k3"))

  rho_fl <- unname(
    ((grad_over_const["k3"] - hess_over_const["k1", "k2"]) *
       hess_over_const["k1", "k2"]) /
      sqrt(
        hess_over_const["k1", "k1"] * (1 - hess_over_const["k1", "k1"])
        * hess_over_const["k2", "k2"] * (1 - hess_over_const["k2", "k2"])
      )
  )

  rho_js <- unname(
    (grad_over_const["k3"] - hess_over_const["k1", "k2"]) /
      sqrt((1 - hess_over_const["k1", "k1"]) *
             (1 - hess_over_const["k2", "k2"]))
  )

  var1 <- unname(1 - grad_over_const["k1"])
  var2 <- unname(1 - grad_over_const["k2"])
  # dat <- rvmcos(N, kappa1, kappa2, kappa3, 0, 0)
  #
  # ave_sin1sin2 <- sum(sin(dat[, 1]) * sin(dat[, 2]))/N
  # ave_cos1cos2 <- sum(cos(dat[, 1]) * cos(dat[, 2]))/N
  #
  # ave_sin1sq <- sum(sin(dat[, 1])^2)/N
  # ave_cos1sq <- 1-ave_sin1sq
  # ave_cos1 <- sum(cos(dat[, 1]))/N
  #
  # ave_sin2sq <- sum(sin(dat[, 2])^2)/N
  # ave_cos2sq <- 1-ave_sin2sq
  # ave_cos2 <- sum(cos(dat[, 2]))/N
  #
  # rho_js <- ave_sin1sin2/sqrt(ave_sin1sq * ave_sin2sq)
  # # ifelse(ave_sin1sin2 >= 0, 1, -1) *
  # # min(abs(ave_sin1sin2)/sqrt(ave_sin1sq * ave_sin2sq), 1)
  #
  # rho_fl <- rho_js *
  #   ave_cos1cos2/sqrt(ave_cos1sq * ave_cos2sq)
  # # ifelse(ave_cos1cos2 >= 0, 1, -1) *
  # # min(abs(ave_cos1cos2)/sqrt(ave_cos1sq * ave_cos2sq), 1)
  #
  # var1 <- min(1 - ave_cos1, 1)
  # var2 <- min(1 - ave_cos2, 1)

  list(var1 = var1, var2 = var2, rho_fl = rho_fl, rho_js = rho_js)
}


vmcos_var_cor_singlepar <- function(kappa1, kappa2, kappa3,
                                    qrnd_grid) {
  if (max(kappa1, kappa2, abs(kappa3)) > 50 |
      kappa3 < 0) {
    out <- vmcos_var_cor_singlepar_numeric(kappa1, kappa2,
                                           kappa3, qrnd_grid)
    # } else if(kappa3 < -1 | max(kappa1, kappa2, abs(kappa3)) > 50) {
    #   vmcos_var_corr_mc(kappa1, kappa2, kappa3, qrnd_grid)
  } else {
    out <- vmcos_var_corr_anltc(kappa1, kappa2, kappa3)
  }

  for (rho in c("rho_js", "rho_fl")) {
    if (out[[rho]] <= -1) {
      out[[rho]] <- -1
    } else if (out[[rho]] >= 1) {
      out[[rho]] <- 1
    }
  }

  for (var in c("var1", "var2")) {
    if (out[[var]] <= 0) {
      out[[var]] <- 0
    } else if (out[[var]] >= 1) {
      out[[var]] <- 1
    }
  }

  out
}



rvmcos_1par <- function(n=1, kappa1, kappa2, kappa3, mu1, mu2, method)
{

  if (abs(abs(kappa3) - kappa1) < 1e-8 |
      abs(abs(kappa3) - kappa2) < 1e-8)
    kappa3 <- kappa3*(1+1e-6)


  if (method == "vmprop") {
    qrnd_grid <- sobol(1e4, 2, FALSE)
    log_const_vmcos <- log(const_vmcos(kappa1, kappa2, kappa3, qrnd_grid))
    log_2pi <- log(2*pi)

    unimodal_y <- TRUE
    phistar <- NULL
    method <- "vmprop"


    # first check if the joint density is bimodal
    if (kappa3 <  - kappa1*kappa2/(kappa1+kappa2+1e-16)) {
      # now check if the y marginal is bimodal
      if (A_bessel(abs(kappa1+kappa3)) >
          -abs(kappa1+kappa3)*kappa2/(kappa1*kappa3 + 1e-16)) {
        unimodal_y <- FALSE
        phistar_eqn <- function(phi) {
          kappa13 <- sqrt(kappa1^2 + kappa3^2 + 2*kappa1*kappa3*cos(phi))
          -kappa1*kappa3*A_bessel(kappa13)/kappa13 - kappa2
        }
        find_root <- uniroot.all(phistar_eqn, c(0, 2*pi))
        # if no root found, use the naive two dimensional rejection sampler
        if (is.null(find_root)) {
          method <- "naive"
        } else {
          phistar <- find_root[1]
        }

      }

    }

    # browser()

    if (method == "vmprop" & unimodal_y) {

      grid_0_2pi <- seq(0, 2*pi, length.out = 100)

      # do minimax to find optimum kappa and K
      # don't worry about mu1 mu2 while optimizing!
      # abs difference between the target log marginal den
      # and log proposal den
      obj_kappa_abs <- function(kappa) {
        obj_kappa_phi <- function(phi) {
          cos_phi <- cos(phi)
          kappa13 <- sqrt(kappa1^2 + kappa3^2 + 2*kappa1*kappa3*cos_phi)
          log_I_k_13 <- log(besselI(kappa13, 0))
          log_target_marginal <- -log_const_vmcos + log_2pi + log_I_k_13 + kappa2*cos_phi
          log_proposal <- -log_2pi - log(besselI(kappa, 0)) + kappa*cos_phi
          abs(log_target_marginal - log_proposal)
        }
        optimize(obj_kappa_phi, c(0, 2*pi), maximum=TRUE)$objective
      }

      # minimize obj_kappa_abs wrt kappa
      kappa_opt <- optimize(obj_kappa_abs, c(0, max(50, kappa2*2)))$minimum


      log_prop_den <- function(kappa, phi) {
        cos_phi <- cos(phi)
        -log_2pi - log(besselI(kappa, 0)) +
          kappa*cos_phi
      }

      # find max of log_prop_den
      max_log_prop_den <- max(log_prop_den(kappa_opt, grid_0_2pi))
      # any point with density < exp(max_log_prop_den)*exp(-10)
      # are less likely to appear, so make exp(max_log_prop_den)*exp(-10)
      # as the lower bound for propsal density to avoid instability


      # browser()

      # now maximize the log ratio of the two densities
      # w.r.t phi, given kappa = kappa_opt
      obj_kappa_phi <- function(kappa, phi) {
        cos_phi <- cos(phi)
        kappa13 <- sqrt(kappa1^2 + kappa3^2 + 2*kappa1*kappa3*cos_phi)
        log_I_k_13 <- log(besselI(kappa13, 0))
        log_target_marginal <- -log_const_vmcos + log_2pi + log_I_k_13 + kappa2*cos_phi
        log_proposal <- pmax(-log_2pi - log(besselI(kappa, 0)) +
                               kappa*cos_phi, max_log_prop_den-10)
        # log_proposals are >= max_log_prop_den -10 to avoid instability
        # also, proposal deviates with log density less than this bound are unlikely
        log_target_marginal - log_proposal
      }

      # maximize the log-ratio over a grid
      (logK <- max(obj_kappa_phi(kappa_opt, seq(0, 2*pi, length.out = 200))))


      # browser()
      # cat(exp(-logK))
      # rcos_unimodal(1, kappa1, kappa2, kappa3, mu1, mu2,
      #               kappa_opt, log(besselI(kappa_opt, 0, TRUE)) + kappa_opt,
      #               logK, log_const_vmcos)

      rcos_unimodal(n, kappa1, kappa2, kappa3, mu1, mu2,
                    kappa_opt, log(BESSI0_C(kappa_opt)), logK,
                    log_const_vmcos)

    }

    else if (method == "vmprop" & !unimodal_y) {

      # change the modes into [0, 2*pi]
      mode_1 <- prncp_reg(prncp_reg.minuspi.pi(mu2)+phistar)
      mode_2 <- prncp_reg(prncp_reg.minuspi.pi(mu2)-phistar)
      sin_phistar <- sin(phistar)
      unifpropn <- 1e-10
      vmpropn <- (1-1e-10)/2
      grid_0_2pi <- seq(0, 2*pi, length.out = 100)

      # proposal for y marginal = vmpropn-vmpropn-unifpropn mix
      # of vm(kappa, mu2 +- phistar) and unif(0, 2*pi)
      # and unif(0, 2*pi) (to avoid overflow of the ratio where
      # the densities are flat)
      # do minimax to find optimum kappa and K
      # abs difference between the target log marginal den
      # and log proposal den
      obj_kappa_abs <- function(kappa) {
        obj_kappa_phi <- function(phi) {
          cos_phi <- cos(phi-mu2)
          kappa13 <- sqrt(kappa1^2 + kappa3^2 + 2*kappa1*kappa3*cos_phi)
          log_I_k_13 <- log(besselI(kappa13, 0))
          log_target_marginal <- -log_const_vmcos + log_2pi + log_I_k_13 + kappa2*cos_phi
          log_proposal <-
            -log_2pi +  log(exp(log(vmpropn) - log(besselI(kappa, 0)) +
                                  kappa*cos(phi-mode_1) +
                                  log(1+exp(kappa*(cos(phi-mode_2)
                                                   - cos(phi-mode_1))))
            ) + unifpropn) # simplified mixture density
          abs(log_target_marginal - log_proposal)
        }
        max(obj_kappa_phi(grid_0_2pi))
      }


      # minimize obj_kappa_abs wrt kappa
      (kappa_opt <- optimize(obj_kappa_abs,
                             c(0, max(kappa1, kappa2,
                                      abs(kappa3))))$minimum)


      log_prop_den <- function(kappa, phi) {
        -log_2pi +  log(exp(log(vmpropn) - log(besselI(kappa, 0)) +
                              kappa*cos(phi-mode_1) +
                              log(1+exp(kappa*(cos(phi-mode_2) - cos(phi-mode_1)))))
                        + unifpropn)
      }


      # find max of log_prop_den
      max_log_prop_den <- max(log_prop_den(kappa_opt, grid_0_2pi))
      # any point with density < exp(max_log_prop_den)*exp(-10)
      # are less likely to appear, so make exp(max_log_prop_den)*exp(-10)
      # as the lower bound for propsal density to avoid instability


      # now maximize the log ratio of the two densities
      # w.r.t phi, given kappa = kappa_opt
      obj_kappa_phi <- function(kappa, phi) {
        cos_phi <- cos(phi-mu2)
        kappa13 <- sqrt(kappa1^2 + kappa3^2 + 2*kappa1*kappa3*cos_phi)
        log_I_k_13 <- log(besselI(kappa13, 0))
        log_target_marginal <- -log_const_vmcos + log_2pi + log_I_k_13 + kappa2*cos_phi
        log_proposal <-
          pmax(-log_2pi +  log(exp(log(vmpropn) - log(besselI(kappa, 0)) +
                                     kappa*cos(phi-mode_1) +
                                     log(1+exp(kappa*(cos(phi-mode_2) - cos(phi-mode_1)))))
                               + unifpropn), max_log_prop_den-10)
        # log_proposals are >= max_log_prop_den -10 to avoid instability
        # also, proposal deviates with log density less than this bound are unlikely
        log_target_marginal - log_proposal
      }

      # browser()
      #
      # target_den <- function(phi) {
      #   cos_phi <- cos(phi-mu2)
      #   kappa13 <- sqrt(kappa1^2 + kappa3^2 + 2*kappa1*kappa3*cos_phi)
      #   log_I_k_13 <- log(besselI(kappa13, 0))
      #   log_target_marginal <- -log_const_vmcos + log_2pi + log_I_k_13 + kappa2*cos_phi
      #   exp(log_target_marginal)
      # }
      #
      # prop_den <- function(kappa, phi) {
      #   cos_phi <- cos(phi)
      #   log_proposal <-
      #     pmax(-log_2pi +  log(exp(log(vmpropn) - log(besselI(kappa, 0)) +
      #                                kappa*cos(phi-mode_1) +
      #                                log(1+exp(kappa*(cos(phi-mode_2) - cos(phi-mode_1)))))
      #                          + unifpropn), max_log_prop_den-30)
      #   exp(log_proposal)
      # }
      # #
      # # integrate(gg, 0, 2*pi)
      # # integrate(function(x) pp(kappa_opt, x), 0, 2*pi)

      # maximize the log-ratio over a grid
      (logK <- max(obj_kappa_phi(kappa_opt, seq(0, 2*pi, length.out = 200))))

      exp(logK)


      # poin <- seq(0, 2*pi, length.out = 100)
      # lggp <- log(target_den(poin))
      # lppp <- log(prop_den(kappa_opt, poin))
      # # difp <- obj_kappa_phi(kappa_opt, poin)
      # #
      # plot(poin, lggp, type="l", ylim=range(lggp, lppp, lggp-lppp))
      # points(poin, lppp, type="l", col = "blue")
      # # points(poin, lggp-lppp, type="l")
      # # points(poin, difp, type="l", col = "red")
      # abline(v=c(prncp_reg(mu2+phistar), prncp_reg(mu2-phistar)))


      rcos_bimodal(n, kappa1, kappa2, kappa3, mu1, mu2,
                   kappa_opt, log(BESSI0_C(kappa_opt)), logK,
                   log_const_vmcos, mode_1, mode_2,
                   vmpropn, unifpropn)

      # rcos_unimodal(n, kappa1, kappa2, kappa3, mu1, mu2,
      #               kappa_opt, log(BESSI0_C(kappa_opt)), logK,
      #               log_const_vmcos)

      # rcos_unimodal_R(kappa1, kappa2, kappa3, mu1, mu2,
      #                 kappa_opt, log(BESSI0_C(kappa_opt)), logK,
      #                 log_const_vmcos)
    }

  }

  else if (method == "naive") {
    opt_obj <- function(k1=1, k2=1, k3=0, mu1=0, mu2=0) {
      obj <- optim(c(0,0), fn = function(x) -(k1*cos(x[1]-mu1)+k2*cos(x[2]-mu2)+k3*cos(x[1]-x[2]-mu1+mu2)),
                   gr = function(x)  -c(-k1*sin(x[1]-mu1)-k3*sin(x[1]-x[2]-mu1+mu2),
                                        -k2*sin(x[2]-mu2)+k3*sin(x[1]-x[2]-mu1+mu2)))
      -obj$value

    }
    upper_bd <- opt_obj(kappa1, kappa2, kappa3, mu1, mu2)
    # qrnd_grid <- sobol(1e4, 2, FALSE)
    # cat(exp(log(const_vmcos(kappa1, kappa2, kappa3,
    #                         qrnd_grid))
    #         - upper_bd)/(4*pi^2))
    rcos_onepar(n, kappa1, kappa2, kappa3, mu1, mu2, upper_bd)
  }

}
