DoubleBlockSamplingSystem.java

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

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

import java.util.ArrayList;
import java.util.List;
import java.util.Map;
import java.util.Set;
import java.util.TreeMap;
import java.util.TreeSet;

import org.mklab.nfc.ode.DoubleSampling;
import org.mklab.tool.control.system.DoubleBlockSystem;
import org.mklab.tool.control.system.DoubleSystemOperator;
import org.mklab.tool.control.system.DoubleZeroSystem;
import org.mklab.tool.control.system.discrete.DoubleDiscreteDynamicSystem;
import org.mklab.tool.control.system.discrete.DoubleDiscreteStaticSystem;
import org.mklab.tool.control.system.discrete.DoubleHoldSystem;


/**
 * サンプリングが存在するブロックシステムを表すクラスです。
 * 
 * @author koga
 * @version $Revision$
 */
public abstract class DoubleBlockSamplingSystem extends DoubleBlockSystem implements DoubleSampling {

  /** ノードの値を求める時刻 */
  protected double time;

  /** 異なるサンプリング周期の集合(昇順にソート済み) */
  private Set<Double> samplingIntervals = new TreeSet<>();
  /** 同一サンプリング周期の離散時間動的システムの集合のマップ(キーで昇順にソート済み) */
  private Map<Double, List<DoubleDiscreteDynamicSystem>> discreteDynamicSystemsWithSameSamplingInterval = new TreeMap<>();
  /** 同一サンプリング周期の離散時間静的システムの集合のマップ(キーで昇順にソート済み) */
  private Map<Double, List<DoubleDiscreteStaticSystem>> discreteStaticSystemsWithSameSamplingInterval = new TreeMap<>();
  /** 同一サンプリング周期のホールドシステムの集合のマップ(キーで昇順にソート済み) */
  private Map<Double, List<DoubleHoldSystem>> holdSystemsWithSameSamplingInterval = new TreeMap<>();

  /** 離散時間動的システムのリスト */
  protected List<DoubleDiscreteDynamicSystem> discreteDynamicSystems = new ArrayList<>();
  /** 離散時間静的システムのリスト */
  protected List<DoubleDiscreteStaticSystem> discreteStaticSystems = new ArrayList<>();
  /** サンプル値動的システムのリスト */
  private List<DoubleHoldSystem> holdSystems = new ArrayList<>();

  /** 次のサンプリング時間で状態を更新すべき離散時間動的システムの集合 */
  protected List<DoubleDiscreteDynamicSystem> discreteDynamicSystemsUpdatedAtNextSamplingPoint;
  /** 次のサンプリング時間で状態を更新すべき離散時間静的システムの集合 */
  protected List<DoubleDiscreteStaticSystem> discreteStaticSystemsUpdatedAtNextSamplingPoint;
  /** 次のサンプリング時間で状態を更新すべきホールドシステムの集合 */
  private ArrayList<DoubleHoldSystem> holdSystemsUpdatedAtNextSamplingPoint;

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

  /**
   * @see org.mklab.tool.control.system.BlockSystem#initialize()
   */
  @Override
  public void initialize() {
    super.initialize();
    if (this.discreteDynamicSystems == null || this.holdSystems == null) {
      return;
    }
    resetSystemsUpdatedAtNextSamplingPoint();
  }

  /**
   * 異なるサンプリング周期の配列を設定します。
   */
  @SuppressWarnings("boxing")
  private void setupSamplingInterval() {

    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 == DoubleZeroSystem.getInstance()) {
          continue;
        }

        if (system instanceof DoubleDiscreteDynamicSystem) {
          final double interval = ((DoubleSampler)system).getSamplingInterval();

          if (this.discreteDynamicSystemsWithSameSamplingInterval.containsKey(interval)) {
            final List<DoubleDiscreteDynamicSystem> systemSet = this.discreteDynamicSystemsWithSameSamplingInterval.get(interval);
            systemSet.add((DoubleDiscreteDynamicSystem)system);
            this.discreteDynamicSystemsWithSameSamplingInterval.remove(interval);
            this.discreteDynamicSystemsWithSameSamplingInterval.put(interval, systemSet);
          } else {
            final List<DoubleDiscreteDynamicSystem> systemSet = new ArrayList<>();
            systemSet.add((DoubleDiscreteDynamicSystem)system);
            this.discreteDynamicSystemsWithSameSamplingInterval.put(interval, systemSet);
          }

          this.samplingIntervals.add(interval);
        } else if (system instanceof DoubleDiscreteStaticSystem && (system instanceof DoubleHoldSystem == false)) {
          final double interval = ((DoubleSampler)system).getSamplingInterval();

          if (this.discreteStaticSystemsWithSameSamplingInterval.containsKey(interval)) {
            final List<DoubleDiscreteStaticSystem> systemSet = this.discreteStaticSystemsWithSameSamplingInterval.get(interval);
            systemSet.add((DoubleDiscreteStaticSystem)system);
            this.discreteStaticSystemsWithSameSamplingInterval.remove(interval);
            this.discreteStaticSystemsWithSameSamplingInterval.put(interval, systemSet);
          } else {
            final List<DoubleDiscreteStaticSystem> systemSet = new ArrayList<>();
            systemSet.add((DoubleDiscreteStaticSystem)system);
            this.discreteStaticSystemsWithSameSamplingInterval.put(interval, systemSet);
          }

          this.samplingIntervals.add(interval);
        } else if (system instanceof DoubleHoldSystem) {
          final double interval = ((DoubleHoldSystem)system).getSamplingInterval();

          if (this.holdSystemsWithSameSamplingInterval.containsKey(interval)) {
            final List<DoubleHoldSystem> systemSet = this.holdSystemsWithSameSamplingInterval.get(interval);
            systemSet.add((DoubleHoldSystem)system);
            this.holdSystemsWithSameSamplingInterval.remove(interval);
            this.holdSystemsWithSameSamplingInterval.put(interval, systemSet);
          } else {
            final List<DoubleHoldSystem> systemSet = new ArrayList<>();
            systemSet.add((DoubleHoldSystem)system);
            this.holdSystemsWithSameSamplingInterval.put(interval, systemSet);
          }

          this.samplingIntervals.add(interval);
        }
      }
    }
  }

  /**
   * 次のサンプリング点の時間を返します。
   * 
   * @param t 現在の時間
   * @param tolerance 許容誤差
   * @return 次のサンプリング点の時間
   */
  @SuppressWarnings("boxing")
  public double getNextSamplingTime(final double t, final double tolerance) {
    final int size = this.samplingIntervals.size();
    final double[][] nextSamplingTimes = new double[size][2];

    int i = 0;
    for (double samplingInterval : this.samplingIntervals) {
      nextSamplingTimes[i][0] = samplingInterval;
      double nextSamplingTime = Math.ceil(t / samplingInterval) * samplingInterval;
      if (Math.abs(t - nextSamplingTime) < tolerance) {
        nextSamplingTime = t + samplingInterval;
      }
      nextSamplingTimes[i][1] = nextSamplingTime;
      i++;
    }

    int k = 0;
    double nextSamplingTime = nextSamplingTimes[k][1];
    for (int j = 1; j < size; j++) {
      if (nextSamplingTime > nextSamplingTimes[j][1]) {
        nextSamplingTime = nextSamplingTimes[j][1];
        k = j;
      }
    }

    this.discreteDynamicSystemsUpdatedAtNextSamplingPoint = new ArrayList<>();
    for (int j = 0; j < size; j++) {
      if (Math.abs(nextSamplingTimes[k][1] - nextSamplingTimes[j][1]) < tolerance) {
        final List<DoubleDiscreteDynamicSystem> systemSet = this.discreteDynamicSystemsWithSameSamplingInterval.get(nextSamplingTimes[j][0]);
        if (systemSet == null) {
          continue;
        }
        this.discreteDynamicSystemsUpdatedAtNextSamplingPoint.addAll(systemSet);
      }
    }

    this.discreteStaticSystemsUpdatedAtNextSamplingPoint = new ArrayList<>();
    for (int j = 0; j < size; j++) {
      if (Math.abs(nextSamplingTimes[k][1] - nextSamplingTimes[j][1]) < tolerance) {
        final List<DoubleDiscreteStaticSystem> systemSet = this.discreteStaticSystemsWithSameSamplingInterval.get(nextSamplingTimes[j][0]);
        if (systemSet == null) {
          continue;
        }
        this.discreteStaticSystemsUpdatedAtNextSamplingPoint.addAll(systemSet);
      }
    }

    this.holdSystemsUpdatedAtNextSamplingPoint = new ArrayList<>();
    for (int j = 0; j < size; j++) {
      if (Math.abs(nextSamplingTimes[k][1] - nextSamplingTimes[j][1]) < tolerance) {
        final List<DoubleHoldSystem> systemSet = this.holdSystemsWithSameSamplingInterval.get(nextSamplingTimes[j][0]);
        if (systemSet == null) {
          continue;
        }
        this.holdSystemsUpdatedAtNextSamplingPoint.addAll(systemSet);
      }
    }

    return nextSamplingTime;
  }

  /**
   * 次のサンプル点で状態を更新すべき離散時間動的システムの集合をリセットします。
   */
  public void resetSystemsUpdatedAtNextSamplingPoint() {
    // 全ての離散時間動的システムが対象となる
    this.discreteDynamicSystemsUpdatedAtNextSamplingPoint = new ArrayList<>();
    for (final List<DoubleDiscreteDynamicSystem> systemSet : this.discreteDynamicSystemsWithSameSamplingInterval.values()) {
      this.discreteDynamicSystemsUpdatedAtNextSamplingPoint.addAll(systemSet);
    }

    // 全ての離散時間静的システムが対象となる
    this.discreteStaticSystemsUpdatedAtNextSamplingPoint = new ArrayList<>();
    for (final List<DoubleDiscreteStaticSystem> systemSet : this.discreteStaticSystemsWithSameSamplingInterval.values()) {
      this.discreteStaticSystemsUpdatedAtNextSamplingPoint.addAll(systemSet);
    }

    // 全てのホールドシステムが対象となる
    this.holdSystemsUpdatedAtNextSamplingPoint = new ArrayList<>();
    for (final List<DoubleHoldSystem> systemSet : this.holdSystemsWithSameSamplingInterval.values()) {
      this.holdSystemsUpdatedAtNextSamplingPoint.addAll(systemSet);
    }
  }

  /**
   * サンプル点であるか判定します。
   * 
   * @return サンプル点ならばtrue
   */
  public boolean isAtSamplingPoint() {
    boolean atSamplingPoint = false;

    for (final DoubleHoldSystem system : this.holdSystems) {
      atSamplingPoint |= system.isAtSamplingPoint();
    }

    for (final DoubleDiscreteDynamicSystem system : this.discreteDynamicSystems) {
      atSamplingPoint |= system.isAtSamplingPoint();
    }

    for (final DoubleDiscreteStaticSystem system : this.discreteStaticSystems) {
      atSamplingPoint |= system.isAtSamplingPoint();
    }

    return atSamplingPoint;
  }

  /**
   * 全てのシステムのサンプリング周期が一致するか判定します。
   * 
   * @return 全てのシステムのサンプリング周期が一致すればtrue、そうでなければfalse
   */
  public boolean isSingleRate() {
    return this.samplingIntervals.size() == 1;
  }

  /**
   * 全てのシステムに共通するサンプリング周期を返します。
   * 
   * @return 全てのシステムに共通するサンプリング周期
   */
  @SuppressWarnings("boxing")
  public double getSingleSamplingInterval() {
    if (isSingleRate() == false) {
      throw new RuntimeException(Messages.getString("BlockSamplingSystem.0")); //$NON-NLS-1$
    }

    return this.samplingIntervals.toArray(new Double[1])[0];
  }

  /**
   * サンプル点であるかを設定します。
   * 
   * @param samplingPoint サンプル点ならばtrue
   */
  public void setAtSamplingPoint(final boolean samplingPoint) {
    if (samplingPoint == true) {
      for (final DoubleHoldSystem system : this.holdSystemsUpdatedAtNextSamplingPoint) {
        system.setAtSamplingPoint(true);
      }
      for (final DoubleDiscreteDynamicSystem system : this.discreteDynamicSystemsUpdatedAtNextSamplingPoint) {
        system.setAtSamplingPoint(true);
      }
      for (final DoubleDiscreteStaticSystem system : this.discreteStaticSystemsUpdatedAtNextSamplingPoint) {
        system.setAtSamplingPoint(true);
      }
    } else {
      for (final DoubleHoldSystem system : this.holdSystemsUpdatedAtNextSamplingPoint) {
        system.setAtSamplingPoint(samplingPoint);
      }
      for (final DoubleDiscreteDynamicSystem system : this.discreteDynamicSystemsUpdatedAtNextSamplingPoint) {
        system.setAtSamplingPoint(samplingPoint);
      }
      for (final DoubleDiscreteStaticSystem system : this.discreteStaticSystemsUpdatedAtNextSamplingPoint) {
        system.setAtSamplingPoint(samplingPoint);
      }
    }
  }

  /**
   * サンプリングが存在するシステムのリストを設定します。
   */
  private void setupSamplingSystems() {
    setupDiscreteDynamicSystems();
    setupDiscreteStaticSystems();
    setupHoldSystems();
  }

  /**
   * ホールドシステムのリストを設定します。
   */
  private void setupHoldSystems() {
    this.holdSystems.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 DoubleHoldSystem) {
          this.holdSystems.add((DoubleHoldSystem)system);
        }
      }
    }
  }

  /**
   * 離散時間動的システムのリストを設定します。
   */
  private void setupDiscreteDynamicSystems() {
    this.discreteDynamicSystems.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 DoubleDiscreteDynamicSystem) {
          this.discreteDynamicSystems.add((DoubleDiscreteDynamicSystem)system);
        }
      }
    }
  }

  /**
   * 離散時間政敵システムのリストを設定します。
   */
  private void setupDiscreteStaticSystems() {
    this.discreteStaticSystems.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 DoubleDiscreteStaticSystem) {
          this.discreteStaticSystems.add((DoubleDiscreteStaticSystem)system);
        }
      }
    }
  }

  /**
   * @see org.mklab.tool.control.system.BlockSystem#replaceDynamicSystemList(org.mklab.tool.control.system.SystemOperator, org.mklab.tool.control.system.SystemOperator)
   */
  @Override
  protected boolean replaceDynamicSystemList(final DoubleSystemOperator oldSystem, final DoubleSystemOperator newSystem) {
    if (oldSystem instanceof DoubleDiscreteDynamicSystem) {
      final int index = this.discreteDynamicSystems.indexOf((DoubleDiscreteDynamicSystem)oldSystem);
      if (index == -1) {
        return false;
      }
      this.discreteDynamicSystems.set(index, (DoubleDiscreteDynamicSystem)newSystem);

      replaceDynamicSystemSetForSampling(oldSystem, newSystem);

      return true;
    } else if (oldSystem instanceof DoubleHoldSystem) {
      final int index = this.holdSystems.indexOf(oldSystem);
      if (index == -1) {
        return false;
      }
      this.holdSystems.set(index, (DoubleHoldSystem)newSystem);
      return true;
    }

    return false;
  }

  /**
   * サンプリング周期に関する離散時間動的システムの集合の成分を入れ替えます。
   * 
   * @param oldSystem 元のシステム
   * @param newSystem 新システム
   */
  private void replaceDynamicSystemSetForSampling(final DoubleSystemOperator oldSystem, final DoubleSystemOperator newSystem) {
    if (this.discreteDynamicSystemsUpdatedAtNextSamplingPoint.contains((DoubleDiscreteDynamicSystem)oldSystem)) {
      this.discreteDynamicSystemsUpdatedAtNextSamplingPoint.remove((DoubleDiscreteDynamicSystem)oldSystem);
      this.discreteDynamicSystemsUpdatedAtNextSamplingPoint.add((DoubleDiscreteDynamicSystem)newSystem);
    }

    final double samplingInterval = ((DoubleSampler)oldSystem).getSamplingInterval();
    final List<DoubleDiscreteDynamicSystem> systemSet = this.discreteDynamicSystemsWithSameSamplingInterval.get(Double.valueOf(samplingInterval));
    if (systemSet.contains((DoubleDiscreteDynamicSystem)oldSystem)) {
      systemSet.remove((DoubleDiscreteDynamicSystem)oldSystem);
      systemSet.add((DoubleDiscreteDynamicSystem)newSystem);
    } else {
      throw new RuntimeException(Messages.getString("BlockSamplingSystem.1")); //$NON-NLS-1$
    }
  }
}