#!/usr/bin/env python
#
# WARNING: This example may not yet be working.  Please check again in
#          the upcoming release.
#
"""
This example is an extension of the robot_continuous.py
code by Petter Nilsson and Nok Wongpiromsarn.

It demonstrates  the use of TuLiP for systems with
piecewise affine dynamics.

Necmiye Ozay, August 26, 2012
"""
import numpy as np

from tulip import spec, synth
from tulip.hybrid import LtiSysDyn, PwaSysDyn
from tulip.polytope import box2poly
from tulip.abstract import prop2part, discretize

# Problem parameters
input_bound = 0.4
uncertainty = 0.05

# Continuous state space
cont_state_space = box2poly([[0., 3.], [0., 2.]])

# Assume, for instance, our robot is traveling on
# a nonhomogenous surface (xy plane),
# resulting in different dynamics at different
# parts of the plane.
#
# Since the continuous state space in this example
# is just xy position, different dynamics in
# different parts of the surface can be modeled
# using LtiSysDyn subsystems subsys0 and subsys1.
#
# Togetger they comprise a Piecewise Affine System:

def subsys0():
    A = np.array([[1.1052, 0.], [ 0., 1.1052]])
    B = np.array([[1.1052, 0.], [ 0., 1.1052]])
    E = np.array([[1,0], [0,1]])
    
    U = box2poly(input_bound * np.array([[-1., 1.], [-1., 1.]]))
    W = box2poly(uncertainty * np.array([[-1., 1.], [-1., 1.]]))
    dom = box2poly([[0., 3.], [0.5, 2.]])
    
    sys_dyn = LtiSysDyn(A, B, E, [], U, W, dom)
    #sys_dyn.plot()
    
    return sys_dyn

def subsys1():
    A = np.array([[0.9948, 0.], [0., 1.1052]])
    B = np.array([[-1.1052, 0.], [0., 1.1052]])
    E = np.array([[1, 0], [0, 1]])
    
    U = box2poly(input_bound * np.array([[-1., 1.], [-1., 1.]]))
    W = box2poly(uncertainty * np.array([[-1., 1.], [-1., 1.]]))
    
    dom = box2poly([[0., 3.], [0., 0.5]])
    
    sys_dyn = LtiSysDyn(A, B, E, [], U, W, dom)
    #sys_dyn.plot()
    
    return sys_dyn

subsystems = [subsys0(), subsys1()]

# Build piecewise affine system from its subsystems
sys_dyn = PwaSysDyn(subsystems, cont_state_space)

# Continuous proposition
cont_props = {}
cont_props['home'] = box2poly([[0., 1.], [0., 1.]])
cont_props['lot'] = box2poly([[2., 3.], [1., 2.]])

# Compute the proposition preserving partition
# of the continuous state space
cont_partition = prop2part(cont_state_space, cont_props)
disc_dynamics = discretize(
    cont_partition, sys_dyn, closed_loop=True,
    N=8, min_cell_volume=0.1
)

# Specifications

# Environment variables and assumptions
env_vars = {'park'}
env_init = set()                # empty set
env_prog = '!park'
env_safe = set()                # empty set

# System variables and requirements
sys_vars = {'X0reach'}

# []<>home
sys_prog = {'home'}

# [](park -> <> lot)
sys_init = {'X0reach'}
sys_safe = {'X(X0reach) <-> lot || (X0reach && !park)'}
sys_prog |= {'X0reach'}

# Create the specification
specs = spec.GRSpec(env_vars, sys_vars, env_init, sys_init,
                    env_safe, sys_safe, env_prog, sys_prog)

# Synthesize
ctrl = synth.synthesize('gr1c', specs,
                        sys=disc_dynamics.ofts, ignore_sys_init=True)

# Save graphical representation of controller for viewing
if not ctrl.save('continuous_pwa.png', 'png'):
    print(ctrl)

# Simulation

@@ -1238,120 +1238,132 @@
 oly(
-input_bound * np.array(%5B%5B-1., 1.%5D, %5B-1., 1.%5D%5D))%0A    W = box2poly(uncertainty * np.array(%5B%5B-1., 1.%5D, %5B-1., 1.%5D%5D))
+%5B%5B-1., 1.%5D, %5B-1., 1.%5D%5D)%0A    U.scale(input_bound)%0A    %0A    W = box2poly(%5B%5B-1., 1.%5D, %5B-1., 1.%5D%5D)%0A    W.scale(uncertainty)%0A    
 %0A   
@@ -1670,31 +1670,8 @@
 oly(
-input_bound * np.array(
 %5B%5B-1
@@ -1685,26 +1685,55 @@
  %5B-1., 1.%5D%5D)
+%0A    U.scale(input_bound
 )%0A
+    %0A
     W = box2
@@ -1741,31 +1741,8 @@
 oly(
-uncertainty * np.array(
 %5B%5B-1
@@ -1760,16 +1760,40 @@
 ., 1.%5D%5D)
+%0A    W.scale(uncertainty
 )%0A    %0A 

use Polytope.scale in continuous_pwa