#!/usr/bin/env python

# this test is taken almost verbatim from RelayBiSimulation_OT2_noCplugin.py
def test_smc_1():
    from Siconos.Kernel import FirstOrderLinearDS, Model, TimeDiscretisation,\
        TimeStepping, Moreau, ControlManager, linearSensor, linearSMC_OT2,\
        getMatrix, SimpleMatrix
    from matplotlib.pyplot import subplot, title, plot, grid, show
    from numpy import array, eye, empty, zeros, savetxt
    from math import ceil, sin
    from numpy.linalg import norm

    # Derive our own version of FirstOrderLinearDS
    class MyFOLDS(FirstOrderLinearDS):
        def computeb(self, time):
            t = sin(50*time)
            tmpz = self.z()
            # XXX fix this !
            if len(tmpz) != 2:
                print("DEBUG z has length ", len(tmpz))
                return
            # XXX we need to find a smarter way to do things here
            # we need to convert from vector (sage) to arrayish
            u = [t, -t] + tmpz
            self.setb(u)

    # variable declaration
    ndof = 2   # Number of degrees of freedom of your system
    t0 = 0.0   # start time
    T = 1    # end time
    h = 1.0e-4  # time step for simulation
    hControl = 1.0e-2 # time step for control
    Xinit = 1.0 # initial position
    theta = 0.5
    N = ceil((T-t0)/h + 10) # number of time steps
    outputSize = 5 # number of variable to store at each time step

    # Matrix declaration
    A = zeros((ndof,ndof))
    x0 = [Xinit, -Xinit]
    sensorC = eye(ndof)
    sensorD = zeros((ndof,ndof))
    Csurface = [0, 1.0]

    # Simple check
    if h > hControl:
        print "hControl must be bigger than h"
        exit(1)

    # Declaration of the Dynamical System
    processDS = MyFOLDS(x0, A)
    # XXX b is not automatically created ...
    processDS.setb([0, 0])
    # Model
    process = Model(t0, T)
    process.nonSmoothDynamicalSystem().insertDynamicalSystem(processDS)
    # time discretization
    processTD = TimeDiscretisation(t0, h)
    tSensor = TimeDiscretisation(t0, hControl)
    tActuator = TimeDiscretisation(t0, hControl)
    # Creation of the Simulation
    processSimulation = TimeStepping(processTD, 0)
    processSimulation.setName("plant simulation")
    # Declaration of the integrator
    processIntegrator = Moreau(processDS, theta)
    processSimulation.insertIntegrator(processIntegrator)
    # Actuator, Sensor & ControlManager
    control = ControlManager(process)
    sens = linearSensor(100, tSensor, process, sensorC, sensorD)

    control.addSensorPtr(sens)
    act = linearSMC_OT2(104, tActuator, process)
    act.addSensorPtr(sens)
    control.addActuatorPtr(act)

    # Initialization.
    process.initialize(processSimulation)
    control.initialize()
    act.setCsurfacePtr(Csurface)
    # This is not working right now
    #eventsManager = s.eventsManager()

    # Matrix for data storage
    dataPlot = empty((3*(N+1), outputSize))
    dataPlot[0, 0] = t0
    dataPlot[0, 1] = processDS.x()[0]
    dataPlot[0, 2] = processDS.x()[1]
    dataPlot[0, 3] = processDS.z()[0]
    dataPlot[0, 4] = processDS.z()[1]

    # Main loop
    k = 1
    while(processSimulation.nextTime() < T):
        processSimulation.computeOneStep()
        dataPlot[k, 0] = processSimulation.nextTime()
        dataPlot[k, 1] = processDS.x()[0]
        dataPlot[k, 2] = processDS.x()[1]
        dataPlot[k, 3] = processDS.z()[0]
        dataPlot[k, 4] = processDS.z()[1]
        k += 1
        processSimulation.nextStep()
    #    print processSimulation.nextTime()
    # Resize matrix
    dataPlot.resize(k, outputSize)


Add new control test for Front-End