import matplotlib.pyplot as plt
import bicycledataprocessor as bdp
from canonicalbicycleid import canonical_bicycle_id as cbi

with open(read('pathToGoodRuns')) as f:
    goodRuns = cPickle.load(f)

with open(read('pathToIdMat')) as f:
    idMat = cPickle.load(f)

dataset = bdp.DataSet()

roll = {k: [] for k in idMat.keys() + ['Whipple', 'Arm']}
steer = {k: [] for k in idMat.keys() + ['Whipple', 'Arm']}

allH = cbi.lateral_force_contribution(['Charlie', 'Jason', 'Luke'])

for runNum in goodRuns:
    trial = bdp.Run(runNum, dataset, filterFreq=15.)
    rider = trial.metadata['Rider']
    timeseries = cbi.benchmark_time_series(trial, subtractMean=True)
    for k, model in idMat.items():
        rollrsq, steerrsq, fig = cbi.input_prediction(timeseries, model)
        roll[k].append(rollrsq)
        steer[k].append(steerrsq)
        fig.savefig('plots/' + str(runNum) + '-' + k + '.png')
        plt.close(fig)
        del fig

    M, C1, K0, K2 = trial.bicycle.canonical(nominal=True)
    rollrsq, steerrsq, fig = cbi.input_prediction(timeseries,
import matplotlib.pyplot as plt
import bicycledataprocessor as bdp
from canonicalbicycleid import canonical_bicycle_id as cbi

with open(read('pathToGoodRuns')) as f:
    goodRuns = cPickle.load(f)

with open(read('pathToIdMat')) as f:
    idMat = cPickle.load(f)

dataset = bdp.DataSet()

roll = {k : [] for k in idMat.keys() + ['Whipple', 'Arm']}
steer = {k : [] for k in idMat.keys() + ['Whipple', 'Arm']}

allH = cbi.lateral_force_contribution(['Charlie', 'Jason', 'Luke'])

for runNum in goodRuns:
    trial = bdp.Run(runNum, dataset, filterFreq=15.)
    rider = trial.metadata['Rider']
    timeseries = cbi.benchmark_time_series(trial, subtractMean=True)
    for k, model in idMat.items():
        rollrsq, steerrsq, fig = cbi.input_prediction(timeseries, model)
        roll[k].append(rollrsq)
        steer[k].append(steerrsq)
        fig.savefig('plots/' + str(runNum) + '-' + k + '.png')
        plt.close(fig)
        del fig

    M, C1, K0, K2 = trial.bicycle.canonical(nominal=True)
    rollrsq, steerrsq, fig = cbi.input_prediction(timeseries, (M, C1, K0, K2,
import cPickle

# external
import pandas
from canonicalbicycleid import canonical_bicycle_id as cbi

riders = ['Charlie', 'Jason', 'Luke']
maneuvers = ['Balance',
             'Track Straight Line',
             'Balance With Disturbance',
             'Track Straight Line With Disturbance']
environments = ['Horse Treadmill', 'Pavillion Floor']

# Get the M, C1, K0, K2, and H matrices for the riders.
canon = cbi.load_benchmark_canon(riders)
H = cbi.lateral_force_contribution(riders)

# Pick the free parameters in the canonical model, i.e. the ones that were
# identified.
roll_parameters = ['Mpd', 'C1pd', 'K0pd']
steer_parameters = ['Mdd', 'C1dp', 'C1dd', 'K0dd', 'K2dd', 'HdF']

# Load in precomputed results.
results_directory = '../data'

with open(os.path.join(results_directory, 'good-runs.p'), 'r') as f:
    good_runs = cPickle.load(f)

with open(os.path.join(results_directory, 'id-matrices.p'), 'r') as f:
    id_matrices = cPickle.load(f)
    # bode plots
    speeds = np.array([2.0, 4.0, 6.0, 9.0])
    null, iAs, iBs = bicycle.benchmark_state_space_vs_speed(iM, iC1, iK0, iK2,
            speeds)
    null, wAs, wBs = bicycle.benchmark_state_space_vs_speed(wM, wC1, wK0, wK2,
            speeds)
    figs = cbi.plot_bode(speeds, iAs, iBs, wAs, wBs, aAs, aBs)
    figs[0].savefig(filename + '-Tphi.png')
    figs[1].savefig(filename + '-Tdel.png')

########################################
allRiders = ['Charlie', 'Jason', 'Luke']
# create the comparison models
canon = cbi.load_benchmark_canon(allRiders)
H = cbi.lateral_force_contribution(allRiders)

with open('../data/idMatrices.p') as f:
    idMatrices = cPickle.load(f)

riders = ['Charlie', 'Jason', 'Luke', 'All']
rMap = {x[0]: [x] for x in riders}
environments = ['Horse Treadmill', 'Pavillion Floor', 'All']
eMap = {x[0]: [x] for x in environments}

for k, v in idMatrices.items():
    r, e = k.split('-')
    if r == 'A':
        riders = ['Charlie', 'Jason', 'Luke']
    else:
        riders = rMap[r]
# generate the eigenvalues an eigenvectors as a function of speed.
iM, iC1, iK0, iK2, iH = id_matrices['L-P']
speeds, iAs, iBs = bicycle.benchmark_state_space_vs_speed(iM,
                                                          iC1,
                                                          iK0,
                                                          iK2,
                                                          v0=v0,
                                                          vf=vf,
                                                          num=num)
w, v = control.eig_of_series(iAs)
iEigenvalues, iEigenvectors = control.sort_modes(w, v)

# Load the Whipple model M, C1, K0, K2, H from first principles and generate
# the eigenvalues and eigenvectors as a function of speed.
wM, wC1, wK0, wK2 = cbi.load_benchmark_canon(['Luke'])['Luke']
wH = cbi.lateral_force_contribution(['Luke'])['Luke']
speeds, wAs, wBs = bicycle.benchmark_state_space_vs_speed(wM,
                                                          wC1,
                                                          wK0,
                                                          wK2,
                                                          v0=v0,
                                                          vf=vf,
                                                          num=num)
w, v = control.eig_of_series(wAs)
wEigenvalues, wEigenvectors = control.sort_modes(w, v)

# Load the Arm model state space for each rider from first principles and
# generate the eigenvalues and eigenvectors as a function of speed.
aAs, aBs, aSpeed = cbi.mean_arm(['Luke'])
indices = np.int32(np.round(speeds * 10))
w, v = control.eig_of_series(aAs[indices])
vf = 10.
num = 100

# Load the identified model from data for rider L and in pavillion floor and
# generate the eigenvalues an eigenvectors as a function of speed.
iM, iC1, iK0, iK2, iH = id_matrices['L-P']
speeds, iAs, iBs = bicycle.benchmark_state_space_vs_speed(iM, iC1, iK0, iK2,
                                                          v0=v0, vf=vf,
                                                          num=num)
w, v = control.eig_of_series(iAs)
iEigenvalues, iEigenvectors = control.sort_modes(w, v)

# Load the Whipple model M, C1, K0, K2, H from first principles and generate
# the eigenvalues and eigenvectors as a function of speed.
wM, wC1, wK0, wK2 = cbi.load_benchmark_canon(['Luke'])['Luke']
wH = cbi.lateral_force_contribution(['Luke'])['Luke']
speeds, wAs, wBs = bicycle.benchmark_state_space_vs_speed(wM, wC1, wK0, wK2,
                                                          v0=v0, vf=vf,
                                                          num=num)
w, v = control.eig_of_series(wAs)
wEigenvalues, wEigenvectors = control.sort_modes(w, v)

# Load the Arm model state space for each rider from first principles and
# generate the eigenvalues and eigenvectors as a function of speed.
aAs, aBs, aSpeed = cbi.mean_arm(['Luke'])
indices = np.int32(np.round(speeds * 10))
w, v = control.eig_of_series(aAs[indices])
aEigenvalues, aEigenvectors = control.sort_modes(w, v)

rlfig = cbi.plot_rlocus_parts(speeds, iEigenvalues, wEigenvalues,
                              aEigenvalues)