DoubleBlockPiecewiseSampledDataDynamicSystem.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.DoubleMatrix;
import org.mklab.nfc.ode.DoublePiecewiseContinuousAlgebraicSystem;
import org.mklab.nfc.ode.DoublePiecewiseContinuousDiscreteAlgebraicSystem;
import org.mklab.nfc.ode.DoublePiecewiseDifferentialDifferenceSystem;
import org.mklab.nfc.ode.DoublePiecewiseDifferentialSystem;
import org.mklab.nfc.ode.SolverStopException;
import org.mklab.tool.control.system.DoubleSystemOperator;
import org.mklab.tool.control.system.continuous.DoubleContinuousExplicitDynamicSystem;


/**
 * ブロック区分的連続サンプル値動的システムを表わすクラスです。
 * 
 * @author koga
 * @version $Revision: 1.10 $, 2007/04/11
 */
public class DoubleBlockPiecewiseSampledDataDynamicSystem extends DoubleBlockSampledDataDynamicSystem implements DoublePiecewiseDifferentialDifferenceSystem {

  /** 区分的微分可能差分システムのリスト */
  private List<DoublePiecewiseDifferentialDifferenceSystem> piecewiseDifferentialDifferenceSystems = new ArrayList<>();

  /** 区分的微分可能システムのリスト */
  private List<DoublePiecewiseDifferentialSystem> piecewiseDifferentialSystems = new ArrayList<>();

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

  /** 区分的連続代数システムのリスト */
  private List<DoublePiecewiseContinuousAlgebraicSystem> piecewiseContinuousAlgebraicSystems = new ArrayList<>();

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

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

    outputEquation(t1, continuousState1, discreteState1);

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

    outputEquation(t2, continuousState2, discreteState2);

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

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

    outputEquation(t1, originalContinuousState, originalDiscreteState);

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

    final Double[] sortedPoints = discontinuousPoints.toArray(new Double[discontinuousPoints.size()]);
    Arrays.sort(sortedPoints);
    return sortedPoints[0];
  }

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

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

    final List<DoubleMatrix> differentialDiffernceContinuousStates = getDifferentialDifferneceSystemContinuousStates();
    final List<DoubleMatrix> differentialContinuousStates = getDifferentialSystemContinuousStates();
    final List<DoubleMatrix> 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(double t) throws SolverStopException {
    final List<Integer> pieces = new ArrayList<>();
    for (DoublePiecewiseContinuousDiscreteAlgebraicSystem system : this.piecewiseContinuousDiscreteAlgebraicSystems) {
      pieces.addAll(system.getPiece(t, getInputNodeValueOf((DoubleSystemOperator)system)));
    }
    return pieces;
  }

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

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

  /**
   * 区分的微分可能システムの区分の番号のリストを返します。
   * 
   * @param t 時刻
   * @param continuousStates 連続状態
   * @return 区分的微分可能システムの区分の番号のリスト
   * @exception SolverStopException ソルバーが停止された場合
   */
  private List<Integer> getDifferentialPieces(double t, List<DoubleMatrix> continuousStates) throws SolverStopException {
    final List<Integer> pieces = new ArrayList<>();
    for (int i = 0; i < this.piecewiseDifferentialSystems.size(); i++) {
      final DoublePiecewiseDifferentialSystem system = this.piecewiseDifferentialSystems.get(i);
      final DoubleMatrix xc = continuousStates.get(i);
      pieces.addAll(system.getPiece(t, xc, getInputNodeValueOf((DoubleSystemOperator)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 DoubleSystemOperator system = getSystemOperator(row, column);
        if (system instanceof DoublePiecewiseDifferentialDifferenceSystem) {
          this.piecewiseDifferentialDifferenceSystems.add((DoublePiecewiseDifferentialDifferenceSystem)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 DoubleSystemOperator system = getSystemOperator(row, column);
        if (system instanceof DoublePiecewiseDifferentialSystem) {
          this.piecewiseDifferentialSystems.add((DoublePiecewiseDifferentialSystem)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 DoubleSystemOperator system = getSystemOperator(row, column);
        if (system instanceof DoublePiecewiseContinuousDiscreteAlgebraicSystem) {
          this.piecewiseContinuousDiscreteAlgebraicSystems.add((DoublePiecewiseContinuousDiscreteAlgebraicSystem)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 DoubleSystemOperator system = getSystemOperator(row, column);
        if (system instanceof DoublePiecewiseContinuousAlgebraicSystem) {
          this.piecewiseContinuousAlgebraicSystems.add((DoublePiecewiseContinuousAlgebraicSystem)system);
        }
      }
    }
  }

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

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

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

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

  /**
   * 指定された区間内([(t1,inputs1),(t2,inputs2)]の連続離散代数システムの不連続点の時刻を返します。
   * 
   * @param t1 不連続点の前の時刻
   * @param inputs1 不連続点の前の入力
   * @param t2 不連続点の後の時刻
   * @param inputs2 不連続点の後の入力
   * @return 指定された区間内([(t1,inputs1),(t2,inputs2)]の不連続点の時刻
   * @exception SolverStopException ソルバーが停止された場合
   */
  @SuppressWarnings("boxing")
  private List<Double> getContinuousDiscreteDiscontinuousPoints(double t1, final List<DoubleMatrix> inputs1, double t2, final List<DoubleMatrix> inputs2) throws SolverStopException {
    final List<Double> points = new ArrayList<>();
    for (int i = 0; i < this.piecewiseContinuousDiscreteAlgebraicSystems.size(); i++) {
      final DoublePiecewiseContinuousDiscreteAlgebraicSystem system = this.piecewiseContinuousDiscreteAlgebraicSystems.get(i);
      final DoubleMatrix u1 = inputs1.get(i);
      final DoubleMatrix 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 ソルバーが停止しば場合
   */
  @SuppressWarnings("boxing")
  private List<Double> getContinuousDiscontinuousPoints(double t1, final List<DoubleMatrix> inputs1, double t2, final List<DoubleMatrix> inputs2) throws SolverStopException {
    final List<Double> points = new ArrayList<>();
    for (int i = 0; i < this.piecewiseContinuousAlgebraicSystems.size(); i++) {
      final DoublePiecewiseContinuousAlgebraicSystem system = this.piecewiseContinuousAlgebraicSystems.get(i);
      final DoubleMatrix u1 = inputs1.get(i);
      final DoubleMatrix 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 ソルバーが停止された場合
   */
  @SuppressWarnings("boxing")
  private List<Double> getDifferentialDifferenceDiscontinuousPoints(final double t1, final List<DoubleMatrix> continuousStates1, final List<DoubleMatrix> discreteStates1, final List<DoubleMatrix> inputs1, double t2,
      List<DoubleMatrix> continuousStates2, final List<DoubleMatrix> discreteStates2, final List<DoubleMatrix> inputs2) throws SolverStopException {
    final List<Double> points = new ArrayList<>();
    for (int i = 0; i < this.piecewiseDifferentialDifferenceSystems.size(); i++) {
      final DoublePiecewiseDifferentialDifferenceSystem system = this.piecewiseDifferentialDifferenceSystems.get(i);
      final DoubleMatrix u1 = inputs1.get(i);
      final DoubleMatrix u2 = inputs2.get(i);
      final DoubleMatrix xc1 = continuousStates1.get(i);
      final DoubleMatrix xc2 = continuousStates2.get(i);
      final DoubleMatrix xd1 = discreteStates1.get(i);
      final DoubleMatrix 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 ソルバーが停止された場合
   */
  @SuppressWarnings("boxing")
  private List<Double> getDifferentialDiscontinuousPoints(final double t1, final List<DoubleMatrix> continuousStates1, final List<DoubleMatrix> inputs1, double t2, List<DoubleMatrix> continuousStates2,
      final List<DoubleMatrix> inputs2) throws SolverStopException {
    final List<Double> points = new ArrayList<>();
    for (int i = 0; i < this.piecewiseDifferentialSystems.size(); i++) {
      final DoublePiecewiseDifferentialSystem system = this.piecewiseDifferentialSystems.get(i);
      final DoubleMatrix u1 = inputs1.get(i);
      final DoubleMatrix u2 = inputs2.get(i);
      final DoubleMatrix xc1 = continuousStates1.get(i);
      final DoubleMatrix xc2 = continuousStates2.get(i);

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

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

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

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