import csv
import numpy as np
from numpy import sin, cos, tan, pi, exp, sqrt
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d.axes3d import Axes3D
from matplotlib import cm
import time
import sys


# optimization parameters
x0 = [0.0, 0.0]

# for line search '' or 'one', 'simple', 'exact', 'golden', 'armijo'
lsmode = 'simple'
nmaxlsiter = 100
# for lsmode == 'golden'
alphaeps = 1.0e-2
# for lsmode == 'exact'
dump = 0.3

# for global optimization
eps   = 1.0e-5
nmaxiter = 200
iprintiterval = 1


# target function
#functype = 'ellipsoid'
functype = 'general'

# differentiation x mesh
h = 0.01

# graph plot parameters
fplot = 1
ngdata = 51
xgmin = -4.0
xgmax =  4.0
ygmin = -4.0
ygmax =  4.0
tsleep = 0.3


if len(sys.argv) >= 2:
    x0[0] = float(sys.argv[1])
if len(sys.argv) >= 3:
    x0[1] = float(sys.argv[2])
if len(sys.argv) >= 4:
    lsmode = sys.argv[3]
if len(sys.argv) >= 5:
    functype = sys.argv[4]

if functype == 'ellipsoid':
    alpha = 0.03
else:
    alpha = 0.01

if lsmode == 'golden':
    alpha = 1.0
elif lsmode == 'armijo':
    alpha = 1.0


def linesearch(mode, func, x0, diff, alpha, dump, nmaxiter, eps):
    if mode == '' or mode == 'one':
        return linesearch_one(func, x0, diff, alpha)
    if mode == 'simple':
        return linesearch_simple(func, x0, diff, alpha, nmaxiter)
    if mode == 'exact':
        return linesearch_exact(func, x0, diff, 1.0e-3*alpha, dump, 1, eps)
    if mode == 'golden':
        return linesearch_golden(func, x0, diff, alpha, nmaxiter, eps)
    if mode == 'armijo':
        return linesearch_armijo(func, x0, diff, alpha, nmaxiter, eps)

def linesearch_armijo(func, x0, diff, alpha, nmaxiter, eps):
    xi  = 0.5   # 0 < xi < 1
    tau = 0.5   # 0 < tau < 1
    dr2 = np.inner(diff, diff)
    f0 = func(x0)
#    print("f0=", f0, "   diff=", diff)
    for i in range(nmaxiter):
        dx = -alpha * diff
        fp = func(x0 + dx)
        fxi = f0 - xi * alpha * dr2
#        print("i={:3d}  alpha={:6.3f}  fp={:6.3f}  fxi={:6.3f}".format(i, alpha, fp, fxi), "   dx=", dx)
        if fp <= fxi or alpha < eps:
            return i, x0 + dx, dx, alpha
        else:
             alpha *= tau
    return -1, x0 + dx, dx, alpha

def linesearch_golden(func, x0, diff, xrange, nmaxiter, eps):
    eta = 0.381966011   # (sqrt(5) - 1) / (sqrt(5) + 1)
    dr2 = np.inner(diff, diff)
    dr  = sqrt(dr2)
    x = [0.0, 0.0 + xrange*eta, xrange - xrange*eta, xrange]
    for i in range(nmaxiter):
        alpha2 = (x[1] + x[2]) / 2.0
        dx = -alpha2 * diff
        if abs(x[0] - x[1]) < eps:
            return i, x0 + dx, dx, alpha2

#        f0 = func(x0 - x[0] * diff)
        f1 = func(x0 - x[1] * diff)
        f2 = func(x0 - x[2] * diff)
#        f3 = func(x0 - x[3] * diff)
#        print("x=[{:6.3f}, {:6.3f}, {:6.3f}, {:6.3f}]  f=[{:8.3f}, {:8.3f}, {:8.3f}, {:8.3f}]".format(
#                x[0], x[1], x[2], x[3], f0, f1, f2, f3))
#        print("eps=", eps, "  eta=", eta)
        if f1 < f2:
            dxe = x[2] - x[0]
            x = [x[0], x[0] + dxe * eta, x[2] - dxe * eta, x[2]]
        else:
            dxe = x[3] - x[1]
            x = [x[1], x[1] + dxe * eta, x[3] - dxe * eta, x[3]]
    return -1, x0 + dx, dx, alpha2

def linesearch_exact(func, x0, diff, h, dump, nmaxiter, eps):
    dr2 = np.inner(diff, diff)
    dr  = sqrt(dr2)
    for i in range(nmaxiter):
        fm = func(x0 - h * diff / dr)
        f0 = func(x0)
        fp = func(x0 + h * diff / dr)
        f1 = (fp - fm) / 2.0 / h
        f2 = (fp - 2.0 * f0 + fm) / h / h
        alpha2 = f1 / (abs(f2) + dump)
#        print("f1 = ", f1, "  f2 = ", f2, "  dr = ", dr)
        dx0 = - alpha2 * diff / dr
        x0 += dx0
        dr0 = sqrt(np.inner(dx0, dx0))
        if abs(alpha2/dr) < eps:
            return i, x0, -alpha2 * diff, alpha2
    return -1, x0, -alpha2 * diff, alpha2


def linesearch_one(func, x0, diff, alpha):
    return 1, x0 - alpha * diff, -alpha * diff, alpha

def linesearch_simple(func, x0, diff, alpha, nmaxiter):
    f0 = func(x0)
#    print("x0=", x0, "   f0=", f0)
    xprev = x0.copy()
    x = x0.copy()
    for i in range(1, nmaxiter+1):
        alpha2 = i * alpha
        for j in range(len(x0)):
           x[j] = x0[j] - alpha2 * diff[j]
        f1 = func(x)
#        print("i:{}".format(i), "  x=", x, "  f1=", f1)
        if f1 > f0:
            return i, xprev, -alpha2 * diff, alpha2
        else:
            xprev = x.copy()
            f0 = f1
    return -1, x, -alpha2 * diff, alpha2


# first derivative of func(x)
def ediff1(i, x):
    if i == 0:
        return 2.0 * x[0]
    if i == 1:
        return 18.0 * x[1]

def gdiff1(i, x):
    if i == 0:
        return - 10.0 - 60.0 * x[0] + 4.5 * x[0]*x[0] + 12.0 * x[0]*x[0]*x[0] \
                + 3.0 * x[1] * x[1]
    if i == 1:
        return 30.0 - 60.0 * x[1] + 12.0 * x[1]*x[1]*x[1] \
                + 6.0 * x[0] * x[1]

def efunc(x):
    return x[0]*x[0] + 9.0*x[1]*x[1]

def gfunc(x):
    return -3.0 - 10.0 * x[0] - 30.0 * x[0]*x[0] + 1.5 * x[0]*x[0]*x[0] + 3.0 * x[0]*x[0]*x[0]*x[0] \
                + 30.0 * x[1] - 30.0 * x[1]*x[1] + 3.0 * x[1]*x[1]*x[1]*x[1] \
                + 3.0 * x[0] * x[1] * x[1]

# global function variables
if functype == 'ellipsoid':
    diff1 = ediff1
    func = efunc
else:
    diff1 = gdiff1
    func = gfunc


def main():
    global plt, ngdata, xgmin, xgmax
    global x0, alpha, dump, eps, nmaxiter, iprintinterval
    global func, diff1

    print("Find minimum / maximum point by Newton-Raphson method")
    print("")

# plot surface graph
    if fplot == 1:
        xgstep = (xgmax - xgmin) / (ngdata - 1)
        ygstep = (ygmax - ygmin) / (ngdata - 1)
        xg = np.empty([ngdata, ngdata])
        yg = np.empty([ngdata, ngdata])
        zg = np.empty([ngdata, ngdata])
        for ix in range(ngdata):
            for iy in range(ngdata):
                xg[ix, iy] = xgmin + ix * xgstep
                yg[ix, iy] = ygmin + iy * ygstep
                zg[ix, iy] = func([xg[ix][iy], yg[ix][iy]])

        fig = plt.figure(figsize = (10, 5))
        ax  = fig.add_subplot(1, 2, 1)
        ax2 = fig.add_subplot(1, 2, 2, projection='3d')
#        ax = fig.add_subplot(111, projection='3d')
#        surf = ax.plot_surface(xg, yg, zg, cmap=cm.coolwarm)
        surf = ax2.plot_wireframe(xg, yg, zg, rstride=2, cstride=2)
#        ax.set_zlim3d(-3.01, 3.01)
#        plt.colorbar(surf, shrink=0.5, aspect=10)
        ax.set_title('contour')
        contour = ax.contourf(xg, yg, zg, levels = 51, cmap = 'Spectral')
#        contour = ax.contour(xg, yg, zg, levels = 51, cmap = 'Spectral')
        solve, = ax.plot([], color = 'blue', marker = 'o', linestyle = '-')
#        solve, = ax.plot([], color = 'blue', marker = 'o', linestyle = '')
        plt.pause(0.1)
#        plt.show()

    n = len(x0)
    Si = np.empty(n)

# calculate initial parameters
    f = func(x0)
    print("x0 = ({}, {}): f = {}".format(x0[0], x0[1], f))
    xt = []
    yt = []
    xt.append(x0[0])
    yt.append(x0[1])

# optimization start
    for iter in range(nmaxiter):
        for i in range(0, n):
            Si[i] = diff1(i, x0)

        insiter, x0, dx, alpha2 = linesearch(lsmode, func, x0, Si, alpha, dump, nmaxlsiter, alphaeps)
        print("   dx=", dx)
        f = func(x0)
        dxmax = max(abs(dx))
        print("{:4d}({:3d}): x = ({:12.6g}, {:12.6g}) dx = {:12.6g}: f = {:12.6g}".format(
                        iter, insiter, x0[0], x0[1], dxmax, f))
        if fplot == 1:
            xt.append(x0[0])
            yt.append(x0[1])
            solve.set_data(xt, yt)
            plt.pause(0.1)
            time.sleep(tsleep)

        if dxmax < eps:
            print("Converged at x = ", x0, " dx = {}   f = {}".format(dxmax, f))
            break;
    else:
        print("Not converged")

    if fplot == 1:
        print("Press enter to terminate:", end = '')
        ret = input()


if __name__ == "__main__":
    main()

