# This file analyses the evolution of the control parameters with
# the forward speed, for straight walking.

# import libraries
import numpy as np
import matplotlib
from matplotlib import pyplot as plt
import AllData as data
import math as mt

matplotlib.rc('text', usetex=True)
matplotlib.rcParams['text.latex.preamble']=[r"\usepackage{amsmath}"]

data_130 = np.array(
[[ 0.26306896 , 0.29468004 , 0.29463085 , 0.29438842 , 0.28060345],
 [ 0.27531314 , 0.43783773 , 0.33456379 , 0.37428996 , 0.1247026 ],
 [ 0.2721999  , 0.02755981 , 0.09001297 , 0.06109174 , 0.18487643],
 [ 0.88572765 , 0.65508136 , 0.85776529 , 0.77833629 , 0.6539538 ],
 [ 0.51115829 , 0.22080703 , 0.71024291 , 0.1696709  , 0.05019016],
 [ 0.39421737 , 0.3465862  , 0.91909111 , 0.89782264 , 0.68197176],
 [ 0.23320543 , 0.57525863 , 0.58228834 , 0.79419303 , 0.4906143 ],
 [ 0.66925576 , 0.01332728 , 0.0752121  , 0.1721729  , 0.31276503],
 [ 0.06532468 , 0.84293599 , 0.90280848 , 0.62074986 , 0.35263979],
 [ 0.19868194 , 0.02685454 , 0.0012362  , 0.00820936 , 0.03686664],
 [ 0.53389895 , 0.76386005 , 0.08435489 , 0.89854559 , 0.65498252]])
speed_130 = np.array([ 1.29193 , 1.31824 , 1.32632 , 1.31473 , 1.31623])


data_135 = np.array(
[[ 0.27129242 , 0.26079315 , 0.27927909 , 0.27056925 , 0.26183364],
 [ 0.08610873 , 0.30316065 , 0.20409642 , 0.16908438 , 0.03295554],
 [ 0.27277156 , 0.20449949 , 0.14139695 , 0.02831467 , 0.2102248 ],
 [ 0.55486595 , 0.63703593 , 0.81418628 , 0.49557138 , 0.80738218],
 [ 0.03899627 , 0.71339649 , 0.07218607 , 0.06754768 , 0.06516773],
 [ 0.27010179 , 0.43302529 , 0.66497029 , 0.41994964 , 0.53987226],
 [ 0.34575278 , 0.20574454 , 0.94652271 , 0.70828554 , 0.53404785],
 [ 0.96413882 , 0.1767621  , 0.60326942 , 0.53276971 , 0.30074214],
 [ 0.57881503 , 0.19061077 , 0.42899488 , 0.92865216 , 0.45416244],
 [ 0.20767772 , 0.11629922 , 0.01907767 , 0.2733392  , 0.01342316],
 [ 0.90156193 , 0.23990507 , 0.82302348 , 0.08732173 , 0.31959929]])
speed_135 = np.array([ 1.3506  , 1.3706  , 1.37489 , 1.36168 , 1.37323])


data_140 = np.array(
[[ 0.27789929 , 0.28092466 , 0.28722329 , 0.27946009 , 0.2818072 ],
 [ 0.29110316 , 0.35374093 , 0.4027871  , 0.21745931 , 0.2660519 ],
 [ 0.23270031 , 0.09827144 , 0.00400048 , 0.21228746 , 0.26183031],
 [ 0.73680477 , 0.87291171 , 0.87559647 , 0.70043049 , 0.90203273],
 [ 0.11404543 , 0.35152542 , 0.40679547 , 0.06588764 , 0.08133952],
 [ 0.4366089  , 0.35698872 , 0.17774074 , 0.32458544 , 0.27118553],
 [ 0.47329742 , 0.6898791  , 0.62815282 , 0.50495946 , 0.63624455],
 [ 0.14440119 , 0.13460993 , 0.49741467 , 0.7955     , 0.66581793],
 [ 0.46274069 , 0.34692936 , 0.95300243 , 0.50851925 , 0.72111339],
 [ 0.05934562 , 0.01293419 , 0.0018083  , 0.04978636 , 0.08725768],
 [ 0.33698389 , 0.73305641 , 0.51199627 , 0.06608048 , 0.02488903]])
speed_140 = np.array([ 1.41982 , 1.42664 , 1.42804 , 1.40221 , 1.42361])


data_145 = np.array(
[[ 0.27270212 , 0.27844669 , 0.28079894 , 0.25664537 , 0.26103501],
 [ 0.17341533 , 0.28896178 , 0.37696949 , 0.02451072 , 0.08824337],
 [ 0.29841198 , 0.15014396 , 0.21392621 , 0.07754241 , 0.08935896],
 [ 0.61055261 , 0.9597376  , 0.75453295 , 0.8330073  , 0.41029232],
 [ 0.10694181 , 0.74049921 , 0.3762206  , 0.04513521 , 0.07524334],
 [ 0.86565872 , 0.52307051 , 0.44292399 , 0.31538837 , 0.543863  ],
 [ 0.35824687 , 0.39549847 , 0.46671403 , 0.41431516 , 0.52254003],
 [ 0.36014003 , 0.44893168 , 0.126237   , 0.81556702 , 0.72713574],
 [ 0.85394772 , 0.21586117 , 0.20837964 , 0.16566835 , 0.99227644],
 [ 0.06767767 , 0.01067683 , 0.08302784 , 0.40127943 , 0.01332673],
 [ 0.83304093 , 0.49063869 , 0.97214621 , 0.44853233 , 0.57187735]])
speed_145 = np.array([ 1.43668 , 1.47348 , 1.47141 , 1.4526 ,  1.43228])


data_150 = np.array(
[[  2.69923690e-01  , 2.67930190e-01  , 2.69104010e-01  , 2.62965700e-01  ,  2.70713760e-01],
 [  2.47441090e-01  , 3.13750110e-01  , 2.43143970e-01  , 1.98547490e-01  ,  3.64696050e-01],
 [  3.23502310e-01  , 9.93122800e-02  , 3.38612180e-01  , 1.43285780e-01  ,  2.32804300e-02],
 [  3.94076120e-01  , 6.36113310e-01  , 3.44500930e-01  , 2.75891180e-01  ,  7.67436600e-01],
 [  1.07328220e-01  , 6.07359200e-01  , 1.24206800e-01  , 1.55856620e-01  ,  2.27236090e-01],
 [  2.50335920e-01  , 2.51200150e-01  , 3.76729760e-01  , 7.83247700e-01  ,  1.79056530e-01],
 [  3.09026900e-01  , 3.71040970e-01  , 4.92739810e-01  , 6.21747140e-01  ,  4.52489920e-01],
 [  8.56310370e-01  , 8.56082800e-01  , 5.93620120e-01  , 5.94238400e-01  ,  8.24120890e-01],
 [  2.08491120e-01  , 2.09943060e-01  , 9.38155500e-02  , 8.15292490e-01  ,  4.68337020e-01],
 [  1.28893690e-01  , 7.36090000e-04  , 5.10895000e-02  , 2.86008500e-02  ,  7.27473000e-03],
 [  9.91600090e-01  , 8.61309880e-01  , 7.91580620e-01  , 7.02769950e-01  ,  6.30694650e-01]])
speed_150 = np.array([ 1.49739 , 1.52626 , 1.50051 , 1.49113 , 1.5013 ])


data_155 = np.array(
[[ 0.26658319 , 0.27305054 , 0.27312091 , 0.26579306 , 0.26436166],
 [ 0.30253821 , 0.37256444 , 0.33840544 , 0.29796575 , 0.15707807],
 [ 0.21204211 , 0.10769307 , 0.23799426 , 0.27195074 , 0.3698293 ],
 [ 0.48908226 , 0.76453847 , 0.73481966 , 0.45271403 , 0.55057466],
 [ 0.20767229 , 0.8156936  , 0.20484671 , 0.19149587 , 0.09184399],
 [ 0.57728039 , 0.54806837 , 0.97323099 , 0.25579257 , 0.35155275],
 [ 0.36644203 , 0.70831511 , 0.53134779 , 0.43446429 , 0.42551948],
 [ 0.7878818  , 0.78565112 , 0.81641546 , 0.54535929 , 0.43659587],
 [ 0.24174943 , 0.86650992 , 0.73597885 , 0.11347332 , 0.11606553],
 [ 0.24612583 , 0.12739929 , 0.2106041  , 0.04586055 , 0.37896457],
 [ 0.38135602 , 0.24285618 , 0.60191124 , 0.74805441 , 0.30219371]])
speed_155 = np.array([ 1.57497 , 1.56674 , 1.55868 , 1.55283 , 1.54557])


data_160 = np.array(
[[ 0.2613178  , 0.26354813 , 0.26617927 , 0.26841701 , 0.25366305],
 [ 0.28620787 , 0.33107034 , 0.3346951  , 0.33215464 , 0.20684461],
 [ 0.0058795  , 0.18476001 , 0.17528543 , 0.25308287 , 0.13123885],
 [ 0.76284775 , 0.60322006 , 0.48332883 , 0.61589553 , 0.73651099],
 [ 0.21641519 , 0.53280361 , 0.3987987  , 0.48182354 , 0.20394831],
 [ 0.44313164 , 0.28343878 , 0.77873331 , 0.78032317 , 0.52161517],
 [ 0.39720481 , 0.53916427 , 0.51845277 , 0.44586053 , 0.35730956],
 [ 0.95171573 , 0.60680106 , 0.77448789 , 0.90649961 , 0.71128466],
 [ 0.17766802 , 0.12488758 , 0.4919229  , 0.51600731 , 0.09485616],
 [ 0.07422612 , 0.0110726  , 0.42349447 , 0.37039381 , 0.01865356],
 [ 0.01807748 , 0.48935937 , 0.69394121 , 0.31613373 , 0.85467006]])
speed_160 = np.array([ 1.62313 , 1.60181 , 1.59737 , 1.6212  , 1.58006])


data_165 = np.array(
[[ 0.23896224 , 0.26218908 , 0.263619   , 0.26027197 , 0.24873412],
 [ 0.11536636 , 0.2347233  , 0.2922405  , 0.28161668 , 0.11631988],
 [ 0.11509372 , 0.28271257 , 0.2723996  , 0.17784956 , 0.34287832],
 [ 0.62176923 , 0.48288352 , 0.67238493 , 0.48894265 , 0.54714303],
 [ 0.62994779 , 0.47210526 , 0.59406435 , 0.93110567 , 0.79139072],
 [ 0.41034317 , 0.82794209 , 0.92744773 , 0.67650134 , 0.5162261 ],
 [ 0.79827403 , 0.50343533 , 0.89553553 , 0.53198502 , 0.44841672],
 [ 0.99709097 , 0.80886311 , 0.99641987 , 0.75454718 , 0.61362067],
 [ 0.09601246 , 0.28452052 , 0.94166841 , 0.49586404 , 0.80632223],
 [ 0.45892735 , 0.3302143  , 0.38989386 , 0.58143928 , 0.02826382],
 [ 0.36623686 , 0.49685538 , 0.49682407 , 0.70416835 , 0.30039028]])
speed_165 = np.array([ 1.64795 , 1.63861 , 1.64418 , 1.64213 , 1.64313])


data_170 = np.array(
[[ 0.24876885 , 0.24383069 , 0.24962667 , 0.24136071 , 0.24265613],
 [ 0.24030027 , 0.14083549 , 0.14406012 , 0.12069611 , 0.11520558],
 [ 0.07440017 , 0.0135754  , 0.21594747 , 0.23099965 , 0.14010845],
 [ 0.65664092 , 0.77406001 , 0.38532043 , 0.58087255 , 0.79656719],
 [ 0.62791419 , 0.83541932 , 0.69584806 , 0.65037416 , 0.81165556],
 [ 0.61553894 , 0.49764556 , 0.86905125 , 0.45867615 , 0.56507225],
 [ 0.65680256 , 0.69289749 , 0.61873304 , 0.72339512 , 0.52959961],
 [ 0.98653262 , 0.72812942 , 0.68345975 , 0.79476858 , 0.79745921],
 [ 0.09545511 , 0.14439721 , 0.13609433 , 0.08998625 , 0.76666679],
 [ 0.28762067 , 0.08425225 , 0.10617406 , 0.15530558 , 0.13464956],
 [ 0.97280991 , 0.46862926 , 0.43226082 , 0.84757306 , 0.47071248]])
speed_170 = np.array([ 1.68574 , 1.695  ,  1.71022 , 1.6766  , 1.69125])

# complementary of 'phi_hip_ref' to invert the sign
for i in range(5):
	data_130[2][i] = 1 - data_130[2][i]
	data_135[2][i] = 1 - data_135[2][i]
	data_140[2][i] = 1 - data_140[2][i]
	data_145[2][i] = 1 - data_145[2][i]
	data_150[2][i] = 1 - data_150[2][i]
	data_155[2][i] = 1 - data_155[2][i]
	data_160[2][i] = 1 - data_160[2][i]
	data_165[2][i] = 1 - data_165[2][i]
	data_170[2][i] = 1 - data_170[2][i]

# names of the variables
names = ['tau', 'theta_ref', 'phi_hip_ref', 'k_HFL1', 'k_HFL2',
                'k_HAM', 'G_sol', 'G_sol_ta', 'G_gas', 'G_vas', 'k_phi_knee']

# plot factors for unit conversions
plot_fac = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]

# labels for plot
labels = [r'$\boldsymbol{\tau~(s)}$', r'$\boldsymbol{\theta_{ref}~(rad)}$', r'$\boldsymbol{\varphi_{h,ref}~(rad)}$', \
	r'$\boldsymbol{k_{HFL,1}~(-)}$', r'$\boldsymbol{k_{HFL,2}~(-)}$', r'$\boldsymbol{k_{HAM}~(-)}$', \
	r'$\boldsymbol{G_{SOL}~(-)}$', r'$\boldsymbol{G_{S-T}~(-)}$', r'$\boldsymbol{G_{GAS}~(-)}$', \
	r'$\boldsymbol{G_{VAS}~(-)}$', r'$\boldsymbol{k_{\varphi,k}~(-)}$']

# approximation order
approx_order = [2, 0, 0, 1, 2, 2, 2, 1, 1, 1, 0]

# parameter bounds
bounds = np.array([
	[ 0.010 , 0.15  ] ,
	[ 0.0   , 0.25  ] ,
	[ -0.9  ,-0.005 ] ,
	[ 0.3   , 8.1   ] ,
	[ 1.0   , 12.0  ] ,
	[ 0.3   , 7.0   ] ,
	[ 0.60  , 5.0   ] ,
	[ 0.4   , 7.0   ] ,
	[ 0.0   , 20.0  ] ,
	[ 0.82  , 5.0   ] ,
	[ 1.0   , 15.0  ]
])

# xlim values
plot_xlim  = np.array([ 1.28 , 1.72 ])

# ylim values
plot_ylim = np.array([
	[ 0.043 , 0.053 ] ,
	[ 0.02  , 0.1   ] ,
	[ -0.3  , -0.05 ] ,
	[ 3.0   , 8.0   ] ,
	[ 1.0   , 11.0  ] ,
	[ 1.0   , 6.0   ] ,
	[ 2.0   , 4.0   ] ,
	[ 1.0   , 7.0   ] ,
	[ 2.0   , 16.0  ] ,
	[ 0.5   , 3.0   ] ,
	[ 2.0   , 14.0  ]
])

# reference speed
x_star = 1.5

# p-value threshold
p_thres = 0.1

output_folder = './'

# class with all data
all_data = data.AllData(names, plot_fac, labels, approx_order, bounds, x_star, p_thres, plot_xlim, plot_ylim, output_folder)

# add data
all_data.add_data(data_130, speed_130, 1.3)
all_data.add_data(data_135, speed_135, 1.35)
all_data.add_data(data_140, speed_140, 1.4)
all_data.add_data(data_145, speed_145, 1.45)
all_data.add_data(data_150, speed_150, 1.5)
all_data.add_data(data_155, speed_155, 1.55)
all_data.add_data(data_160, speed_160, 1.6)
all_data.add_data(data_165, speed_165, 1.65)
all_data.add_data(data_170, speed_170, 1.7)

# option to save the graphs
flag_save = 0

all_data.flag_save = flag_save

# graphs
all_data.param_plot('tau')
all_data.param_plot('theta_ref')
all_data.param_plot('phi_hip_ref')
all_data.param_plot('k_HFL1')
all_data.param_plot('k_HFL2')
all_data.param_plot('k_HAM')
all_data.param_plot('G_sol')
all_data.param_plot('G_sol_ta')
all_data.param_plot('G_gas')
all_data.param_plot('G_vas')
all_data.param_plot('k_phi_knee')

# polynomial order
	
print('')
all_data.lack_of_fit_012('tau')
all_data.lack_of_fit_012('theta_ref')
all_data.lack_of_fit_012('phi_hip_ref')
all_data.lack_of_fit_012('k_HFL1')
all_data.lack_of_fit_012('k_HFL2')
all_data.lack_of_fit_012('k_HAM')
all_data.lack_of_fit_012('G_sol')
all_data.lack_of_fit_012('G_sol_ta')
all_data.lack_of_fit_012('G_gas')
all_data.lack_of_fit_012('G_vas')
all_data.lack_of_fit_012('k_phi_knee')

# show plots
plt.show()
