compareKernelSmoothing <- function(y, bandwidth) {
  n <- length(y)
  kernel <- function(x) ifelse(abs(x) <= 1 + 1e-15, 3 / 4 * (1 - x^2), 0)
  L <- max(0, min(as.integer(bandwidth * n + 1e-14), n - 1))
  K <- kernel(-L:L / (bandwidth * n))
  
  ret <- numeric(n)
  
  for (i in 1:n) {
    ret[i] <- sum(K[max(L + 2 - i, 1):min(n - i + L + 1, 2 * L + 1)] * y[max(i - L, 1):min(i + L, n)]) / sum(K[max(L + 2 - i, 1):min(n - i + L + 1, 2 * L + 1)])
  }
  ret
}

compareImSy <- function(y, bandwidth) {
  n <- length(y)
  kernel <- function(x) ifelse(abs(x) <= 1 + 1e-15, 3 / 4 * (1 - x^2), 0)
  L <- max(0, min(as.integer(bandwidth * n + 1e-14), n - 1))
  K <- kernel(-L:L / (bandwidth * n))
  
  y - .kernelSmoothing(y, K)
}

compareImSX <- function(y, bandwidth) {
  n <- length(y)
  kernel <- function(x) ifelse(abs(x) <= 1 + 1e-15, 3 / 4 * (1 - x^2), 0)
  L <- max(0, min(as.integer(bandwidth * n + 1e-14), n - 1))
  K <- kernel(-L:L / (bandwidth * n))
  Matrix::bandSparse(n, n - 1, k = (-L + 1):L - 1, diagonals = .createImSX(n = n, K = rev(cumsum(K))))
}

compareXty <- function(y, bandwidth, x = NULL) {
  if (is.null(x)) {
    x <- compareImSX(y, bandwidth)
  }
  Matrix::t(x) %*% y / length(y)
}

compareXtX <- function(y, bandwidth, x = NULL) {
  if (is.null(x)) {
    x <- compareImSX(y, bandwidth)
  }
  Matrix::t(x) %*% x / length(y)
}

comparePcplus <- function(y, lambda, bandwidth, sd, thresh, maxit) {
  ImSX <- compareImSX(y, bandwidth)
  ImSy <- compareImSy(y, bandwidth)
  
  ret <- glmnet::glmnet(x = ImSX, y = ImSy, lambda = lambda, standardize = FALSE, intercept = FALSE)
  fit <- c(0, cumsum(as.numeric(ret$beta)))
  
  n <- length(y)
  kernel <- function(x) ifelse(abs(x) <= 1 + 1e-15, 3 / 4 * (1 - x^2), 0)
  L <- max(0, min(as.integer(bandwidth * n + 1e-14), n - 1))
  K <- kernel(-L:L / (bandwidth * n))
  smooth <- .kernelSmoothing(y - fit, K)
  
  pelt <- .pelt(obs = as.numeric(y - smooth), sd = sd)
  
  J <- pelt$cps
  ImSX <- cbind(rep(0, n), ImSX)
  
  if (length(J) > 0) {
    fitJ <- as.numeric(Matrix::solve(Matrix::t(ImSX[, J, drop = FALSE]) %*% ImSX[, J, drop = FALSE],
                                     as.numeric(Matrix::t(ImSX[, J, drop = FALSE]) %*% ImSy)))
    fit <- numeric(n)
    fit[J] <- fitJ
    fit <- cumsum(fit)
    smooth <- .kernelSmoothing(y - fit, K)
  } else {
    fit <- rep(0, n)
    smooth <- .kernelSmoothing(y, K)
  }
  
  list(est = fit + smooth, smooth = smooth, cpfun = fit, cps = J)
}

comparePcplusInf <- function(y, sd) {
  pelt <- .pelt(obs = y - mean(y), sd = sd)
  list(est = pelt$est + mean(y), smooth = rep(mean(y), length(y)), cpfun = pelt$est, cps = pelt$cps)
}

test_that("pcplus is giving correct results I", {
  testy <- rnorm(100)
  testbandwidth <- 0.05
  testlambda <- 0.01
  testthresh <- 1e-7
  testmaxit <- 1e5
  testsd <- 1
  
  object <- pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, sd = testsd,
                   nlambda = 30L, thresh = testthresh, maxit = testmaxit)
  expected <- comparePcplus(testy, testlambda, testbandwidth, testsd, testthresh, testmaxit)
  
  expect_equal(object, expected)
})

test_that("pcplus is giving correct results II", {
  testy <- rnorm(100)
  testy[40:100] <- testy[40:100] + 3
  testy[70:100] <- testy[70:100] + 5
  testbandwidth <- 0.15
  testlambda <- 0.03
  testthresh <- 1e-7
  testmaxit <- 1e5
  testsd <- 1
  
  object <- pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, sd = testsd,
                   nlambda = 30L, thresh = testthresh, maxit = testmaxit)
  expected <- comparePcplus(testy, testlambda, testbandwidth, testsd, testthresh, testmaxit)
  
  expect_equal(object, expected)
})

test_that("pcplus is giving correct results III", {
  testy <- rnorm(100)
  testy[7:100] <- testy[7:100] + 50
  testy[12:100] <- testy[12:100] + 20
  testy[90:100] <- testy[90:100] + 20
  testbandwidth <- 0.15
  testlambda <- 0.04
  testthresh <- 1e-7
  testmaxit <- 1e5
  testsd <- 0.5
  
  object <- pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, sd = testsd,
                   nlambda = 30L, thresh = testthresh, maxit = testmaxit)
  expected <- comparePcplus(testy, testlambda, testbandwidth, testsd, testthresh, testmaxit)
  
  expect_equal(object, expected)
})

test_that("argument y is checked", {
  testy <- rnorm(100)
  testbandwidth <- 0.1
  testlambda <- 0.03
  
  expect_error(pcplus())
  expect_error(pcplus(lambda = testlambda, bandwidth = testbandwidth))
  expect_error(pcplus(y = NA, lambda = testlambda, bandwidth = testbandwidth))
  expect_error(pcplus(y = as.numeric(NA), lambda = testlambda, bandwidth = testbandwidth))
  expect_error(pcplus(y = c(testy, "s"), lambda = testlambda, bandwidth = testbandwidth))
  expect_error(pcplus(y = c(testy, Inf), lambda = testlambda, bandwidth = testbandwidth))
  expect_error(pcplus(y = c(-Inf, testy), lambda = testlambda, bandwidth = testbandwidth))
  expect_error(pcplus(y = c(NaN, testy), lambda = testlambda, bandwidth = testbandwidth))
})

test_that("argument bandwidth is checked", {
  testy <- rnorm(100)
  testy[7:100] <- testy[7:100] + 50
  testy[12:100] <- testy[12:100] + 20
  testy[90:100] <- testy[90:100] + 20
  testlambda <- seq(1, 0.05, -0.05)
  testthresh <- 1e-7
  testmaxit <- 1e5
  testsd <- 0.5
  
  expect_error(pcplus(y = testy, lambda = testlambda))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = "s"))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = NA))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = -0.05))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = as.numeric(NA)))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = c(0.05, 0.1)))
  
  expect_equal(pcplus(y = testy, lambda = testlambda, bandwidth = Inf, sd = testsd),
               comparePcplusInf(testy, testsd))
  expect_warning(object <- pcplus(y = testy, lambda = testlambda, bandwidth = 0.5, sd = testsd))
  expect_equal(object, comparePcplusInf(testy, testsd))
  
  expect_identical(pcplus(y = testy, lambda = Inf, bandwidth = Inf),
                   list(est = rep(mean(testy), length(testy)), smooth = rep(mean(testy), length(testy)),
                        cpfun = rep(0, length(testy)), cps = integer(0)))
  
  expect_warning(object <- pcplus(y = testy, lambda = testlambda, bandwidth = 0.005, sd = testsd,
                                  nlambda = 30L, thresh = testthresh, maxit = testmaxit))
  expected <- pcplus(y = testy, lambda = testlambda, bandwidth = 0.01, sd = testsd,
                     nlambda = 30L, thresh = testthresh, maxit = testmaxit)
  expect_equal(object, expected)
})

test_that("argument lambda is checked", {
  testy <- rnorm(100)
  testy[7:100] <- testy[7:100] + 50
  testy[12:100] <- testy[12:100] + 20
  testy[90:100] <- testy[90:100] + 20
  testbandwidth <- 0.1
  testthresh <- 1e-7
  testmaxit <- 1e5
  testsd <- 0.5
  
  expect_error(pcplus(y = testy, bandwidth = testbandwidth))
  expect_error(pcplus(y = testy, lambda = "s", bandwidth = testbandwidth))
  expect_error(pcplus(y = testy, lambda = NA, bandwidth = testbandwidth))
  expect_error(pcplus(y = testy, lambda = -0.5, bandwidth = testbandwidth))
  expect_error(pcplus(y = testy, lambda = as.numeric(NA), bandwidth = testbandwidth))
  
  expect_equal(pcplus(y = testy, lambda = seq(1, 0.05, -0.05), bandwidth = testbandwidth),
               pcplus(y = testy, lambda = 0.05, bandwidth = testbandwidth, nlambda = 1), tolerance = 1e-12)
  expect_warning(object <- pcplus(y = testy, lambda = rev(seq(1, 0.05, -0.05)), bandwidth = testbandwidth))
  expect_equal(object,
               pcplus(y = testy, lambda = 0.05, bandwidth = testbandwidth, nlambda = 1), tolerance = 1e-12)
  expect_warning(object <- pcplus(y = testy, lambda = c(seq(1, 0.2, -0.05), 0.05, 0.15, 0.1), bandwidth = testbandwidth))
  expect_equal(object,
               pcplus(y = testy, lambda = 0.05, bandwidth = testbandwidth, nlambda = 1), tolerance = 1e-12)
  
  expect_error(pcplus(y = testy, lambda = c(seq(1, 0.05, -0.05), "s"), bandwidth = testbandwidth))
  expect_error(pcplus(y = testy, lambda = c(as.numeric(NA), seq(1, 0.05, -0.05)), bandwidth = testbandwidth))
  expect_error(pcplus(y = testy, lambda = c(seq(1, 0.5, -0.05), -0.05, seq(0.45, 0.05, -0.05)), bandwidth = testbandwidth))
  
  object <- pcplus(y = testy, lambda = 1e12, bandwidth = testbandwidth, sd = testsd,
                   nlambda = 1L, thresh = testthresh, maxit = testmaxit)
  expected <- comparePcplus(testy, 1e12, testbandwidth, testsd, testthresh, testmaxit)
  expect_equal(object, expected)
  
  compareSmooth <- compareKernelSmoothing(testy, testbandwidth)
  expect_equal(pcplus(y = testy, lambda = Inf, bandwidth = testbandwidth, nlambda = 1L),
               list(est = compareSmooth, smooth = compareSmooth, cpfun = rep(0, length(testy)), cps = integer(0)))
})

test_that("argument sd is checked", {
  testy <- rnorm(100)
  testy[7:100] <- testy[7:100] + 50
  testy[12:100] <- testy[12:100] + 20
  testy[90:100] <- testy[90:100] + 20
  testlambda <- seq(1, 0.05, -0.05)
  testbandwidth <- 0.05
  testthresh <- 1e-7
  testmaxit <- 1e5
  
  testsd <- as.numeric(diff(quantile(diff(testy), c(0.25, 0.75))) / diff(qnorm(c(0.25, 0.75))) / sqrt(2)) 
  expect_equal(pcplus(y = testy, lambda = testlambda, bandwidth = Inf),
               comparePcplusInf(testy, testsd))
  
  object <- pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth,
                   nlambda = 30L, thresh = testthresh, maxit = testmaxit)
  expected <- comparePcplus(testy, testlambda[length(testlambda)], testbandwidth, testsd, testthresh, testmaxit)
  expect_equal(object, expected)
  
  object <- pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, sd = NULL,
                   nlambda = 30L, thresh = testthresh, maxit = testmaxit)
  expect_equal(object, expected)
  
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, sd = "s"))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, sd = NA))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, sd = -0.05))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, sd = NaN))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, sd = as.numeric(NA)))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, sd = c(0.05, 0.1)))
})

test_that("argument nlambda is checked", {
  testy <- rnorm(100)
  testy[7:100] <- testy[7:100] + 50
  testy[12:100] <- testy[12:100] + 20
  testy[90:100] <- testy[90:100] + 20
  testbandwidth <- 0.1
  testlambda <- 0.05
  testthresh <- 1e-7
  testmaxit <- 1e5
  testsd <- 0.5
  
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, nlambda = "s"))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, nlambda = NA))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, nlambda = NaN))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, nlambda = Inf))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, nlambda = -10))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, nlambda = as.numeric(NA)))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, nlambda = c(5, 10)))
  
  expect_identical(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth),
                   pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, nlambda = 30L))
  expect_identical(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, nlambda = 10),
                   pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, nlambda = 10L))
  
  
  expect_equal(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, nlambda = 1L),
               pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth))
  expect_equal(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, nlambda = 2L),
               pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth))
  expect_equal(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, nlambda = 5L),
               pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth))
  expect_equal(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, nlambda = 10L),
               pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth))
  expect_equal(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, nlambda = 20L),
               pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth))
  expect_equal(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, nlambda = 50L),
               pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth))
  expect_equal(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, nlambda = 100L),
               pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth))
  
  expect_identical(pcplus(y = testy, lambda = Inf, bandwidth = testbandwidth, nlambda = 30L),
                   pcplus(y = testy, lambda = Inf, bandwidth = testbandwidth, nlambda = 1L))
})

test_that("argument thresh is checked", {
  testy <- rnorm(100)
  testy[7:100] <- testy[7:100] + 50
  testy[12:100] <- testy[12:100] + 20
  testy[90:100] <- testy[90:100] + 20
  testbandwidth <- 0.1
  testlambda <- seq(1, 0.05, -0.05)
  testmaxit <- 1e5
  testsd <- 0.5
  
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, thresh = "s"))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, thresh = NA))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, thresh = NaN))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, thresh = Inf))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, thresh = -10))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, thresh = 0))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, thresh = as.numeric(NA)))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, thresh = c(5, 10)))
  
  expect_identical(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth),
                   pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, thresh = 1e-7))
  
  object <- pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, sd = testsd,
                   nlambda = 30L, thresh = 1e-9, maxit = testmaxit)
  expected <- comparePcplus(testy, testlambda[length(testlambda)], testbandwidth, testsd, 1e-9, testmaxit)
  expect_equal(object, expected)
  
  object <- pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, sd = testsd,
                   nlambda = 30L, thresh = 1e-3, maxit = testmaxit)
  expected <- comparePcplus(testy, testlambda[length(testlambda)], testbandwidth, testsd, 1e-3, testmaxit)
  expect_equal(object, expected)
  
  object <- pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, sd = testsd,
                   nlambda = 30L, thresh = 1e-2, maxit = testmaxit)
  expected <- comparePcplus(testy, testlambda[length(testlambda)], testbandwidth, testsd, 1e-2, testmaxit)
  expect_equal(object, expected, tolerance = 1e-1)
})

test_that("argument maxit is checked", {
  testy <- rnorm(100)
  testy[7:100] <- testy[7:100] + 50
  testy[12:100] <- testy[12:100] + 20
  testy[90:100] <- testy[90:100] + 20
  testbandwidth <- 0.1
  testlambda <- c(0.5, 0.4)
  testthresh <- 1e7
  testsd <- 0.5
  
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, maxit = "s"))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, maxit = NA))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, maxit = NaN))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, maxit = Inf))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, maxit = -10))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, maxit = 0))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, maxit = as.numeric(NA)))
  expect_error(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, maxit = c(5, 10)))
  
  expect_identical(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth),
                   pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, maxit = 1e5L))
  expect_identical(pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth),
                   pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, maxit = 1e5))
  
  object <- pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, sd = testsd,
                   nlambda = 30L, thresh = testthresh, maxit = 1e3)
  expected <- comparePcplus(testy, testlambda[length(testlambda)], testbandwidth, testsd, testthresh, 1e3)
  expect_equal(object, expected)
  
  object <- pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, sd = testsd,
                   nlambda = 30L, thresh = testthresh, maxit = 5)
  expected <- comparePcplus(testy, testlambda[length(testlambda)], testbandwidth, testsd, testthresh, 5)
  expect_equal(object, expected)
  
  object <- pcplus(y = testy, lambda = testlambda, bandwidth = testbandwidth, sd = testsd,
                   nlambda = 30L, thresh = testthresh, maxit = 1)
  expected <- comparePcplus(testy, testlambda[length(testlambda)], testbandwidth, testsd, testthresh, 1)
  expect_equal(object, expected)
})