'''
Created originally by Hyst v1.3
Hybrid Automaton in Hylaa
'''

import numpy as np
from hylaa.hybrid_automaton import LinearHybridAutomaton, LinearConstraint, HyperRectangle
from hylaa.engine import HylaaSettings
from hylaa.engine import HylaaEngine
from hylaa.plotutil import PlotSettings
from hylaa.timerutil import Timers

def define_ha():
    '''make the hybrid automaton and return it'''

    ha = LinearHybridAutomaton()
    ha.variables = ["x1", "x2", "x3", "x4", "x5", "x6", "x7", "x8", "x9", "t"]

    negAngle = ha.new_mode('negAngle')
    negAngle.a_matrix = np.array([[0, 0, 0, 0, 0, 0, 0.0833333333333333, 0, -1, 0], [13828.8888888889, -26.6666666666667, 60, 60, 0, 0, -5, -60, 0, 0], [0, 0, 0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0, 0, 0], [0, 0, 0, 0, -714.285714285714, -0.04, 0, 714.285714285714, 0, 0], [-2777.77777777778, 3.33333333333333, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0], [100, 0, 0, 0, 1000, 0, 0, -1000, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]], dtype=float)
    negAngle.c_vector = np.array([0, 716.666666666667, 0, 5, 0, 0, -83.3333333333333, 0, 3, 1], dtype=float)
    negAngle.inv_list.append(LinearConstraint([1, 0, 0, 0, 0, 0, 0, 0, 0, 0], -0.03)) # x1 <= -0.03

    deadzone = ha.new_mode('deadzone')
    deadzone.a_matrix = np.array([[0, 0, 0, 0, 0, 0, 0.0833333333333333, 0, -1, 0], [-60, -26.6666666666667, 60, 60, 0, 0, -5, -60, 0, 0], [0, 0, 0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0, 0, 0], [0, 0, 0, 0, -714.285714285714, -0.04, 0, 714.285714285714, 0, 0], [0, 3.33333333333333, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 1000, 0, 0, -1000, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]], dtype=float)
    deadzone.c_vector = np.array([0, 300, 0, 5, 0, 0, 0, 0, 0, 1], dtype=float)
    deadzone.inv_list.append(LinearConstraint([-1, 0, 0, 0, 0, 0, 0, 0, 0, 0], 0.03)) # -0.03 <= x1
    deadzone.inv_list.append(LinearConstraint([1, 0, 0, 0, 0, 0, 0, 0, 0, 0], 0.03)) # x1 <= 0.03

    posAngle = ha.new_mode('posAngle')
    posAngle.a_matrix = np.array([[0, 0, 0, 0, 0, 0, 0.0833333333333333, 0, -1, 0], [13828.8888888889, -26.6666666666667, 60, 60, 0, 0, -5, -60, 0, 0], [0, 0, 0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0, 0, 0], [0, 0, 0, 0, -714.285714285714, -0.04, 0, 714.285714285714, 0, 0], [-2777.77777777778, 3.33333333333333, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0], [100, 0, 0, 0, 1000, 0, 0, -1000, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]], dtype=float)
    posAngle.c_vector = np.array([0, -116.666666666667, 0, 5, 0, 0, 83.3333333333333, 0, -3, 1], dtype=float)
    posAngle.inv_list.append(LinearConstraint([-1, 0, 0, 0, 0, 0, 0, 0, 0, 0], -0.03)) # 0.03 <= x1

    negAngleInit = ha.new_mode('negAngleInit')
    negAngleInit.a_matrix = np.array([[0, 0, 0, 0, 0, 0, 0.0833333333333333, 0, -1, 0], [13828.8888888889, -26.6666666666667, 60, 60, 0, 0, -5, -60, 0, 0], [0, 0, 0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0, 0, 0], [0, 0, 0, 0, -714.285714285714, -0.04, 0, 714.285714285714, 0, 0], [-2777.77777777778, 3.33333333333333, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0], [100, 0, 0, 0, 1000, 0, 0, -1000, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]], dtype=float)
    negAngleInit.c_vector = np.array([0, 116.666666666667, 0, -5, 0, 0, -83.3333333333333, 0, 3, 1], dtype=float)
    negAngleInit.inv_list.append(LinearConstraint([0, 0, 0, 0, 0, 0, 0, 0, 0, 1], 0.199999)) # t <= 0.199999

    error = ha.new_mode('error')
    error.is_error = True

    trans = ha.new_transition(negAngleInit, negAngle)
    trans.condition_list.append(LinearConstraint([-0, -0, -0, -0, -0, -0, -0, -0, -0, -1], -0.199999)) # t >= 0.199999

    trans = ha.new_transition(negAngle, deadzone)
    trans.condition_list.append(LinearConstraint([-1, -0, -0, -0, -0, -0, -0, -0, -0, -0], 0.03)) # x1 >= -0.03

    trans = ha.new_transition(deadzone, error)
    trans.condition_list.append(LinearConstraint([1, 0, 0, 0, 0, 0, 0, 0, 0, 0], -0.03)) # x1 <= -0.03

    trans = ha.new_transition(deadzone, posAngle)
    trans.condition_list.append(LinearConstraint([-1, -0, -0, -0, -0, -0, -0, -0, -0, -0], -0.03)) # x1 >= 0.03

    trans = ha.new_transition(posAngle, error)
    trans.condition_list.append(LinearConstraint([1, 0, 0, 0, 0, 0, 0, 0, 0, 0], 0.03)) # x1 <= 0.03

    return ha

def define_init_states(ha):
    '''returns a list of (mode, list(LinearConstraint])'''
    # Variable ordering: [x1, x2, x3, x4, x5, x6, x7, x8, x9, t]
    rv = []
    
    constraints = []
    constraints.append(LinearConstraint([1, 0, 0, -0.00056, 0, 0, 0, 0, 0, 0], -0.060000000000000005)) # 1.0 * x1 + 0.0488 = 0.00056 * x4 - 0.0112
    constraints.append(LinearConstraint([-1, -0, -0, 0.00056, -0, -0, -0, -0, -0, -0], 0.060000000000000005)) # 1.0 * x1 + 0.0488 = 0.00056 * x4 - 0.0112
    constraints.append(LinearConstraint([0, 1, 0, -0.467, 0, 0, 0, 0, 0, 0], -25.009999999999998)) # 1.0 * x2 + 15.67 = 0.467 * x4 - 9.34
    constraints.append(LinearConstraint([-0, -1, -0, 0.467, -0, -0, -0, -0, -0, -0], 25.009999999999998)) # 1.0 * x2 + 15.67 = 0.467 * x4 - 9.34
    constraints.append(LinearConstraint([0, 0, 1, 0, 0, 0, 0, 0, 0, 0], 0)) # x3 = 0.0
    constraints.append(LinearConstraint([-0, -0, -1, -0, -0, -0, -0, -0, -0, -0], -0)) # x3 = 0.0
    constraints.append(LinearConstraint([0, 0, 0, 0, 1, 0, 0, 0, 0, 0], 0)) # x5 = 0.0
    constraints.append(LinearConstraint([-0, -0, -0, -0, -1, -0, -0, -0, -0, -0], -0)) # x5 = 0.0
    constraints.append(LinearConstraint([0, 0, 0, -1, 0, 1, 0, 0, 0, 0], 0)) # 1.0 * x6 - 20.0 = 1.0 * x4 - 20.0
    constraints.append(LinearConstraint([-0, -0, -0, 1, -0, -1, -0, -0, -0, -0], -0)) # 1.0 * x6 - 20.0 = 1.0 * x4 - 20.0
    constraints.append(LinearConstraint([0, 0, 0, -12, 0, 0, 1, 0, 0, 0], 0)) # 1.0 * x7 - 240.0 = 12.0 * x4 - 240.0
    constraints.append(LinearConstraint([-0, -0, -0, 12, -0, -0, -1, -0, -0, -0], -0)) # 1.0 * x7 - 240.0 = 12.0 * x4 - 240.0
    constraints.append(LinearConstraint([0, 0, 0, -0.00006, 0, 0, 0, 1, 0, 0], -0.00312)) # 1.0 * x8 + 0.00192 = 0.00006 * x4 - 0.0012
    constraints.append(LinearConstraint([-0, -0, -0, 0.00006, -0, -0, -0, -1, -0, -0], 0.00312)) # 1.0 * x8 + 0.00192 = 0.00006 * x4 - 0.0012
    constraints.append(LinearConstraint([0, 0, 0, -1, 0, 0, 0, 0, 1, 0], 0)) # 1.0 * x9 - 20.0 = 1.0 * x4 - 20.0
    constraints.append(LinearConstraint([-0, -0, -0, 1, -0, -0, -0, -0, -1, -0], -0)) # 1.0 * x9 - 20.0 = 1.0 * x4 - 20.0
    constraints.append(LinearConstraint([0, 0, 0, -1, 0, 0, 0, 0, 0, 0], -20)) # 20.0 <= x4
    constraints.append(LinearConstraint([0, 0, 0, 1, 0, 0, 0, 0, 0, 0], 40)) # x4 <= 40.0
    constraints.append(LinearConstraint([0, 0, 0, 0, 0, 0, 0, 0, 0, 1], 0)) # t = 0.0
    constraints.append(LinearConstraint([-0, -0, -0, -0, -0, -0, -0, -0, -0, -1], -0)) # t = 0.0
    rv.append((ha.modes['negAngleInit'], constraints))

    return rv


def define_settings(plot):
    'get the hylaa settings object'
    plot_settings = PlotSettings()
    plot_settings.plot_mode = PlotSettings.PLOT_IMAGE if plot else PlotSettings.PLOT_NONE
    plot_settings.xdim = 0
    plot_settings.ydim = 2
    plot_settings.max_shown_polys = None

    # step was 5.0E-4
    settings = HylaaSettings(step=5.0E-4, max_time=2.0, plot_settings=plot_settings)
    settings.counter_example_filename = None

    #settings.deaggregation=False
    #settings.aggregation=False

    plot_settings.filename = 'hylaa_drivetrain_deagg.png'
    plot_settings.label.x_label = '$x_1$'
    plot_settings.label.y_label = '$x_3$'
    plot_settings.label.title = 'Drivetrain (Hylaa)'
    plot_settings.plot_size = (12, 12)
    plot_settings.label.big(size=40)

    plot_settings.extra_lines = [[(-0.03, -100.0), (-0.03, 100.0)], [(0.03, -100.0), (0.03, 100.0)]]

    return settings

def run_hylaa(plot=False):
    'Runs hylaa with the given settings, returning the HylaaResult object.'

    settings = define_settings(plot)
    ha = define_ha()
    init = define_init_states(ha)

    engine = HylaaEngine(ha, settings)
    engine.run(init)

    return engine.result.time, not engine.reached_error

if __name__ == '__main__':
    run_hylaa(plot=True) # uncomment this to produce plot (~75 seconds)
    Timers.reset()

    runtime, is_safe = run_hylaa(plot=False)

    print "\nSummary:"
    print "Drivetrain Time={:.2f}sec, safe={}".format(runtime, is_safe)