#include "ANCcontroller.h"
//#include <STM32ADC.h>
//#include <libmaple/adc.h>
//#include <CMSIS_DSP.h>

HardwareTimer Timer1(TIM1);
// the setup function runs once when you press reset or power the board
uint8_t sw = LOW;
int sensorPin = A6;    // select the input pin for the potentiometer
int biasPin = A12;
int audioOutPin = A13;

const int Q_BIT_NUM = 12;
const int AS_SYS_SAMPLERATE = 4000*2;
const int AUDIO_BIAS = 2482;//3.3V中の2V(12bit)

char anc_state;
bool Sendflg = true;
bool calocReadyFlg = false;
int sendTimingCnt=0;
int audioInValue = 0;  // variable to store the value coming from the sensor
int audioOutValue = 0;

uint32_t t1,t2;

WaveGenerator s_sound(AS_SYS_SAMPLERATE,Q_BIT_NUM);
ANC_Controller ANC;

void sendDebugdat(){
  sw ^=1;
  digitalWrite(LED_GREEN, sw);
  printf("%d\n",audioInValue);
}

//adcは最適値を設定すれば高速化できる余地あり現在30μs
void audioreader() {

  audioInValue = analogRead(sensorPin);//多分12bitで読み取る

  analogWrite(audioOutPin,audioOutValue);
  sendTimingCnt++;
  calocReadyFlg = true;
}


void audioreader_debug() {
  t1 = micros();

  audioInValue = analogRead(sensorPin);//多分12bitで読み取る
  sendTimingCnt++;

  t2 = micros();
  printf(" t=%d \n",t2-t1);
}

void setup() {
  // initialize digital pin LED_BUILTIN as an output.
  pinMode(LED_GREEN, OUTPUT);
  pinMode(audioOutPin, OUTPUT);
  pinMode(biasPin, OUTPUT);
  pinMode(sensorPin,INPUT_ANALOG);

  analogWriteResolution(Q_BIT_NUM);//analogwriteを12bit(MAX4095)で利用できるように設定
  analogReadResolution(Q_BIT_NUM);
  analogWrite(biasPin,2482);//MAX3.3vの2.0V


  Timer1.pause();                   // タイマー停止

  Timer1.setOverflow(AS_SYS_SAMPLERATE,HERTZ_FORMAT);//サンプルレートHzでよびだす
  Timer1.attachInterrupt(           // 割り込みハンドラの登録
      audioreader                  // 呼び出す関数
    );

  Timer1.setCount(0);               // カウンタを0に設定
  Timer1.refresh();                 // タイマ更新
  Timer1.resume();                  // タイマースタート
  tone(PB4,200);
  analogWrite(biasPin,AUDIO_BIAS);

  anc_state = ANC.IDENTIFICAT_COEFC;
}


// the loop function runs over and over again forever
void loop() {
  //デバグ情報の送信
  if(sendTimingCnt >= AS_SYS_SAMPLERATE / 4){
    sendDebugdat();
    sendTimingCnt=0;
  }


  //ANC状態の管理
  if (anc_state == ANC.IDENTIFICAT_COEFC) {
    t1 = micros();
    //伝達関数の推定をしている。非同期処理なのでタイミングがずれるかも
    if(calocReadyFlg){
      ANC.calc2ndPassTF(audioInValue-AUDIO_BIAS,audioOutValue);
      audioOutValue = s_sound.playWaveCharp(s_sound.UNSINGED_INT_WAVE);

      calocReadyFlg=false;
    }
    t2 = micros();
    printf(" t=%d \n",t2-t1);

  //起動から15sでモード移行 正確に処理したい場合はタイマーを使おう！
    if (millis() > 15 * 1000) {
      ANC.printCoefC();
      anc_state = ANC.ACTIVATION_ANC;
    }
  }
  else if (anc_state == ANC.ACTIVATION_ANC) {
    audioOutValue = 0;
  }

}