AdjacencyConstantMatrix.java

/*
 * Created on 2007/12/08
 * Copyright (C) 2007 Koga Laboratory. All rights reserved.
 *
 */
package org.mklab.tool.control.system;

import java.io.Writer;
import java.util.ArrayList;

import org.mklab.nfc.matrix.ComplexNumericalMatrix;
import org.mklab.nfc.matrix.MatrixSizeException;
import org.mklab.nfc.matrix.RealNumericalMatrix;
import org.mklab.nfc.rpn.ExpressionProcessor;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.RealNumericalScalar;
import org.mklab.tool.control.system.continuous.ContinuousLinearDynamicSystem;
import org.mklab.tool.control.system.continuous.IntegratorSystem;
import org.mklab.tool.control.system.math.ConstantSystem;
import org.mklab.tool.control.system.math.NegativeUnitSystem;
import org.mklab.tool.control.system.math.UnitSystem;


/**
 * 隣接定数行列を表すクラスです。
 * 
 * @author Anan
 * @version $Revision: 1.8 $, 2007/12/08
 * @param <RS> type of real scalar
 * @param <RM> type of real matrix
 * @param <CS> type of complex scalar
 * @param <CM> type of complex matrix
 */
public class AdjacencyConstantMatrix<RS extends RealNumericalScalar<RS, RM, CS, CM>, RM extends RealNumericalMatrix<RS, RM, CS, CM>, CS extends ComplexNumericalScalar<RS, RM, CS, CM>, CM extends ComplexNumericalMatrix<RS, RM, CS, CM>>
    extends AdjacencyMatrix<RS, RM, CS, CM> {

  /** serialVersionUID */
  private static final long serialVersionUID = 6753883274746323945L;

  /**
   * 1行1列の隣接定数行列を生成します。
   * 
   * @param sunit unit of scalar
   */
  AdjacencyConstantMatrix(RS sunit) {
    this(0, 0, sunit);
  }

  /**
   * rowSizeとcolumnSizeを持つ隣接定数行列を生成します。
   * 
   * @param rowSize 行の数
   * @param columnSize 列の数
   * @param sunit unit of scalar
   */
  AdjacencyConstantMatrix(final int rowSize, final int columnSize, RS sunit) {
    this(new SystemOperator[rowSize][columnSize], sunit);
  }

  /**
   * 引数を成分とする隣接定数行列を生成します。
   * 
   * @param elements 成分をもつ配列
   * @param sunit unit of scalar
   */
  AdjacencyConstantMatrix(final SystemOperator<RS, RM, CS, CM>[][] elements, RS sunit) {
    super(elements, sunit);
    setZeroSystemToNullElement(elements);
  }

  /**
   * 引数の隣接定数行列に対応する単位行列をもつ隣接定数行列を作成します。
   * 
   * @param matrix 隣接定数行列
   * @param isNegative trueならば負の単位行列，falseならば正の単位行列
   * @param sunit unit of scalar
   */
  private AdjacencyConstantMatrix(final AdjacencyConstantMatrix<RS, RM, CS, CM> matrix, final boolean isNegative, RS sunit) {
    this(matrix.getRowSize(), matrix.getRowSize(), sunit);
    if (isNegative == true) {
      for (int i = 0; i < this.getMatrixSize(); i++) {
        for (int j = 0; j < matrix.getColumnSize(); j++) {
          if (matrix.getElement(i + 1, j + 1) == ZeroSystem.getInstance(this.sunit)) {
            continue;
          }
          this.elements[i][i] = new NegativeUnitSystem<>((matrix.getElement(i + 1, j + 1)).getInputSize(), sunit);
          break;
        }
      }
    } else {
      for (int i = 0; i < this.getMatrixSize(); i++) {
        for (int j = 0; j < matrix.getColumnSize(); j++) {
          if (matrix.getElement(i + 1, j + 1) == ZeroSystem.getInstance(this.sunit)) {
            continue;
          }
          this.elements[i][i] = new UnitSystem<>((matrix.getElement(i + 1, j + 1)).getInputSize(), sunit);
          break;
        }
      }
    }
  }

  /**
   * 各成分の符号を反転したシステムを成分とする隣接定数行列を返します。
   * 
   * @return 各成分の符号を反転したシステムを成分とする隣接定数行列
   */
  AdjacencyConstantMatrix<RS, RM, CS, CM> unaryMinus() {
    AdjacencyConstantMatrix<RS, RM, CS, CM> ans = new AdjacencyConstantMatrix<>(getRowSize(), getColumnSize(), this.sunit);

    for (int i = 0; i < getRowSize(); i++) {
      for (int j = 0; j < getColumnSize(); j++) {
        if (getElement(i + 1, j + 1) == ZeroSystem.getInstance(this.sunit)) {
          continue;
        }
        ans.elements[i][j] = ((ConstantSystem<RS, RM, CS, CM>)getElement(i + 1, j + 1)).unaryMinus();
      }
    }

    return ans;
  }

  /**
   * matrixSizeをもつ正方隣接定数行列を作成します。
   * 
   * @param matrixSize 行列の次数
   * @param sunit unit of scalar
   */
  AdjacencyConstantMatrix(int matrixSize, RS sunit) {
    this(matrixSize, matrixSize, sunit);
  }

  /**
   * 隣接定数行列同士を足し合わせます。
   * 
   * @param addend 加える行列
   * @return 足し合わされた結果
   */
  AdjacencyConstantMatrix<RS, RM, CS, CM> add(final AdjacencyConstantMatrix<RS, RM, CS, CM> addend) {
    if ((this.getRowSize() != addend.getRowSize()) || (this.getColumnSize() != addend.getColumnSize())) {
      throw new MatrixSizeException(this, addend, Messages.getString("AdjacencyConstantMatrix.0")); //$NON-NLS-1$
    }

    final SystemOperator<RS, RM, CS, CM>[][] answerMatrix = new SystemOperator[this.getRowSize()][this.getColumnSize()];

    for (int row = 1; row <= this.getRowSize(); row++) {
      for (int column = 1; column <= this.getColumnSize(); column++) {
        if (this.getElement(row, column) == ZeroSystem.getInstance(this.sunit) && addend.getElement(row, column) == ZeroSystem.getInstance(this.sunit)) {
          answerMatrix[row - 1][column - 1] = ZeroSystem.getInstance(this.sunit);
        } else if (addend.getElement(row, column) == ZeroSystem.getInstance(this.sunit)) {
          answerMatrix[row - 1][column - 1] = this.getElement(row, column);
        } else if (this.getElement(row, column) == ZeroSystem.getInstance(this.sunit)) {
          answerMatrix[row - 1][column - 1] = addend.getElement(row, column);
        } else {
          answerMatrix[row - 1][column - 1] = ((ConstantSystem<RS, RM, CS, CM>)this.getElement(row, column)).add((ConstantSystem<RS, RM, CS, CM>)addend.getElement(row, column));
        }
      }
    }

    return new AdjacencyConstantMatrix<>(answerMatrix, this.sunit);
  }

  /**
   * 隣接定数行列同士を掛け合わせます。
   * 
   * @param multiplier 掛ける行列
   * @return 掛け合わされた結果
   */
  AdjacencyConstantMatrix<RS, RM, CS, CM> multiply(final AdjacencyConstantMatrix<RS, RM, CS, CM> multiplier) {
    if (this.getColumnSize() != multiplier.getRowSize()) {
      throw new MatrixSizeException(this, multiplier, Messages.getString("AdjacencyConstantMatrix.1")); //$NON-NLS-1$
    }
    final SystemOperator<RS, RM, CS, CM>[][] answerMatrix = new SystemOperator[this.getRowSize()][multiplier.getColumnSize()];

    for (int row = 1; row <= this.getRowSize(); row++) {
      for (int column = 1; column <= multiplier.getColumnSize(); column++) {
        for (int k = 1; k <= this.getColumnSize(); k++) {
          if (this.getElement(row, k) == ZeroSystem.getInstance(this.sunit) && multiplier.getElement(k, column) == ZeroSystem.getInstance(this.sunit)) {
            continue;
          } else if (multiplier.getElement(k, column) == ZeroSystem.getInstance(this.sunit)) {
            continue;
          } else if (this.getElement(row, k) == ZeroSystem.getInstance(this.sunit)) {
            continue;
          } else {
            if (answerMatrix[row - 1][column - 1] == null) {
              answerMatrix[row - 1][column - 1] = ((ConstantSystem<RS, RM, CS, CM>)this.getElement(row, k)).multiply((ConstantSystem<RS, RM, CS, CM>)multiplier.getElement(k, column));
            } else {
              answerMatrix[row - 1][column - 1] = ((ConstantSystem<RS, RM, CS, CM>)answerMatrix[row - 1][column - 1])
                  .add(((ConstantSystem<RS, RM, CS, CM>)this.getElement(row, k)).multiply((ConstantSystem<RS, RM, CS, CM>)multiplier.getElement(k, column)));
            }
          }
        }
      }
    }
    return new AdjacencyConstantMatrix<>(answerMatrix, this.sunit);
  }

  /**
   * 逆行列を計算します。
   * 
   * @return 逆行列の結果
   */
  AdjacencyConstantMatrix<RS, RM, CS, CM> inverse() {
    if (this.getColumnSize() == 1 && this.getColumnSize() == 1) {
      final SystemOperator<RS, RM, CS, CM>[][] inverse = new SystemOperator[1][1];
      if (this.elements[0][0].getLinearSystem().getD().isZero()) {
        throw new RuntimeException(Messages.getString("AdjacencyConstantMatrix.2")); //$NON-NLS-1$
      }
      inverse[0][0] = ((ConstantSystem<RS, RM, CS, CM>)this.elements[0][0]).inverse();
      return new AdjacencyConstantMatrix<>(inverse, this.sunit);
    }

    int i;
    for (i = 0; i < this.getRowSize(); i++) {
      if (this.elements[i][0] == ZeroSystem.getInstance(this.sunit) || ((ConstantSystem<RS, RM, CS, CM>)this.elements[i][0]).getGain().isFullRank() == false) {
        continue;
      }
      this.exchangeRow(1, i + 1);
      break;
    }

    if (((ConstantSystem<RS, RM, CS, CM>)this.getElement(1, 1)).getGain().isFullRank()) {
      final AdjacencyConstantMatrix<RS, RM, CS, CM> A = getSubMatrix(1, 1, 1, 1).createAdjacencyConstantMatrix();
      final AdjacencyConstantMatrix<RS, RM, CS, CM> B = (getSubMatrix(1, 1, 2, this.getColumnSize())).createAdjacencyConstantMatrix();
      final AdjacencyConstantMatrix<RS, RM, CS, CM> C = (getSubMatrix(2, this.getRowSize(), 1, 1)).createAdjacencyConstantMatrix();
      final AdjacencyConstantMatrix<RS, RM, CS, CM> D = (getSubMatrix(2, this.getRowSize(), 2, this.getColumnSize())).createAdjacencyConstantMatrix();
      final AdjacencyConstantMatrix<RS, RM, CS, CM> ans = createMatrixInversionLemmaWhenAIsRegular(A, B, C, D);
      ans.exchangeColumn(1, i + 1);
      return ans;
    }

    throw new RuntimeException(Messages.getString("AdjacencyConstantMatrix.2")); //$NON-NLS-1$
  }

  /**
   * 逆行列レンマを用いて逆行列を求めます。
   * 
   * @param a A
   * @param b B
   * @param c C
   * @param d D
   * @return ConstantSystem[][] MatrixInversionLemma
   */
  private AdjacencyConstantMatrix<RS, RM, CS, CM> createMatrixInversionLemmaWhenAIsRegular(AdjacencyConstantMatrix<RS, RM, CS, CM> a, AdjacencyConstantMatrix<RS, RM, CS, CM> b,
      AdjacencyConstantMatrix<RS, RM, CS, CM> c, AdjacencyConstantMatrix<RS, RM, CS, CM> d) {

    final AdjacencyConstantMatrix<RS, RM, CS, CM> aInverse = a.inverse();
    final AdjacencyConstantMatrix<RS, RM, CS, CM> aDet = d.add(c.unaryMinus().multiply(aInverse).multiply(b));
    final AdjacencyConstantMatrix<RS, RM, CS, CM> aDetInverse = aDet.inverse();
    final AdjacencyConstantMatrix<RS, RM, CS, CM> A = aInverse.add(aInverse.multiply(b).multiply(aDetInverse).multiply(c).multiply(aInverse));
    final AdjacencyConstantMatrix<RS, RM, CS, CM> B = aInverse.unaryMinus().multiply(b).multiply(aDetInverse);
    final AdjacencyConstantMatrix<RS, RM, CS, CM> C = aDetInverse.unaryMinus().multiply(c).multiply(aInverse);
    final AdjacencyConstantMatrix<RS, RM, CS, CM> D = aDetInverse;

    return combineABCDBlock(A, B, C, D);
  }

  /**
   * @see org.mklab.nfc.matrix.Grid#printElements(java.io.Writer)
   */
  @Override
  public void printElements(Writer writer) {
    throw new UnsupportedOperationException();
  }

  /**
   * {@inheritDoc}
   */
  @Override
  public void printElements(Writer writer, int maxColumnSize) {
    throw new UnsupportedOperationException();
  }

  //  /**
  //   * {@inheritDoc}
  //   */
  //  @Override
  //  public SystemOperator<RS,RM,CS,CM>[][] getElements() {
  //    return this.elements;
  //  }

  /**
   * thisの隣接定数行列に対応する単位行列表す隣接定数行列を作り、返します。
   * 
   * @param isNegative trueならば負の単位行列，falseならば正の単位行列
   * @return AdjacencyConstantMatrix<RS,RM,CS,CM> thisの隣接定数行列に対応する単位行列表す隣接定数行列
   */
  AdjacencyConstantMatrix<RS, RM, CS, CM> createUnitAdjacencyConstantMatrix(boolean isNegative) {
    return new AdjacencyConstantMatrix<>(this, isNegative, this.sunit);
  }

  /**
   * 隣接定数行列に対応する単位行列を表す隣接定数行列を返します．
   * 
   * @return AdjacencyConstantMatrix<RS,RM,CS,CM> 単位行列表す隣接定数行列
   */
  AdjacencyConstantMatrix<RS, RM, CS, CM> createUnit() {
    AdjacencyConstantMatrix<RS, RM, CS, CM> unit = new AdjacencyConstantMatrix<>(this.getRowSize(), this.getRowSize(), this.sunit);

    for (int i = 0; i < this.getMatrixSize(); i++) {
      final SystemOperator<RS, RM, CS, CM> element = this.getElement(i + 1, i + 1);

      if (element == ZeroSystem.getInstance(this.sunit)) {
        unit.elements[i][i] = new UnitSystem<>(1, this.sunit);
      } else {
        unit.elements[i][i] = new UnitSystem<>(element.getInputSize(), this.sunit);
      }
    }

    return unit;
  }

  /**
   * 隣接定数行列のサイズを返します。
   * 
   * @return int 隣接定数行列のサイズ
   */
  private int getMatrixSize() {
    return this.elements.length;
  }

  /**
   * 隣接定数行列同士で減算をします。
   * 
   * @param subtrahend 引く行列
   * @return AdjacencyConstantMatrix<RS,RM,CS,CM> 減算を行った隣接定数行列
   */
  AdjacencyConstantMatrix<RS, RM, CS, CM> subtract(final AdjacencyConstantMatrix<RS, RM, CS, CM> subtrahend) {
    return this.add(subtrahend.unaryMinus());
  }

  /**
   * 隣接定数行列を単位行列から減算します。
   * 
   * @return AdjacencyConstantMatrix<RS,RM,CS,CM> 単位行列から減算を行った隣接定数行列
   */
  AdjacencyConstantMatrix<RS, RM, CS, CM> subtractFromUnit() {
    return this.createUnit().subtract(this);
  }

  /**
   * A,B,C,Dのそれぞれの隣接定数行列を結合した隣接定数行列を返します。
   * 
   * @param a A
   * @param b B
   * @param c C
   * @param d D
   * @return AdjacencyConstantMatrix<RS,RM,CS,CM> 連結された隣接定数行列
   */
  AdjacencyConstantMatrix<RS, RM, CS, CM> combineABCDBlock(AdjacencyConstantMatrix<RS, RM, CS, CM> a, AdjacencyConstantMatrix<RS, RM, CS, CM> b, AdjacencyConstantMatrix<RS, RM, CS, CM> c,
      AdjacencyConstantMatrix<RS, RM, CS, CM> d) {
    final AdjacencyConstantMatrix<RS, RM, CS, CM> upperMatrix = a.appendRight(b).createAdjacencyConstantMatrix();
    final AdjacencyConstantMatrix<RS, RM, CS, CM> lowerMatrix = c.appendRight(d).createAdjacencyConstantMatrix();
    return upperMatrix.appendDown(lowerMatrix).createAdjacencyConstantMatrix();
  }

  /**
   * 代数ループのサブマトリックスを取り出します。
   * 
   * @param loopList 代数ループが存在する行番号
   * @return 代数ループのサブマトリックス
   */
  @SuppressWarnings("boxing")
  AdjacencyConstantMatrix<RS, RM, CS, CM> getLoopMatrix(final ArrayList<Integer> loopList) {
    return (getSubMatrix(loopList.get(0) + 1, loopList.get(0) + 1 + loopList.size() - 1, loopList.get(0) + 1, loopList.get(0) + loopList.size() + 1 - 1)).createAdjacencyConstantMatrix();
  }

  /**
   * 隣接定数行列からA行列(システム行列)を生成します．
   * 
   * @param stateSizes サブシステムの状態の数の配列
   * @param inputNodeSize 全システムの状態の数
   * @return A行列(システム行列)
   */
  AdjacencyConstantMatrix<RS, RM, CS, CM> createA(final int[] stateSizes, final int inputNodeSize) {
    final int numberOfSubsystem = stateSizes.length;
    final int rowMin = inputNodeSize + 1;
    final int rowMax = inputNodeSize + numberOfSubsystem;
    final int columnMin = inputNodeSize + numberOfSubsystem + 1;
    final int columnMax = inputNodeSize + 2 * numberOfSubsystem;
    return getSubMatrix(rowMin, rowMax, columnMin, columnMax).transpose().createAdjacencyConstantMatrix();
  }

  /**
   * 隣接定数行列からB行列(入力行列)を生成します．
   * 
   * @param stateSizes サブシステムの状態の数の配列
   * @param inputNodeSize 全システムの状態の数
   * @return B行列(入力行列)
   */
  AdjacencyConstantMatrix<RS, RM, CS, CM> createB(int[] stateSizes, int inputNodeSize) {
    final int numberOfSubsystem = stateSizes.length;
    final int rowMin = 1;
    final int rowMax = inputNodeSize;
    final int columnMin = inputNodeSize + numberOfSubsystem + 1;
    final int columnMax = inputNodeSize + 2 * numberOfSubsystem;
    return getSubMatrix(rowMin, rowMax, columnMin, columnMax).transpose().createAdjacencyConstantMatrix();
  }

  /**
   * 隣接定数行列からC行列(出力行列)を生成します．
   * 
   * @param stateSizes サブシステムの状態の数の配列
   * @param inputNodeSize 全システムの入力ノードの数
   * @param outputNodeSize 全システムの出力ノードの数
   * @return C行列(出力行列)
   */
  AdjacencyConstantMatrix<RS, RM, CS, CM> createC(int[] stateSizes, int inputNodeSize, int outputNodeSize) {
    final int numberOfSubsystem = stateSizes.length;
    final int rowMin = inputNodeSize + 1;
    final int rowMax = inputNodeSize + numberOfSubsystem;
    final int columnMin = inputNodeSize + 2 * numberOfSubsystem + 1;
    final int columnMax = inputNodeSize + 2 * numberOfSubsystem + outputNodeSize;
    return getSubMatrix(rowMin, rowMax, columnMin, columnMax).transpose().createAdjacencyConstantMatrix();
  }

  /**
   * 隣接定数行列からD行列(直達行列)を生成します．
   * 
   * @param inputNodeSize 全システムの入力ノードの数
   * @param outputNodeSize 全システムの出力ノードの数
   * @return D行列(直達行列)
   */
  AdjacencyConstantMatrix<RS, RM, CS, CM> createD(int inputNodeSize, int outputNodeSize) {
    final int rowMin = 1;
    final int rowMax = inputNodeSize;
    final int columnMax = this.getColumnSize();
    final int columnMin = this.getColumnSize() - outputNodeSize + 1;
    return getSubMatrix(rowMin, rowMax, columnMin, columnMax).transpose().createAdjacencyConstantMatrix();
  }

  /**
   * 定数行列からE行列(ディスクリプタ行列)を生成します．
   * 
   * @param stateSizes サブシステムの状態の数の配列
   * @param inputNodeSize 全システムの入力ノードの数
   * @return E行列(ディスクリプタ行列)
   */
  AdjacencyConstantMatrix<RS, RM, CS, CM> createE(int[] stateSizes, int inputNodeSize) {
    final int numberOfSubsystem = stateSizes.length;
    final int rowMin = inputNodeSize + numberOfSubsystem + 1;
    final int rowMax = inputNodeSize + 2 * numberOfSubsystem;
    final int columnMin = inputNodeSize + numberOfSubsystem + 1;
    final int columnMax = inputNodeSize + 2 * numberOfSubsystem;
    final AdjacencyConstantMatrix<RS, RM, CS, CM> E = getSubMatrix(rowMin, rowMax, columnMin, columnMax).transpose().createAdjacencyConstantMatrix();
    final AdjacencyConstantMatrix<RS, RM, CS, CM> I = E.createUnit();
    return I.subtract(E);
  }

  /**
   * 成分を文字列で返します。
   * 
   * @return 成分の文字列
   */
  String getAsString() {
    final SystemOperator<RS, RM, CS, CM>[][] matrix = getElements();
    final StringBuffer string = new StringBuffer();
    final String lineSeparator = System.getProperty("line.separator"); //$NON-NLS-1$

    for (int row = 0; row < matrix.length; row++) {
      for (int column = 0; column < matrix[0].length; column++) {
        final SystemOperator<RS, RM, CS, CM> system = matrix[row][column];
        if (system instanceof ZeroSystem) {
          string.append("0   "); //$NON-NLS-1$
        } else if (system instanceof ConstantSystem && ((ConstantSystem<RS, RM, CS, CM>)system).isZeroOperand()) {
          string.append(new ExpressionProcessor<RS, RM>().getResult(((ConstantSystem<RS, RM, CS, CM>)system)) + "   "); //$NON-NLS-1$
        } else if (system instanceof ConstantSystem) {
          string.append(new ExpressionProcessor<RS, RM>().getResult(((ConstantSystem<RS, RM, CS, CM>)system)) + "   "); //$NON-NLS-1$
        }
      }
      string.append(lineSeparator);
    }

    return string.toString();
  }

  /**
   * 中身を文字列で返します。
   * 
   * @return String 中身の文字列
   */
  @SuppressWarnings("nls")
  String getElementsAsString() {
    final SystemOperator<RS, RM, CS, CM>[][] matrix = getElements();
    final StringBuffer string = new StringBuffer();
    final String lineSeparator = System.getProperty("line.separator");

    for (int row = 0; row < matrix.length; row++) {
      for (int column = 0; column < matrix.length; column++) {
        SystemOperator<RS, RM, CS, CM> system = matrix[row][column];
        if (system instanceof ZeroSystem) {
          string.append("0 ");
        } else if (system instanceof UnitSystem) {
          string.append("P ");
        } else if (system instanceof NegativeUnitSystem) {
          string.append("N ");
        } else if (system instanceof IntegratorSystem) {
          string.append("q ");
        } else if (system instanceof ContinuousLinearDynamicSystem) {
          string.append("L ");
        } else if (system instanceof ConstantSystem) {
          string.append(new ExpressionProcessor<RS, RM>().getResult(((ConstantSystem<RS, RM, CS, CM>)system)) + " ");
        }
      }
      string.append(lineSeparator);
    }

    return string.toString();
  }

  /**
   * 隣接行列の成分を文字列で返します。
   * 
   * @return 隣接行列の成分の文字列
   */
  public String getAdjacencyConstantMatrixAsString() {
    return getAsString();
  }
}