import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
import seaborn as sns

import pysindy as ps
import random

import sys
sys.path.append("../")
import os
os.environ["CUDA_DEVICE_ORDER"] = "PCI_BUS_ID"

def fourth_order_diff(x, dt):
    dx = np.zeros([x.shape[0], x.shape[1]])
    dx[0] = (-11.0 / 6) * x[0] + 3 * x[1] - 1.5 * x[2] + x[3] / 3
    dx[1] = (-11.0 / 6) * x[1] + 3 * x[2] - 1.5 * x[3] + x[4] / 3
    dx[2:-2] = (-1.0 / 12) * x[4:] + (2.0 / 3) * x[3:-1] - (2.0 / 3) * x[1:-3] + (1.0 / 12) * x[:-4]
    dx[-2] = (11.0 / 6) * x[-2] - 3.0 * x[-3] + 1.5 * x[-4] - x[-5] / 3.0
    dx[-1] = (11.0 / 6) * x[-1] - 3.0 * x[-2] + 1.5 * x[-3] - x[-4] / 3.0
    return dx / dt

def sample_trajectory(x0, coefs, library, timesteps, dt, batch_size):
    # bug with intel mk4 if you import torch outside of this function and try
    # to fit pysindy model, leaving import torch in here works for some reason
    import torch
    from library_utils import Library
    library = Library(n=3, poly_order=2, include_constant=True)
    coefs = np.transpose(coefs, (0, 2, 1))
    coefs = torch.from_numpy(coefs).type(torch.FloatTensor).to(1)
    xs = []
    curr = np.array([x0 for i in range(batch_size)])
    curr = torch.from_numpy(curr).type(torch.FloatTensor).to(1)
    for i in range(timesteps):
        curr_lib = library.transform(curr)
        coef_idx = np.random.randint(0, len(coefs), batch_size)
        curr_coefs = coefs[coef_idx]
        dx = torch.matmul(curr_lib.unsqueeze(1), curr_coefs).squeeze(1)
        curr = curr + dx * dt
        xs.append(curr.detach().cpu().numpy())
    xs = np.array(xs)
    return np.transpose(xs, (1, 0, 2))

def sample_trajectory2(x0, coefs, library, timesteps, dt, batch_size):
    coefs = np.transpose(coefs, (0, 2, 1))
    coefs_mean, coefs_std = coefs.mean(0), coefs.std(0)
    coefs_mean = np.array([coefs_mean for _ in range(batch_size)])
    coefs_std = np.array([coefs_std for _ in range(batch_size)])
    xs = []
    curr = np.array([x0 for _ in range(batch_size)])
    for i in range(timesteps):
        curr_lib = library.transform(curr).reshape(10, 1, 10)
        noise = np.random.normal(0, 1, (batch_size, coefs.shape[1], coefs.shape[2]))
        curr_coefs = coefs_mean + coefs_std * noise
        dx = np.matmul(curr_lib, curr_coefs).squeeze(1)
        curr = curr + dx * dt
        xs.append(curr)
    xs = np.array(xs)
    return np.transpose(xs, (1, 0, 2))

def plot_samples(xs, samples, num_samples=4, dpi=300, figsize=None, filename=None):
    sns.set()

    # https://dawes.wordpress.com/2014/06/27/publication-ready-3d-figures-from-matplotlib/
    # fig = plt.figure(figsize=(batch_size + 1, 3.5), dpi=300)
    if figsize is not None:
        fig = plt.figure(figsize=figsize, dpi=dpi)
    else:
        fig = plt.figure(dpi=dpi)
    fig.tight_layout()
    ct = 0
    sample_idx = 0
    for i in range(num_samples):
        ax = fig.add_subplot(1, num_samples, ct + 1, projection='3d')
        if i == 0:
            ax.plot(xs[:, 0], xs[:, 1], xs[:,2], color='red')
        else:
            curr_sample = samples[sample_idx]
            while np.any(np.isnan(curr_sample)):
                sample_idx += 1
                curr_sample = samples[sample_idx]
            ax.plot(curr_sample[:, 0], curr_sample[:, 1], curr_sample[:,2], color='blue')
            sample_idx += 1
        ct += 1

        ax.grid(False)
        color_tuple = (1.0, 1.0, 1.0, 0.0)

        ax.xaxis.set_pane_color(color_tuple)
        ax.yaxis.set_pane_color(color_tuple)
        ax.zaxis.set_pane_color(color_tuple)
        ax.xaxis.line.set_color(color_tuple)
        ax.yaxis.line.set_color(color_tuple)
        ax.zaxis.line.set_color(color_tuple)

        ax.set_xticks([])
        ax.set_yticks([])
        ax.set_zticks([])

    plt.subplots_adjust(wspace=0)
    
    if filename is not None:
        plt.savefig(filename)
        plt.close()
    plt.show()
    plt.close()


# 1
np.random.seed(862023)
random.seed(862023)
dt = 0.01
x_train = np.load('../data/rossler/scale-1.0/x_train.npy')
x_dot = fourth_order_diff(x_train, dt)
x_test = np.load('../data/rossler/scale-1.0/x_test_0.npy')
x0 = x_test[0]
feature_names = ['x', 'y', 'z']
# Instantiate and fit the SINDy model 
library = ps.PolynomialLibrary(degree=2, include_bias=True)
optimizer = ps.SR3(
    threshold=0.05, thresholder="l0", max_iter=1000, normalize_columns=False, tol=1e-1
)
model = ps.SINDy(feature_names=feature_names, feature_library=library, optimizer=optimizer)
model.fit(x_train, x_dot=x_dot, t=dt, ensemble=True, quiet=True, n_models=500)
ensemble_coefs = np.array(model.coef_list)
model.print()
model.coef_list = np.mean(ensemble_coefs, 0)
the_std = ensemble_coefs.std(0)
print(the_std)
print(np.max(the_std))
np.random.seed(862023)
random.seed(862023)
samples = sample_trajectory(x0, ensemble_coefs, library, 10000, dt, 10)
plot_samples(x_test, samples, 5, 300, (20, 20), "../results/esindy_rossler_gen_1")

# 5
np.random.seed(862023)
random.seed(862023)
dt = 0.01
x_train = np.load('../data/rossler/scale-5.0/x_train.npy')
x_dot = fourth_order_diff(x_train, dt)
x_test = np.load('../data/rossler/scale-5.0/x_test_0.npy')
x0 = x_test[0]
feature_names = ['x', 'y', 'z']
# Instantiate and fit the SINDy model 
library = ps.PolynomialLibrary(degree=2, include_bias=True)
optimizer = ps.SR3(
    threshold=0.1, thresholder="l0", max_iter=1000, normalize_columns=False, tol=1e-1
)
model = ps.SINDy(feature_names=feature_names, feature_library=library, optimizer=optimizer)
model.fit(x_train, x_dot=x_dot, t=dt, ensemble=True, quiet=True, n_models=500)
ensemble_coefs = np.array(model.coef_list)
model.print()
model.coef_list = np.mean(ensemble_coefs, 0)
the_std = ensemble_coefs.std(0)
print(the_std)
print(np.max(the_std))
np.random.seed(862023)
random.seed(862023)
samples = sample_trajectory(x0, ensemble_coefs, library, 10000, dt, 10)
plot_samples(x_test, samples, 5, 300, (20, 20), "../results/esindy_rossler_gen_5")

# 10
np.random.seed(862023)
random.seed(862023)
dt = 0.01
x_train = np.load('../data/rossler/scale-10.0/x_train.npy')
x_dot = fourth_order_diff(x_train, dt)
x_test = np.load('../data/rossler/scale-10.0/x_test_0.npy')
x0 = x_test[0]
feature_names = ['x', 'y', 'z']
# Instantiate and fit the SINDy model 
library = ps.PolynomialLibrary(degree=2, include_bias=True)
optimizer = ps.SR3(
    threshold=0.2175, thresholder="l0", max_iter=1000, normalize_columns=False, tol=1e-1
)
model = ps.SINDy(feature_names=feature_names, feature_library=library, optimizer=optimizer)
model.fit(x_train, x_dot=x_dot, t=dt, ensemble=True, quiet=True, n_models=500)
ensemble_coefs = np.array(model.coef_list)
model.print()
model.coef_list = np.mean(ensemble_coefs, 0)
the_std = ensemble_coefs.std(0)
print(the_std)
print(np.max(the_std))
np.random.seed(862023)
random.seed(862023)
samples = sample_trajectory(x0, ensemble_coefs, library, 14500, dt, 100)
plot_samples(x_test, samples, 5, 300, (20, 20), None)
plot_samples(x_test, samples, 5, 300, (20, 20), "../results/esindy_rossler_gen_10")