# Zaznam prikazov pre vypocet geometrickeho (L1) medianu za ohraniceni
# Predmet "Stochasticke optimalizacne metody", Radoslav Harman, FMFI UK

sumdist <- function(x, X) {
   # x ... m-rozmerny vektor
   # X ... matica rozmeru nxm s polohami n datovych bodov v R^m
   # Vysledok je suma vzdialenosti bodu x ku vsetkym datovym bodom
   
   return(sum(sqrt(rowSums((X - rep(1,nrow(X)) %*% t(x))^2))))
}

minus.sumdist <- function(x, X) {
   return(-sum(sqrt(rowSums((X - rep(1,nrow(X)) %*% t(x))^2))))
}

objective.function <- function(x, X, l, u, r) {
   sumdist <- sum(sqrt(rowSums((X - rep(1,nrow(X)) %*% t(x))^2)))
   penalty <- r*sum(pmin(0, c(x - l, u - x))^2)
   return(sumdist + penalty)
}

plot.objective.function <- function(X, l, u, r) 
{
   # X ... matica rozmeru nxm s polohami n datovych bodov v R^m
   # Nakresli 201x201 filled contour pre funkciu sumdist s penalizaciou
   
   min1 <- min(X[,1]); max1 <- max(X[,1])
   min2 <- min(X[,2]); max2 <- max(X[,2])
   x1 <- seq(min1 - 0.1*(max1 - min1), max1 + 0.1*(max1 - min1), length = 201)
   x2 <- seq(min2 - 0.1*(max2 - min2), max2 + 0.1*(max2 - min2), length = 201)
   Z <- matrix(0, ncol = 201, nrow = 201)
   for (i1 in 1:201) {
      for (i2 in 1:201) {
         Z[i1, i2] <- min(objective.function(c(x1[i1],x2[i2]),
                                             X, l, u, r), 10000)
      }
   }
   
   library(viridisLite)
   image(x1, x2, Z, col = viridis(100), asp = 1)
   points(X, pch = 19, col = "red")
}

# Nakreslime si ucelovu fciu pre niektore hodnoty r

X <- matrix(runif(200, min = -10, max = 10), ncol = 2)
plot.objective.function(X, l = c(-5, 5), u = c(5, 10), r = 100)

# Vypocitajme optimum pomocou Nelder-Mead (Zangwillov postup)

res1 <- optim(c(0, 0), objective.function, X = X, l = c(-5, 5), u = c(5, 10),
      r = 1, method = "Nelder-Mead")$par
res2 <- optim(res1, objective.function, X = X, l = c(-5, 5), u = c(5, 10),
              r = 10, method = "Nelder-Mead")$par
res3 <- optim(res2, objective.function, X = X, l = c(-5, 5), u = c(5, 10),
              r = 100, method = "Nelder-Mead")$par
res4 <- optim(res3, objective.function, X = X, l = c(-5, 5), u = c(5, 10),
              r = 1000, method = "Nelder-Mead")$par
res5 <- optim(res4, objective.function, X = X, l = c(-5, 5), u = c(5, 10),
              r = 10000, method = "Nelder-Mead")$par
res6 <- optim(res5, objective.function, X = X, l = c(-5, 5), u = c(5, 10),
              r = 100000, method = "Nelder-Mead")$par
res7 <- optim(res6, objective.function, X = X, l = c(-5, 5), u = c(5, 10),
              r = 1000000, method = "Nelder-Mead")$par
print(rbind(res1, res2, res3, res4, res5, res6, res7))


# Idealne je, ak ma procedura vlastny sposob zohladnenia ohraniceni:

optim(c(0, 0), sumdist, X = X, method = "L-BFGS-B", 
      lower = c(-5, 5), upper = c(5, 10)) 

pso::psoptim(c(0, 0), sumdist, X = X, 
             lower = c(-5, 5), upper = c(5, 10))

summary(GA::ga(type = "real-valued", fitness = minus.sumdist, X = X,
       lower = c(-5, 5), upper = c(5, 10), monitor = FALSE))

# Zlozitejsie ohranicenia ako "box constraints" ma ovela menej procedur
help("constrOptim")