DoubleBlockSampledDataDynamicSystem.java

/**
 * $Id$
 *
 * Copyright (C) 2004-2005 Koga Laboratory. All rights reserved.
 */

package org.mklab.tool.control.system.sampled;

import java.text.MessageFormat;
import java.util.List;

import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.ode.SolverStopException;
import org.mklab.tool.control.system.DoubleSystemOperator;
import org.mklab.tool.control.system.continuous.DoubleContinuousExplicitDynamicSystem;
import org.mklab.tool.control.system.discrete.DoubleDiscreteDynamicSystem;


/**
 * ブロックサンプル値動的システムを表わすクラスです。
 * 
 * @author koga
 * @version $Revision$
 */
public class DoubleBlockSampledDataDynamicSystem extends DoubleBlockSampledDataSystem implements DoubleSampledDataDynamicSystem {

  /** 連続時間システムの状態の数 */
  private int continuousStateSize;
  /** 離散時間システムの状態の数 */
  private int discreteStateSize;

  /**
   * 新しく生成された<code>BlockSampledDataDynamicSystem</code>オブジェクトを初期化します。
   * 
   * @param elements 隣接行列
   * @param inputNodes 入力ノードの番号のリスト(番号は1から始まる)
   * @param outputNodes 出力ノードの番号のリスト(番号は1から始まる)
   */
  public DoubleBlockSampledDataDynamicSystem(final DoubleSystemOperator[][] elements, final List<Integer> inputNodes, final List<Integer> outputNodes) {
    super(elements, inputNodes, outputNodes);
    this.continuousStateSize = calcContinuousStateSize();
    this.discreteStateSize = calcDiscreteStateSize();
    setStateSize(this.continuousStateSize + this.discreteStateSize);
    setDynamic(true);
  }

  /**
   * 連続時間システムの状態の数を返します。
   * 
   * @return 連続時間システムの状態の数
   */
  private int calcContinuousStateSize() {
    int count = 0;
    for (final DoubleContinuousExplicitDynamicSystem system : this.continuousDynamicSystems) {
      count = count + ((DoubleSystemOperator)system).getStateSize();
    }
    return count;
  }

  /**
   * 離散時間システムの状態の数を返します。
   * 
   * @return 離散時間システムの状態の数
   */
  private int calcDiscreteStateSize() {
    int count = 0;

    for (final DoubleDiscreteDynamicSystem system : this.discreteDynamicSystems) {
      count = count + ((DoubleSystemOperator)system).getStateSize();
    }

    return count;
  }

  /**
   * @see org.mklab.tool.control.system.sampled.SampledDataDynamicSystem#getContinuousStateSize()
   */
  public int getContinuousStateSize() {
    return this.continuousStateSize;
  }

  /**
   * @see org.mklab.tool.control.system.sampled.SampledDataDynamicSystem#getDiscreteStateSize()
   */
  public int getDiscreteStateSize() {
    return this.discreteStateSize;
  }

  /**
   * 連続時間システムの状態の数を設定します。
   * 
   * @param stateSize 連続時間システムの状態の数
   */
  protected void setContinuousStateSize(final int stateSize) {
    this.continuousStateSize = stateSize;
  }

  /**
   * 離散時間システムの状態の数を設定します。
   * 
   * @param stateSize 離散時間システムの状態の数
   */
  protected void setDiscreteStateSize(final int stateSize) {
    this.discreteStateSize = stateSize;
  }

  /**
   * @see org.mklab.tool.control.system.sampled.SampledDataDynamicSystem#getContinuousInitialState()
   */
  public DoubleMatrix getContinuousInitialState() {
    DoubleMatrix x = new DoubleMatrix(0, 1);

    if (this.continuousDynamicSystems == null) {
      return x;
    }

    for (final DoubleContinuousExplicitDynamicSystem system : this.continuousDynamicSystems) {
      x = x.appendDown(system.getInitialState());
    }
    return x;
  }

  /**
   * @see org.mklab.tool.control.system.sampled.SampledDataDynamicSystem#getDiscreteInitialState()
   */
  public DoubleMatrix getDiscreteInitialState() {
    DoubleMatrix x = new DoubleMatrix(0, 1);

    if (this.discreteDynamicSystems == null) {
      return x;
    }

    for (final DoubleDiscreteDynamicSystem system : this.discreteDynamicSystems) {
      x = x.appendDown(system.getInitialState());
    }
    return x;
  }

  /**
   * @see org.mklab.tool.control.system.sampled.SampledDataDynamicSystem#getContinuousState()
   */
  public DoubleMatrix getContinuousState() {
    DoubleMatrix x = new DoubleMatrix(0, 1);
    for (final DoubleContinuousExplicitDynamicSystem system : this.continuousDynamicSystems) {
      x = x.appendDown(system.getState());
    }
    return x;
  }

  /**
   * @see org.mklab.tool.control.system.sampled.SampledDataDynamicSystem#getDiscreteState()
   */
  public DoubleMatrix getDiscreteState() {
    DoubleMatrix x = new DoubleMatrix(0, 1);
    for (final DoubleDiscreteDynamicSystem system : this.discreteDynamicSystems) {
      x = x.appendDown(system.getState());
    }

    return x;
  }

  /**
   * {@inheritDoc}
   */
  public void setContinuousInitialState(final DoubleMatrix initialState) {
    if (this.continuousDynamicSystems == null) {
      return;
    }

    int stateSize = 0;
    int offset = 1;
    for (final DoubleContinuousExplicitDynamicSystem system : this.continuousDynamicSystems) {
      final int size = ((DoubleSystemOperator)system).getStateSize();
      if (size > 0) {
        final int end = offset + size - 1;
        if (end > initialState.getRowSize()) {
          throw new IllegalArgumentException(MessageFormat.format(Messages.getString("BlockSampledDataDynamicSystem.0"), system.getClass().getName())); //$NON-NLS-1$
        }
        system.setInitialState(initialState.getSubVector(offset, end));
        offset += size;
        stateSize += size;
      }
    }

    if (initialState.getRowSize() != stateSize) {
      throw new IllegalArgumentException(MessageFormat.format(Messages.getString("BlockSampledDynamicSystem.1"), Integer.valueOf(initialState.getRowSize()), Integer.valueOf(stateSize))); //$NON-NLS-1$
    }
  }

  /**
   * {@inheritDoc}
   */
  public void setDiscreteInitialState(final DoubleMatrix initialState) {
    if (this.discreteDynamicSystems == null) {
      return;
    }

    int stateSize = 0;
    int offset = 1;
    for (final DoubleDiscreteDynamicSystem system : this.discreteDynamicSystems) {
      final int size = ((DoubleSystemOperator)system).getStateSize();
      if (size > 0) {
        final int end = offset + size - 1;
        if (end > initialState.getRowSize()) {
          throw new IllegalArgumentException(MessageFormat.format(Messages.getString("BlockSampledDataDynamicSystem.0"), system.getClass().getName())); //$NON-NLS-1$
        }
        system.setInitialState(initialState.getSubVector(offset, end));
        offset += size;
        stateSize += size;
      }
    }

    if (initialState.getRowSize() != stateSize) {
      throw new IllegalArgumentException(MessageFormat.format(Messages.getString("BlockSampledDynamicSystem.1"), Integer.valueOf(initialState.getRowSize()), Integer.valueOf(stateSize))); //$NON-NLS-1$
    }
  }

  /**
   * {@inheritDoc}
   */
  public void setContinuousState(final DoubleMatrix state) {
    if (this.continuousDynamicSystems == null) {
      return;
    }

    int stateSize = 0;
    int offset = 1;
    for (final DoubleContinuousExplicitDynamicSystem system : this.continuousDynamicSystems) {
      final int size = ((DoubleSystemOperator)system).getStateSize();
      if (size > 0) {
        final int end = offset + size - 1;
        if (end > state.getRowSize()) {
          throw new IllegalArgumentException(MessageFormat.format(Messages.getString("BlockSampledDataDynamicSystem.0"), system.getClass().getName())); //$NON-NLS-1$
        }
        system.setState(state.getSubVector(offset, end));
        offset += size;
        stateSize += size;
      }
    }

    if (state.getRowSize() != stateSize) {
      throw new IllegalArgumentException(MessageFormat.format(Messages.getString("BlockSampledDynamicSystem.1"), Integer.valueOf(state.getRowSize()), Integer.valueOf(stateSize))); //$NON-NLS-1$
    }
  }

  /**
   * {@inheritDoc}
   */
  public void setDiscreteState(final DoubleMatrix state) {
    if (this.discreteDynamicSystems == null) {
      return;
    }

    int stateSize = 0;
    int offset = 1;
    for (final DoubleDiscreteDynamicSystem system : this.discreteDynamicSystems) {
      final int size = ((DoubleSystemOperator)system).getStateSize();
      if (size > 0) {
        final int end = offset + size - 1;
        if (end > state.getRowSize()) {
          throw new IllegalArgumentException(MessageFormat.format(Messages.getString("BlockSampledDataDynamicSystem.0"), system.getClass().getName())); //$NON-NLS-1$
        }
        system.setState(state.getSubVector(offset, end));
        offset += size;
        stateSize += size;
      }
    }

    if (state.getRowSize() != stateSize) {
      throw new IllegalArgumentException(MessageFormat.format(Messages.getString("BlockSampledDynamicSystem.1"), Integer.valueOf(state.getRowSize()), Integer.valueOf(stateSize))); //$NON-NLS-1$
    }
  }

  /**
   * 次のサンプリング周期で状態を更新すべき離散時間システムの状態を設定します。
   * 
   * @param state 現在の全離散時間システムの状態
   */
  private void setDiscreteStateForUpdating(final DoubleMatrix state) {
    int offset = 1;
    for (final DoubleDiscreteDynamicSystem system : this.discreteDynamicSystems) {
      final int size = ((DoubleSystemOperator)system).getStateSize();
      if (size > 0 && this.discreteDynamicSystemsUpdatedAtNextSamplingPoint.contains(system)) {
        system.setState(state.getSubVector(offset, offset + size - 1));
        offset += size;
      }
    }
  }

  /**
   * {@inheritDoc}
   */
  public DoubleMatrix continuousStateEquation(final double t, final DoubleMatrix xc, final DoubleMatrix xd,  final DoubleMatrix inputOutput) throws SolverStopException {
    setContinuousState(xc);
    setDiscreteStateForUpdating(xd);

    DoubleMatrix dx = new DoubleMatrix(0, 1);
    for (final DoubleContinuousExplicitDynamicSystem system : this.continuousDynamicSystems) {
      dx = dx.appendDown(system.stateEquation(t, system.getState(), getInputNodeValueOf((DoubleSystemOperator)system)));
    }

    return dx;
  }

  /**
   * {@inheritDoc}
   */
  public DoubleMatrix discreteStateEquation(final double t, final DoubleMatrix xc, final DoubleMatrix xd,  final DoubleMatrix inputOutput) throws SolverStopException {
    setContinuousState(xc);
    setDiscreteStateForUpdating(xd);

    DoubleMatrix xNext = new DoubleMatrix(0, 1);
    for (final DoubleDiscreteDynamicSystem system : this.discreteDynamicSystems) {
      if (this.discreteDynamicSystemsUpdatedAtNextSamplingPoint.contains(system)) {
        final int k = (int)Math.rint(t / ((DoubleSampler)system).getSamplingInterval());
        final DoubleMatrix nextX = system.stateEquation(k, system.getState(), getInputNodeValueOf((DoubleSystemOperator)system));
        system.setState(nextX); // ??
        xNext = xNext.appendDown(nextX);
      } else {
        xNext = xNext.appendDown(system.getState());
      }
    }

    return xNext;
  }

  /**
   * {@inheritDoc}
   */
  public DoubleMatrix outputEquation(final double t, final DoubleMatrix xc, final DoubleMatrix xd) throws SolverStopException {
    setContinuousState(xc);
    setDiscreteStateForUpdating(xd);

    resetNodeValue();
    setInputNodeValue(new DoubleMatrix(getInputSize(), 1));
    calcNodeValue(t);

    return getOutputNodeValue();
  }

  /**
   * {@inheritDoc}
   */
  public DoubleMatrix outputEquation(final double t, final DoubleMatrix xc, final DoubleMatrix xd, final DoubleMatrix input) throws SolverStopException {
    setContinuousState(xc);
    setDiscreteStateForUpdating(xd);

    resetNodeValue();
    setInputNodeValue(input);
    calcNodeValue(t);

    return getOutputNodeValue();
  }

  /**
   * {@inheritDoc}
   */
  public DoubleMatrix differentialEquation(final double t, final DoubleMatrix xc, final DoubleMatrix xd, final DoubleMatrix inputOutput) throws SolverStopException {
    return continuousStateEquation(t, xc, xd, inputOutput);
  }

  /**
   * {@inheritDoc}
   */
  public DoubleMatrix differenceEquation(final double t, final DoubleMatrix xc, final DoubleMatrix xd, final DoubleMatrix inputOutput) throws SolverStopException {
    return discreteStateEquation(t, xc, xd, inputOutput);
  }

  /**
   * {@inheritDoc}
   */
  public DoubleMatrix inputOutputEquation(final double t, final DoubleMatrix xc, final DoubleMatrix xd) throws SolverStopException {
    final DoubleMatrix u = new DoubleMatrix(getInputSize(), 1);
    final DoubleMatrix y = outputEquation(t, xc, xd);
    return u.appendDown(y);
  }

  /**
   * @see org.mklab.tool.control.system.SystemOperator#initialize()
   */
  @Override
  public void initialize() {
    super.initialize();
    if (this.continuousDynamicSystems != null) {
      setContinuousState(getContinuousInitialState());
    }
    if (this.discreteDynamicSystems != null) {
      setDiscreteState(getDiscreteInitialState());
    }
  }
}