Backlash.java

/*
 * $Id: Backlash.java,v 1.16 2008/07/15 15:27:15 koga Exp $
 *
 * Copyright (C) 2004 Masanobu Koga. All rights reserved.
 */
package org.mklab.tool.control.system.discontinuous;

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

import org.mklab.nfc.matrix.ComplexNumericalMatrix;
import org.mklab.nfc.matrix.RealNumericalMatrix;
import org.mklab.nfc.ode.EquationSolver;
import org.mklab.nfc.ode.PiecewiseContinuousAlgebraicSystem;
import org.mklab.nfc.ode.PiecewiseUtil;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.RealNumericalScalar;
import org.mklab.tool.control.system.continuous.BaseContinuousStaticSystem;
import org.mklab.tool.control.system.parameter.Parameter;
import org.mklab.tool.control.system.parameter.StringExternalizable;


/**
 * バックラッシュ(遊びのある)システムを表すクラスです。
 * 
 * @author koga
 * @version $Revision: 1.16 $
 * @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 Backlash<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 BaseContinuousStaticSystem<RS,RM,CS,CM> implements PiecewiseContinuousAlgebraicSystem<RS,RM,CS,CM>, StringExternalizable {

  /** 遮断帯域の幅 */
  @Parameter(name = "deadBandWidth", description = "Backlash.1", internationalization = true)
  private RS deadBandWidth;

  /** 初期出力 */
  @Parameter(name = "initialOutput", description = "Backlash.3", internationalization = true)
  private RM initialOutput = this.sunit.createZeroGrid(1, 1);

  /** 初期状態ならばtrue */
  private boolean initialState = true;

  /** 1刻み前の時刻 */
  private RS previousOutputTime;

  /** 1刻み前の入力 */
  private RM previousInput;

  /** 1刻み前の変化率 */
  private RM previousRate;

  /** 1刻み前の出力 */
  private RM previousOutput;

  /** 保持された出力 */
  private RM holdOutput;

  /** 出力が保持されたときの入力の変化率の符号 */
  private int holdRateSign = 0;

  /**
   * 新しく生成された<code>Backlash</code>オブジェクトを初期化します。
   * @param sunit unit of scalar
   */
  public Backlash(RS sunit) {
    this(sunit.create(1), sunit.createZeroGrid(1, 1), sunit);
  }

  /**
   * 新しく生成された<code>Saturation</code>オブジェクトを初期化します。
   * 
   * @param rateLowerBound 出力の最小値
   * @param sunit unit of scalar
   */
  public Backlash(final RS rateLowerBound, RS sunit) {
    this(rateLowerBound,  sunit.createZeroGrid(1, 1), sunit);
  }

  /**
   * 新しく生成された<code>Saturation</code>オブジェクトを初期化します。
   * 
   * @param rateLowerBound 出力の最小値
   * @param initialOutput 初期出力
   * @param sunit unit of scalar
   */
  public Backlash(final RS rateLowerBound, final RM initialOutput, RS sunit) {
    super(0, 0, sunit);
    setAutoSize(true);
    setHasDirectFeedthrough(true);
    setDeadBandWidth(rateLowerBound);
    this.initialOutput = initialOutput.createClone();
    initialize();
  }

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

  /**
   * @see org.mklab.tool.control.system.continuous.BaseContinuousStaticSystem#initialize()
   */
  @Override
  public void initialize() {
    this.previousInput = this.initialOutput.createClone();
    this.previousOutput = this.initialOutput.createClone();
    this.holdOutput = this.initialOutput.createClone();
    this.previousRate = this.initialOutput.createZero();
    this.previousOutputTime = this.sunit.create(0);
    this.initialState = true;

    final int size = this.initialOutput.getRowSize();

    setInputSize(size);
    setOutputSize(size);
  }

  /**
   * {@inheritDoc}
   */
  @Override
  public RM outputEquation(final RS t, final RM u) {
    if (t == this.previousOutputTime) {
      return this.previousOutput.createClone();
    }

    final RS pTime = this.previousOutputTime;
    final RM pInput = this.previousInput;
    final RM inputRate = u.subtract(pInput).divide(t.subtract(pTime));

    if (EquationSolver.isTrial() == false) {
      this.previousOutputTime = t;
      this.previousInput = u.createClone();
      this.previousRate = inputRate.createClone();

      if (this.deadBandWidth.divide(2).isLessThanOrEquals(u.absElementWise().max())) {
        this.initialState = false;
      }
    }

    if (this.initialState) {
      this.previousOutput = this.initialOutput.createClone();
      return this.previousOutput;
    }

    final int size = u.getRowSize();
    this.previousOutput = this.sunit.createZeroGrid(size, 1);

    for (int i = 1; i <= size; i++) {
      final RS uc = u.getElement(i, 1);
      final RS rate = inputRate.getElement(i, 1);
      final RS hoc = this.holdOutput.getElement(i, 1);

      if (rate.isGreaterThanOrEquals(0)) {
        if (this.holdRateSign < 0 && uc.subtract(hoc).isLessThan(this.deadBandWidth.divide(2))) {
          this.previousOutput.setElement(i, 1, hoc);
        } else {
          this.previousOutput.setElement(i, 1, uc.subtract(this.deadBandWidth.divide(2)));
          if (EquationSolver.isTrial() == false) {
            this.holdOutput = this.previousOutput.createClone();
            this.holdRateSign = 1;
          }
        }
      } else {
        if (this.holdRateSign > 0 && hoc.subtract(uc).isLessThan(this.deadBandWidth.divide(2))) {
          this.previousOutput.setElement(i, 1, hoc);
        } else {
          this.previousOutput.setElement(i, 1, uc.add(this.deadBandWidth.divide(2)));
          if (EquationSolver.isTrial() == false) {
            this.holdOutput = this.previousOutput.createClone();
            this.holdRateSign = -1;
          }
        }
      }
    }

    return this.previousOutput;
  }

  /**
   * 遮断帯域の幅を設定します。
   * 
   * @param deadBandWidth 遮断帯域の幅
   */
  public void setDeadBandWidth(final RS deadBandWidth) {
    this.deadBandWidth = deadBandWidth;
  }

  /**
   * 遮断帯域の幅を返します。
   * 
   * @return 遮断帯域の幅
   */
  public RS getDeadBandWidth() {
    return this.deadBandWidth;
  }

  /**
   * {@inheritDoc}
   */
  @SuppressWarnings("boxing")
  public List<Integer> getPiece(final RS t, final RM u) {
    final int size = u.getRowSize();
    final List<Integer> piece = new ArrayList<>(size);

    final RS pTime = this.previousOutputTime;
    final RM pInput = this.previousInput;
    final RM inputRate = (t == pTime ? this.previousRate : u.subtract(pInput).divide(t.subtract(pTime)));

    for (int i = 1; i <= size; i++) {
      final RS uu = u.getElement(i, 1);
      final RS hh = this.holdOutput.getElement(i, 1);
      final RS rate = inputRate.getElement(i, 1);

      if (this.initialState && u.absElementWise().max().isLessThan(this.deadBandWidth.divide(2))) {
        piece.add(0);
      } else if (rate.isGreaterThan(0)) {
        if (this.holdRateSign < 0 && uu.subtract((hh.subtract(this.deadBandWidth.divide(2)))).isLessThan(this.deadBandWidth.divide(2))) {
          piece.add(4);
        } else {
          piece.add(1);
        }
      } else {
        if (this.holdRateSign > 0 && (hh.add(this.deadBandWidth.divide(2))).subtract(uu).isLessThan(this.deadBandWidth.divide(2))) {
          piece.add(2);
        } else {
          piece.add(3);
        }
      }
    }

    return piece;
  }

  /**
   * {@inheritDoc}
   */
  @SuppressWarnings("boxing")
  public RS getDiscontinuousPoint(final RS t1, final RM u1, final RS t2, final RM u2) {
    final List<Integer> pieces1 = getPiece(t1, u1);
    final List<Integer> pieces2 = getPiece(t2, u2);

    if (pieces1.equals(pieces2)) {
      return this.sunit.getNaN();
    }

    final int number = PiecewiseUtil.getDistinctPiece(pieces1, pieces2);

    final RS uu1 = u1.getElement(number, 1);
    final RS uu2 = u2.getElement(number, 1);

    final int piece1 = pieces1.get(number - 1);
    final int piece2 = pieces2.get(number - 1);

    final boolean fromZeroToOne = piece1 == 0 && piece2 == 1;

    if (fromZeroToOne) {
      return t1.add((this.deadBandWidth.divide(2).subtract(uu1)).divide((uu2.subtract(uu1))).multiply((t2.subtract(t1))));
    }

    final boolean fromOneToTwo = piece1 == 1 && piece2 == 2;

    if (fromOneToTwo) {
      return t1.add(t2.subtract(t1).divide(2));
    }

    final boolean fromTwoToThree = piece1 == 2 && piece2 == 3;

    if (fromTwoToThree) {
      return t1.add((this.deadBandWidth.divide(2).subtract((uu1.subtract(uu2)))).divide((this.deadBandWidth.divide(2))).multiply(t2.subtract(t1)));
    }

    final boolean fromThreeToFour = piece1 == 3 && piece2 == 4;

    if (fromThreeToFour) {
      return t1.add(t2.subtract(t1).divide(2));
    }

    final boolean fromFourToOne = piece1 == 4 && piece2 == 1;

    if (fromFourToOne) {
      return t1.add(this.deadBandWidth.divide(2).subtract(uu2.subtract(uu1)).divide(this.deadBandWidth.divide(2)).multiply(t2.subtract(t1)));
    }

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

    return t1.add(t2).divide(2);
  }

  /**
   * @see org.mklab.tool.control.system.SystemOperator#setInputSize(int)
   */
  @Override
  public void setInputSize(final int size) {
    super.setInputSize(size);
    super.setOutputSize(size);
  }

  /**
   * @see org.mklab.tool.control.system.SystemOperator#setOutputSize(int)
   */
  @Override
  public void setOutputSize(final int size) {
    super.setInputSize(size);
    super.setOutputSize(size);
  }

  /**
   * @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;
    }
    Backlash<RS,RM,CS,CM> castedObj = (Backlash<RS,RM,CS,CM>)o;
    return ((this.deadBandWidth == castedObj.deadBandWidth) && (this.initialOutput == null ? castedObj.initialOutput == null : this.initialOutput.equals(castedObj.initialOutput))
        && (this.initialState == castedObj.initialState) && (this.previousOutputTime == castedObj.previousOutputTime)
        && (this.previousInput == null ? castedObj.previousInput == null : this.previousInput.equals(castedObj.previousInput))
        && (this.previousRate == null ? castedObj.previousRate == null : this.previousRate.equals(castedObj.previousRate))
        && (this.previousOutput == null ? castedObj.previousOutput == null : this.previousOutput.equals(castedObj.previousOutput))
        && (this.holdOutput == null ? castedObj.holdOutput == null : this.holdOutput.equals(castedObj.holdOutput)) && (this.holdRateSign == castedObj.holdRateSign));
  }

  /**
   * @see org.mklab.tool.control.system.SystemOperator#hashCode()
   */
  @Override
  public int hashCode() {
    int hashCode = super.hashCode();
    hashCode = 31 * hashCode + (this.deadBandWidth.hashCode() ^ (this.deadBandWidth.hashCode() >>> 32));
    hashCode = 31 * hashCode + (this.initialOutput == null ? 0 : this.initialOutput.hashCode());
    hashCode = 31 * hashCode + (this.initialState ? 1231 : 1237);
    hashCode = 31 * hashCode + (this.previousOutputTime.hashCode() ^ (this.previousOutputTime.hashCode() >>> 32));
    hashCode = 31 * hashCode + (this.previousInput == null ? 0 : this.previousInput.hashCode());
    hashCode = 31 * hashCode + (this.previousRate == null ? 0 : this.previousRate.hashCode());
    hashCode = 31 * hashCode + (this.previousOutput == null ? 0 : this.previousOutput.hashCode());
    hashCode = 31 * hashCode + (this.holdOutput == null ? 0 : this.holdOutput.hashCode());
    hashCode = 31 * hashCode + this.holdRateSign;
    return hashCode;
  }
}