#ifndef __MISC_H
#define __MISC_H

/** @file
 * @brief 操舵入力の調整方法やノイズジェネレータなどについて記述
 *
 */

#include "util/util.h"

/**
 * Control surface class
 *
 * deflection = parameter*(input pulse width - neutral pulse width),
 * with rate limitation (rate limit = max deg of change / time step)
 * no saturation
 */
template<class FloatT = double>
class ControlInput{
  protected:
    FloatT lin_parameter, old_deflection, rate_limit;
    FloatT neutral_pulse;

    int sign(const FloatT &d){
      return (d > 0) ? 1 : -1;
    }
  public:
    /**
     * 舵面の設定
     * 
     * @param up_pulse 正の最大偏角でのパルス長
     * @param down_pulse 負の最大偏角でのパルス長
     * @param up_deflection 正の最大偏角
     * @param down_deflection 負の最大偏角
     * @rl レートリミット(ゼロ以下の値に設定するとリミットなし)
     */
    ControlInput(
        const FloatT &up_pulse, const FloatT &down_pulse,
        const FloatT &up_deflection, const FloatT &down_deflection, const FloatT &rl)
        : neutral_pulse(down_pulse),
          lin_parameter((up_deflection - down_deflection) / (up_pulse - down_pulse)),
          rate_limit(rl),
          old_deflection(0) {}
    /**
     * 舵面の設定
     * 
     * @param up_pulse 正の最大偏角でのパルス長
     * @param down_pulse 負の最大偏角でのパルス長
     * @param n_pulse 中立位置でのパルス長
     * @param up_deflection 正の最大偏角
     * @param down_deflection 負の最大偏角
     * @rl レートリミット(ゼロ以下の値に設定するとリミットなし)
     */
    ControlInput(
        const FloatT &up_pulse, const FloatT &down_pulse, const FloatT &n_pulse,
        const FloatT &up_deflection, const FloatT &down_deflection, const FloatT &rl)
        : neutral_pulse(n_pulse),
          lin_parameter(sqrt(pow(up_deflection / (up_pulse - n_pulse), 2)
              + pow(down_deflection / (down_pulse - n_pulse), 2))),
          rate_limit(rl),
          old_deflection(0) {}
    /**
     * 舵面の設定
     * 
     * @param n_pulse 中立位置でのパルス長
     * @param sf スケールファクタ
     * @rl レートリミット(ゼロ以下の値に設定するとリミットなし)
     */
    ControlInput(
        const FloatT &n_pulse, const FloatT &sf, const FloatT &rl)
        : neutral_pulse(n_pulse),
          lin_parameter(sf),
          rate_limit(rl),
          old_deflection(0) {}

    FloatT deflection_no_limit(const FloatT &pulse_width){
      return lin_parameter * (pulse_width - neutral_pulse);
    }

    FloatT deflection_init(const FloatT &pulse_width){
      return old_deflection = deflection_no_limit(pulse_width);
    }

    FloatT deflection(const FloatT &pulse_width){
      FloatT virtual_deflection(deflection_no_limit(pulse_width));
      if((rate_limit > 0) 
          && (fabs(virtual_deflection - old_deflection) > rate_limit)){
        virtual_deflection = old_deflection 
            + rate_limit * sign(virtual_deflection - old_deflection);
      }
      old_deflection = virtual_deflection;
      return virtual_deflection;
    }
};

/**
 * ピッチ用ノイズ生成器
 * Random Walk FM + White Noise
 * "SIMULATION OF OSCILLATOR NOISE" by James A. Barnes
 * 38th Annual Frequency Control Symposium - 1984
 * を参考に
 */
template<class FloatT = double>
class PitchNoiseGenerator{
  protected:
    static const FloatT mu0, mu1, mu2;
    FloatT x_n1, x_n2;
    FloatT a_n, a_n1;
  public:
    FloatT next(){
      FloatT x_n((x_n1 * 2 - x_n2) * (1.0 - 8E-6) + a_n + 0.286 * a_n1);
      x_n2 = x_n1;
      x_n1 = x_n;
      a_n1 = a_n;
      a_n = rand_regularized(0, mu1);
      return x_n + rand_regularized(0, mu2);
    }
    PitchNoiseGenerator()
        : x_n1(rand_regularized(0, mu0)), x_n2(x_n1),
          a_n(rand_regularized(0, mu1)), a_n1(rand_regularized(0, mu1)) {
      for(int i(0); i < 1000; i++){next();}
    }
};
template<class FloatT>
const FloatT PitchNoiseGenerator<FloatT>::mu0 = 0.35;
template<class FloatT>
const FloatT PitchNoiseGenerator<FloatT>::mu1 = 2.9E-6;
template<class FloatT>
const FloatT PitchNoiseGenerator<FloatT>::mu2 = 1E-3;


/**
 * ピッチ角速度用ノイズ生成器
 * White Noise FM + White Noise
 * "SIMULATION OF OSCILLATOR NOISE" by James A. Barnes
 * 38th Annual Frequency Control Symposium - 1984
 * を参考に
 */
template<class FloatT = double>
class QNoiseGenerator{
  protected:
    static const FloatT mu0, mu1, mu2;
    FloatT x_n1;
    FloatT a_n;
  public:
    FloatT next(){
      FloatT x_n(x_n1 * (1. - 3E-3) + a_n);
      x_n1 = x_n;
      a_n = rand_regularized(0, mu1);
      return x_n + rand_regularized(0, mu2);
    }
    QNoiseGenerator()
        : x_n1(rand_regularized(0, mu0)),
          a_n(rand_regularized(0, mu1)) {
      for(int i(0); i < 100; i++){next();}
    }
};
template<class FloatT>
const FloatT QNoiseGenerator<FloatT>::mu0 = 0.35;
template<class FloatT>
const FloatT QNoiseGenerator<FloatT>::mu1 = 6E-1;
template<class FloatT>
const FloatT QNoiseGenerator<FloatT>::mu2 = 5E-1;


#endif /* __MISC_H */