#!/usr/bin/env python
# coding: utf-8

# # Introduction

# This notebook is intended to address this question:
# 
# > Does the data processing introduce a bias? The joint moment calculation makes use of marker coordinates. The joint angle calculation uses the same marker coordinates. If there was a random measurement error in a marker, it would affect both and this may result in correlations that show up in the sys id results. We should be able to test this effect and conclude whether it is a problem. Here is an idea: (1) make an average (periodic and unperturbed) gait cycle from raw data (marker coordinates and GRF) and repeat it over and over to create the same amount of data that you usually work with. (2) add small random numbers to all samples, (3) compute angles and torques in the same way you normally do, (4) do the sys id and see what comes out. If this result is the same as what you get from the perturbation experiment, that would be bad news...

# # Imports

# In[1]:


import sys
sys.path.append('../src')


# In[2]:


import operator


# In[3]:


import numpy as np
import pandas
import matplotlib.pyplot as plt
from gaitanalysis.motek import markers_for_2D_inverse_dynamics
from gaitanalysis.gait import GaitData
from gaitanalysis.controlid import SimpleControlSolver


# In[4]:


import utils
from gait_landmark_settings import settings


# In[5]:


get_ipython().run_line_magic('matplotlib', 'inline')


# In[6]:


from IPython.core.pylabtools import figsize


# # Load and Process Data

# Load the path to the directory with the experimental data.

# In[7]:


trials_dir = utils.config_paths()['raw_data_dir']


# Pick a trial.

# In[8]:


trial_number = '018'


# In[9]:


trial = utils.Trial(trial_number)


# In[10]:


trial.prep_data('First Normal Walking')


# First, extract the data from the first normal walking period in the trial.

# In[11]:


gait_data = GaitData(trial.event_data_frames['First Normal Walking'])
gait_data.grf_landmarks('FP2.ForY', 'FP1.ForY',
                        filter_frequency=trial.grf_filter_frequency,
                        threshold=trial.grf_threshold)
cycles = gait_data.split_at('right', num_samples=trial.num_samples_lower_bound)


# In[12]:


cycles.shape


# In[13]:


figsize(10, 10)
axes = gait_data.gait_cycle_stats.hist()


# # Generate Fake Data

# Now extract one good step from the bunch. The 21st step looks as good as any:

# In[14]:


cycles.iloc[20]['FP2.ForY'].plot()


# This gives a list of measurements needed for the 2D inverse dynamics caluclations and also a list of just the marker labels.

# In[15]:


measurements = markers_for_2D_inverse_dynamics('full')
measurement_list = reduce(operator.add, measurements)
marker_list = reduce(operator.add, measurements[:2])


# Copy the cycle so it no longer points to the original data arrays.

# In[16]:


cycle = cycles.iloc[20][measurement_list + ['Original Time']].copy()


# This code fits an nth order Fourier series to each signal needed for the 2D dynamics and then builds a smoothed step data frame.

# In[17]:


smoothed_cycle = cycle.copy()

time = cycle['Original Time'].values

freq = 1.0 / (time[-1] - time[0]) # cycle / sec
print('Cycle frequency = {:0.2f} hz'.format(freq))
omega = 2.0 * np.pi * freq # rad /sec
print('Cycle frequency = {:0.2f} rad/s'.format(omega))

fourier_order = 20
initial_coeff = np.ones(1 + 2 * fourier_order)

for measurement in measurement_list:
    signal = cycle[measurement].values
    popt, pcov = utils.fit_fourier(time, signal, initial_coeff, omega)
    eval_fourier = utils.fourier_series(omega)
    smoothed_cycle[measurement] = eval_fourier(time, *popt)


# This plot shows the Fourier series (green line) vs the original data (blue dots).

# In[18]:


figsize(14, 40)
axes = cycle.plot(style='.', subplots=True)
for i, ax in enumerate(axes):
    smoothed_cycle.iloc[:, i].plot(ax=ax)


# Now build some fake data by repeating the smoothed steps..

# In[19]:


fakedata = {}
for i in range(500):
    fakedata[i] = smoothed_cycle.copy()
fake_data_df = pandas.concat(fakedata, ignore_index=True)
fake_data_df.index = np.linspace(0.0, len(fake_data_df) * 0.01 - 0.01, len(fake_data_df))


# Now add random Gaussian noise with a mean of zero and standard deviation of 5 mm to all of the marker measurements.

# In[20]:


shape = fake_data_df[marker_list].shape
fake_data_df[marker_list] += np.random.normal(scale=0.005, size=shape)


# This is a plot of the first ~11 gait cycles of the fake data (includes the noise on the markers).

# In[21]:


figsize(14, 28)
axes = fake_data_df[['FP2.ForY'] + marker_list].iloc[:1000].plot(subplots=True)


# Now compute the inverse dynamics on the noisy data.

# In[22]:


gait_data = GaitData(fake_data_df)
subject_mass = float(trial.meta_data['subject']['mass'])
args = list(measurements) + [subject_mass, 6.0]
df = gait_data.inverse_dynamics_2d(*args)


# This shows an example of the joint angles, rates, and torques.

# In[23]:


figsize(14, 24)
subset = ['FP2.ForY',
          'Right.Ankle.PlantarFlexion.Moment',
          'Right.Ankle.PlantarFlexion.Angle',
          'Right.Ankle.PlantarFlexion.Rate']
axes = gait_data.data[subset].iloc[:1000].plot(subplots=True)


# # Identify the Controller

# Now, to identify the controller the steps need to be found.

# In[24]:


null = gait_data.grf_landmarks('FP2.ForY', 'FP1.ForY', threshold=30.0)


# In[25]:


fake_steps = gait_data.split_at('right', num_samples=20)


# The only variation in the steps is due to the marker coordinates noise.

# In[26]:


figsize(12, 12)
axes = gait_data.plot_gait_cycles(*subset)


# Now identify a joint centric gain scheduled controller as usual.

# In[27]:


solver = SimpleControlSolver(fake_steps, trial.sensors, trial.controls)


# In[28]:


gain_inclusion_matrix = np.zeros((len(trial.controls), len(trial.sensors))).astype(bool)
for i, row in enumerate(gain_inclusion_matrix):
    row[2 * i:2 * i + 2] = True


# In[29]:


result = solver.solve(gain_inclusion_matrix=gain_inclusion_matrix)


# In[30]:


fig, axes = utils.plot_joint_isolated_gains(trial.sensors, trial.controls, result[0], result[3])


# Interesting that we see a curve similar in shape, but not magnitude, in the angle to moment gain curves. Below I calculate the ratio of the right vertical ground reaction force to the gain curve and then plot the gain curves, normalized to by this ratio with respect to the vertical ground reaction force.

# In[31]:


for m, a, seg in zip([0, 1, 2], [0, 2, 4], ['Ankle', 'Knee', 'Hip']):
    ratios = fake_steps.iloc[0]['FP2.ForY'] / result[0][:, m, a]
    ratio = ratios[:11].mean()
    print('GRF to {} Gain ratio = {:0.3f}'.format(seg, ratio))
    plt.plot(ratio * result[0][:, m, a], label=seg)
    
plt.plot(fake_steps.iloc[0]['FP2.ForY'], label='ForY')
plt.legend()


# # Footer

# In[32]:


get_ipython().system('git rev-parse HEAD')


# In[33]:


get_ipython().system('git --git-dir=/home/moorepants/src/GaitAnalysisToolKit/.git --work-tree=/home/moorepants/src/GaitAnalysisToolKit rev-parse HEAD')


# In[34]:


get_ipython().run_line_magic('install_ext', 'http://raw.github.com/jrjohansson/version_information/master/version_information.py')


# In[35]:


get_ipython().run_line_magic('load_ext', 'version_information')


# In[36]:


get_ipython().run_line_magic('version_information', 'numpy, scipy, pandas, matplotlib, tables, oct2py, dtk, gaitanalysis')


# In[37]:


get_ipython().system('conda list')


# In[38]:


get_ipython().system('pip freeze')