ass4_q2

# Eli Simhayev - 2017
# Question 2

# ===== Task C =====

mu = 20; # Global variable for penalty function.

"""
Backtracking line search using the Armijo condition.
(source: lecture notes, page 108)
"""
function linesearch(f::Function,x,d,gk,alpha0=1.0,beta=0.5,c=1e-4)
    alphaj = alpha0;
    for jj = 1:10
        x_temp = x + alphaj*d;
        if f(x_temp) <= f(x) + alphaj*c*dot(d,gk)
            break;
        else
            alphaj = alphaj*beta;
        end
    end
    return alphaj;
end

"""
Steepest Descent:
    x⁽ᵏ⁺¹⁾ = x⁽ᵏ⁾ - α⁽ᵏ⁾*∇f(x⁽ᵏ⁾)
"""
function sd(f::Function, grad_f::Function, x_SD, maxIter)
    grad = grad_f(x_SD); # ∇f(x⁽⁰⁾)
    alpha_SD = linesearch(f,x_SD,-grad,grad); # α⁽⁰⁾
    for k=1:maxIter
        x_SD = x_SD - alpha_SD*grad;
        grad = grad_f(x_SD); # ∇f(x⁽ᵏ⁾)
        alpha_SD = linesearch(f,x_SD,-grad,grad); # α⁽ᵏ⁾
    end

    return x_SD;
end

### main
f = (x) -> (x[1]+x[2])^2 - 10(x[1]+x[2]);
c1eq = (x) -> 3*x[1] + x[2] - 6;
c2ieq = (x) -> x[1]^2+x[2]^2-5;
c3ieq = (x) -> -x[1];

f_penalty = (x) -> f(x) + mu*(c1eq(x)^2 + max(0,c2ieq(x))^2 + max(0,c3ieq(x))^2);
grad_f_penalty = (x) -> [2(x[1]+x[2])-10 + mu*(6(3*x[1]+x[2]-6)+max(0,4*x[1]*(x[1]^2+x[2]^2-5))+max(0,-2*x[1]));
                        2(x[1]+x[2])-10 + mu*(2(3*x[1]+x[2]-6)+max(0,4*x[2]*(x[1]^2+x[2]^2-5)))];

x_SD = [1; 2]; # inital guess
ans = sd(f_penalty, grad_f_penalty, x_SD, 100);
println("ans = ", ans); # ans = [1.42599,1.72947]

# Note: mu = 20