#!/usr/bin/env python
# coding: utf-8

# # Sequential Quadratic Programming
# 
# Copyright (C) 2020 Andreas Kloeckner
# 
# <details>
# <summary>MIT License</summary>
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
# 
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
# 
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
# THE SOFTWARE.
# </details>

# (which is just using Newton on a Lagrangian)

# In[2]:


import numpy as np
import numpy.linalg as la


# Here's an objective function $f$ and a constraint $g(x)=0$:

# In[3]:


def f(vec):
    x = vec[0]
    y = vec[1]
    return (x-2)**4 + 2*(y-1)**2

def g(vec):
    x = vec[0]
    y = vec[1]
    return x + 4*y - 3


# Now define the Lagrangian, its gradient, and its Hessian:

# In[4]:


def L(vec):
    lam = vec[2]
    return f(vec) + lam * g(vec)

def grad_L(vec):
    x = vec[0]
    y = vec[1]
    lam = vec[2]
    
    return np.array([
        4*(x-2)**3 + lam,
        4*(y-1) + 4*lam,
        x+4*y-3
        ])

def hess_L(vec):
    x = vec[0]
    y = vec[1]
    lam = vec[2]
    
    return np.array([
        [12*(x-2)**2, 0, 1],
        [0, 4, 4],
        [1, 4, 0]
        ])    


# At this point, we only need to find an *unconstrained* minimum of the Lagrangian!
# 
# Let's fix a starting vector `vec`:

# In[5]:


vec = np.zeros(3)


# Implement Newton and run this cell in place a number of times (Ctrl-Enter):

# In[18]:


vec = vec - la.solve(hess_L(vec), grad_L(vec))
vec


# Let's first check that we satisfy the constraint:

# In[21]:


g(vec)


# Next, let's look at a plot:

# In[22]:


import matplotlib.pyplot as pt
x, y = np.mgrid[0:4:30j, -3:5:30j]
pt.contour(x, y, f(np.array([x,y])), 20)
pt.contour(x, y, g(np.array([x,y])), levels=[0])
pt.plot(vec[0], vec[1], "o")


# In[ ]: