#ifndef __MEMS_H__
#define __MEMS_H__

/** @file
 * @brief MEMS慣性センサ
 * 
 * MEMS慣性センサのモデル化について定義したファイル。
 * 
 * @see sensor_err.h
 */

#include "sensor_err.h"
#include "util/util.h"

/**
 * @brief MEMS加速度計
 * 
 * MEMS加速度計クラス。
 * ランダムドリフトの効果は無視できると仮定し、
 * ノイズはホワイトノイズ成分のみで構成されています。
 * 各定数はAnalog DevicesのADXL103のカタログスペックを基準に
 * 設定しています。
 * 
 * なお、ホワイトノイズの標準偏差 @f$ \sigma_{\mathrm{WN}} @f$ と
 * 観測時間間隔 @f$ \tau @f$の間には
 * @f[
 *    \log \sigma_{\mathrm{WN}} 
 *      = k_\mathrm{slope} \log \tau + C_{\mathrm{intercept}}
 * @f]
 * の関係があります。
 * 
 * @see http://www.analog.com/jp/prod/0,2877,ADXL103,00.html
 */
class MEMS_Accelerometer : public SensorErrorModel<double>{
  
  public:
    // ノイズ関連
    static const double WN_SLOPE;     ///< @f$ k_\mathrm{slope} @f$、ホワイトノイズ成分の観測時間に対する傾き
    static const double WN_INTERCEPT; ///< @f$ C_\mathrm{intercept} @f$、ホワイトノイズ成分の観測時間に対する切片

    // その他特性
    static const double SF; ///< スケールファクタ

  public:
    MEMS_Accelerometer() : SensorErrorModel<double>(0, 0){} /**< コンストラクタ */
    double convert(const double &true_value, const double &delta){
      return rand_regularized(0, pow(delta, WN_SLOPE) * WN_INTERCEPT) + true_value;
    }
};

const double MEMS_Accelerometer::WN_SLOPE = -0.5;
const double MEMS_Accelerometer::WN_INTERCEPT = 300E-6 * 10;
const double MEMS_Accelerometer::SF = 1;

/**
 * @brief MEMSジャイロ
 * 
 * MEMSジャイロクラス。
 * ノイズはランダムドリフト成分とホワイトノイズ成分で構成されています。
 * 各定数はAnalog DevicesのADXRS150のカタログスペックを基準に
 * 設定しています。
 * 
 * ホワイトノイズ成分についてはMEMS_Accelerometerを参考にしてください。
 * 
 * @see MEMS_Accelerometer
 * @see http://www.analog.com/en/prod/0,2877,ADXRS150,00.html
 */
class MEMS_Gyro : public SensorErrorModel<double>{
  
  public:
    // ノイズ関連
    static const double BETA;         ///< @f$ \beta @f$、ランダムドリフトの減衰効果
    static const double ROOT_N;       ///< @f$ \sqrt{N} @f$、ランダムドリフトの大きさ
    static const double WN_SLOPE;     ///< @f$ k_\mathrm{slope} @f$、ホワイトノイズ成分の観測周波数に対する傾き
    static const double WN_INTERCEPT; ///< @f$ C_\mathrm{intercept} @f$、ホワイトノイズ成分の観測時間に対する切片

    // その他特性
    static const double SF;           ///< スケールファクタ[V / rad / sec]

  public:
    MEMS_Gyro() : SensorErrorModel<double>(ROOT_N, BETA){}
    
    /**
     * ホワイトノイズ成分を返します
     * 
     * @param delta 時間間隔[sec]
     * @return (double) ホワイトノイズ[rad/sec]
     */
    double pure_WN(const double &delta){
      return rand_regularized(0, pow(delta, WN_SLOPE) * WN_INTERCEPT) / SF;
    }
    
    /**
     * ランダムドリフト成分を返します
     * 
     * @param delta 時間間隔[sec]
     * @return (double) ランダムドリフト[rad/sec]
     */
    double pure_RD(const double &delta){
      return next(delta) / SF;
    }
    
    /**
     * 真値→出力値にコンバート
     * 
     * @param true_value 真値[rad/sec]
     * @param delta 時間間隔[sec]
     * @return (double) 出力値[rad/sec]
     */
    double convert(const double &true_value, const double &delta){
      return pure_WN(delta) + pure_RD(delta) + true_value;
    }
};

// 実測値を元に
const double MEMS_Gyro::BETA = 0.016;
const double MEMS_Gyro::ROOT_N = 0.00009;
const double MEMS_Gyro::WN_SLOPE = -0.438498;
const double MEMS_Gyro::WN_INTERCEPT = 0.000476415;
const double MEMS_Gyro::SF = 12.5E-3 / (PI / 180);   // [V / rad / sec]

/**
 * @brief MEMS加速度計 その2
 * 
 * MEMS加速度計クラス。
 * MEMS_Accelerometerよりも性能がオーダーで1桁良いものを仮定しています。
 * 
 * @see MEMS_Accelerometer
 */
class Better_Accelerometer : public SensorErrorModel<double>{
  
  public:
    // ノイズ関連
    static const double WN_SLOPE;
    static const double WN_INTERCEPT;

    // その他特性
    static const double SF;

  public:
    Better_Accelerometer() : SensorErrorModel<double>(0, 0){}
    double convert(const double &true_value, const double &delta){
      return rand_regularized(0, pow(delta, WN_SLOPE) * WN_INTERCEPT) + true_value;
    }
};

// オーダーで1桁良い
const double Better_Accelerometer::WN_SLOPE = -0.5;
const double Better_Accelerometer::WN_INTERCEPT = 30E-6 * 10;
const double Better_Accelerometer::SF = 1;

/**
 * @brief MEMSジャイロ その2
 * 
 * MEMSジャイロクラス。
 * MEMS_Gyroよりも性能がオーダーで1桁良いものを仮定しています。
 * 
 * @see MEMS_Gyro
 */
class Better_Gyro : public SensorErrorModel<double>{
  
  public:
    // ノイズ関連
    static const double BETA;
    static const double ROOT_N;
    static const double WN_SLOPE;
    static const double WN_INTERCEPT;

    // その他特性
    static const double SF;

  public:
    Better_Gyro() : SensorErrorModel<double>(ROOT_N, BETA){}
    /**
     * 真値→出力値にコンバート
     * 
     * @param true_value 真値[rad/sec]
     * @param delta 時間間隔[sec]
     * @return (double) 出力値[rad/sec]
     */
    double convert(const double &true_value, const double &delta){
      return (next(delta) 
                + rand_regularized(0, pow(delta, WN_SLOPE) * WN_INTERCEPT)) / SF * PI / 180
                + true_value;
    }
};

// オーダーで1桁良い
const double Better_Gyro::BETA = 0.016;
const double Better_Gyro::ROOT_N = 0.000008;
const double Better_Gyro::WN_SLOPE = -0.438498;
const double Better_Gyro::WN_INTERCEPT = 0.0000476415;
const double Better_Gyro::SF = 12.5E-3;   // [V / deg / sec]


#endif /* __MEMS_H__ */