BlockPiecewiseSampledDataDynamicSystem.java

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

import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;

import org.mklab.nfc.matrix.ComplexNumericalMatrix;
import org.mklab.nfc.matrix.RealNumericalMatrix;
import org.mklab.nfc.ode.PiecewiseContinuousAlgebraicSystem;
import org.mklab.nfc.ode.PiecewiseContinuousDiscreteAlgebraicSystem;
import org.mklab.nfc.ode.PiecewiseDifferentialDifferenceSystem;
import org.mklab.nfc.ode.PiecewiseDifferentialSystem;
import org.mklab.nfc.ode.SolverStopException;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.RealNumericalScalar;
import org.mklab.tool.control.system.SystemOperator;
import org.mklab.tool.control.system.continuous.ContinuousExplicitDynamicSystem;


/**
 * ブロック区分的連続サンプル値動的システムを表わすクラスです。
 * 
 * @author koga
 * @version $Revision: 1.10 $, 2007/04/11
 * @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 BlockPiecewiseSampledDataDynamicSystem<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 BlockSampledDataDynamicSystem<RS, RM, CS, CM> implements PiecewiseDifferentialDifferenceSystem<RS, RM, CS, CM> {

  /** 区分的微分可能差分システムのリスト */
  private List<PiecewiseDifferentialDifferenceSystem<RS, RM, CS, CM>> piecewiseDifferentialDifferenceSystems = new ArrayList<>();

  /** 区分的微分可能システムのリスト */
  private List<PiecewiseDifferentialSystem<RS, RM, CS, CM>> piecewiseDifferentialSystems = new ArrayList<>();

  /** 区分的連続離散代数システムのリスト */
  private List<PiecewiseContinuousDiscreteAlgebraicSystem<RS, RM, CS, CM>> piecewiseContinuousDiscreteAlgebraicSystems = new ArrayList<>();

  /** 区分的連続代数システムのリスト */
  private List<PiecewiseContinuousAlgebraicSystem<RS, RM, CS, CM>> piecewiseContinuousAlgebraicSystems = new ArrayList<>();

  /**
   * 新しく生成された<code>BlockPiecewiseSampledDataDynamicSystem</code>オブジェクトを初期化します。
   * 
   * @param elements 隣接行列
   * @param inputNodes 入力ノード番号のリスト(番号は1から始まる)
   * @param outputNodes 出力ノード番号のリスト(番号は1から始まる)
   * @param sunit unit of scalar
   */
  public BlockPiecewiseSampledDataDynamicSystem(SystemOperator<RS, RM, CS, CM>[][] elements, List<Integer> inputNodes, List<Integer> outputNodes, RS sunit) {
    super(elements, inputNodes, outputNodes, sunit);
    setupPiecewiseDifferentialDifferenceSystems();
    setupPiecewiseDifferentialSystems();
    setupPiecewiseContinuousDiscreteAlgebraicSystems();
    setupPiecewiseContinuousAlgebraicSystems();
  }

  /**
   * {@inheritDoc}
   */
  public RS getDiscontinuousPoint(RS t1, RM continuousState1, RM discreteState1, RM u1, RS t2, RM continuousState2, RM discreteState2, RM u2) throws SolverStopException {
    final RM originalContinuousState = getContinuousState();
    final RM originalDiscreteState = getDiscreteState();

    outputEquation(t1, continuousState1, discreteState1);

    final List<RM> differentialDifferenceSystemInputs1 = getDifferentialDifferenceSystemInputs();
    final List<RM> differenceContinuousStates1 = getDifferentialDifferneceSystemContinuousStates();
    final List<RM> differenceDiscreteStates1 = getDifferentialDifferneceSystemDiscreteStates();
    final List<RM> differentialSystemInputs1 = getDifferentialSystemInputs();
    final List<RM> differentialContinuousStates1 = getDifferentialSystemContinuousStates();
    final List<RM> continuousDiscreteSystemInputs1 = getContinuousDiscreteAlgebraicSystemInputs();
    final List<RM> continuousSystemInputs1 = getContinuousAlgebraicSystemInputs();

    outputEquation(t2, continuousState2, discreteState2);

    final List<RM> differentialDifferenceSystemInputs2 = getDifferentialDifferenceSystemInputs();
    final List<RM> differenceContinuousStates2 = getDifferentialDifferneceSystemContinuousStates();
    final List<RM> differenceDiscreteStates2 = getDifferentialDifferneceSystemDiscreteStates();
    final List<RM> differentialSystemInputs2 = getDifferentialSystemInputs();
    final List<RM> differentialContinuousStates2 = getDifferentialSystemContinuousStates();
    final List<RM> continuousDiscreteSystemInputs2 = getContinuousDiscreteAlgebraicSystemInputs();
    final List<RM> continuousSystemInputs2 = getContinuousAlgebraicSystemInputs();

    final List<RS> differentialDifferenceSystemDiscontinuousPoints = getDifferentialDifferenceDiscontinuousPoints(t1, differenceContinuousStates1, differenceDiscreteStates1,
        differentialDifferenceSystemInputs1, t2, differenceContinuousStates2, differenceDiscreteStates2, differentialDifferenceSystemInputs2);
    final List<RS> differentialSystemDiscontinuousPoints = getDifferentialDiscontinuousPoints(t1, differentialContinuousStates1, differentialSystemInputs1, t2, differentialContinuousStates2,
        differentialSystemInputs2);
    final List<RS> continuousDiscreteSystemDiscontinuousPoints = getContinuousDiscreteDiscontinuousPoints(t1, continuousDiscreteSystemInputs1, t2, continuousDiscreteSystemInputs2);
    final List<RS> continuousSystemDiscontinuousPoints = getContinuousDiscontinuousPoints(t1, continuousSystemInputs1, t2, continuousSystemInputs2);

    outputEquation(t1, originalContinuousState, originalDiscreteState);

    final List<RS> discontinuousPoints = differentialDifferenceSystemDiscontinuousPoints;
    discontinuousPoints.addAll(differentialSystemDiscontinuousPoints);
    discontinuousPoints.addAll(continuousDiscreteSystemDiscontinuousPoints);
    discontinuousPoints.addAll(continuousSystemDiscontinuousPoints);

    final RS[] sortedPoints = discontinuousPoints.toArray(this.sunit.createArray(discontinuousPoints.size()));
    Arrays.sort(sortedPoints);
    return sortedPoints[0];
  }

  /**
   * {@inheritDoc}
   */
  public List<Integer> getPiece(RS t, RM continuousState, RM discreteState, RM u) throws SolverStopException {
    final RM originalContinuousState = getContinuousState();
    final RM originalDiscreteState = getDiscreteState();

    setContinuousState(continuousState);
    setDiscreteState(discreteState);
    calcNodeValue(t);

    final List<RM> differentialDiffernceContinuousStates = getDifferentialDifferneceSystemContinuousStates();
    final List<RM> differentialContinuousStates = getDifferentialSystemContinuousStates();
    final List<RM> discreteStates = getDifferentialDifferneceSystemDiscreteStates();

    final List<Integer> differentialDifferencePieces = getDifferentialDifferencePieces(t, differentialDiffernceContinuousStates, discreteStates);
    final List<Integer> differentialPieces = getDifferentialPieces(t, differentialContinuousStates);
    final List<Integer> continuousDiscretePieces = getContinuousDiscretePieces(t);
    final List<Integer> continuousPieces = getContinuousPieces(t);

    setContinuousState(originalContinuousState);
    setDiscreteState(originalDiscreteState);
    calcNodeValue(t);

    final List<Integer> pieces = differentialDifferencePieces;
    pieces.addAll(differentialPieces);
    pieces.addAll(continuousDiscretePieces);
    pieces.addAll(continuousPieces);

    return pieces;
  }

  /**
   * 区分的連続離散代数システムの区分の番号のリストを返します。
   * 
   * @param t 時刻
   * @return 区分的連続離散代数システムの区分の番号のリスト
   * @exception SolverStopException ソルバーが停止された場合
   */
  private List<Integer> getContinuousDiscretePieces(RS t) throws SolverStopException {
    final List<Integer> pieces = new ArrayList<>();
    for (PiecewiseContinuousDiscreteAlgebraicSystem<RS, RM, CS, CM> system : this.piecewiseContinuousDiscreteAlgebraicSystems) {
      pieces.addAll(system.getPiece(t, getInputNodeValueOf((SystemOperator<RS, RM, CS, CM>)system)));
    }
    return pieces;
  }

  /**
   * 区分的連続代数システムの区分の番号のリストを返します。
   * 
   * @param t 時刻
   * @return 区分的連続代数システムの区分の番号のリスト
   * @exception SolverStopException ソルバーが停止された場合
   */
  private List<Integer> getContinuousPieces(RS t) throws SolverStopException {
    final List<Integer> pieces = new ArrayList<>();
    for (PiecewiseContinuousAlgebraicSystem<RS, RM, CS, CM> system : this.piecewiseContinuousAlgebraicSystems) {
      pieces.addAll(system.getPiece(t, getInputNodeValueOf((SystemOperator<RS, RM, CS, CM>)system)));
    }
    return pieces;
  }

  /**
   * 区分的微分可能差分システムの区分の番号のリストを返します。
   * 
   * @param t 時刻
   * @param continuousStates 連続状態
   * @param discreteStates 離散状態
   * @return 区分的微分可能差分システムの区分の番号のリスト
   * @exception SolverStopException ソルバーが停止された場合
   */
  private List<Integer> getDifferentialDifferencePieces(RS t, List<RM> continuousStates, List<RM> discreteStates) throws SolverStopException {
    final List<Integer> pieces = new ArrayList<>();
    for (int i = 0; i < this.piecewiseDifferentialDifferenceSystems.size(); i++) {
      final PiecewiseDifferentialDifferenceSystem<RS, RM, CS, CM> system = this.piecewiseDifferentialDifferenceSystems.get(i);
      final RM xc = continuousStates.get(i);
      final RM xd = discreteStates.get(i);
      pieces.addAll(system.getPiece(t, xc, xd, getInputNodeValueOf((SystemOperator<RS, RM, CS, CM>)system)));
    }
    return pieces;
  }

  /**
   * 区分的微分可能システムの区分の番号のリストを返します。
   * 
   * @param t 時刻
   * @param continuousStates 連続状態
   * @return 区分的微分可能システムの区分の番号のリスト
   * @exception SolverStopException ソルバーが停止された場合
   */
  private List<Integer> getDifferentialPieces(RS t, List<RM> continuousStates) throws SolverStopException {
    final List<Integer> pieces = new ArrayList<>();
    for (int i = 0; i < this.piecewiseDifferentialSystems.size(); i++) {
      final PiecewiseDifferentialSystem<RS, RM, CS, CM> system = this.piecewiseDifferentialSystems.get(i);
      final RM xc = continuousStates.get(i);
      pieces.addAll(system.getPiece(t, xc, getInputNodeValueOf((SystemOperator<RS, RM, CS, CM>)system)));
    }
    return pieces;
  }

  /**
   * 区分的微分可能差分システムのリストを設定します。
   */
  private void setupPiecewiseDifferentialDifferenceSystems() {
    this.piecewiseDifferentialDifferenceSystems.clear();

    final int size = getNodeSize();

    for (int row = 0; row < size; row++) {
      for (int column = 0; column < size; column++) {
        final SystemOperator<RS, RM, CS, CM> system = getSystemOperator(row, column);
        if (system instanceof PiecewiseDifferentialDifferenceSystem) {
          this.piecewiseDifferentialDifferenceSystems.add((PiecewiseDifferentialDifferenceSystem<RS, RM, CS, CM>)system);
        }
      }
    }
  }

  /**
   * 区分的微分可能システムのリストを設定します。
   */
  private void setupPiecewiseDifferentialSystems() {
    this.piecewiseDifferentialSystems.clear();

    final int size = getNodeSize();

    for (int row = 0; row < size; row++) {
      for (int column = 0; column < size; column++) {
        final SystemOperator<RS, RM, CS, CM> system = getSystemOperator(row, column);
        if (system instanceof PiecewiseDifferentialSystem) {
          this.piecewiseDifferentialSystems.add((PiecewiseDifferentialSystem<RS, RM, CS, CM>)system);
        }
      }
    }
  }

  /**
   * 区分的連続離散代数システムのリストを設定します。
   */
  private void setupPiecewiseContinuousDiscreteAlgebraicSystems() {
    this.piecewiseContinuousDiscreteAlgebraicSystems.clear();

    final int size = getNodeSize();

    for (int row = 0; row < size; row++) {
      for (int column = 0; column < size; column++) {
        final SystemOperator<RS, RM, CS, CM> system = getSystemOperator(row, column);
        if (system instanceof PiecewiseContinuousDiscreteAlgebraicSystem) {
          this.piecewiseContinuousDiscreteAlgebraicSystems.add((PiecewiseContinuousDiscreteAlgebraicSystem<RS, RM, CS, CM>)system);
        }
      }
    }
  }

  /**
   * 区分的連続代数システムのリストを設定します。
   */
  private void setupPiecewiseContinuousAlgebraicSystems() {
    this.piecewiseContinuousAlgebraicSystems.clear();

    final int size = getNodeSize();

    for (int row = 0; row < size; row++) {
      for (int column = 0; column < size; column++) {
        final SystemOperator<RS, RM, CS, CM> system = getSystemOperator(row, column);
        if (system instanceof PiecewiseContinuousAlgebraicSystem) {
          this.piecewiseContinuousAlgebraicSystems.add((PiecewiseContinuousAlgebraicSystem<RS, RM, CS, CM>)system);
        }
      }
    }
  }

  /**
   * 区分的微分可能差分システムのへの入力を返します。
   * 
   * @return 区分的微分可能差分システムのへの入力
   */
  private List<RM> getDifferentialDifferenceSystemInputs() {
    final List<RM> inputs = new ArrayList<>();
    for (PiecewiseDifferentialDifferenceSystem<RS, RM, CS, CM> system : this.piecewiseDifferentialDifferenceSystems) {
      inputs.add(getInputNodeValueOf((SystemOperator<RS, RM, CS, CM>)system).createClone());
    }
    return inputs;
  }

  /**
   * 区分的微分可能システムのへの入力を返します。
   * 
   * @return 区分的微分可能システムのへの入力
   */
  private List<RM> getDifferentialSystemInputs() {
    final List<RM> inputs = new ArrayList<>();
    for (PiecewiseDifferentialSystem<RS, RM, CS, CM> system : this.piecewiseDifferentialSystems) {
      inputs.add(getInputNodeValueOf((SystemOperator<RS, RM, CS, CM>)system).createClone());
    }
    return inputs;
  }

  /**
   * 区分的連続離散代数システムへの入力を返します。
   * 
   * @return 区分的連続離散代数システムへの入力
   */
  private List<RM> getContinuousDiscreteAlgebraicSystemInputs() {
    final List<RM> inputs = new ArrayList<>();
    for (PiecewiseContinuousDiscreteAlgebraicSystem<RS, RM, CS, CM> system : this.piecewiseContinuousDiscreteAlgebraicSystems) {
      inputs.add(getInputNodeValueOf((SystemOperator<RS, RM, CS, CM>)system).createClone());
    }
    return inputs;
  }

  /**
   * 区分的連続代数システムへの入力を返します。
   * 
   * @return 区分的連続代数システムへの入力
   */
  private List<RM> getContinuousAlgebraicSystemInputs() {
    final List<RM> inputs = new ArrayList<>();
    for (PiecewiseContinuousAlgebraicSystem<RS, RM, CS, CM> system : this.piecewiseContinuousAlgebraicSystems) {
      inputs.add(getInputNodeValueOf((SystemOperator<RS, RM, CS, CM>)system).createClone());
    }
    return inputs;
  }

  /**
   * 指定された区間内([(t1,inputs1),(t2,inputs2)]の連続離散代数システムの不連続点の時刻を返します。
   * 
   * @param t1 不連続点の前の時刻
   * @param inputs1 不連続点の前の入力
   * @param t2 不連続点の後の時刻
   * @param inputs2 不連続点の後の入力
   * @return 指定された区間内([(t1,inputs1),(t2,inputs2)]の不連続点の時刻
   * @exception SolverStopException ソルバーが停止された場合
   */
  private List<RS> getContinuousDiscreteDiscontinuousPoints(RS t1, final List<RM> inputs1, RS t2, final List<RM> inputs2) throws SolverStopException {
    final List<RS> points = new ArrayList<>();
    for (int i = 0; i < this.piecewiseContinuousDiscreteAlgebraicSystems.size(); i++) {
      final PiecewiseContinuousDiscreteAlgebraicSystem<RS, RM, CS, CM> system = this.piecewiseContinuousDiscreteAlgebraicSystems.get(i);
      final RM u1 = inputs1.get(i);
      final RM u2 = inputs2.get(i);
      points.add(system.getDiscontinuousPoint(t1, u1, t2, u2));
    }
    return points;
  }

  /**
   * 指定された区間内([(t1,inputs1),(t2,inputs2)]の連続代数システムの不連続点の時刻を返します。
   * 
   * @param t1 不連続点の前の時刻
   * @param inputs1 不連続点の前の入力
   * @param t2 不連続点の後の時刻
   * @param inputs2 不連続点の後の入力
   * @return 指定された区間内([(t1,inputs1),(t2,inputs2)]の不連続点の時刻
   * @throws SolverStopException ソルバーが停止しば場合
   */
  private List<RS> getContinuousDiscontinuousPoints(RS t1, final List<RM> inputs1, RS t2, final List<RM> inputs2) throws SolverStopException {
    final List<RS> points = new ArrayList<>();
    for (int i = 0; i < this.piecewiseContinuousAlgebraicSystems.size(); i++) {
      final PiecewiseContinuousAlgebraicSystem<RS, RM, CS, CM> system = this.piecewiseContinuousAlgebraicSystems.get(i);
      final RM u1 = inputs1.get(i);
      final RM u2 = inputs2.get(i);
      points.add(system.getDiscontinuousPoint(t1, u1, t2, u2));
    }
    return points;
  }

  /**
   * 指定された区間内([(t1,continuousState1,discreteState1,inputs1),(t2,continuousState2,discreteState2,inputs2)]の不連続点の時刻を返します。
   * 
   * @param t1 不連続点の前の時刻
   * @param continuousStates1 不連続点の前の連続状態
   * @param discreteStates1 不連続点の前の離散状態
   * @param inputs1 不連続点の前の入力
   * @param t2 不連続点の後の時刻
   * @param continuousStates2 不連続点の後の連続状態
   * @param discreteStates2 不連続点の後の離散状態
   * @param inputs2 不連続点の後の入力
   * @return 指定された区間内([(t1,x1,inputs1),(t2,x2,inputs2)]の不連続点の時刻
   * @exception SolverStopException ソルバーが停止された場合
   */
  private List<RS> getDifferentialDifferenceDiscontinuousPoints(final RS t1, final List<RM> continuousStates1, final List<RM> discreteStates1, final List<RM> inputs1, RS t2,
      List<RM> continuousStates2, final List<RM> discreteStates2, final List<RM> inputs2) throws SolverStopException {
    final List<RS> points = new ArrayList<>();
    for (int i = 0; i < this.piecewiseDifferentialDifferenceSystems.size(); i++) {
      final PiecewiseDifferentialDifferenceSystem<RS, RM, CS, CM> system = this.piecewiseDifferentialDifferenceSystems.get(i);
      final RM u1 = inputs1.get(i);
      final RM u2 = inputs2.get(i);
      final RM xc1 = continuousStates1.get(i);
      final RM xc2 = continuousStates2.get(i);
      final RM xd1 = discreteStates1.get(i);
      final RM xd2 = discreteStates2.get(i);

      points.add(system.getDiscontinuousPoint(t1, xc1, xd1, u1, t2, xc2, xd2, u2));
    }
    return points;
  }

  /**
   * 指定された区間内([(t1,continuousState1,discreteState1,inputs1),(t2,continuousState2,discreteState2,inputs2)]の不連続点の時刻を返します。
   * 
   * @param t1 不連続点の前の時刻
   * @param continuousStates1 不連続点の前の連続状態
   * @param inputs1 不連続点の前の入力
   * @param t2 不連続点の後の時刻
   * @param continuousStates2 不連続点の後の連続状態
   * @param inputs2 不連続点の後の入力
   * @return 指定された区間内([(t1,x1,inputs1),(t2,x2,inputs2)]の不連続点の時刻
   * @throws SolverStopException ソルバーが停止された場合
   */
  private List<RS> getDifferentialDiscontinuousPoints(final RS t1, final List<RM> continuousStates1, final List<RM> inputs1, RS t2, List<RM> continuousStates2, final List<RM> inputs2)
      throws SolverStopException {
    final List<RS> points = new ArrayList<>();
    for (int i = 0; i < this.piecewiseDifferentialSystems.size(); i++) {
      final PiecewiseDifferentialSystem<RS, RM, CS, CM> system = this.piecewiseDifferentialSystems.get(i);
      final RM u1 = inputs1.get(i);
      final RM u2 = inputs2.get(i);
      final RM xc1 = continuousStates1.get(i);
      final RM xc2 = continuousStates2.get(i);

      points.add(system.getDiscontinuousPoint(t1, xc1, u1, t2, xc2, u2));
    }
    return points;
  }

  /**
   * 区分的微分可能差分システムの連続状態を返します。
   * 
   * @return 区分的微分可能差分システムの連続状態
   */
  private List<RM> getDifferentialDifferneceSystemContinuousStates() {
    final List<RM> states = new ArrayList<>();
    for (PiecewiseDifferentialDifferenceSystem<RS, RM, CS, CM> system : this.piecewiseDifferentialDifferenceSystems) {
      states.add(((SampledDataDynamicSystem<RS, RM, CS, CM>)system).getContinuousState().createClone());
    }
    return states;
  }

  /**
   * 区分的微分可能システムの連続状態を返します。
   * 
   * @return 区分的微分可能システムの連続状態
   */
  private List<RM> getDifferentialSystemContinuousStates() {
    final List<RM> states = new ArrayList<>();
    for (PiecewiseDifferentialSystem<RS, RM, CS, CM> system : this.piecewiseDifferentialSystems) {
      states.add(((ContinuousExplicitDynamicSystem<RS, RM, CS, CM>)system).getState().createClone());
    }
    return states;
  }

  /**
   * 区分的微分可能差分システムの離散状態を返します。
   * 
   * @return 区分的微分可能差分システムの離散状態
   */
  private List<RM> getDifferentialDifferneceSystemDiscreteStates() {
    final List<RM> states = new ArrayList<>();
    for (PiecewiseDifferentialDifferenceSystem<RS, RM, CS, CM> system : this.piecewiseDifferentialDifferenceSystems) {
      states.add(((SampledDataDynamicSystem<RS, RM, CS, CM>)system).getDiscreteState().createClone());
    }
    return states;
  }
}