DoubleBlockSystem.java

/**
 * $Id: BlockSystem.java,v 1.162 2008/07/15 15:27:16 koga Exp $
 *
 * Copyright (C) 2004-2005 Koga Laboratory. All rights reserved.
 */

package org.mklab.tool.control.system;

import java.text.MessageFormat;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.HashMap;
import java.util.List;
import java.util.Map;

import org.mklab.nfc.SolverException;
import org.mklab.nfc.matrix.BooleanMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.nleq.NewtonRaphsonFixedPointSolver;
import org.mklab.nfc.nleq.NonLinearEquationSolver;
import org.mklab.nfc.nleq.NonLinearFunction;
import org.mklab.nfc.ode.SolverStopException;
import org.mklab.nfc.scalar.DoubleNumber;
import org.mklab.tool.control.DoubleLinearSystem;
import org.mklab.tool.control.system.graph.DoubleMinimumUnknownEquation;
import org.mklab.tool.control.system.graph.DoubleNodeIdentityFunctionEquation;
import org.mklab.tool.control.system.math.DoubleConstantSystem;
import org.mklab.tool.control.system.math.DoubleUnitSystem;
import org.mklab.tool.control.system.sink.DoubleContinuousSink;
import org.mklab.tool.control.system.sink.DoubleOutputPort;
import org.mklab.tool.control.system.source.DoubleContinuousSource;
import org.mklab.tool.control.system.source.DoubleInputPort;


/**
 * ブロックシステムを表わすクラスです。
 * 
 * @author koga
 * @version $Revision: 1.162 $
 */
public abstract class DoubleBlockSystem extends DoubleSystemOperator {

  /** ノードの数 */
  private int nodeSize;

  /** 入力ノードの番号のリスト */
  private List<Integer> inputNodes;

  /** 出力ノードの番号のリスト */
  private List<Integer> outputNodes;

  /** ノードの大きさのリスト */
  private int[] nodeSizes;

  /** 隣接行列 */
  private DoubleSystemOperator[][] elements;

  /** 非直達項成分の隣接行列 */
  private DoubleSystemOperator[][] nonDirectFeedthroughElements;

  /** 直達項成分の隣接行列 */
  private DoubleSystemOperator[][] directFeedthroughElements;

  /** システムとその入力ノードの番号の対のマップ */
  private Map<String, Integer> inputMap = new HashMap<>();

  /** ノードの値 */
  private DoubleMatrix[] nodeValue;

  /** 切るべきノードの番号のリストのリスト */
  private List<List<Integer>> cuttingNodes;

  /** 値が決まっていないノードの1ステップ前の値を結合したベクトルの配列 */
  private DoubleMatrix[] cuttingNodeValues;

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

    this.elements = elements;
    final Integer[] inNodes = new Integer[inputNodes.size()];
    this.inputNodes = new ArrayList<>(Arrays.asList(inputNodes.toArray(inNodes)));
    final Integer[] outNodes = new Integer[outputNodes.size()];
    this.outputNodes = new ArrayList<>(Arrays.asList(outputNodes.toArray(outNodes)));
    this.nodeSize = elements.length;
    this.nodeValue = new DoubleMatrix[this.nodeSize];

    initialize();

    setInputSize(findInputSize());
    setOutputSize(findOutputSize());
    setupLinear();
    setupInputMap();

    //setZeroSizeToUnDefinedInputPort();
    //setZeroSizeToUnDefinedOutputPort();

    setupNodeSize();

    setInputSizeAndOutpuSize();
  }

  /**
   * 隣接行列構成要素のチェックを行います。
   * 
   * @param elements チェックする要素配列
   */
  @SuppressWarnings("boxing")
  private static void checkElements(DoubleSystemOperator[][] elements) {
    if (elements.length == 0) throw new IllegalArgumentException();

    final int baseColumnCount = elements[0].length;
    for (int i = 0; i < elements.length; i++) {
      if (elements[i].length != baseColumnCount) throw new IllegalArgumentException();

      for (int j = 0; j < baseColumnCount; j++) {
        if (elements[i][j] == null) throw new IllegalArgumentException(MessageFormat.format("Null system was detected : (row={0},column={1})", i, j)); //$NON-NLS-1$
      }
    }
  }

  /**
   * 出力数が未定のInputPortの出力数を0に、入力数が未定のOutputPortの入力数を0に設定します。
   */
  public void setZeroSizeToUnDefinedInputPortOutputPort() {
    setZeroSizeToUnDefinedInputPort();
    setZeroSizeToUnDefinedOutputPort();

    setupNodeSize();

    setInputSizeAndOutpuSize();
  }

  /**
   * 他のノードから入るエッジがないノードの値を確定します。 <p> ノードの仮の値を正式の値へコピーします。
   * 
   * @param matrix 隣接行列を転置した行列
   * @param nodeValue ノードの値
   * @param nodeTmpValue 仮のノードの値
   * @param skipping 値が決まっているノードについての処理をスキップするならばtrue、そうでなければfalse
   */
  public void setNodeValueOfNoInputNode(final DoubleSystemOperator[][] matrix, final DoubleMatrix[] nodeValue, final DoubleMatrix[] nodeTmpValue, final boolean skipping) {
    final int localNodeNumber = nodeTmpValue.length;
    for (int i = 0; i < localNodeNumber; i++) {
      if (skipping && nodeValue[i] != null) {
        continue;
      }

      boolean noSystem = true;
      for (int j = 0; j < localNodeNumber; j++) {
        if (matrix[i][j] != DoubleZeroSystem.getInstance()) {
          noSystem = false;
        }
      }
      if (noSystem) {
        nodeValue[i] = nodeTmpValue[i];
      }
    }
  }

  /**
   * 全ての列成分がゼロまたは定数システムであるか判定します。
   * 
   * @param matrix 隣接行列の転置
   * @param column 調べる列
   * @return 全ての列成分がゼロまたは定数システムならばtrue、そうでなければfalse
   */
  private boolean isAllConstantOrZeroInColumn(final DoubleSystemOperator[][] matrix, final int column) {
    final int rowSize = matrix.length;

    for (int row = 0; row < rowSize; row++) {
      DoubleSystemOperator system = matrix[row][column];
      if (system != DoubleZeroSystem.getInstance() && !(system instanceof DoubleConstantSystem)) {
        return false;
      }
    }

    return true;
  }

  /**
   * 極大閉路を形成するノードのリストのリストを返します。
   * 
   * @param matrix 隣接行列
   * @return 極大閉路を形成するノードのリストのリスト
   */
  @SuppressWarnings("boxing")
  static List<List<Integer>> getLocalMaximumCycles(final DoubleSystemOperator[][] matrix) {

    final int size = matrix.length;

    final List<List<Integer>> loopList = new ArrayList<>();

    // 対角成分に値が入っている場合、ループがあると判断
    for (int i = 0; i < size; i++) {
      if (matrix[i][i] != DoubleZeroSystem.getInstance()) {
        final List<Integer> loop = new ArrayList<>();
        loop.add(i + 1);
        loopList.add(loop);
      }
    }

    // 零でない値が入っている成分と対角成分にtrue、それ以外の成分にfalse
    final BooleanMatrix nodeMatrix = BooleanMatrix.unit(size, size);
    for (int row = 0; row < size; row++) {
      for (int column = 0; column < size; column++) {
        if (matrix[row][column] != DoubleZeroSystem.getInstance()) {
          nodeMatrix.setElement(row + 1, column + 1, true);
        }
      }
      nodeMatrix.setElement(row + 1, row + 1, true);
    }

    // ノードの数だけ累乗を求める
    final BooleanMatrix loopMatrix = nodeMatrix.power(size);

    // iノードからjノードへのパスがあり、jノードからiノードへのパスがあれば、
    // iノードとjノードは同一ループ中に存在する
    for (int j = 1; j <= size; j++) {
      final List<Integer> loop = new ArrayList<>();
      for (int i = 1; i <= size; i++) {
        if (loopMatrix.getElement(i, j) && loopMatrix.getElement(j, i)) {
          loop.add(i);
        }
      }

      if (loop.size() > 1 && loopList.contains(loop) == false) {
        loopList.add(loop);
      }
    }

    return loopList;
  }

  /**
   * 自己ループをもつノード(システム)の入出力を求め、このシステムを隣接行列から削除します。
   * 
   * @param matrix 隣接行列
   * @param tmpValue ノードの仮の値
   * @return 縮約したならばtrue、そうでなければfalse
   */
  private boolean resolveConstantSelfLoop(final DoubleSystemOperator[][] matrix, final DoubleMatrix[] tmpValue) {
    final int rowSize = matrix.length;

    boolean changed = false;

    for (int i = 0; i < rowSize; i++) {
      if (!(matrix[i][i] instanceof DoubleConstantSystem) || !DoubleAdjacencyMatrixUtil.isAllZeroInRow(matrix, i, i)) {
        continue;
      }

      changed = true;

      final DoubleMatrix D = ((DoubleConstantSystem)matrix[i][i]).getGain();
      final int size = D.getRowSize();
      tmpValue[i] = DoubleMatrix.unit(size, size).subtract(D).leftDivide(tmpValue[i]);
      matrix[i][i] = DoubleZeroSystem.getInstance();
    }

    return changed;
  }

  /**
   * (行方向に)定数システムと定数システムの直列を結合(定数直列エッジを縮約)します。
   * 
   * @param matrix 隣接行列
   * @param nodeTmpValue 仮のノードの値
   * @return 縮約したならばtrue、そうでなければfalse
   * @throws SolverStopException ソルバーが停止された場合
   */
  private boolean contractConstantEdgeForwardRowWise(final DoubleSystemOperator[][] matrix, final DoubleMatrix[] nodeTmpValue) throws SolverStopException {
    final int rowSize = matrix.length;
    final int columnSize = rowSize == 0 ? 0 : matrix[0].length;

    boolean changed = false;

    for (int row = 0; row < rowSize; row++) {
      final boolean hasSelfLoop = (matrix[row][row] != DoubleZeroSystem.getInstance());
      if (hasSelfLoop) {
        return changed;
      }

      for (int column = 0; column < columnSize; column++) {
        if (!(matrix[row][column] instanceof DoubleConstantSystem)) {
          continue;
        }

        if (!DoubleAdjacencyMatrixUtil.isAllZeroInRow(matrix, row, column) && !isAllConstantOrZeroInColumn(matrix, row)) {
          continue;
        }

        if (!DoubleAdjacencyMatrixUtil.isEitherColumnZeroOrBothConstant(matrix, row, column)) {
          continue;
        }

        if (!DoubleAdjacencyMatrixUtil.isAllLinearOrConstantOrZeroInColumn(matrix, row)) {
          continue;
        }

        final DoubleConstantSystem gainSystem = ((DoubleConstantSystem)matrix[row][column]);
        final DoubleMatrix gain1 = gainSystem.getGain();

        if (nodeTmpValue[row].isZero()) {
          for (int k = 0; k < rowSize; k++) {
            final DoubleSystemOperator systemInRow = matrix[k][row];
            final DoubleSystemOperator systemInColumn = matrix[k][column];

            if (systemInRow == DoubleZeroSystem.getInstance()) {
              continue;
            }

            if (DoubleAdjacencyMatrixUtil.isAllZeroInRow(matrix, row, column) == false) {
              continue;
            }

            if (systemInRow instanceof DoubleConstantSystem && systemInColumn instanceof DoubleConstantSystem) {
              matrix[k][column] = ((DoubleConstantSystem)systemInColumn).add(((DoubleConstantSystem)systemInRow).multiply(gainSystem));
            } else {
              final DoubleSystemOperator contractedSystem;
              if (systemInRow instanceof DoubleConstantSystem) {
                contractedSystem = ((DoubleConstantSystem)systemInRow).multiply(gainSystem);
              } else {
                contractedSystem = DoubleAdjacencyMatrixUtil.multiplyGain((DoubleLinearSystemOperator)systemInRow, gain1);
              }

              if (contractedSystem.hasDirectFeedthrough()) {
                matrix[k][column] = contractedSystem;
              } else {
                nodeTmpValue[k] = nodeTmpValue[k].add(calcOutputOfNonDirectFeedthroughSystem(contractedSystem));
                matrix[k][column] = DoubleZeroSystem.getInstance();
              }
            }

            changed = true;

            if (DoubleAdjacencyMatrixUtil.isAllZeroInRow(matrix, row, column)) {
              matrix[k][row] = DoubleZeroSystem.getInstance();
            }
          }
        } else if (isAllConstantOrZeroInColumn(matrix, row)) {
          for (int k = 0; k < rowSize; k++) {
            final DoubleSystemOperator systemInRow = matrix[k][row];
            final DoubleSystemOperator systemInColumn = matrix[k][column];

            if (systemInRow == DoubleZeroSystem.getInstance()) {
              continue;
            }

            if (DoubleAdjacencyMatrixUtil.isAllZeroInRow(matrix, row, column) == false) {
              continue;
            }

            if (systemInRow instanceof DoubleConstantSystem && systemInColumn instanceof DoubleConstantSystem) {
              matrix[k][column] = ((DoubleConstantSystem)systemInColumn).add(((DoubleConstantSystem)systemInRow).multiply(gainSystem));
            } else {
              final DoubleSystemOperator contractedSystem;
              if (systemInRow instanceof DoubleConstantSystem) {
                contractedSystem = ((DoubleConstantSystem)systemInRow).multiply(gainSystem);
              } else {
                contractedSystem = DoubleAdjacencyMatrixUtil.multiplyGain((DoubleLinearSystemOperator)systemInRow, gain1);
              }

              if (contractedSystem.hasDirectFeedthrough()) {
                matrix[k][column] = contractedSystem;
              } else {
                nodeTmpValue[k] = nodeTmpValue[k].add(calcOutputOfNonDirectFeedthroughSystem(contractedSystem));
                matrix[k][column] = DoubleZeroSystem.getInstance();
              }
            }

            final DoubleMatrix gain2 = ((DoubleConstantSystem)systemInRow).getGain();
            nodeTmpValue[k] = nodeTmpValue[k].add(gain2.multiply(nodeTmpValue[row]));

            changed = true;

            if (DoubleAdjacencyMatrixUtil.isAllZeroInRow(matrix, row, column)) {
              matrix[k][row] = DoubleZeroSystem.getInstance();
            }
          }
        }
      }
    }

    return changed;
  }

  /**
   * (行方向に)単位システムを前方のシステムに結合(単位エッジを縮約)します。
   * 
   * @param matrix 隣接行列
   * @param tmpValue 仮のノードの値
   * @return 縮約したならばtrue、そうでなければfalse
   */
  boolean contractUnitEdgeForwardRowWise(final DoubleSystemOperator[][] matrix, final DoubleMatrix[] tmpValue) {
    final int rowSize = matrix.length;
    final int columnSize = rowSize == 0 ? 0 : matrix[0].length;

    boolean changed = false;

    for (int row = 0; row < rowSize; row++) {
      for (int column = 0; column < columnSize; column++) {
        boolean hasSelfLoop = (row == column && matrix[row][column] != DoubleZeroSystem.getInstance());
        if (hasSelfLoop) {
          return changed;
        }

        if (!(matrix[row][column] instanceof DoubleUnitSystem)) {
          continue;
        }

        if (!DoubleAdjacencyMatrixUtil.isAllZeroInRow(matrix, row, column)) {
          continue;
        }

        if (!DoubleAdjacencyMatrixUtil.isEitherColumnZeroOrBothConstant(matrix, row, column)) {
          continue;
        }

        if (tmpValue[row].isZero()) {
          for (int k = 0; k < rowSize; k++) {
            if (matrix[k][row] == DoubleZeroSystem.getInstance()) {
              continue;
            }

            changed = true;
            matrix[k][column] = matrix[k][row];
            matrix[k][row] = DoubleZeroSystem.getInstance();
          }
        } else if (isAllConstantOrZeroInColumn(matrix, column)) {
          for (int k = 0; k < rowSize; k++) {
            if (matrix[k][row] == DoubleZeroSystem.getInstance()) {
              continue;
            }

            changed = true;

            matrix[k][column] = matrix[k][row];
            DoubleMatrix gain = ((DoubleConstantSystem)matrix[k][row]).getGain();
            tmpValue[k] = tmpValue[k].add(gain.multiply(tmpValue[row]));

            matrix[k][row] = DoubleZeroSystem.getInstance();
          }
        }
      }
    }

    return changed;
  }

  /**
   * 入力の数を返します。
   * 
   * @return 入力ノードの入力の数
   */
  private int findInputSize() {
    if (this.inputNodes.size() == 0) {
      return 0;
    }

    int inputSize = 0;

    for (int inputNode : this.inputNodes) {
      inputSize += Math.max(findNodeSize(inputNode), 0);
    }

    return inputSize;
  }

  /**
   * 出力数が未定のInputPortの出力数を0に設定します。
   */
  private void setZeroSizeToUnDefinedInputPort() {
    for (int inputNode : this.inputNodes) {
      for (int column = 0; column < this.nodeSize; column++) {
        DoubleSystemOperator system = this.elements[inputNode - 1][column];
        if (system == null || system == DoubleZeroSystem.getInstance()) {
          continue;
        }
        if (system instanceof DoubleUnitSystem && system.getOutputSize() == -1) {
          system.setOutputSize(0);
        }
      }
    }
  }

  /**
   * 入力数が未定のOutputPortの入力数を0に設定します。
   */
  private void setZeroSizeToUnDefinedOutputPort() {
    for (int outputNode : this.outputNodes) {
      for (int row = 0; row < this.nodeSize; row++) {
        DoubleSystemOperator system = this.elements[row][outputNode - 1];
        if (system == null || system == DoubleZeroSystem.getInstance()) {
          continue;
        }
        if (system instanceof DoubleUnitSystem && system.getInputSize() == -1) {
          system.setInputSize(0);
        }
      }
    }
  }

  /**
   * 出力の数を返します。
   * 
   * @return 出力ノードの出力の数
   */
  private int findOutputSize() {
    if (this.outputNodes.size() == 0) {
      return 0;
    }

    int outputSize = 0;

    for (int outputNode : this.outputNodes) {
      outputSize += Math.max(findNodeSize(outputNode), 0);
    }

    return outputSize;
  }

  /**
   * ノードの大きさ(信号の数)を返します。
   * 
   * @param nodeNumber ノードの番号
   * @return ノードの大きさ(信号の数)
   */
  private int findNodeSize(int nodeNumber) {
    for (int column = 0; column < this.nodeSize; column++) {
      final DoubleSystemOperator system = this.elements[nodeNumber - 1][column];
      if (system != DoubleZeroSystem.getInstance()) {
        // inlet の場合は入力の数が0
        final int size = system.getInputSize();
        if (size >= 0) {
          return size;
        }
      }
    }

    for (int row = 0; row < this.nodeSize; row++) {
      final DoubleSystemOperator system = this.elements[row][nodeNumber - 1];
      if (system != DoubleZeroSystem.getInstance()) {
        // outlet の場合は出力の数が0
        final int size = system.getOutputSize();
        if (size >= 0) {
          return size;
        }
      }
    }
    return -1;
  }

  /**
   * 入力数と出力数が決定されていないシステムの入出力数を設定します。
   */
  private void setInputSizeAndOutpuSize() {
    boolean nodeSizeResized = false;

    do {
      nodeSizeResized = false;

      for (int column = 0; column < this.nodeSize; column++) {
        for (int row = 0; row < this.nodeSize; row++) {
          final DoubleSystemOperator system = this.elements[row][column];
          if (system == DoubleZeroSystem.getInstance()) {
            continue;
          }

          final int inputNodeSize = this.nodeSizes[row];
          if (inputNodeSize != -1) {
            if (system.isAutoSize() == false && system.getInputSize() != inputNodeSize) {
              throw new RuntimeException("" + system + ": " + Messages.getString("BlockSystem.3")); //$NON-NLS-1$ //$NON-NLS-2$ //$NON-NLS-3$
            }

            if (inputNodeSize != system.getInputSize()) {
              system.setInputSize(inputNodeSize);

              if (this.nodeSizes[column] != system.getOutputSize() && system.getOutputSize() != -1) {
                this.nodeSizes[column] = system.getOutputSize();
                nodeSizeResized = true;
              }
            }
          }

          final int outputNodeSize = this.nodeSizes[column];
          if (outputNodeSize != -1) {
            if (system.isAutoSize() == false && system.getOutputSize() != outputNodeSize) {
              throw new RuntimeException("" + system + ": " + Messages.getString("BlockSystem.4")); //$NON-NLS-1$ //$NON-NLS-2$ //$NON-NLS-3$
            }

            if (outputNodeSize != system.getOutputSize()) {
              system.setOutputSize(outputNodeSize);

              if (this.nodeSizes[row] != system.getInputSize() && system.getInputSize() != -1) {
                this.nodeSizes[row] = system.getInputSize();
                nodeSizeResized = true;
              }
            }
          }
        }
      }
    } while (nodeSizeResized);
  }

  /**
   * システムと入力ノード番号の対を設定します。
   */
  @SuppressWarnings("boxing")
  private void setupInputMap() {
    this.inputMap.clear();

    final int size = this.nodeSize;

    for (int row = 0; row < size; row++) {
      for (int column = 0; column < size; column++) {
        final DoubleSystemOperator system = this.elements[row][column];
        if (system != DoubleZeroSystem.getInstance()) {
          this.inputMap.put(system.getID(), row);
        }
      }
    }
  }

  /**
   * ノードの大きさ(信号の数)を設定します。
   */
  private void setupNodeSize() {
    this.nodeSizes = new int[this.nodeSize];

    for (int i = 0; i < this.nodeSize; i++) {
      this.nodeSizes[i] = findNodeSize(i + 1);
    }
  }

  /**
   * 入力ノードに値を設定します。
   * 
   * @param u 入力ノードの値
   */
  public void setInputNodeValue(DoubleMatrix u) {
    if (u.length() != getInputSize()) {
      throw new RuntimeException(Messages.getString("BlockSystem.8")); //$NON-NLS-1$
    }

    int offset = 1;
    for (int inputNode : this.inputNodes) {
      int size = this.nodeSizes[inputNode - 1];
      if (size > 0) {
        this.nodeValue[inputNode - 1] = u.getSubVector(offset, offset + size - 1);
        offset += size;
      }
    }
  }

  /**
   * 出力ノードの値を返します。
   * 
   * @return 出力ノードの値
   */
  protected DoubleMatrix getOutputNodeValue() {
    DoubleMatrix y = new DoubleMatrix(0, 1);

    for (int outputNode : this.outputNodes) {
      y = y.appendDown(this.nodeValue[outputNode - 1]);
    }

    return y;
  }

  /**
   * システムへ入力するノードの値を返します。
   * 
   * @param system 入力するノードの値を調べるシステム
   * @return システムへ入力するノードの値
   */
  @SuppressWarnings("boxing")
  public DoubleMatrix getInputNodeValueOf(DoubleSystemOperator system) {
    if (this.inputMap.containsKey(system.getID()) == false) {
      throw new RuntimeException(Messages.getString("BlockSystem.11")); //$NON-NLS-1$
    }
    return this.nodeValue[this.inputMap.get(system.getID())];
  }

  /**
   * ノードの数を返します。
   * 
   * @return ノードの数
   */
  public int getNodeSize() {
    return this.nodeSize;
  }

  /**
   * 入力ノードの数を返します。
   * 
   * @return 入力ノードの数
   */
  public int getInputNodeSize() {
    return this.inputNodes.size();
  }

  /**
   * 出力ノードの数を返します。
   * 
   * @return 出力ノードの数
   */
  public int getOutputNodeSize() {
    return this.outputNodes.size();
  }

  /**
   * 線形でないシステムが含まれているか調べ、線形性を設定します。
   */
  private void setupLinear() {
    setLinear(true);

    final int size = this.nodeSize;

    for (int row = 0; row < size; row++) {
      for (int column = 0; column < size; column++) {
        final DoubleSystemOperator system = this.elements[row][column];
        final boolean notLinear = system.isLinear() == false;
        final boolean notInputPort = (system instanceof DoubleInputPort) == false;
        final boolean notOutputPort = (system instanceof DoubleOutputPort) == false;
        final boolean notZeroSystem = system != DoubleZeroSystem.getInstance();
        if (notZeroSystem && notInputPort && notOutputPort && notLinear) {
          setLinear(false);
          return;
        }
      }
    }
  }

  /**
   * ノードの値をリセットします。
   */
  protected void resetNodeValue() {
    final int size = this.nodeSize;

    for (int i = 0; i < size; i++) {
      this.nodeValue[i] = null;
    }
  }

  /**
   * ノードの仮の値をゼロに設定します。
   * 
   * @param nodeTmpValue ノードの仮の値
   */
  private void setZeroNodeTmpValue(final DoubleMatrix[] nodeTmpValue) {
    final int size = this.nodeSize;

    for (int column = 0; column < size; column++) {
      for (int row = 0; row < size; row++) {
        if (this.elements[row][column] == DoubleZeroSystem.getInstance()) {
          continue;
        }

        int nodeSizeI = this.elements[row][column].getOutputSize();
        if (nodeTmpValue[column] == null) {
          nodeTmpValue[column] = new DoubleMatrix(nodeSizeI, 1);
        }else if (nodeTmpValue[column].getRowSize() != nodeSizeI) {
          throw new RuntimeException(Messages.getString("BlockSystem.12")); //$NON-NLS-1$
        }

        int nodeSizeJ = this.elements[row][column].getInputSize();
        if (nodeTmpValue[row] == null) {
          nodeTmpValue[row] = new DoubleMatrix(nodeSizeJ, 1);
        } else if (nodeTmpValue[row].getRowSize() != nodeSizeJ) {
          throw new RuntimeException(Messages.getString("BlockSystem.13")); //$NON-NLS-1$
        }
      }
    }

    for (int i = 0; i < size; i++) {
      if (nodeTmpValue[i] == null) {
        throw new RuntimeException(Messages.getString("BlockSystem.14") + i + Messages.getString("BlockSystem.15")); //$NON-NLS-1$ //$NON-NLS-2$
      }
    }
  }

  /**
   * 値が決定されていない(nullである)番号を返します。
   * 
   * @param values ノードの値の配列
   * @return 値が決定されていない(nullである)番号の配列
   */
  private int[] getUnknownNode(final DoubleMatrix[] values) {
    final int size = values.length;
    int count = 0;
    for (int i = 0; i < size; i++) {
      if (values[i] == null) {
        count++;
      }
    }

    int[] unknownNode = new int[count];

    for (int i = 0, j = 0; i < size; i++) {
      if (values[i] == null) {
        unknownNode[j++] = i;
      }

    }

    return unknownNode;
  }

  /**
   * 直達項成分の隣接行列の転置を生成します。
   * 
   * @param matrix 隣接行列
   * @return 直達項成分の隣接行列の転置
   */
  @SuppressWarnings("boxing")
  private DoubleSystemOperator[][] getDirectFeedthroughPart(final DoubleSystemOperator[][] matrix) {
    final int size = this.nodeSize;

    final DoubleSystemOperator[][] newMatrix = new DoubleSystemOperator[size][size];

    for (int row = 0; row < size; row++) {
      for (int column = 0; column < size; column++) {
        final DoubleSystemOperator system = matrix[row][column];

        if (!system.hasDirectFeedthrough()) {
          newMatrix[row][column] = DoubleZeroSystem.getInstance();
        } else if (system instanceof DoubleLinearSystemOperator && ((DoubleLinearSystemOperator)system).hasVariableE()==false && system instanceof DoubleDynamicSystem) {
          final DoubleMatrix D = ((DoubleLinearSystemOperator)system).getLinearSystem().getD();
          final DoubleSystemOperator newSystem = new DoubleConstantSystem(D);
          newMatrix[row][column] = newSystem;
          newMatrix[row][column].setID(system.getID() + "D"); //$NON-NLS-1$

          final int inputNumber = this.inputMap.get(system.getID());
          this.inputMap.put(newSystem.getID(), inputNumber);
        } else {
          newMatrix[row][column] = system;
        }
      }
    }

    return newMatrix;
  }

  /**
   * 転置グリッドを生成します。
   * 
   * @param grid 元のグリッド
   * @return 転置グリッド
   */
  private DoubleSystemOperator[][] transpose(final DoubleSystemOperator[][] grid) {
    final int rowSize = grid.length;
    final int columnSize = rowSize == 0 ? 0 : grid[0].length;
    DoubleSystemOperator[][] ans = new DoubleSystemOperator[columnSize][rowSize];

    for (int row = 0; row < rowSize; row++) {
      DoubleSystemOperator[] gridi = grid[row];
      for (int column = 0; column < columnSize; column++) {
        ans[column][row] = gridi[column]; // not generate clone
      }
    }
    return ans;
  }

  /**
   * 部分グリッドを返します。
   * 
   * @param grid 元のグリッド
   * @param rowIndex 該当する行の番号
   * @param columnIndex 該当する列の番号
   * @return 部分グリッド
   */
  static final DoubleSystemOperator[][] getSubMatrix(final DoubleSystemOperator[][] grid, final int[] rowIndex, final int[] columnIndex) {
    final int rowSize = rowIndex.length;
    final int columnSize = columnIndex.length;
    DoubleSystemOperator[][] ans = new DoubleSystemOperator[rowSize][columnSize];

    for (int row = 0; row < rowSize; row++) {
      DoubleSystemOperator[] ansi = ans[row];
      DoubleSystemOperator[] gridi = grid[rowIndex[row]];
      for (int column = 0; column < columnSize; column++) {
        ansi[column] = gridi[columnIndex[column]]; // not generage clone
      }
    }
    return ans;
  }

  /**
   * 直達項の無いシステムの出力を求めます。
   * 
   * @param system 直達項の無いシステム
   * @return 直達項の無いシステムの出力
   * @exception SolverStopException ソルバーが停止された場合
   */
  protected abstract DoubleMatrix calcOutputOfNonDirectFeedthroughSystem(DoubleSystemOperator system) throws SolverStopException;

  /**
   * 直達項のあるシステムの出力を求めます。
   * 
   * @param system 直達項のあるシステム
   * @param u 入力
   * @return 直達項のあるシステムの出力
   * @exception SolverStopException ソルバーが停止された場合
   */
  protected abstract DoubleMatrix calcOutputOfDirectFeedthroughSystem(DoubleSystemOperator system, DoubleMatrix u) throws SolverStopException;

  /**
   * 強プロパーな線形動的システムを生成します。
   * 
   * @param system バイプロパーな線形動的システム
   * @return 強プロパーな線形動的システム
   */
  protected abstract DoubleSystemOperator createStrictlyProperLinearDynamicSystem(DoubleSystemOperator system);

  /**
   * 動的システムのリストの成分を新しいシステムに入れ替えます。
   * 
   * @param oldSystem リストに登録されている旧システム
   * @param newSystem リストに登録する新システム
   * @return 入れ替えが成功した場合true
   */
  protected abstract boolean replaceDynamicSystemList(DoubleSystemOperator oldSystem, DoubleSystemOperator newSystem);

  /**
   * 次数最小の未知のノードの値を計算します。
   * 
   * @param matrix 隣接行列を転置した行列
   * @param nodeTmpValue ノードの仮の値
   * @param unknownNode 未知のノードの番号の配列
   */
  private void calcUnknownMinimumNodeValues(final DoubleSystemOperator[][] matrix, final DoubleMatrix[] nodeTmpValue, final int[] unknownNode) {
    DoubleSystemOperator[][] newMatrix = getSubMatrix(matrix, unknownNode, unknownNode);

    DoubleMatrix[] newNodeTmpValue = new DoubleMatrix[unknownNode.length];
    for (int i = 0; i < unknownNode.length; i++) {
      newNodeTmpValue[i] = nodeTmpValue[unknownNode[i]];
    }

    if (this.cuttingNodes == null) {
      this.cuttingNodes = new DoubleMinimumUnknownEquation(new DoubleAdjacencyMatrix(newMatrix)).getCuttingNodes();
      //this.cuttingNodes = getCuttingNodes(newMatrix);
    }

    // 初期値の設定
    if (this.cuttingNodeValues == null) {
      this.cuttingNodeValues = new DoubleMatrix[this.cuttingNodes.size()];
      int k = 0;
      for (List<Integer> nodes : this.cuttingNodes) {
        int n = 0;
        for (int i : nodes) {
          n = n + newNodeTmpValue[i - 1].length();
        }

        this.cuttingNodeValues[k++] = new DoubleMatrix(n, 1);
      }
    }

    final NonLinearEquationSolver<DoubleNumber,DoubleMatrix> solver = new NewtonRaphsonFixedPointSolver<>();

    int k = 0;
    for (List<Integer> nodes : this.cuttingNodes) {
      NonLinearFunction<DoubleNumber,DoubleMatrix> ne = new DoubleNodeIdentityFunctionEquation(this, newMatrix, newNodeTmpValue, nodes);

      try {
        this.cuttingNodeValues[k] = solver.solve(ne, this.cuttingNodeValues[k]);
      } catch (SolverException e) {
        throw new RuntimeException(e);
      }

      int offset = 0;
      for (int i : nodes) {
        int n = newNodeTmpValue[i - 1].length();
        this.nodeValue[unknownNode[i - 1]] = this.cuttingNodeValues[k].getSubVector(offset + 1, offset + n);
        offset = offset + n;
      }

      k++;
    }
  }

  /**
   * 直達項の無いシステムの出力を仮のノードへ加え、そのシステムを隣接行列から削除します。
   * 
   * @param matrix 隣接行列を転置した行列
   * @param nodeTmpValue 仮のノードの値
   * @exception SolverStopException ソルバーが停止された場合
   */
  private void calcOutputOfNonDirectFeedthroughSystem(final DoubleSystemOperator[][] matrix, final DoubleMatrix[] nodeTmpValue) throws SolverStopException {
    final int size = this.nodeSize;

    for (int row = 0; row < size; row++) {
      for (int column = 0; column < size; column++) {
        DoubleSystemOperator system = matrix[row][column];
        if (system == null) throw new IllegalArgumentException("Adjacency matrix contained null system."); //$NON-NLS-1$

        if (system == DoubleZeroSystem.getInstance()) {
          continue;
        }
        if (system.hasDirectFeedthrough()) {
          continue;
        }

        DoubleMatrix output = calcOutputOfNonDirectFeedthroughSystem(system);

        nodeTmpValue[row] = nodeTmpValue[row].add(output);
        matrix[row][column] = DoubleZeroSystem.getInstance();
      }
    }
  }

  /**
   * 非直達項成分の隣接行列の転置を生成します。
   * 
   * @param matrix 隣接行列
   * @return 非直達項成分の隣接行列の転置
   */
  @SuppressWarnings("boxing")
  private DoubleSystemOperator[][] getNonDirectFeedthroughPart(final DoubleSystemOperator[][] matrix) {
    final int size = this.nodeSize;
    final DoubleSystemOperator[][] newMatrix = new DoubleSystemOperator[size][size];

    for (int row = 0; row < size; row++) {
      for (int column = 0; column < size; column++) {
        final DoubleSystemOperator system = matrix[row][column];
        if (!system.hasDirectFeedthrough()) {
          newMatrix[row][column] = system;
        } else if (system instanceof DoubleLinearSystemOperator && ((DoubleLinearSystemOperator)system).hasVariableE()==false && system instanceof DoubleDynamicSystem) {
          final DoubleSystemOperator newSystem = createStrictlyProperLinearDynamicSystem(system);
          newMatrix[row][column] = newSystem;
          newMatrix[row][column].setID(system.getID() + "N"); //$NON-NLS-1$

          final int inputNumber = this.inputMap.get(system.getID());
          this.inputMap.put(newSystem.getID(), inputNumber);

          replaceDynamicSystemList(system, newSystem);
        } else {
          newMatrix[row][column] = DoubleZeroSystem.getInstance();
        }
      }
    }

    return newMatrix;
  }

  /**
   * 確定したノードの値を用いて各直達項の有るシステムの出力を計算し、その値を仮のノードへ加えます。 <p> そして、そのシステムを隣接行列から削除します。
   * 
   * @param matrix 隣接行列を転置した行列
   * @param localNodeValue ノードの値
   * @param nodeTmpValue 仮のノードの値
   * @param skip 値が決定しているノードについて計算をスキップする場合true
   * @return 隣接行列から削除されたシステムがある場合true
   * @exception SolverStopException ソルバーが停止された場合
   */
  public boolean calcOutputOfDirectFeedthroughSystem(final DoubleSystemOperator[][] matrix, final DoubleMatrix[] localNodeValue, final DoubleMatrix[] nodeTmpValue, final boolean skip) throws SolverStopException {
    final int localNodeNumber = nodeTmpValue.length;
    boolean changed = false;

    for (int row = 0; row < localNodeNumber; row++) {
      if (skip && localNodeValue[row] != null) {
        continue;
      }
      for (int column = 0; column < localNodeNumber; column++) {
        DoubleMatrix u = localNodeValue[column];
        DoubleSystemOperator system = matrix[row][column];

        if (u == null || system == DoubleZeroSystem.getInstance()) {
          continue;
        }

        DoubleMatrix output = calcOutputOfDirectFeedthroughSystem(system, u);

        nodeTmpValue[row] = nodeTmpValue[row].add(output);
        matrix[row][column] = DoubleZeroSystem.getInstance();
        changed = true;
      }
    }

    return changed;
  }

  /**
   * ノードの値を計算します。
   * 
   * @exception SolverStopException ソルバーが停止された場合
   */
  protected void calcNodeValue() throws SolverStopException {
    DoubleSystemOperator[][] noDirectMatrix = transpose(this.nonDirectFeedthroughElements);
    DoubleSystemOperator[][] matrix = transpose(this.directFeedthroughElements);
    DoubleMatrix[] nodeTmpValue = new DoubleMatrix[this.nodeSize];

    setZeroNodeTmpValue(nodeTmpValue);
    calcOutputOfNonDirectFeedthroughSystem(noDirectMatrix, nodeTmpValue);
    setNodeValueOfNoInputNode(matrix, this.nodeValue, nodeTmpValue, true);

    do {
      while (calcOutputOfDirectFeedthroughSystem(matrix, this.nodeValue, nodeTmpValue, true)) {
        setNodeValueOfNoInputNode(matrix, this.nodeValue, nodeTmpValue, true);
      }

      boolean changed1 = contractConstantEdgeForwardRowWise(matrix, nodeTmpValue);
      if (changed1) {
        setNodeValueOfNoInputNode(matrix, this.nodeValue, nodeTmpValue, true);
      }

      boolean changed2 = resolveConstantSelfLoop(matrix, nodeTmpValue);
      if (changed2) {
        setNodeValueOfNoInputNode(matrix, this.nodeValue, nodeTmpValue, true);
      }

      if (!changed1 && !changed2) {
        break;
      }
    } while (true);

    int[] unknownNode = getUnknownNode(this.nodeValue);
    if (unknownNode.length == 0) {
      return;
    }

    calcUnknownMinimumNodeValues(matrix, nodeTmpValue, unknownNode);

    while (calcOutputOfDirectFeedthroughSystem(matrix, this.nodeValue, nodeTmpValue, true)) {
      setNodeValueOfNoInputNode(matrix, this.nodeValue, nodeTmpValue, true);
    }
  }

  /**
   * @see org.mklab.tool.control.system.SystemOperator#getLinearSystem()
   */
  @Override
  public DoubleLinearSystem getLinearSystem() {
    final DoubleAdjacencyMatrix adjMatrix = new DoubleAdjacencyMatrix(this.elements).createClone();
    adjMatrix.setInputNodes(this.inputNodes);
    adjMatrix.setOutputNodes(this.outputNodes);
    adjMatrix.setRequiringLinearSystem(true);

    return adjMatrix.getLinearSystem(false);
  }

  /**
   * @see org.mklab.tool.control.system.SystemOperator#initialize()
   */
  @Override
  public void initialize() {
    final int rowSize = this.elements.length;
    final int columnSize = rowSize == 0 ? 0 : this.elements[0].length;

    for (int row = 0; row < rowSize; row++) {
      for (int column = 0; column < columnSize; column++) {
        DoubleSystemOperator system = this.elements[row][column];
        if (system == DoubleZeroSystem.getInstance()) {
          continue;
        }
        system.initialize();
      }
    }
  }

  /**
   * 非直達項成分と直達項成分を分離し、2個の隣接行列を生成します。
   */
  public void separateDirectFeedthroughAndNonDirectFeedthrough() {
    this.directFeedthroughElements = getDirectFeedthroughPart(this.elements);
    this.nonDirectFeedthroughElements = getNonDirectFeedthroughPart(this.elements);
  }

  /**
   * 単一のシステムであるか判定します。
   * 
   * @return 単一のシステムならばtrue、そうでなければfalse
   */
  public boolean isSingleSystem() {
    if (this.nodeSize != 2) {
      return false;
    }

    if (this.elements[0][0] != DoubleZeroSystem.getInstance()) {
      return false;
    }
    if (this.elements[1][0] != DoubleZeroSystem.getInstance()) {
      return false;
    }
    if (this.elements[1][1] != DoubleZeroSystem.getInstance()) {
      return false;
    }
    if (this.elements[0][1] == DoubleZeroSystem.getInstance()) {
      return false;
    }

    return true;
  }

  /**
   * 単一のシステムであるならば、単一システムを返します。
   * 
   * @return 単一システム
   * @throws RuntimeException 単一のシステムでない場合
   */
  public DoubleSystemOperator getSingleSystem() {
    if (isSingleSystem() == false) {
      throw new RuntimeException(Messages.getString("BlockSystem.19")); //$NON-NLS-1$
    }

    return this.elements[0][1];
  }

  /**
   * ブロックシステムを拡張された隣接行列に代入します。
   * 
   * @param matrix 拡張された隣接行列
   * @param row ブロックシステムの行番号
   * @param column ブロックシステムの列番号
   * @param blockSystem ブロックシステムの成分
   * @return Sourceが接続されているノードのリスト、Sinkが接続されているノードのリスト
   */
  public static List<List<Integer>> setBlockMatrix(final DoubleSystemOperator[][] matrix, final int row, final int column, final DoubleBlockSystem blockSystem) {
    final DoubleSystemOperator[][] blockMatrix = blockSystem.elements;
    final int blockSize = blockMatrix.length - 1; // 最初のinputNodeSize列と最終のoutputNodeSize行は全て零成分

    final int inputNodeSize = blockSystem.getInputNodeSize();
    final int outputNodeSize = blockSystem.getOutputNodeSize();

    final int inputOutputNodeSize = Math.min(inputNodeSize, outputNodeSize);

    final int firstColumn = column - inputOutputNodeSize + 1;
    //final int firstColumn = column - outputNodeSize + 1;

    final List<Integer> sourceNodes = new ArrayList<>();
    final List<Integer> sinkNodes = new ArrayList<>();

    if (row <= column) {
      // input row
      for (int i = 0; i < inputNodeSize; i++) {
        for (int j = 0; j < blockSize; j++) {
          final DoubleSystemOperator system = blockMatrix[i][j + 1];
          final int input = row + i;
          final int output = firstColumn + j;
          matrix[input][output] = system;
          setupSourceSink(system, input + 1, output + 1, sourceNodes, sinkNodes);
        }
      }

      for (int i = inputNodeSize; i < blockSize; i++) {
        for (int j = 0; j < blockSize; j++) {
          final DoubleSystemOperator system = blockMatrix[i][j + 1];
          final int input = firstColumn - 1 + i;
          final int output = firstColumn + j;
          matrix[input][output] = system;
          setupSourceSink(system, input + 1, output + 1, sourceNodes, sinkNodes);
        }
      }
    } else {
      // input row
      for (int i = 0; i < inputNodeSize; i++) {
        for (int j = 0; j < blockSize; j++) {
          final DoubleSystemOperator system = blockMatrix[i][j + 1];
          final int input = row + i + blockSize - outputNodeSize;
          final int output = firstColumn + j;
          matrix[input][output] = system;
          setupSourceSink(system, input + 1, output + 1, sourceNodes, sinkNodes);
        }
      }

      for (int i = inputNodeSize; i < blockSize; i++) {
        for (int j = 0; j < blockSize; j++) {
          final DoubleSystemOperator system = blockMatrix[i][j + 1];
          final int input = firstColumn - 1 + i;
          final int output = firstColumn + j;
          matrix[input][output] = system;
          setupSourceSink(system, input + 1, output + 1, sourceNodes, sinkNodes);
        }
      }
    }

    final List<List<Integer>> sourceNodesSinkNodes = new ArrayList<>();
    sourceNodesSinkNodes.add(sourceNodes);
    sourceNodesSinkNodes.add(sinkNodes);

    return sourceNodesSinkNodes;
  }

  /**
   * システムがSourceまたはSinkならば、接続されているノードをリストに追加します。
   * 
   * @param system システム
   * @param inputNode 入力ノード
   * @param outputNode 出力ノード
   * @param sourceNodes Sourceが接続されているノードのリスト
   * @param sinkNodes Sinkが接続されているノードのリスト
   */
  @SuppressWarnings("boxing")
  private static void setupSourceSink(final DoubleSystemOperator system, final int inputNode, final int outputNode, final List<Integer> sourceNodes, final List<Integer> sinkNodes) {
    if (system == null || system == DoubleZeroSystem.getInstance()) {
      return;
    }
    if (system instanceof DoubleContinuousSource && system instanceof DoubleInputPort == false) {
      sourceNodes.add(inputNode);
    }
    if (system instanceof DoubleContinuousSink && system instanceof DoubleOutputPort == false) {
      sinkNodes.add(outputNode);
    }
  }

  /**
   * 指定されたノード間にあるシステムを返します。
   * 
   * @param inputNode 入力ノード番号
   * @param outputNode 出力ノード番号
   * @return 指定されたノード間にあるシステム
   */
  protected DoubleSystemOperator getSystemOperator(final int inputNode, final int outputNode) {
    return this.elements[inputNode][outputNode];
  }

  /**
   * @see org.mklab.tool.control.system.SystemOperator#isAutoSize()
   */
  @Override
  public boolean isAutoSize() {
    if (this.isSingleSystem()) {
      return getSingleSystem().isAutoSize();
    }

    return super.isAutoSize();
  }

  /**
   * @see org.mklab.tool.control.system.SystemOperator#setAutoSize(boolean)
   */
  @Override
  public void setAutoSize(final boolean autoSize) {
    if (this.isSingleSystem()) {
      this.getSingleSystem().setAutoSize(autoSize);
    }

    super.setAutoSize(autoSize);
  }

  /**
   * @see org.mklab.tool.control.system.SystemOperator#resetAutoSize()
   */
  @Override
  public void resetAutoSize() {
    if (this.isSingleSystem() == false) {
      return;
    }

    super.setInputSize(-1);
    super.setOutputSize(-1);
    this.getSingleSystem().resetAutoSize();
  }

  /**
   * @see org.mklab.tool.control.system.SystemOperator#setInputSize(int)
   */
  @Override
  public void setInputSize(final int inputSize) {
    if (this.isSingleSystem()) {
      final int singleInputSize = this.getSingleSystem().getInputSize();
      if (singleInputSize != -1) {
        super.setInputSize(singleInputSize);
      }
    } else {
      super.setInputSize(inputSize);
    }
  }

  /**
   * @see org.mklab.tool.control.system.SystemOperator#setOutputSize(int)
   */
  @Override
  public void setOutputSize(final int outputSize) {
    if (this.isSingleSystem()) {
      final int singoleOutputSize = this.getSingleSystem().getOutputSize();
      if (singoleOutputSize != -1) {
        super.setOutputSize(singoleOutputSize);
      }
    } else {
      super.setOutputSize(outputSize);
    }
  }
}