//-----------------------------------------------------------
//  出力を 4 倍にアップサンプリングするクラス
//  補間処理で使うフィルタ：直線位相 FIR フィルタ
//  
//  2022/09/29, Copyright (c) 2022 MIKAMI, Naoki
//-----------------------------------------------------------

#include "MultirateLiPh.hpp"
#include "MultirateLiPhCoefs.hpp"   // デフォルトの補間用フィルタの係数

namespace Mikami
{
    // コンストラクタ（デフォルトの補間フィルタの係数を使う場合）
    MultirateLiPh::MultirateLiPh(float fSampling,
                                 PinName pin, ADC_TypeDef* const adc)
        : indexW_(0), FIR_LOOP_((ORDER_+2)/UR_), CENTER_((ORDER_+2)/(UR_*2)),
          vn_((ORDER_+2)/UR_, 0.0f), h1_((ORDER_+2)/UR_, HK1_),
          h2_((ORDER_+2)/UR_, HK2_), h3_((ORDER_+2)/UR_, HK3_)
    {   Init(fSampling, pin, adc); }

    // コンストラクタ（デフォルト以外の補間フィルタの係数を使う場合）
    MultirateLiPh::MultirateLiPh(float fSampling, int order,
                                 const float hk1[], const float hk2[],
                                 const float hk3[],
                                 PinName pin, ADC_TypeDef* const adc)
        : indexW_(0), FIR_LOOP_((order+2)/UR_), CENTER_((order+2)/(UR_*2)),
          vn_((order+2)/UR_, 0.0f), h1_((order+2)/UR_, hk1),
          h2_((order+2)/UR_, hk2), h3_((order+2)/UR_, hk3)
    {   Init(fSampling, pin, adc); }

    // 標本化の実行開始
    void MultirateLiPh::Start(void (*Func)())
    {
        // CAN2_TX によるソフトウェア割り込みに対応する設定
        NVIC_SetVector(CAN2_TX_IRQn, (uint64_t)Func);   // OK
        NVIC_EnableIRQ(CAN2_TX_IRQn);       

        // AD 変換器を使うための準備
        wait_us(1000);    // ある程度の待ち時間が必要
        adc_->SetIntrVec(&MultirateLiPh::AdcIsr);  // AD 変換終了に対応する ISR の設定
    }

    // 補間用フィルタを実行し，処理結果を出力用バッファへ書き込む
    void MultirateLiPh::Output(float yn)
    {
        vn_[0] = yn;    // 補間フィルタ用バッファの先頭に書き込む

        buf_[ModIndex(indexW_)] = vn_[CENTER_];
        buf_[ModIndex(indexW_)] = Interpolate(h1_);
        buf_[ModIndex(indexW_)] = Interpolate(h2_);
        buf_[ModIndex(indexW_)] = Interpolate(h3_);

        for (int k=FIR_LOOP_-1; k>0; k--) vn_[k] = vn_[k-1];
    }

    // ADC 変換終了割り込みに対する割り込みサービス･ルーチン
    void MultirateLiPh::AdcIsr()
    {
        static int count = 0;

        xn_ = adc_->Read();     // AD変換器の値を読み込む
        dac_.Write(buf_[ModIndex(indexR_)]);  // 出力バッファの内容を DAC へ書き込む

        if (count == 0)                 // AD変換器からの入力信号は４回に１回使う
            NVIC->STIR = CAN2_TX_IRQn;  // ソフトウェア割込み発生，信号処理を起動
        count = ++count & MASK_UR_; // 入力を４回に１回行うための処理
    }

    // 補間用 FIR フィルタ
    float MultirateLiPh::Interpolate(const float hk[]) const
    {
        float y = 0;
        for (int n=0; n<FIR_LOOP_; n++) y += vn_[n]*hk[n];
        return y;
    }

    // ADC の初期化と割込み優先順位の設定
    void MultirateLiPh::Init(float fSampling, PinName pin, ADC_TypeDef* const adc)
    {
        NVIC_SetPriority(ADC_IRQn, 0);      // ADC 終了割り込み：最優先
        NVIC_SetPriority(CAN2_TX_IRQn, 1);  // ソフトウェア割り込みで使用：２番目に優先

        adc_ = new DspAdcIntr(fSampling*UR_, pin, adc);
    }

    // static メンバの実体の宣言/初期化
    DspAdcIntr *MultirateLiPh::adc_;     // AD変換器のオブジェクトのポインタ
    DspDac MultirateLiPh::dac_;          // DA変換器のオブジェクト
    Array<float> MultirateLiPh::buf_(2*UR_, 0.0f);
    int MultirateLiPh::indexR_ = UR_;
    float MultirateLiPh::xn_;
}
