#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Mon May 29 08:59:36 2018

@author: helge
"""

import numpy as np
import matplotlib.pyplot as plt

setting = 4

#Setting 1
if setting == 1:
    N = 4
    K_max = 10**3
    sigma = 10**-3

#Setting 2
elif setting == 2:
    N = 7
    K_max = 10**5#10**2#10**10#
    sigma = 10**-5

#Setting 3
elif setting == 3:
    N = 10
    K_max = float("inf")
    sigma = 0

#Setting 4
elif setting == 4:
    N = 42
    K_max = 10**2#10**10#10**5#
    sigma = 0

M = 1

#Function that executes the model
def HilbertMatrix(N):     
    id_i = np.array([range(N),]*N, dtype = float).transpose()+1
    id_j = np.array([range(N),]*N, dtype = float)+1
    HM_out = np.divide(float(1),id_i+id_j-1)
    return HM_out

def invsvd(U,w,V,b,K_max,Amat):    
    w_inv = np.divide(1,w)
    w_inv[np.divide(w[1],w) > K_max] = 0
    S_inv = np.diag(w_inv)
    x_estim = np.matmul(np.transpose(V), np.matmul(S_inv, np.matmul(np.transpose(U), b)))
    x_invEst = np.matmul(np.linalg.inv(Amat), b)
    return [x_estim,x_invEst]

HM = HilbertMatrix(N)
[U,w,V] = np.linalg.svd(HM)

K = np.amax(w)/np.amin(w)
print("Konditionszahl ist " + str(K))
x = np.array([np.sin(2*np.pi*(N_i)/(N-1)) for N_i in range(N)])

b_true = np.matmul(HM, x)
x_est = np.empty([M,N])
x_estInv = np.empty([M,N])

for im in range(M):
    b = b_true + sigma*np.random.randn(*np.shape(b_true))
    (x_est[im,],x_estInv[im,]) = invsvd(U,w,V,b,K_max,HM)
    
    
fig_traj = plt.figure()
plt.plot(range(N),x, label="true")
plt.plot(range(N),x_est.transpose(),'o', label="estimate" ) 
if setting==3:
    plt.plot(range(N),x_estInv.transpose(),'xg', markersize = 17, label="build in inv") 
plt.title('Estimation of X via matrix inversion, setting ' + str(setting))
plt.legend()
plt.ylabel('x')