#!/usr/bin/env python3
# -*- coding: UTF-8 -*-
#from scipy.special import gamma, binom
import numpy as np
import matplotlib.pyplot as plt

from mpmath import mp, mpf
mp.dps = 1000

voxel_num = 2
phase_range = mp.pi/2
phase_init = mp.pi/20 #mpf(0.0)
U_points = voxel_num * 1000

# noise_ratio = mpf(0.0) #1e8

total_periods = 10
rf_samples_per_period = 5
# max polynomial order equals  rf_samples_per_period * total_periods 

# B0=1.5T freq=64Mhz, period = 15.6 ns
period = mpf(15.6/1000/1000) #ms
omega = 2.0*mp.pi/period
#T2s_scale = 0.01 #ms # need to be 10ms
T2s_scale = total_periods*period #ms # need to be 10ms
T2s_min = T2s_scale/1000.0
#print(period)
#ms
time_steps = np.array(mp.linspace(mpf(0), mpf(period*total_periods), rf_samples_per_period*total_periods))
tmp = [mp.rand() for n in range(voxel_num)]
voxel_amplitudes = np.array(tmp)
tmp = [mp.rand() for n in range(voxel_num)]
voxel_T2s_decay = np.array(tmp)*(T2s_scale-T2s_min) + T2s_min
voxel_all = np.append(voxel_amplitudes,voxel_T2s_decay/T2s_scale)
if voxel_num == 5:
    voxel_all = np.array([mpf(0.822628),mpf(0.691376),mpf(0.282906),mpf(0.226013),mpf(0.90703),mpf(0.144985),mpf(0.328563),mpf(0.440353),mpf(0.662462),mpf(0.720518)])
    #voxel_all = np.array([mpf(0.592606),mpf(0.135168),mpf(0.365712),mpf(0.667536),mpf(0.437378),mpf(0.918822),mpf(0.943879),mpf(0.590338),mpf(0.685997),mpf(0.658839)])
    voxel_amplitudes = voxel_all[:voxel_num]
    voxel_T2s_decay = voxel_all[voxel_num:]*T2s_scale

voxel_phases = np.array(mp.linspace(0,phase_range, voxel_num))
if len(voxel_amplitudes) != len(voxel_phases):
    print("ERROR! Size of amplitude and phase arrays do not match!")
    raise

def gen_rf_signal(time):
    '''Generates demodulated signal at radio frequence using voxels
    amplitudes, T2s decays, and phases.

    '''
    tmp = [mpf(0.0) for n in range(len(time))]
    mag_sin = np.array(tmp)
    mag_cos = np.array(tmp)
    for t in range(len(time)):
        for i in range(voxel_num):
            amp = voxel_amplitudes[i] * (
                mp.exp(-time[t]/voxel_T2s_decay[i])
                ) + ( 0.0 
                          # + np.random.rand()*noise_ratio
                        )
            mag_sin[t] += amp * mp.sin(
                voxel_phases[i] + phase_init
                )
            mag_cos[t] += amp * mp.cos(
                voxel_phases[i] + phase_init
                )
    return mag_sin, mag_cos

def factorial(n):
    return mp.gamma(n+1)

def binom(n,k):
    return factorial(n)/(factorial(k)*factorial(n-k))

def shiftedLegendre(n):
    coeffs = []
    for k in range(n+1):
        val = mpf(-1)**n * binom(mpf(n),mpf(k)) * binom(n+k,k) * (-1)**k
        coeffs.insert(0,val)
    return np.poly1d(coeffs)

def K ( i, j):
    polyL = L[i] #precomputed shiftedLegendre(i)
    return polyL.coeffs[-j-1]

def GetU (lambdas):
    x = np.array(mp.linspace(0,1, U_points))
    tmp = [mpf(0.0) for n in range(len(lambdas))]
    mag_sin = np.array(tmp)
    tmp = [mpf(0.0) for n in range(U_points)]
    U = np.array(tmp)
    for i in range (len(lambdas)):
        polyL = L[i] #shiftedLegendre(i)        
        U += lambdas[i]*polyL(x)
    return U

def GetLambda(mag_rf):
    M_cutoff = len(mag_rf)
    all_lambda = []
    for i in range(M_cutoff):
        lambd = mpf(0)
        for j in range(i+1):
            lambd += K(i,j)*mag_rf[i]
        all_lambda.append(lambd)
    # tmp = [mpf(0.0) for n in range(M_cutoff)]
    # all_lambda = np.array(tmp)
    # all_lambda[29] = mpf(1.0)
    return all_lambda


mag_sin, mag_cos = gen_rf_signal(time_steps)

sign = ""
for i in range(voxel_num):
    if i%5 == 0:
        sign+="\n"
    sign = sign + '{:3.2g}'.format(float(voxel_amplitudes[i] * mp.sin(
        voxel_phases[i] + phase_init
        )))+"/"+'{:3.2g}'.format(float(voxel_T2s_decay[i]))+", "


plt.plot(mag_sin, ls='-', marker='o')
plt.title("Signal to restore amp/decay_T:"+sign)
plt.savefig("signal.pdf")
plt.clf()


L = [] # Shifted Legendre polinomials
for i in range(len(mag_sin)):
    polyL = shiftedLegendre(i)        
    L += [polyL]

x = np.linspace(0,1, U_points)
polyL_val = np.array([float(L[-1](x[n])) for n in range(U_points)])


plt.plot(x,polyL_val)
plt.title("Legendre polynom of order "+str(len(L)))
plt.savefig("polyL.pdf")
plt.clf()
print("Output of last poly done.")

lambdas = GetLambda(mag_sin)
print(len(lambdas))
U = GetU(lambdas)

x = np.linspace(0,1, U_points)
mag_x = np.linspace(0,1, len(mag_sin))
plt.plot(x,U)
plt.savefig("plt.pdf")
plt.clf()