DoubleTransportDelay.java

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

import java.util.ArrayDeque;
import java.util.Deque;

import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.ode.EquationSolver;
import org.mklab.tool.control.system.parameter.Parameter;
import org.mklab.tool.control.system.parameter.ParameterUpdator;
import org.mklab.tool.control.system.parameter.SIunit;
import org.mklab.tool.control.system.parameter.StringExternalizable;


/**
 * 入力を遅延させるシステム(無駄時間システム)を表わすクラスです。
 * 
 * @author koga
 * @version $Revision: 1.18 $, 2007/04/16
 */
public class DoubleTransportDelay extends DoubleBaseContinuousStaticSystem implements ParameterUpdator, StringExternalizable {

  /** 無駄時間 */
  @Parameter(name = "timeDelay", unit = SIunit.s, description = "TransportDelay.1", internationalization = true)
  private double timeDelay = 1;

  /** 初期出力 */
  @Parameter(name = "initialOutput", description = "TransportDelay.3", update = true, internationalization = true)
  private DoubleMatrix initialOutput = new DoubleMatrix(1, 1);

  /** 初期バッファサイズ */
  @Parameter(name = "initialBufferSize", description = "TransportDelay.5", internationalization = true)
  private int initialBufferSize = 1024;

  /** 時刻のキュー */
  private Deque<Double> timeQue;

  /** 入力のキュー */
  private Deque<DoubleMatrix> inputQue;

  /**
   * 新しく生成された<code>TransportDelay</code>オブジェクトを初期化します。
   */
  public DoubleTransportDelay() {
    this(1);
  }

  /**
   * 新しく生成された<code>TransportDelay</code>オブジェクトを初期化します。
   * 
   * @param timeDelay 無駄時間
   */
  public DoubleTransportDelay(final double timeDelay) {
    super(-1, -1);
    setAutoSize(true);
    setHasDirectFeedthrough(true);
    this.timeDelay = timeDelay;
    initialize();
  }

  /**
   * 新しく生成された<code>TransportDelay</code>オブジェクトを初期化します。
   * 
   * @param timeDelay 無駄時間
   * @param initialOutput 初期出力
   */
  public DoubleTransportDelay(final double timeDelay, final DoubleMatrix initialOutput) {
    super(initialOutput.getRowSize(), initialOutput.getRowSize());
    setHasDirectFeedthrough(true);
    this.timeDelay = timeDelay;
    this.initialOutput = initialOutput.createClone();
    initialize();
  }

  /**
   * @see org.mklab.tool.control.system.continuous.BaseContinuousStaticSystem#initialize()
   */
  @Override
  public void initialize() {
    this.timeQue = new ArrayDeque<>(this.initialBufferSize);
    this.inputQue = new ArrayDeque<>(this.initialBufferSize);
  }

  /**
   * {@inheritDoc}
   */
  @SuppressWarnings({"boxing"})
  @Override
  public DoubleMatrix outputEquation(final double t, final DoubleMatrix u) {
    final int dataSize = this.timeQue.size();

    if (EquationSolver.isTrial() == false) {
      if (dataSize == 0 || this.timeQue.getLast() != t) {
        this.timeQue.add(t);
        this.inputQue.add(u);
      }
    }

    if (t < this.timeDelay || dataSize == 0) {
      return this.initialOutput.createClone();
    }

    final double delayPoint = t - this.timeDelay;

    if (dataSize == 1) {
      final double t2 = this.timeQue.getLast();
      final double t1 = 0;
      final DoubleMatrix u2 = this.inputQue.getLast();
      final DoubleMatrix u1 = this.initialOutput;

      return u2.subtract(u2.subtract(u1).multiply((delayPoint - t1) / (t2 - t1)));
    }

    if (this.timeQue.getLast() < delayPoint) {
      final double t2 = this.timeQue.pollLast();
      final double t1 = this.timeQue.pollLast();

      final DoubleMatrix u2 = this.inputQue.pollLast();
      final DoubleMatrix u1 = this.inputQue.pollLast();

      this.timeQue.add(t1);
      this.timeQue.add(t2);

      this.inputQue.add(u1);
      this.inputQue.add(u2);

      return u1.add(u2.subtract(u1).multiply((delayPoint - t1) / (t2 - t1)));
    }

    double t1 = this.timeQue.pollFirst();
    DoubleMatrix u1 = this.inputQue.pollFirst();

    for (int i = 0; i < dataSize; i++) {
      double t2 = this.timeQue.pollFirst();
      final DoubleMatrix u2 = this.inputQue.pollFirst();

      if (t2 < delayPoint) {
        t1 = t2;
        u1 = u2;
        continue;
      }

      this.timeQue.addFirst(t2);
      this.timeQue.addFirst(t1);
      this.inputQue.addFirst(u2);
      this.inputQue.addFirst(u1);

      return u1.add(u2.subtract(u1).multiply((delayPoint - t1) / (t2 - t1)));
    }

    assert false : "never reached"; //$NON-NLS-1$

    return this.initialOutput;
  }

  /**
   * 無駄時間を設定します。
   * 
   * @param timeDelay 無駄時間
   */
  public void setTimeDelay(final double timeDelay) {
    this.timeDelay = timeDelay;
  }

  /**
   * 無駄時間を返します。
   * 
   * @return 無駄時間
   */
  public double getTimeDelay() {
    return this.timeDelay;
  }

  /**
   * 初期出力を設定します。
   * 
   * @param initialOutput 初期出力
   */
  public void setInitialOutput(final DoubleMatrix initialOutput) {
    this.initialOutput = initialOutput.createClone();
    final int size = initialOutput.getRowSize();
    if (getInputSize() != size) {
      super.setInputSize(size);
    }
    if (getOutputSize() != size) {
      super.setOutputSize(size);
    }
  }

  /**
   * @see org.mklab.tool.control.system.SystemOperator#setInputSize(int)
   */
  @Override
  public void setInputSize(final int inputSize) {
    if (inputSize != -1) {
      super.setOutputSize(inputSize);
      if (this.initialOutput == null || this.initialOutput.length() != inputSize) {
        setInitialOutput(new DoubleMatrix(inputSize, 1));
      }
    }
    super.setInputSize(inputSize);
  }

  /**
   * @see org.mklab.tool.control.system.SystemOperator#setOutputSize(int)
   */
  @Override
  public void setOutputSize(final int outputSize) {
    if (outputSize != -1) {
      super.setInputSize(outputSize);
      if (this.initialOutput == null || this.initialOutput.length() != outputSize) {
        setInitialOutput(new DoubleMatrix(outputSize, 1));
      }
    }
    super.setOutputSize(outputSize);
  }

  /**
   * 初期出力を返します。
   * 
   * @return 初期出力
   */
  public DoubleMatrix getInitialOutput() {
    return this.initialOutput.createClone();
  }

  /**
   * 初期バッファサイズを設定します。
   * 
   * @param initialBufferSize 初期バッファサイズ
   */
  public void setInitialBufferSize(final int initialBufferSize) {
    this.initialBufferSize = initialBufferSize;
  }

  /**
   * 初期バッファサイズを返します。
   * 
   * @return 初期バッファサイズ
   */
  public int getInitialBufferSize() {
    return this.initialBufferSize;
  }

  /**
   * @see org.mklab.tool.control.system.parameter.ParameterUpdator#updateWith(java.lang.String)
   */
  public boolean updateWith(String parameter) {
    if (parameter.equals("initialOutput")) { //$NON-NLS-1$
      final int size = this.initialOutput.getRowSize();
      setInputSize(size);
      setOutputSize(size);
      return true;
    }
    return false;
  }

  /**
   * @see org.mklab.tool.control.system.parameter.StringExternalizable#getString(java.lang.String)
   */
  public String getString(String key) {
    return Messages.getString(key);
  }

  /**
   * @see org.mklab.tool.control.system.SystemOperator#equals(java.lang.Object)
   */
  @Override
  public boolean equals(Object o) {
    if (this == o) {
      return true;
    }
    if (!super.equals(o)) {
      return false;
    }
    if (o == null) {
      return false;
    }
    if (o.getClass() != getClass()) {
      return false;
    }

    final DoubleTransportDelay castedObj = (DoubleTransportDelay)o;
    return ((this.timeDelay == castedObj.timeDelay) && (this.initialOutput == null ? castedObj.initialOutput == null : this.initialOutput.equals(castedObj.initialOutput))
        && (this.initialBufferSize == castedObj.initialBufferSize) && (this.timeQue == null ? castedObj.timeQue == null : this.timeQue.equals(castedObj.timeQue)) && (this.inputQue == null
          ? castedObj.inputQue == null : this.inputQue.equals(castedObj.inputQue)));
  }

  /**
   * @see org.mklab.tool.control.system.SystemOperator#hashCode()
   */
  @Override
  public int hashCode() {
    int hashCode = super.hashCode();
    hashCode = 31 * hashCode + (int)(Double.doubleToLongBits(this.timeDelay) ^ (Double.doubleToLongBits(this.timeDelay) >>> 32));
    hashCode = 31 * hashCode + (this.initialOutput == null ? 0 : this.initialOutput.hashCode());
    hashCode = 31 * hashCode + this.initialBufferSize;
    hashCode = 31 * hashCode + (this.timeQue == null ? 0 : this.timeQue.hashCode());
    hashCode = 31 * hashCode + (this.inputQue == null ? 0 : this.inputQue.hashCode());
    return hashCode;
  }
}