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

HardwareTimer Timer1(TIM1);
// the setup function runs once when you press reset or power the board
uint8_t sw = LOW;
const int audioInErrPin = A6;    // select the input pin for the potentiometer
const int audioInRefPin = A7;    // select the input pin for the potentiometer

const int biasPin = D27;
const int audioOutPin = A13;

const int Q_BIT_NUM = 12;
const int AS_SYS_SAMPLERATE = 4000*2;
const int AS_MAX_BIT_NUM = 4095;
//const int AUDIO_BIAS = 2048;//3.3V中の1.65V(12bit) 3.3Vの中心で交流を受け付け+-1.65Vのレンジを作る

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

float32_t inValueErrf32=0;
float32_t inValueReff32=0;

float32_t ancSig=0;

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);
  Serial.println(t2-t1);
  //Serial.println(ancSig);
  //Serial.println(audioOutValue);
}

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

  inValueErr = analogRead(audioInErrPin);//12bitで読み取る　range:+-1
  inValueErrf32 = (((float32_t)inValueErr - 2048) /2048);
  inValueRef = analogRead(audioInRefPin);//12bitで読み取る　range:+-1
  inValueReff32 = (((float32_t)inValueRef - 2048) /2048);
  analogWrite(audioOutPin,audioOutValue);
  sendTimingCnt++;
  calocReadyFlg = true;
}


void audioreader_debug() {
  t1 = micros();

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

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

void setup() {
  Serial.begin(9600);
  // initialize digital pin LED_BUILTIN as an output.
  pinMode(LED_GREEN, OUTPUT);
  pinMode(LED_RED, OUTPUT);
  pinMode(biasPin,OUTPUT);
  digitalWrite(LED_RED,LOW);
  digitalWrite(biasPin,HIGH);

  pinMode(audioOutPin, OUTPUT);
  //pinMode(biasPin, OUTPUT);

  pinMode(audioInErrPin,INPUT_ANALOG);
  pinMode(audioInRefPin,INPUT_ANALOG);

  analogWriteResolution(Q_BIT_NUM);//analogwriteを12bit(MAX4095)で利用できるように設定
  analogReadResolution(Q_BIT_NUM);
  //analogWrite(biasPin,AUDIO_BIAS);//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();                  // タイマースタート


  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) {
    //伝達関数の推定をしている。非同期処理なのでタイミングがずれるかも
    if(calocReadyFlg){
      //ANC.calc2ndPassTF(inValueErr-AUDIO_BIAS,audioOutValue);
      //audioOutValue = s_sound.playWaveCharp(s_sound.UNSINGED_INT_WAVE);

      calocReadyFlg=false;
    }

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

  else if (anc_state == ANC.ACTIVATION_ANC) {

    if(calocReadyFlg){
      t1 = micros();
      ancSig = ANC.calcANCwave(AS_NP_FF,inValueErrf32,inValueReff32);
      t2 = micros();
      audioOutValue = (int)((ancSig+1)/2 * AS_MAX_BIT_NUM);
      //audioOutValue = s_sound.playWaveSin(UNSINGED_INT_WAVE);

      //audioOutValue = 0;

      calocReadyFlg=false;
    }

  }

}