#define _USE_MATH_DEFINES //for Visual Studio

#include "ANCcontroller.h"
WaveGenerator::WaveGenerator()
{
  CreateSinWave();
  CreateChirpWave();
}
//sin波作る
void WaveGenerator::CreateSinWave() {
  auto rad_step = 2 * M_PI * FREQ / SAMPLE_RATE;
  for (int i = 0; i < SAMPLE_RATE / FREQ; i++)
  {
    wave_sin.push_back(AS_16BIT_MAX_NUM * sin(rad_step * i) *GAIN);
  }
}

void WaveGenerator::CreateChirpWave() {
  std::deque<float> CharpBase(int(SAMPLE_RATE * tLength / 2), 0);
  std::deque<float>  waveChirpSignal;
  //float f = fMax;
  float fz = fMin;
  float t = 0;
  float t_step = 1.0 / SAMPLE_RATE;
  float f_step = ((fMax - fMin) / (tLength * 0.5));

  for (auto& w : CharpBase)
  {
    w = cos(2.0 * M_PI * ((fz * t) + (f_step * 0.5 * t * t))); //アップチャープ生成
    t += t_step;
  }

  float w = 0;//波形を連続にする
  while (-0.99 < w && w < 0.99) {
    w = cos(2.0 * M_PI * ((fz * t) + (f_step * 0.5 * t * t))); //アップチャープ生成
    t += t_step;
    CharpBase.emplace_back(w);//アップチャープ挿入
  }

  waveChirpSignal = CharpBase;
  //ダウンチャープ生成
  //こう…イテレーター使うより…早い気がしたねんな…
  for (auto i = CharpBase.size() - 1; i > 0; i--) {

    waveChirpSignal.emplace_back(CharpBase[i]);//挿入
  }


  for (const auto & w : waveChirpSignal) {
    waveChirp.emplace_back(int(w * AS_16BIT_MAX_NUM * GAIN));
  }
}


//16bit情報に変換したsinを吐き出す
int16_t WaveGenerator::playWaveSin()
{
  auto out = wave_sin[sound_cnt % wave_sin.size()];
  sound_cnt++;
  return out;
}
//16bit情報に変換したチャープ信号を吐き出す
int16_t WaveGenerator::playWaveCharp()
{
  auto out = waveChirp[sound_cnt % waveChirp.size()];
  sound_cnt++;
  return out;
}



ANC_DataBase::ANC_DataBase() {
  squaredXBuf.assign(WaveBufLen, 0);
  inputXBuf.assign(WaveBufLen, 0);
  filteredXBuf.assign(WaveBufLen, 0);
  outputYBuf.assign(WaveBufLen, 0);

}

template <class T>
void ANC_DataBase::shiftBuffer(T inValue, std::deque<T>& ioBuffer)
{
  if (isnan(inValue))inValue = 0;//数字のみ受け取り
  ioBuffer.emplace_front(inValue);//行頭に追加
  ioBuffer.pop_back();//行末を消す
}

template<class T, class U, size_t FilterSize >
float ANC_DataBase::calcFIR(const std::deque<T>& inDataBuf, inoArray<U, FilterSize>& filter_ary)
{
  //assert(inDataBuf.size() >= filter_ary.size());

  //内積
  float outValue = std::inner_product(
                     filter_ary.begin(), filter_ary.end(), // ひとつ目のベクトル
                     inDataBuf.begin(),          // ふたつ目のベクトル(v以上の次元数を持つこと)
                     0.0f);//ここの型にキャストされる

  return outValue;
}

template<class T, size_t FilterSize >
void ANC_DataBase::calcAptiveFilter(float inErrValue, inoArray<T, FilterSize> &ioW, float inStepSize, const std::deque<T>& inXBuf, const std::deque<T>& xAvaHelp )
{

  auto xpower = 1.0e-9;
  xpower += std::accumulate(xAvaHelp.begin(), xAvaHelp.begin() + FilterSize , xpower); //対象フィルタサイズ分のパワーを計算


  //conf = conf - inStepSize * inErrValue * inXBuf[idx] / xpower;
  float Alpha = inStepSize * inErrValue / xpower;//定数部分を先に計算し、キャッシュして高速化
  int idx = 0;
  for (auto& conf : ioW)
  {
    //e(n)の導出を+,LMS更新式を-にするとノイズキャンセリング同定になる（正負を反転させるとシステム同定）

    conf = conf - Alpha * inXBuf[idx];
    idx++;
  }
}

template <class T>
void ANC_DataBase::InitFilter(T &Filter)
{
  Filter.fill(0);

  inputXBuf.assign(WaveBufLen, 0);
  squaredXBuf.assign(WaveBufLen, 0);
}


void ANC_DataBase::calcANCwave_IMC(float errGain) {
  //消音波を出力する
  //Filtered N-LMS with IMC-FeedBack
  // coefC : 同定により決定。以下の処理においては定数的。
  // coefW : 適応フィルタ係数。動的に変化する。
  // coefH : エコーキャンセル(ハウリング減退)を行う
  //errGain: 誤差信号は観測値となる e = d(所望信号) + y(CtrSpk_out)


  shiftBuffer(errGain - yc, inputXBuf);//音声バッファにデータを溜め込み、フィルタの適応対象とする

  shiftBuffer(inputXBuf[0] * inputXBuf[0], squaredXBuf);//NLMS高速化のためxbufの各要素の二乗を作っておく。

  //X(n)をロハ信号化
  shiftBuffer(calcFIR(inputXBuf, coefC), filteredXBuf);

  // 適応フィルタをかけ、制御信号y(n)を発行する
  y = calcFIR(inputXBuf, coefW);
  shiftBuffer(y, outputYBuf);


  //二次経路の影響を受けるycを予想する
  yc = calcFIR(outputYBuf, coefC);

  //Filtered N-LMS by IMC
  calcAptiveFilter(errGain, coefW, StepSizeW, filteredXBuf, squaredXBuf);
  if (abs(y) > 32767) InitFilter(coefW);//出力値が大きすぎる場合系が不安定になっているのでリセット
}

void ANC_DataBase::calcANCwave_FF(float errGain,float refGain) {
}

void ANC_DataBase::calcANCwave_HI(float errGain) {
}


//入力波形を利用して任意の経路の伝達関数を推測(とりあえずチャープ信号がおすすめ)
template<class T, size_t FilterSize >
void ANC_DataBase::IdentTransferFunction(float errGain, float TrainingWave, inoArray<T, FilterSize>& TF, float inStepSize) {


  shiftBuffer(TrainingWave, inputXBuf);//出力した信号をトレーニングデータとする
  shiftBuffer(TrainingWave * TrainingWave, squaredXBuf);//NLMS高速化のためxbufの各要素の二乗を作っておく。

  //CofH測定
  auto InternalErrGain = errGain + yc;//システム内部でCofc利用のキャンセリングを行う。
  yc = calcFIR(inputXBuf, TF);
  calcAptiveFilter(InternalErrGain, TF, inStepSize, inputXBuf, squaredXBuf);

  if (abs(yc) > 32767) InitFilter(coefC);//出力値が大きすぎる場合系が不安定になっているのでリセット

}


ANC_Controller::ANC_Controller() {

}

void ANC_Controller::calc2ndPassTF(float errGain, float TrainingWave) {

  DB.IdentTransferFunction(errGain, TrainingWave , DB.coefC, DB.StepSizeC);

}

void ANC_Controller::calcEchoPassTF(float errGain, float TrainingWave) {

  DB.IdentTransferFunction(errGain, TrainingWave , DB.coefH, DB.StepSizeH);

}

float ANC_Controller::calcANCwave(const char methodNum , float errGain , float refGain = 0 ) {
  switch (methodNum)
  {
    case AS_IMC:
      DB.calcANCwave_IMC(errGain);
      break;

    case AS_FF:
      DB.calcANCwave_FF(errGain, refGain);
      break;

    case AS_HI:
      DB.calcANCwave_HI(errGain);
      break;

    default:
      break;
  }

  return DB.y;
}