DoubleBlockContinuousDynamicSystem.java

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

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

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

import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.IntMatrix;
import org.mklab.nfc.ode.SolverStopException;
import org.mklab.tool.control.system.DoubleSystemOperator;


/**
 * 微分方程式で表現されるブロック連続時間動的システムを表わすクラスです。
 * 
 * @author koga
 * @version $Revision$
 */
public abstract class DoubleBlockContinuousDynamicSystem extends DoubleBlockContinuousSystem implements DoubleContinuousDynamicSystem {
  /** 結合システムにおける状態の順番 */
  private int number;

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

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

    return count;
  }

  /**
   * {@inheritDoc}
   */
  public DoubleMatrix getInitialState() {
    DoubleMatrix x = new DoubleMatrix(0, 1);

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

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

  /**
   * {@inheritDoc}
   */
  public DoubleMatrix getInitialStateDerivative() {
    DoubleMatrix x = new DoubleMatrix(0, 1);

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

    for (final DoubleContinuousDynamicSystem system : this.continuousDynamicSystems) {
      x = x.appendDown(system.getInitialStateDerivative());
    }
    return x;
  }
  
  /**
   * {@inheritDoc}
   */
  public IntMatrix getIndex() {
    IntMatrix index = new IntMatrix(0, 1);

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

    for (final DoubleContinuousDynamicSystem system : this.continuousDynamicSystems) {
      index = index.appendDown(system.getIndex());
    }
    return index;
  }

  /**
   * {@inheritDoc}
   */
  public DoubleMatrix getState() {
    DoubleMatrix x = new DoubleMatrix(0, 1);

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

    for (final DoubleContinuousDynamicSystem system : this.continuousDynamicSystems) {
      x = x.appendDown(system.getState());
    }

    return x;
  }

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

    int stateSize = 0;
    int offset = 1;
    for (final DoubleContinuousDynamicSystem 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("BlockContinuousDynamicSystem.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("BlockContinuousDynamicSystem.1"), Integer.valueOf(initialState.getRowSize()), Integer.valueOf(stateSize))); //$NON-NLS-1$
    }
  }
  
  /**
   * {@inheritDoc}
   */
  public void setInitialStateDerivative(final DoubleMatrix initialStateDerivative) {
    if (this.continuousDynamicSystems == null) {
      return;
    }

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

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

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

    int stateSize = 0;
    int offset = 1;
    for (final DoubleContinuousDynamicSystem 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("BlockContinuousDynamicSystem.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("BlockContinuousDynamicSystem.1"), Integer.valueOf(state.getRowSize()), Integer.valueOf(stateSize))); //$NON-NLS-1$
    }
  }

  /**
   * {@inheritDoc}
   */
  public void setIndex(final IntMatrix index) {
    if (this.continuousDynamicSystems == null) {
      return;
    }

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

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

  /**
   * {@inheritDoc}
   */
  public DoubleMatrix outputEquation(final double t, final DoubleMatrix x) throws SolverStopException {
    setState(x);
    resetNodeValue();
    setInputNodeValue(new DoubleMatrix(getInputSize(), 1));
    calcNodeValue(t);
    return getOutputNodeValue();
  }

  /**
   * {@inheritDoc}
   */
  public DoubleMatrix outputEquation(final double t, final DoubleMatrix x, final DoubleMatrix u) throws SolverStopException {
    setState(x);
    resetNodeValue();
    setInputNodeValue(u);
    calcNodeValue(t);
    return getOutputNodeValue();
  }

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

  /**
   * {@inheritDoc}
   */
  @Override
  public void initialize() {
    super.initialize();
    setState(getInitialState());
  }

  /**
   * {@inheritDoc}
   */
  public int getStateNumber() {
    return this.number;
  }

  /**
   * {@inheritDoc}
   */
  public void setStateNumber(final int number) {
    this.number = number;
  }
  
  /**
   * {@inheritDoc}
   */
  public DoubleMatrix getStateDerivative() {
    DoubleMatrix x = new DoubleMatrix(0, 1);

    if (this.continuousDynamicSystems == null) {
      return x;
    }
 
    for (final DoubleContinuousDynamicSystem system : this.continuousDynamicSystems) {
      x = x.appendDown(system.getStateDerivative());
    }

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

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

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

}