/*
 * Nonlinear state space model in the CT domain.
 *
 * The State-Space model implements a system whose behavior is defined by:
 * 
 * (d/dt)x(t) = f(x(t),u(t))
 * y(t) = h(x(t),u(t))
 * x(0) = x0
 * 
 * where x is the state vector, u is the input vector, and y is the output
 * vector.  The number of states, inputs, and outputs are parameters of the
 * actor.  We abbreviate these with
 *
 * n, the number of states
 * m, the number of inputs
 * r, the number of outputs
 *
 * After these are assigned, parameters f1, f2, ..., fn and h1,
 * h2, ..., hr are created, as well as an initalStates parameter.  
 * To these the user can assign expressions to represent the 
 * nonlinear system.  Setting the expressions defines the functions
 * 
 * f0(x(0, 0), x(0, 1), ..., x(0, n-1), u(0, 0), u(0, 1), ..., u(0, m-1))
 * f1(x(0, 0), x(0, 1), ..., x(0, n-1), u(0, 0), u(0, 1), ..., u(0, m-1))
 * ...
 * fn-1(x(0, 0), x(0, 1), ..., x(0, n-1), u(0, 0), u(0, 1), ..., u(0, m-1))
 * h0(x(0, 0), x(0, 1), ..., x(0, n-1), u(0, 0), u(0, 1), ..., u(0, m-1))
 * h1(x(0, 0), x(0, 1), ..., x(0, n-1), u(0, 0), u(0, 1), ..., u(0, m-1))
 * ...
 * hr-1(x(0, 0), x(0, 1), ..., x(0, n-1), u(0, 0), u(0, 1), ..., u(0, m-1))
 *
 * These expressions must be passed as strings, such as
 * "u(0, 1) * cos(x(0, 1)) + u(0, 1) * sin(x(0, 2))"
 *
 * The initialStates parameter should be given a column vector whose
 * length equals the number of states, such as:
 *
 * [0.0; 1.0; 0.0]
 *
 * @author Adam Cataldo
 */

/*
 * This line is used to prevent Ptalon from trying to bring
 * unconnected ports of actors to the outside of the Ptalon
 * actor.  It overrides the default behavior.
 */
danglingPortsOkay;

NonlinearStateSpace is {
	/*
	 * These lines define some actor "constants" to be
	 * used later.
	 */
	actor integrator = ptolemy.domains.continuous.lib.Integrator;
	actor assembler = ptolemy.actor.lib.VectorAssembler;
	actor disassembler = ptolemy.actor.lib.VectorDisassembler;
	actor expression = ptolemy.actor.lib.Expression;
	actor typeConverter = ptolemy.actor.lib.conversions.AnythingToDouble;
	
	/*
	 * These lines create the ports for the actor, which are
	 * all multiports.
	 */
	inport[] input;
	outport[] output;
	outport[] stateOutput;
	
	/*
	 * These lines create the initial parameters for the actor.
	 */
	parameter numberOfInputs;
	parameter numberOfStates;
	parameter numberOfOutputs;
	
	/*
	 * Set up the f0, f1, ..., fn-1 parameters.  Note that the loop won't 
	 * be iterated until the previous  parameters have been set.
	 */
	for a initially [[ 0 ]] [[ a < numberOfStates ]] {
		parameter f[[a]];		
	} next [[ a + 1 ]]
	
	/*
	 * This loop sets up the h0, h1, ..., hr-1 parameters and
	 * connects the inputs to the input vector assembler.
	 */
	for a initially [[ 0 ]] [[ a < numberOfOutputs ]] {
		parameter h[[a]];
	} next [[ a + 1 ]]
	
	/*
	 * This if block is used to delay creating this parameter
	 * until after the numberOfStates parameter is defined.
	 * It's not really needed, but this way, there's not too
	 * much clutter on the screen at first.
	 */
	if [[ numberOfOutputs > 0 ]] {
		parameter initialStates;
	} else {}

	/*
	 * The relation x and u represent the vector data for
	 * the inputs and outputs correspondingly.
	 */
	relation x;
	relation u;
	
	/*
	 * These transparent relations are used to aggregate
	 * inputs and states, respectively.
	 */
	transparent relation inputRelation;
	transparent relation stateRelation;
		
	/*
	 * Assemblers aggregate the data into vectors, and
	 * disassemblers do the reverse.
	 */
	assembler(input := inputRelation, output := u);
	assembler(input := stateRelation, output := x);
	transparent relation stateAsOutput;
	disassembler(input := x, output := stateAsOutput);

	/*
	 * Connect the inputs to the vector assembler
	 */
	for a initially [[ 0 ]] [[ a < numberOfInputs ]] {
		this(input := inputRelation);
	} next [[ a + 1 ]]
	
	/*
	 * Create the expression evaluators for f0, f1, ...,
	 * fn-1 and the corresponding integrators.  Also,
	 * make the appropriate connectors.
	 */
	for a initially [[ 0 ]] [[ a < numberOfStates ]] {
		/*
		 * Note that the expression actor does not
		 * have ports named x and u, so these ports
		 * will be created.
		 */
		relation x[[a]];
		expression(x := x, u := u, output := x[[a]],
			expression := [[ eval(eval("f" + a.toString)) ]]);
		relation convertedX[[a]];
		typeConverter(input := x[[a]], output := convertedX[[a]]);
		integrator(derivative := convertedX[[a]], state := stateRelation,
			initialState := [[initialStates(0, a)]]);
		this(stateOutput := stateAsOutput);
	} next [[ a + 1 ]]

	/*
	 * Create the expression evaluators for h0, h0, ...,
	 * hm-1.
	 */
	for a initially [[ 0 ]] [[ a < numberOfOutputs ]] {
		expression(x := x, u := u, output := output,
			expression := [[ eval(eval("h" + a.toString)) ]]);
	} next [[ a + 1 ]]

}