BlockSamplingSystem.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.matrix.ComplexNumericalMatrix;
import org.mklab.nfc.matrix.RealNumericalMatrix;
import org.mklab.nfc.ode.Sampling;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.RealNumericalScalar;
import org.mklab.tool.control.system.BlockSystem;
import org.mklab.tool.control.system.SystemOperator;
import org.mklab.tool.control.system.ZeroSystem;
import org.mklab.tool.control.system.discrete.DiscreteDynamicSystem;
import org.mklab.tool.control.system.discrete.DiscreteStaticSystem;
import org.mklab.tool.control.system.discrete.HoldSystem;


/**
 * サンプリングが存在するブロックシステムを表すクラスです。
 * 
 * @author koga
 * @version $Revision$
 * @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 abstract class BlockSamplingSystem<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 BlockSystem<RS,RM,CS,CM> implements Sampling<RS,RM,CS,CM> {

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

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

  /** 離散時間動的システムのリスト */
  protected List<DiscreteDynamicSystem<RS,RM,CS,CM>> discreteDynamicSystems = new ArrayList<>();
  /** 離散時間静的システムのリスト */
  protected List<DiscreteStaticSystem<RS,RM,CS,CM>> discreteStaticSystems = new ArrayList<>();
  /** サンプル値動的システムのリスト */
  private List<HoldSystem<RS,RM,CS,CM>> holdSystems = new ArrayList<>();

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

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

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

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

        if (system instanceof DiscreteDynamicSystem) {
          final RS interval = ((Sampler<RS,RM,CS,CM>)system).getSamplingInterval();

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

          this.samplingIntervals.add(interval);
        } else if (system instanceof DiscreteStaticSystem && (system instanceof HoldSystem == false)) {
          final RS interval = ((Sampler<RS,RM,CS,CM>)system).getSamplingInterval();

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

          this.samplingIntervals.add(interval);
        } else if (system instanceof HoldSystem) {
          final RS interval = ((HoldSystem<RS,RM,CS,CM>)system).getSamplingInterval();

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

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

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

    int i = 0;
    for (RS samplingInterval : this.samplingIntervals) {
      nextSamplingTimes[i][0] = samplingInterval;
      RS nextSamplingTime = t.divide(samplingInterval).ceil().multiply(samplingInterval);
      if (t.subtract(nextSamplingTime).abs().isLessThan(tolerance)) {
        nextSamplingTime = t.add(samplingInterval);
      }
      nextSamplingTimes[i][1] = nextSamplingTime;
      i++;
    }

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

    this.discreteDynamicSystemsUpdatedAtNextSamplingPoint = new ArrayList<>();
    for (int j = 0; j < size; j++) {
      if (nextSamplingTimes[k][1].subtract(nextSamplingTimes[j][1]).abs().isLessThan(tolerance)) {
        final List<DiscreteDynamicSystem<RS,RM,CS,CM>> 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 (nextSamplingTimes[k][1].subtract(nextSamplingTimes[j][1]).abs().isLessThan(tolerance)) {
        final List<DiscreteStaticSystem<RS,RM,CS,CM>> 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 (nextSamplingTimes[k][1].subtract(nextSamplingTimes[j][1]).abs().isLessThan(tolerance)) {
        final List<HoldSystem<RS,RM,CS,CM>> 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<DiscreteDynamicSystem<RS,RM,CS,CM>> systemSet : this.discreteDynamicSystemsWithSameSamplingInterval.values()) {
      this.discreteDynamicSystemsUpdatedAtNextSamplingPoint.addAll(systemSet);
    }

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

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

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

    for (final HoldSystem<RS,RM,CS,CM> system : this.holdSystems) {
      atSamplingPoint |= system.isAtSamplingPoint();
    }

    for (final DiscreteDynamicSystem<RS,RM,CS,CM> system : this.discreteDynamicSystems) {
      atSamplingPoint |= system.isAtSamplingPoint();
    }

    for (final DiscreteStaticSystem<RS,RM,CS,CM> system : this.discreteStaticSystems) {
      atSamplingPoint |= system.isAtSamplingPoint();
    }

    return atSamplingPoint;
  }

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

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

    return this.samplingIntervals.toArray(this.sunit.createArray(1))[0];
  }

  /**
   * サンプル点であるかを設定します。
   * 
   * @param samplingPoint サンプル点ならばtrue
   */
  public void setAtSamplingPoint(final boolean samplingPoint) {
    if (samplingPoint == true) {
      for (final HoldSystem<RS,RM,CS,CM> system : this.holdSystemsUpdatedAtNextSamplingPoint) {
        system.setAtSamplingPoint(true);
      }
      for (final DiscreteDynamicSystem<RS,RM,CS,CM> system : this.discreteDynamicSystemsUpdatedAtNextSamplingPoint) {
        system.setAtSamplingPoint(true);
      }
      for (final DiscreteStaticSystem<RS,RM,CS,CM> system : this.discreteStaticSystemsUpdatedAtNextSamplingPoint) {
        system.setAtSamplingPoint(true);
      }
    } else {
      for (final HoldSystem<RS,RM,CS,CM> system : this.holdSystemsUpdatedAtNextSamplingPoint) {
        system.setAtSamplingPoint(samplingPoint);
      }
      for (final DiscreteDynamicSystem<RS,RM,CS,CM> system : this.discreteDynamicSystemsUpdatedAtNextSamplingPoint) {
        system.setAtSamplingPoint(samplingPoint);
      }
      for (final DiscreteStaticSystem<RS,RM,CS,CM> 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 SystemOperator<RS,RM,CS,CM> system = getSystemOperator(row, column);
        if (system instanceof HoldSystem) {
          this.holdSystems.add((HoldSystem<RS,RM,CS,CM>)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 SystemOperator<RS,RM,CS,CM> system = getSystemOperator(row, column);
        if (system instanceof DiscreteDynamicSystem) {
          this.discreteDynamicSystems.add((DiscreteDynamicSystem<RS,RM,CS,CM>)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 SystemOperator<RS,RM,CS,CM> system = getSystemOperator(row, column);
        if (system instanceof DiscreteStaticSystem) {
          this.discreteStaticSystems.add((DiscreteStaticSystem<RS,RM,CS,CM>)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 SystemOperator<RS,RM,CS,CM> oldSystem, final SystemOperator<RS,RM,CS,CM> newSystem) {
    if (oldSystem instanceof DiscreteDynamicSystem) {
      final int index = this.discreteDynamicSystems.indexOf((DiscreteDynamicSystem<RS,RM,CS,CM>)oldSystem);
      if (index == -1) {
        return false;
      }
      this.discreteDynamicSystems.set(index, (DiscreteDynamicSystem<RS,RM,CS,CM>)newSystem);

      replaceDynamicSystemSetForSampling(oldSystem, newSystem);

      return true;
    } else if (oldSystem instanceof HoldSystem) {
      final int index = this.holdSystems.indexOf(oldSystem);
      if (index == -1) {
        return false;
      }
      this.holdSystems.set(index, (HoldSystem<RS,RM,CS,CM>)newSystem);
      return true;
    }

    return false;
  }

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

    final RS samplingInterval = ((Sampler<RS,RM,CS,CM>)oldSystem).getSamplingInterval();
    final List<DiscreteDynamicSystem<RS,RM,CS,CM>> systemSet = this.discreteDynamicSystemsWithSameSamplingInterval.get(samplingInterval);
    if (systemSet.contains((DiscreteDynamicSystem<RS,RM,CS,CM>)oldSystem)) {
      systemSet.remove((DiscreteDynamicSystem<RS,RM,CS,CM>)oldSystem);
      systemSet.add((DiscreteDynamicSystem<RS,RM,CS,CM>)newSystem);
    } else {
      throw new RuntimeException(Messages.getString("BlockSamplingSystem.1")); //$NON-NLS-1$
    }
  }
}